diff --git a/.gitignore b/.gitignore index 03fcc03..b9527af 100644 --- a/.gitignore +++ b/.gitignore @@ -2,7 +2,7 @@ .pioenvs/ .vscode/ MDK-ARM/DebugConfig/ -MDK-ARM/Listing/ +MDK-ARM/Listings/ MDK-ARM/Objects/ MDK-ARM/RTE/ MDK-ARM/*.uvguix.* diff --git a/Inc/config.h b/Inc/config.h index b6dd519..c3581cc 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -72,8 +72,8 @@ #ifdef BYPASS_CUBEMX_DEFINES #define USART_MAIN_BAUD 38400 // [bit/s] MAIN Serial Tx/Rx baud rate #endif -#define SERIAL_START_FRAME 0xAAAA // [-] Start frame definition for reliable serial communication -#define SERIAL_TIMEOUT 300 // [-] Numer of wrong received data for Serial timeout detection +#define SERIAL_START_FRAME 0xABCD // [-] Start frame definition for reliable serial communication +#define SERIAL_TIMEOUT 500 // [-] Numer of wrong received data for Serial timeout detection /* ==================================== VALIDATE SETTINGS ==================================== */ diff --git a/Inc/defines.h b/Inc/defines.h index 127401f..4a1b9bc 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -90,6 +90,7 @@ /* =========================== Defines MPU-6050 =========================== */ #define log_i printf // redirect the log_i debug function to printf #define RAD2DEG 57.295779513082323 // RAD2DEG = 180/pi. Example: angle[deg] = angle[rad] * RAD2DEG +#define q30 1073741824 // 1073741824 = 2^30 #define ACCEL_ON (0x01) #define GYRO_ON (0x02) #define COMPASS_ON (0x04) diff --git a/MDK-ARM/sideboard-hack.uvoptx b/MDK-ARM/sideboard-hack.uvoptx index 3d397e3..da4809b 100644 --- a/MDK-ARM/sideboard-hack.uvoptx +++ b/MDK-ARM/sideboard-hack.uvoptx @@ -75,7 +75,7 @@ 1 0 - 1 + 0 18 @@ -233,7 +233,7 @@ 1 0 - 0 + 1 18 diff --git a/README.md b/README.md index 2dc4a95..0d0cc4f 100644 --- a/README.md +++ b/README.md @@ -74,14 +74,14 @@ make flash This firmware offers currently these variants (selectable in [platformio.ini](/platformio.ini) or [config.h](/Inc/config.h)): - **VARIANT_DEBUG**: In this variant the user can interact with sideboard by sending commands via a Serial Monitor to observe and check the capabilities of the sideboard. -- **VARIANT_HOVERBOARD**: In this variant the sideboard is communicating with the mainboard of a hoverboard using the [FOC firmware repository](https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC). This Variant is not yet fully tested! +- **VARIANT_HOVERBOARD**: In this variant the sideboard is communicating with the mainboard of a hoverboard using the [FOC firmware repository](https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC). Of course the firmware can be further customized for other needs or projects. --- ## 3D Visualization Demo -By [converting Quaternions to Euler angles](https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles), we can make a [3D visualization example](/docs/sketch_processing/sketch_processing.pde) in [Processing](https://processing.org/) as shown below. For this Demo VARIANT_HOVERBOARD was used. +By [converting Quaternions to Euler angles](https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles), we can make a [3D visualization example](/docs/sketch_processing/sketch_processing.pde) in [Processing](https://processing.org/) as shown below. For this Demo VARIANT_DEBUG was used. ![sketch_pic](/docs/pictures/sketch_processing_pic.png) diff --git a/Src/main.c b/Src/main.c index fe83cd2..d392519 100644 --- a/Src/main.c +++ b/Src/main.c @@ -100,8 +100,6 @@ 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; @@ -117,10 +115,10 @@ static uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial comman #endif extern MPU_Data mpu; // holds the MPU-6050 data -ErrorStatus mpuStatus = SUCCESS; // holds the MPU-6050 status: SUCCESS or ERROR +ErrorStatus mpuStatus; // holds the MPU-6050 status: SUCCESS or ERROR -FlagStatus sensor1, sensor2; // holds the sensor1 and sensor 2 values -FlagStatus sensor1_read, sensor2_read; // holds the instantaneous Read for sensor1 and sensor 2 +GPIO_PinState sensor1, sensor2; // holds the sensor1 and sensor 2 values +GPIO_PinState 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 */ @@ -164,7 +162,7 @@ int main(void) #endif #ifdef SERIAL_CONTROL __HAL_UART_FLUSH_DRREGISTER(&huart2); - HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&Sideboard, sizeof(Sideboard)); + HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&Sideboard, sizeof(Sideboard)); #endif #ifdef SERIAL_FEEDBACK __HAL_UART_FLUSH_DRREGISTER(&huart2); @@ -177,6 +175,7 @@ int main(void) HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); // Turn on RED LED } else { + mpuStatus = SUCCESS; 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 @@ -216,6 +215,8 @@ int main(void) // Get MPU data. Because the MPU-6050 interrupt pin is not wired we have to check DMP data by pooling periodically if (SUCCESS == mpuStatus) { mpu_get_data(); + } else if (ERROR == mpuStatus && main_loop_counter % 100 == 0) { + HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin); // Toggle the Red LED every 100 ms } // Print MPU data to Console if (main_loop_counter % 50 == 0) { @@ -228,80 +229,77 @@ int main(void) sensor2_read = HAL_GPIO_ReadPin(SENSOR2_GPIO_Port, SENSOR2_Pin); // SENSOR1 - if (sensor1 == RESET && sensor1_read == SET) { - sensor1 = SET; + if (sensor1 == GPIO_PIN_RESET && sensor1_read == GPIO_PIN_SET) { + sensor1 = GPIO_PIN_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"); - } else if(sensor1 == SET && sensor1_read == RESET) { - sensor1 = RESET; + } else if(sensor1 == GPIO_PIN_SET && sensor1_read == GPIO_PIN_RESET) { + sensor1 = GPIO_PIN_RESET; HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET); } // SENSOR2 - if (sensor2 == RESET && sensor2_read == SET) { - sensor2 = SET; + if (sensor2 == GPIO_PIN_RESET && sensor2_read == GPIO_PIN_SET) { + sensor2 = GPIO_PIN_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"); - } else if (sensor2 == SET && sensor2_read == RESET) { - sensor2 = RESET; + } else if (sensor2 == GPIO_PIN_SET && sensor2_read == GPIO_PIN_RESET) { + sensor2 = GPIO_PIN_RESET; HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET); } - if (sensor1 == SET) { + if (sensor1 == GPIO_PIN_SET) { // Sensor ACTIVE: Do something here (continuous task) } - if (sensor2 == SET) { + if (sensor2 == GPIO_PIN_SET) { // Sensor ACTIVE: Do something here (continuous task) } // ==================================== SERIAL Tx/Rx Handling ==================================== #ifdef SERIAL_CONTROL - if (main_loop_counter % 50 == 0) { // Transmit Tx data periodically using DMA + if (main_loop_counter % 5 == 0) { // Transmit Tx data periodically using DMA Sideboard.start = (uint16_t)SERIAL_START_FRAME; Sideboard.roll = (int16_t)mpu.euler.roll; Sideboard.pitch = (int16_t)mpu.euler.pitch; Sideboard.yaw = (int16_t)mpu.euler.yaw; - Sideboard.sensors = (uint16_t)(sensor1 | (sensor2 << 1)); + Sideboard.sensors = (uint16_t)(sensor1 | (sensor2 << 1) | (mpuStatus << 2)); Sideboard.checksum = (uint16_t)(Sideboard.start ^ Sideboard.roll ^ Sideboard.pitch ^ Sideboard.yaw ^ Sideboard.sensors); - HAL_UART_Transmit_DMA (&huart2, (uint8_t *)&Sideboard, sizeof(Sideboard)); + HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&Sideboard, sizeof(Sideboard)); } #endif #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.cmdLed); - if (NewFeedback.start == SERIAL_START_FRAME && NewFeedback.checksum == checksum) { - if (timeoutFlagSerial) { // Check for previous timeout flag - if (timeoutCntSerial-- <= 0) // Timeout de-qualification - timeoutFlagSerial = 0; // Timeout flag cleared + uint16_t checksum; + checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ 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 + timeoutFlagSerial = 0; // Timeout flag cleared } else { - memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); // Copy the new data - NewFeedback.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle - timeoutCntSerial = 0; // Reset the timeout counter + memcpy(&Feedback, &NewFeedback, sizeof(Feedback)); // Copy the new data + NewFeedback.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle + timeoutCntSerial = 0; // Reset the timeout counter } - } else { - if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification - timeoutFlagSerial = 1; // Timeout detected - timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value - } - // Check periodically the received Start Frame. If it is NOT OK, most probably we are out-of-sync. Try to re-sync by reseting the DMA - if (main_loop_counter % 50 == 0 && NewFeedback.start != SERIAL_START_FRAME && NewFeedback.start != 0xFFFF) { - HAL_UART_DMAStop(&huart2); - HAL_UART_Receive_DMA(&huart2, (uint8_t *)&NewFeedback, sizeof(NewFeedback)); - NewFeedback.start = 0xFFFF; // Change the Start Frame to avoid entering again here if no data is received - } - } - - if (timeoutFlagSerial) { // In case of timeout bring the system to a Safe State and indicate error if desired - HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET); // Turn on Red LED } else { - HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET); // Follow the Normal behavior - } + if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification + timeoutFlagSerial = 1; // Timeout detected + timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value + } + // Most probably we are out-of-sync. Try to re-sync by reseting the DMA + if (main_loop_counter % 150 == 0) { + HAL_UART_DMAStop(&huart2); + HAL_UART_Receive_DMA(&huart2, (uint8_t *)&NewFeedback, sizeof(NewFeedback)); + } + } + + if (timeoutFlagSerial && main_loop_counter % 100 == 0) { // In case of timeout bring the system to a Safe State and indicate error if desired + HAL_GPIO_TogglePin(LED3_GPIO_Port, LED3_Pin); // Toggle the Yellow LED every 100 ms + } #endif main_loop_counter++; diff --git a/Src/mpu6050.c b/Src/mpu6050.c index 255fc5c..683c0a5 100644 --- a/Src/mpu6050.c +++ b/Src/mpu6050.c @@ -3230,19 +3230,19 @@ void mpu_start_self_test(void) #elif defined (MPU6050) || defined (MPU9150) result = mpu_run_self_test(gyro, accel); #endif + #ifdef SERIAL_DEBUG + 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]); + #endif if (result == 0x7) { - #ifdef SERIAL_DEBUG - consoleLog("Passed!\n"); - 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 + consoleLog("Passed!\n"); + /* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/ #ifdef USE_CAL_HW_REGISTERS /* @@ -3639,10 +3639,10 @@ void mpu_calc_euler_angles(void) { float yaw, pitch, roll; // Convert quaternions[q30] to quaternion[float] - w = (float)mpu.quat.w / 1073741824; // 1073741824 = 2^30 - x = (float)mpu.quat.x / 1073741824; - y = (float)mpu.quat.y / 1073741824; - z = (float)mpu.quat.z / 1073741824; + w = (float)mpu.quat.w / q30; // q30 = 2^30 + x = (float)mpu.quat.x / q30; + y = (float)mpu.quat.y / q30; + z = (float)mpu.quat.z / q30; // Calculate Euler angles: source roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y)); // roll (x-axis rotation) diff --git a/Src/util.c b/Src/util.c index 45e238e..1e7b2aa 100644 --- a/Src/util.c +++ b/Src/util.c @@ -69,7 +69,7 @@ void intro_demo_led(uint32_t tDelay) { int i; - for (i = 0; i < 6; i++) { + for (i = 0; i < 3; 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); diff --git a/build/VARIANT_DEBUG/firmware.bin b/build/VARIANT_DEBUG/firmware.bin index 79b0a60..9903c12 100644 Binary files a/build/VARIANT_DEBUG/firmware.bin and b/build/VARIANT_DEBUG/firmware.bin differ diff --git a/build/VARIANT_DEBUG/firmware.elf b/build/VARIANT_DEBUG/firmware.elf index 57067b0..f54853f 100644 Binary files a/build/VARIANT_DEBUG/firmware.elf and b/build/VARIANT_DEBUG/firmware.elf differ diff --git a/build/VARIANT_HOVERBOARD/firmware.bin b/build/VARIANT_HOVERBOARD/firmware.bin index 6107ce6..aab0544 100644 Binary files a/build/VARIANT_HOVERBOARD/firmware.bin and b/build/VARIANT_HOVERBOARD/firmware.bin differ diff --git a/build/VARIANT_HOVERBOARD/firmware.elf b/build/VARIANT_HOVERBOARD/firmware.elf index 959cce1..d22d54c 100644 Binary files a/build/VARIANT_HOVERBOARD/firmware.elf and b/build/VARIANT_HOVERBOARD/firmware.elf differ diff --git a/docs/sketch_processing/sketch_processing.pde b/docs/sketch_processing/sketch_processing.pde index 6bf7e0b..41a43bb 100644 --- a/docs/sketch_processing/sketch_processing.pde +++ b/docs/sketch_processing/sketch_processing.pde @@ -1,5 +1,5 @@ /* - hoverboard-sidebboard-hack MPU6050 IMU - 3D Visualization Example + hoverboard-sidebboard-hack MPU6050 IMU - 3D Visualization Example. Use with VARIANT_DEBUG. Copyright (C) 2020-2021 Emanuel FERU */ import processing.serial.*; @@ -9,40 +9,32 @@ import java.io.IOException; Serial myPort; float roll, pitch,yaw; int idx = 0; -int inBytePrev; -short bufWord; + +String data=""; +String check=""; void setup() { size (1400, 800, P3D); printArray(Serial.list()); // List all the available serial ports myPort = new Serial(this, "COM5", 38400); // starts the serial communication - + myPort.bufferUntil('\n'); } void draw() { - - while (myPort.available() > 0) { - int inByte = myPort.read(); - bufWord = (short)(inBytePrev | (inByte << 8)); - idx++; - if(bufWord == -21846) { // check START_FRAME = 0xAAAA - idx = 0; + + // If no data is received, send 'e' command to read the Euler angles + if(idx != -1 && myPort.available() == 0) { + idx++; + if(idx > 20) { + myPort.write('e'); + idx = -1; } - if (idx == 2) { - roll = float(bufWord) / 100; - } - if (idx == 4) { - pitch = float(bufWord) / 100; - } - if (idx == 6) { - yaw = float(bufWord) / 100; - } - inBytePrev = inByte; + } else { + idx = -1; } - - // println(bufWord); //<>// - - translate(width/2, height/2, 0); + + // Display text + translate(width/2, height/2, 0); //<>// background(51); textSize(22); text("Roll: " + roll + " Pitch: " + pitch + " Yaw: " + yaw, -200, 300); @@ -52,8 +44,7 @@ void draw() { rotateZ(radians(-pitch)); rotateY(radians(yaw)); - // 3D 0bject - + // 3D 0bject // Draw box with text fill(0, 76, 153);; // Make board BLUE box (426, 30, 220); @@ -80,3 +71,21 @@ void draw() { box (40, 40, 15); // Blue Led connector } + +// Read data from the Serial Port +void serialEvent (Serial myPort) { + // reads the data from the Serial Port up to the character '\n' and puts it into the String variable "data". + data = myPort.readStringUntil('\n'); + // if you got any bytes other than the linefeed: + if (data != null) { + data = trim(data); + // split the string at " " (character space) + String items[] = split(data, ' '); + if (items.length > 5) { + //--- Roll,Pitch in degrees + roll = float(items[2]) / 100; + pitch = float(items[4]) / 100; + yaw = float(items[6]) / 100; + } + } +}