diff --git a/.gitignore b/.gitignore index 66b80f5..956c171 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,7 @@ .pio/ .pioenvs/ .vscode/ -MDK-ARM/Listing/ +MDK-ARM/Listings/ MDK-ARM/Objects/ MDK-ARM/*.uvgui.* !build/VARIANT_DEBUG/firmware.hex diff --git a/Inc/config.h b/Inc/config.h index d9641f0..c0367ca 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -67,8 +67,8 @@ #define SERIAL_FEEDBACK // [-] Define for Serial Feedback via the serial port #endif #define USART_MAIN_BAUD 38400 // [bit/s] MAIN Serial Tx/Rx baud rate -#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 /* ==================================== SETTINGS AUX ==================================== */ diff --git a/Inc/defines.h b/Inc/defines.h index bf821b4..8139000 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -134,6 +134,7 @@ typedef enum {READ = 0, WRITE = !READ} i2c_cmd; /* =========================== 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.uvopt b/MDK-ARM/sideboard-hack.uvopt index 9f5f0f6..e233441 100644 --- a/MDK-ARM/sideboard-hack.uvopt +++ b/MDK-ARM/sideboard-hack.uvopt @@ -677,7 +677,7 @@ Src - 0 + 1 0 0 0 diff --git a/README.md b/README.md index 56dc3a8..eba7b01 100644 --- a/README.md +++ b/README.md @@ -89,14 +89,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/i2c_it.c b/Src/i2c_it.c index d607797..b9f70f4 100644 --- a/Src/i2c_it.c +++ b/Src/i2c_it.c @@ -346,7 +346,7 @@ void I2C1_ErrorIRQ_Handler(void) i2c_interrupt_flag_clear(I2C1, I2C_INT_FLAG_PECERR); } /* disable the error interrupt */ - i2c_interrupt_disable(I2C0,I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV); + i2c_interrupt_disable(I2C1,I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV); } #endif diff --git a/Src/main.c b/Src/main.c index 390b87b..31e546d 100644 --- a/Src/main.c +++ b/Src/main.c @@ -46,8 +46,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; @@ -63,7 +61,7 @@ static uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial comman #endif extern MPU_Data mpu; // holds the MPU-6050 data -ErrStatus mpuStatus = SUCCESS; // holds the MPU-6050 status: SUCCESS or ERROR +ErrStatus mpuStatus; // holds the MPU-6050 status: SUCCESS or ERROR uint8_t userCommand; // holds the user command input FlagStatus sensor1, sensor2; // holds the sensor1 and sensor 2 values @@ -81,10 +79,10 @@ int main(void) i2c_nvic_config(); // NVIC peripheral config #ifdef SERIAL_CONTROL - usart_Tx_DMA_config(USART_MAIN, (uint8_t *)&Sideboard, sizeof(Sideboard)); + usart_Tx_DMA_config(USART_MAIN, (uint8_t *)&Sideboard, sizeof(Sideboard)); #endif #ifdef SERIAL_FEEDBACK - usart_Rx_DMA_config(USART_MAIN, (uint8_t *)&NewFeedback, sizeof(NewFeedback)); + usart_Rx_DMA_config(USART_MAIN, (uint8_t *)&NewFeedback, sizeof(NewFeedback)); #endif intro_demo_led(100); // Short LEDs intro demo with 100 ms delay. This also gives some time for the MPU-6050 to power-up. @@ -93,6 +91,7 @@ int main(void) gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on RED LED } else { + mpuStatus = SUCCESS; gpio_bit_set(LED2_GPIO_Port, LED2_Pin); // Turn on GREEN LED } mpu_handle_input('h'); // Print the User Help commands to serial @@ -131,11 +130,13 @@ 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) { + toggle_led(LED1_GPIO_Port, LED1_Pin); // Toggle the Red LED every 100 ms } // Print MPU data to Console if (main_loop_counter % 50 == 0) { mpu_print_to_console(); - } + } // ==================================== SENSORS Handling ==================================== @@ -174,13 +175,12 @@ int main(void) // ==================================== SERIAL Tx/Rx Handling ==================================== #ifdef SERIAL_CONTROL // To transmit on USART - if (main_loop_counter % 50 == 0 && SET == dma_flag_get(DMA_CH3, DMA_FLAG_FTF)) { // check if DMA channel transfer complete (Full Transfer Finish flag == 1) - + if (main_loop_counter % 5 == 0 && SET == dma_flag_get(DMA_CH3, DMA_FLAG_FTF)) { // check if DMA channel transfer complete (Full Transfer Finish flag == 1) 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); dma_channel_disable(DMA_CH3); @@ -192,36 +192,33 @@ 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.cmdLed); + 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 - } + if (timeoutFlagSerial) { // Check for previous timeout flag + if (timeoutCntSerial-- <= 0) // Timeout de-qualification + timeoutFlagSerial = 0; // Timeout flag cleared + } else { + 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) { + // Most probably we are out-of-sync. Try to re-sync by reseting the DMA + if (main_loop_counter % 150 == 0) { dma_channel_disable(DMA_CH4); - usart_Rx_DMA_config(USART_MAIN, (uint8_t *)&NewFeedback, sizeof(NewFeedback)); + usart_Rx_DMA_config(USART_MAIN, (uint8_t *)&NewFeedback, sizeof(NewFeedback)); } } - - if (timeoutFlagSerial) { // In case of timeout bring the system to a Safe State and indicate error if desired - gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on Red LED - } else { - gpio_bit_reset(LED1_GPIO_Port, LED1_Pin); // Follow the Normal behavior - } - #endif - + + if (timeoutFlagSerial && main_loop_counter % 100 == 0) { // In case of timeout bring the system to a Safe State and indicate error if desired + toggle_led(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 a88b6a1..6220b38 100644 --- a/Src/mpu6050.c +++ b/Src/mpu6050.c @@ -3231,19 +3231,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 /* @@ -3640,10 +3640,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 16edadd..38f04b8 100644 --- a/Src/util.c +++ b/Src/util.c @@ -92,7 +92,7 @@ void intro_demo_led(uint32_t tDelay) { int i; - for (i = 0; i < 6; i++) { + for (i = 0; i < 3; i++) { gpio_bit_set(LED1_GPIO_Port, LED1_Pin); gpio_bit_reset(LED3_GPIO_Port, LED3_Pin); delay_1ms(tDelay); diff --git a/build/VARIANT_DEBUG/firmware.bin b/build/VARIANT_DEBUG/firmware.bin index 643b186..4b409a5 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 681634e..c42d0e6 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 b618f06..4a940a2 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 be97ee7..1adac2a 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 45a0079..34a60e0 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(35, 133, 54); // Make board GREEN 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; + } + } +}