aleks/hoverboard-sideboard-hack-STM
1
0
Fork 0
mirror of https://github.com/EFeru/hoverboard-sideboard-hack-STM.git synced 2025-08-17 00:56:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

Visual update and minor fixes

Browse Source 
- minor bug fixes
- added LED board picture
This commit is contained in:
EmanuelFeru
2020-02-13 18:19:18 +01:00
parent 4a470e97b6
commit e849a9ded8
19 changed files with 493 additions and 464 deletions
Download Patch File Download Diff File Expand all files Collapse all files

97
Src/main.c
View File

@@ -59,10 +59,10 @@ void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
__HAL_UART_FLUSH_DRREGISTER(&huart2); // Clear the buffer to prevent overrun
__HAL_UART_FLUSH_DRREGISTER(&huart2); // Clear the buffer to prevent overrun
#ifdef SERIAL_DEBUG
if (rxBuffer != '\n' && rxBuffer != '\r') { // Do not accept 'new line' (ascii 10) and 'carriage return' (ascii 13) commands
if (rxBuffer != '\n' && rxBuffer != '\r') { // Do not accept 'new line' (ascii 10) and 'carriage return' (ascii 13) commands
userCommand = rxBuffer;
}
#endif
@@ -88,8 +88,8 @@ typedef struct{
uint16_t start;
int16_t roll;
int16_t pitch;
int16_t yaw;
uint16_t sensors;
int16_t yaw;
uint16_t sensors;
uint16_t checksum;
} SerialSideboard;
SerialSideboard Sideboard;
@@ -97,16 +97,17 @@ SerialSideboard Sideboard;
#ifdef SERIAL_FEEDBACK
typedef struct{
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedR;
int16_t speedL;
int16_t speedR_meas;
int16_t speedL_meas;
int16_t batVoltage;
int16_t boardTemp;
int16_t checksum;
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedR;
int16_t speedL;
int16_t speedR_meas;
int16_t speedL_meas;
int16_t batVoltage;
int16_t boardTemp;
uint16_t cmdLed;
uint16_t checksum;
} SerialFeedback;
SerialFeedback Feedback;
SerialFeedback NewFeedback;
@@ -118,8 +119,8 @@ static uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial comman
extern MPU_Data mpu; // holds the MPU-6050 data
ErrorStatus mpuStatus = SUCCESS; // holds the MPU-6050 status: SUCCESS or ERROR
uint8_t sensor1, sensor2; // holds the sensor1 and sensor 2 values
FlagStatus sensor1, sensor2; // holds the sensor1 and sensor 2 values
FlagStatus sensor1_read, sensor2_read; // holds the instantaneous Read for sensor1 and sensor 2
static uint32_t main_loop_counter; // main loop counter to perform task squeduling inside main()
/* USER CODE END 0 */
@@ -170,11 +171,15 @@ int main(void)
HAL_UART_Receive_DMA (&huart2, (uint8_t *)&NewFeedback, sizeof(NewFeedback));
#endif
introDemoLED(100); // Short LEDs intro demo with 100 ms delay. This also gives some time for the MPU-6050 to initialize.
if(mpu_config()) { // IMU MPU-6050 config
intro_demo_led(100); // Short LEDs intro demo with 100 ms delay. This also gives some time for the MPU-6050 to power-up.
if(mpu_config()) { // IMU MPU-6050 config
mpuStatus = ERROR;
}
mpu_handle_input('h'); // Print the User Help commands to serial
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); // Turn on RED LED
}
else {
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); // Turn on GREEN LED
}
mpu_handle_input('h'); // Print the User Help commands to serial
/* USER CODE END 2 */
@@ -186,11 +191,15 @@ int main(void)
// ==================================== LEDs Handling ====================================
// HAL_GPIO_TogglePin(LED4_GPIO_Port, LED4_Pin); // Toggle BLUE1 LED
if (SUCCESS == mpuStatus) {
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); // Turn on GREEN LED
} else {
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET); // Turn on RED LED
}
#ifdef SERIAL_FEEDBACK
if (!timeoutFlagSerial) {
if (Feedback.cmdLed & LED1_SET) { HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED2_SET) { HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED3_SET) { HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED4_SET) { HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED5_SET) { HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET); }
}
#endif
// ==================================== USER Handling ====================================
#ifdef SERIAL_DEBUG
@@ -209,35 +218,43 @@ int main(void)
mpu_get_data();
}
// Print MPU data to Console
if (main_loop_counter % 50 == 0 && SUCCESS == mpuStatus) {
if (main_loop_counter % 50 == 0) {
mpu_print_to_console();
}
// ==================================== SENSORS Handling ====================================
sensor1_read = HAL_GPIO_ReadPin(SENSOR1_GPIO_Port, SENSOR1_Pin);
sensor2_read = HAL_GPIO_ReadPin(SENSOR2_GPIO_Port, SENSOR2_Pin);
// SENSOR1
if (HAL_GPIO_ReadPin(SENSOR1_GPIO_Port, SENSOR1_Pin)) {
sensor1 = 1;
// Sensor ACTIVE: Do something here
if (sensor1 == RESET && sensor1_read == SET) {
sensor1 = SET;
// Sensor ACTIVE: Do something here (one time task on activation)
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_SET);
consoleLog("-- SENSOR 1 Active --\n");
HAL_Delay(50);
} else {
sensor1 = 0;
consoleLog("-- SENSOR 1 Active --\n");
} else if(sensor1 == SET && sensor1_read == RESET) {
sensor1 = RESET;
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET);
}
// SENSOR2
if (HAL_GPIO_ReadPin(SENSOR2_GPIO_Port, SENSOR2_Pin)) {
sensor2 = 1;
// Sensor ACTIVE: Do something here
if (sensor2 == RESET && sensor2_read == SET) {
sensor2 = SET;
// Sensor ACTIVE: Do something here (one time task on activation)
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_SET);
consoleLog("-- SENSOR 2 Active --\n");
HAL_Delay(50);
} else {
sensor2 = 0;
} else if (sensor2 == SET && sensor2_read == RESET) {
sensor2 = RESET;
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET);
}
if (sensor1 == SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
if (sensor2 == SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
// ==================================== SERIAL Tx/Rx Handling ====================================
@@ -257,7 +274,7 @@ int main(void)
#ifdef SERIAL_FEEDBACK
uint16_t checksum;
checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp);
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.cmdLed);
if (NewFeedback.start == SERIAL_START_FRAME && NewFeedback.checksum == checksum) {
if (timeoutFlagSerial) { // Check for previous timeout flag
if (timeoutCntSerial-- <= 0) // Timeout de-qualification

336
Src/mpu6050.c
View File

@@ -521,13 +521,13 @@ const struct hw_s hw = {
const struct test_s test = {
.gyro_sens = 32768/250,
.accel_sens = 32768/2, // FSR = +-2G = 16384 LSB/G
.accel_sens = 32768/2, // FSR = +-2G = 16384 LSB/G
.reg_rate_div = 0, // 1kHz.
.reg_lpf = 2, // 92Hz low pass filter
.reg_gyro_fsr = 0, // 250dps.
.reg_accel_fsr = 0x0, // Accel FSR setting = 2g.
.wait_ms = 200, // 200ms stabilization time
.packet_thresh = 200, // 200 samples
.packet_thresh = 200, // 200 samples
.min_dps = 20.f, // 20 dps for Gyro Criteria C
.max_dps = 60.f, // Must exceed 60 dps threshold for Gyro Criteria B
.max_gyro_var = .5f, // Must exceed +50% variation for Gyro Criteria A
@@ -2182,17 +2182,17 @@ static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
for (i = 0; i < 3; i++) {
st_shift_cust[i] = bias_st[i] - bias_regular[i];
if(debug) {
log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n",
st_shift_cust[i]/1.f, bias_regular[i]/1.f,
bias_st[i]/1.f);
log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f);
log_i("Bias_Shift=%ld, Bias_Reg=%ld, Bias_HWST=%ld\r\n",
(long)st_shift_cust[i], bias_regular[i],
bias_st[i]);
log_i("OTP value: %ld\r\n", (long)ct_shift_prod[i]);
}
st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f;
if(debug)
log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f,
test.max_accel_var/1.f);
log_i("ratio=%ld, threshold=%ld\r\n", (long)st_shift_ratio[i],
(long)test.max_accel_var);
if (fabs(st_shift_ratio[i]) > test.max_accel_var) {
if(debug)
@@ -2208,15 +2208,15 @@ static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
if(debug) {
log_i("ACCEL:CRITERIA B\r\n");
log_i("Min MG: %7.4f\r\n", accel_st_al_min/1.f);
log_i("Max MG: %7.4f\r\n", accel_st_al_max/1.f);
log_i("Min MG: %ld\r\n", (long)accel_st_al_min);
log_i("Max MG: %ld\r\n", (long)accel_st_al_max);
}
for (i = 0; i < 3; i++) {
st_shift_cust[i] = bias_st[i] - bias_regular[i];
if(debug)
log_i("Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\n", st_shift_cust[i]/1.f, bias_st[i]/1.f, bias_regular[i]/1.f);
log_i("Bias_shift=%ld, st=%ld, reg=%ld\n", (long)st_shift_cust[i], bias_st[i], bias_regular[i]);
if(st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max) {
if(debug)
log_i("Accel FAIL axis:%d <= 225mg or >= 675mg\n", i);
@@ -2229,7 +2229,7 @@ static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
/* Self Test Pass/Fail Criteria C */
accel_offset_max = test.max_g_offset * 65536.f;
if(debug)
log_i("Accel:CRITERIA C: bias less than %7.4f\n", accel_offset_max/1.f);
log_i("Accel:CRITERIA C: bias less than %ld\n", (long)accel_offset_max);
for (i = 0; i < 3; i++) {
if(fabs(bias_regular[i]) > accel_offset_max) {
if(debug)
@@ -2278,17 +2278,17 @@ static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
st_shift_cust[i] = bias_st[i] - bias_regular[i];
if(debug) {
log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n",
st_shift_cust[i]/1.f, bias_regular[i]/1.f,
bias_st[i]/1.f);
log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f);
log_i("Bias_Shift=%ld, Bias_Reg=%ld, Bias_HWST=%ld\r\n",
(long)st_shift_cust[i], bias_regular[i],
bias_st[i]);
log_i("OTP value: %ld\r\n", (long)ct_shift_prod[i]);
}
st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i];
if(debug)
log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f,
test.max_gyro_var/1.f);
log_i("ratio=%ld, threshold=%ld\r\n", (long)st_shift_ratio[i],
(long)test.max_gyro_var);
if (fabs(st_shift_ratio[i]) < test.max_gyro_var) {
if(debug)
@@ -2303,14 +2303,14 @@ static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
if(debug) {
log_i("GYRO:CRITERIA B\r\n");
log_i("Max DPS: %7.4f\r\n", gyro_st_al_max/1.f);
log_i("Max DPS: %ld\r\n", (long)gyro_st_al_max);
}
for (i = 0; i < 3; i++) {
st_shift_cust[i] = bias_st[i] - bias_regular[i];
if(debug)
log_i("Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\n", st_shift_cust[i]/1.f, bias_st[i]/1.f, bias_regular[i]/1.f);
log_i("Bias_shift=%ld, st=%ld, reg=%ld\n", (long)st_shift_cust[i], bias_st[i], bias_regular[i]);
if(st_shift_cust[i] < gyro_st_al_max) {
if(debug)
log_i("GYRO FAIL axis:%d greater than 60dps\n", i);
@@ -2323,7 +2323,7 @@ static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
/* Self Test Pass/Fail Criteria C */
gyro_offset_max = test.min_dps * 65536.f;
if(debug)
log_i("Gyro:CRITERIA C: bias less than %7.4f\n", gyro_offset_max/1.f);
log_i("Gyro:CRITERIA C: bias less than %ld\n", (long)gyro_offset_max);
for (i = 0; i < 3; i++) {
if(fabs(bias_regular[i]) > gyro_offset_max) {
if(debug)
@@ -2455,8 +2455,8 @@ static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, in
if(debug) {
log_i("Accel offset data HWST bit=%d: %7.4f %7.4f %7.4f\r\n", hw_test, accel[0]/65536.f, accel[1]/65536.f, accel[2]/65536.f);
log_i("Gyro offset data HWST bit=%d: %7.4f %7.4f %7.4f\r\n", hw_test, gyro[0]/65536.f, gyro[1]/65536.f, gyro[2]/65536.f);
log_i("Accel offset data HWST bit=%d: %ld %ld %ld\r\n", hw_test, accel[0], accel[1], accel[2]);
log_i("Gyro offset data HWST bit=%d: %ld %ld %ld\r\n", hw_test, gyro[0], gyro[1], gyro[2]);
}
return 0;
@@ -2871,9 +2871,9 @@ static int setup_compass(void)
if (akm_addr > 0x0F) {
/* TODO: Handle this case in all compass-related functions. */
#ifdef SERIAL_DEBUG
log_i("Compass not found.\n");
#endif
#ifdef SERIAL_DEBUG
log_i("Compass not found.\n");
#endif
return -1;
}
@@ -3228,14 +3228,14 @@ void mpu_start_self_test(void)
if (result == 0x7) {
#ifdef SERIAL_DEBUG
consoleLog("Passed!\n");
log_i("accel: %7.4f %7.4f %7.4f\n",
accel[0]/65536.f,
accel[1]/65536.f,
accel[2]/65536.f);
log_i("gyro: %7.4f %7.4f %7.4f\n",
gyro[0]/65536.f,
gyro[1]/65536.f,
gyro[2]/65536.f);
log_i("accel: %ld %ld %ld\n",
accel[0],
accel[1],
accel[2]);
log_i("gyro: %ld %ld %ld\n",
gyro[0],
gyro[1],
gyro[2]);
/* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/
#endif
@@ -3301,17 +3301,17 @@ void mpu_setup_gyro(void)
unsigned char mask = 0, lp_accel_was_on = 0;
if (hal.sensors & ACCEL_ON) {
mask |= INV_XYZ_ACCEL;
consoleLog("Accel sensor On.\n");
consoleLog("Accel sensor On.\n");
} else {
consoleLog("Accel sensor Off.\n");
}
consoleLog("Accel sensor Off.\n");
}
if (hal.sensors & GYRO_ON) {
mask |= INV_XYZ_GYRO;
lp_accel_was_on |= hal.lp_accel_mode;
consoleLog("Gyro sensor On.\n");
consoleLog("Gyro sensor On.\n");
} else {
consoleLog("Gyro sensor Off.\n");
}
consoleLog("Gyro sensor Off.\n");
}
#ifdef COMPASS_ENABLED
if (hal.sensors & COMPASS_ON) {
mask |= INV_XYZ_COMPASS;
@@ -3337,22 +3337,22 @@ void mpu_setup_gyro(void)
/* =========================== MPU-6050 Configuration =========================== */
int mpu_config(void)
{
consoleLog("-- Configuring MPU6050... ");
if(mpu_init()) {
consoleLog("FAIL (MPU).\n");
return -1;
}
consoleLog("-- Configuring MPU6050... ");
if(mpu_init()) {
consoleLog("FAIL (MPU).\n");
return -1;
}
/* Get/set hardware configuration. Start gyro. */
/* Get/set hardware configuration. Start gyro. */
/* Wake up all sensors. */
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
/* Push both gyro and accel data into the FIFO. */
/* Push both gyro and accel data into the FIFO. */
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(MPU_DEFAULT_HZ);
/* Read back configuration in case it was set improperly. */
/* Read back configuration in case it was set improperly. */
// mpu_get_sample_rate(&gyro_rate);
// mpu_get_gyro_fsr(&gyro_fsr);
// mpu_get_accel_fsr(&accel_fsr);
@@ -3365,7 +3365,7 @@ int mpu_config(void)
hal.next_temp_ms = 0;
#ifdef MPU_DMP_ENABLE
/* To initialize the DMP:
/* To initialize the DMP:
* 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in
* inv_mpu_dmp_motion_driver.h into the MPU memory.
* 2. Push the gyro and accel orientation matrix to the DMP.
@@ -3395,11 +3395,11 @@ int mpu_config(void)
* DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot
* be used in combination with DMP_FEATURE_SEND_RAW_GYRO.
*/
consoleLog(" writing DMP... ");
if (dmp_load_motion_driver_firmware()) {
consoleLog(" FAIL (DMP) --\r\n");
return -1;
}
consoleLog(" writing DMP... ");
if (dmp_load_motion_driver_firmware()) {
consoleLog(" FAIL (DMP) --\r\n");
return -1;
}
// dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_pdata.orientation));
dmp_register_tap_cb(mpu_tap_func);
dmp_register_android_orient_cb(mpu_android_orient_func);
@@ -3424,8 +3424,8 @@ int mpu_config(void)
hal.dmp_on = 1;
#endif
consoleLog(" OK --\r\n");
return 0;
consoleLog(" OK --\r\n");
return 0;
}
@@ -3452,93 +3452,93 @@ void mpu_get_data(void)
* Let's make them timer-based.
*/
if (timestamp > hal.next_temp_ms) {
hal.next_temp_ms = timestamp + TEMP_READ_MS;
new_temp = 1;
hal.next_temp_ms = timestamp + TEMP_READ_MS;
new_temp = 1;
}
if (hal.new_gyro && hal.dmp_on) {
short gyro[3], accel[3], sensors;
static long quat[4], temperature;
unsigned char more;
/* This function gets new data from the FIFO when the DMP is in
* use. The FIFO can contain any combination of gyro, accel,
* quaternion, and gesture data. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
* the FIFO isn't being filled with accel data.
* The driver parses the gesture data to determine if a gesture
* event has occurred; on an event, the application will be notified
* via a callback (assuming that a callback function was properly
* registered). The more parameter is non-zero if there are
* leftover packets in the FIFO.
*/
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (!more)
hal.new_gyro = 0;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
if (sensors & INV_WXYZ_QUAT) {
mpu.quat.w = quat[0];
mpu.quat.x = quat[1];
mpu.quat.y = quat[2];
mpu.quat.z = quat[3];
mpu_calc_euler_angles(); // Calculate Euler angles
new_data = 1;
}
short gyro[3], accel[3], sensors;
static long quat[4], temperature;
unsigned char more;
/* This function gets new data from the FIFO when the DMP is in
* use. The FIFO can contain any combination of gyro, accel,
* quaternion, and gesture data. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
* the FIFO isn't being filled with accel data.
* The driver parses the gesture data to determine if a gesture
* event has occurred; on an event, the application will be notified
* via a callback (assuming that a callback function was properly
* registered). The more parameter is non-zero if there are
* leftover packets in the FIFO.
*/
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (!more)
hal.new_gyro = 0;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
if (sensors & INV_WXYZ_QUAT) {
mpu.quat.w = quat[0];
mpu.quat.x = quat[1];
mpu.quat.y = quat[2];
mpu.quat.z = quat[3];
mpu_calc_euler_angles(); // Calculate Euler angles
new_data = 1;
}
} else if (hal.new_gyro) {
short gyro[3], accel[3];
long temperature;
unsigned char sensors, more;
/* This function gets new data from the FIFO. The FIFO can contain
* gyro, accel, both, or neither. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
* being filled with accel data. The more parameter is non-zero if
* there are leftover packets in the FIFO. The HAL can use this
* information to increase the frequency at which this function is
* called.
*/
hal.new_gyro = 0;
mpu_read_fifo(gyro, accel, &sensor_timestamp, &sensors, &more);
if (more)
hal.new_gyro = 1;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
short gyro[3], accel[3];
long temperature;
unsigned char sensors, more;
/* This function gets new data from the FIFO. The FIFO can contain
* gyro, accel, both, or neither. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
* being filled with accel data. The more parameter is non-zero if
* there are leftover packets in the FIFO. The HAL can use this
* information to increase the frequency at which this function is
* called.
*/
hal.new_gyro = 0;
mpu_read_fifo(gyro, accel, &sensor_timestamp, &sensors, &more);
if (more)
hal.new_gyro = 1;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
}
if (new_data) {
// do something if needed
// do something if needed
}
}
@@ -3608,7 +3608,7 @@ void mpu_calc_euler_angles(void) {
// Calculate Euler angles: source <https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles>
roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y)); // roll (x-axis rotation)
pitch = asin(2*(w*y - z*x)); // pitch (y-axis rotation)
pitch = asin(2*(w*y - z*x)); // pitch (y-axis rotation)
yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z)); // yaw (z-axis rotation)
// Convert [rad] to [deg*100]
@@ -3677,30 +3677,30 @@ void mpu_handle_input(char c)
switch (c) {
/* This command prints the Help text. */
case 'h':
consoleLog("=================== HELP COMMANDS ===================\n");
consoleLog("h: Print Help commands\n");
consoleLog("8: Set Accelerometer sensor on/off\n");
consoleLog("9: Set Gyroscope sensor on/off\n");
consoleLog("r: Print Registers value\n");
consoleLog("a: Print Accelerometer data\n");
consoleLog("g: Print Gyroscope data\n");
consoleLog("q: Print Quaternion data\n");
consoleLog("e: Print Euler angles in degree * 100\n");
consoleLog("t: Print Temperature in degC * 100\n");
consoleLog("p: Print Pedometer data\n");
consoleLog("0: Reset Pedometer\n");
consoleLog("1: Set DMP/MPU frequency 10 Hz\n");
consoleLog("2: Set DMP/MPU frequency 20 Hz\n");
consoleLog("3: Set DMP/MPU frequency 40 Hz\n");
consoleLog("4: Set DMP/MPU frequency 50 Hz\n");
consoleLog("5: Set DMP/MPU frequency 100 Hz\n");
consoleLog(",: Set DMP interrupt to gesture event only\n");
consoleLog(".: Set DMP interrupt to continuous\n");
consoleLog("f: Set DMP on/off\n");
consoleLog("v: Set Quaternion on/off\n");
consoleLog("w: Test out low-power accel mode\n");
consoleLog("s: Run self-test (device must be facing up or down)\n");
consoleLog("=====================================================\n");
consoleLog("=================== HELP COMMANDS ===================\n");
consoleLog("h: Print Help commands\n");
consoleLog("8: Set Accelerometer sensor on/off\n");
consoleLog("9: Set Gyroscope sensor on/off\n");
consoleLog("r: Print Registers value\n");
consoleLog("a: Print Accelerometer data\n");
consoleLog("g: Print Gyroscope data\n");
consoleLog("q: Print Quaternion data\n");
consoleLog("e: Print Euler angles in degree * 100\n");
consoleLog("t: Print Temperature in degC * 100\n");
consoleLog("p: Print Pedometer data\n");
consoleLog("0: Reset Pedometer\n");
consoleLog("1: Set DMP/MPU frequency 10 Hz\n");
consoleLog("2: Set DMP/MPU frequency 20 Hz\n");
consoleLog("3: Set DMP/MPU frequency 40 Hz\n");
consoleLog("4: Set DMP/MPU frequency 50 Hz\n");
consoleLog("5: Set DMP/MPU frequency 100 Hz\n");
consoleLog(",: Set DMP interrupt to gesture event only\n");
consoleLog(".: Set DMP interrupt to continuous\n");
consoleLog("f: Set DMP on/off\n");
consoleLog("v: Set Quaternion on/off\n");
consoleLog("w: Test out low-power accel mode\n");
consoleLog("s: Run self-test (device must be facing up or down)\n");
consoleLog("=====================================================\n");
break;
/* These commands turn off individual sensors. */
@@ -3713,14 +3713,14 @@ void mpu_handle_input(char c)
mpu_setup_gyro();
break;
/* This command prints out the value of each gyro register for debugging.
/* This command prints out the value of each gyro register for debugging.
* If logging is disabled, this function has no effect.
*/
case 'r':
mpu_reg_dump();
break;
/* These commands print individual sensor data. */
/* These commands print individual sensor data. */
case 'a':
hal.report ^= PRINT_ACCEL;
break;
@@ -3849,9 +3849,9 @@ void mpu_handle_input(char c)
/* Test out low-power accel mode. */
case 'w':
if (hal.dmp_on) {
consoleLog("Warning: For low-power mode, DMP needs to be disabled.\n");
consoleLog("Warning: For low-power mode, DMP needs to be disabled.\n");
break; /* LP accel is not compatible with the DMP. */
}
}
mpu_lp_accel_mode(20);
/* When LP accel mode is enabled, the driver automatically configures
* the hardware for latched interrupts. However, the MCU sometimes
@@ -3868,8 +3868,8 @@ void mpu_handle_input(char c)
break;
/* The hardware self test is completely localized in the gyro driver.
* Logging is assumed to be enabled; otherwise, a couple LEDs could
* probably be used here to display the test results.
* Logging is assumed to be enabled; otherwise, a couple LEDs could
* probably be used here to display the test results.
*/
case 's':
mpu_start_self_test();

80
Src/util.c
View File

@@ -35,7 +35,7 @@ extern I2C_HandleTypeDef hi2c1;
void consoleLog(char *message)
{
#ifdef SERIAL_DEBUG
log_i("%s", message);
log_i("%s", message);
#endif
}
@@ -51,51 +51,49 @@ void get_tick_count_ms(unsigned long *count)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif
PUTCHAR_PROTOTYPE
{
PUTCHAR_PROTOTYPE {
HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1, 1000);
return ch;
}
#ifdef __GNUC__
int _write(int file, char *ptr, int len)
{
int DataIdx;
for (DataIdx = 0; DataIdx < len; DataIdx++) { __io_putchar( *ptr++ );}
return len;
}
int _write(int file, char *data, int len) {
int i;
for (i = 0; i < len; i++) { __io_putchar( *data++ );}
return len;
}
#endif
#endif
void introDemoLED(uint32_t tDelay)
void intro_demo_led(uint32_t tDelay)
{
int i;
int i;
for (i = 0; i < 6; i++) {
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
}
for (i = 0; i < 6; i++) {
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
}
for (i = 0; i < 2; i++) {
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_SET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET);
}
for (i = 0; i < 2; i++) {
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_SET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET);
}
}
@@ -106,12 +104,12 @@ void introDemoLED(uint32_t tDelay)
*/
int8_t i2c_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
// !! Using the I2C Interrupt will fail writing the DMP.. could be that DMP memory writing requires more time !! So use the I2C without interrupt.
// HAL_I2C_Mem_Write_IT(&hi2c1, slaveAddr << 1, regAddr, 1, data, length);
// !! Using the I2C Interrupt will fail writing the DMP.. could be that DMP memory writing requires more time !! So use the I2C without interrupt.
// HAL_I2C_Mem_Write_IT(&hi2c1, slaveAddr << 1, regAddr, 1, data, length);
// while(HAL_I2C_STATE_READY != HAL_I2C_GetState(&hi2c1)); // Wait until all data bytes are sent/received
// return 0;
return HAL_I2C_Mem_Write(&hi2c1, slaveAddr << 1, regAddr, 1, data, length, 100); // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
return HAL_I2C_Mem_Write(&hi2c1, slaveAddr << 1, regAddr, 1, data, length, 100); // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
}
@@ -140,8 +138,8 @@ int8_t i2c_writeBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t
*/
int8_t i2c_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
// !! Using the I2C Interrupt will fail writing the DMP.. could be that DMP memory writing requires more time !! So use the I2C without interrupt.
// HAL_I2C_Mem_Read(&hi2c1, slaveAddr << 1, regAddr, 1, data, length);
// !! Using the I2C Interrupt will fail writing the DMP.. could be that DMP memory writing requires more time !! So use the I2C without interrupt.
// HAL_I2C_Mem_Read(&hi2c1, slaveAddr << 1, regAddr, 1, data, length);
// while(HAL_I2C_STATE_READY != HAL_I2C_GetState(&hi2c1)); // Wait until all data bytes are sent/received
// return 0;