mirror of
https://github.com/EFeru/hoverboard-sideboard-hack-STM.git
synced 2025-07-27 01:09:33 +00:00
Major UART communication improvement
- the UART communication is improved based on UART Idle line detection interrupt - an Rx ring buffer is used to manage the UART incoming data - both Tx and Rx are efficiently handled using DMA - fixed #1 Other: - minor visual improvements
This commit is contained in:
EmanuelFeru
parent
9edd22b6b7
commit
ee6ab3a886
59
Inc/config.h
59
Inc/config.h
@ -27,54 +27,54 @@
|
||||
// Ubuntu: define the desired build variant here if you want to use make in console
|
||||
// or use VARIANT environment variable for example like "make -e VARIANT=VARIANT_DEBUG". Select only one at a time.
|
||||
#if !defined(PLATFORMIO)
|
||||
// #define VARIANT_DEBUG // Variant for debugging and checking the capabilities of the side-board
|
||||
// #define VARIANT_HOVERBOARD // Variant for using the side-boards connected to the Hoverboard mainboard
|
||||
// #define VARIANT_DEBUG // Variant for debugging and checking the capabilities of the side-board
|
||||
// #define VARIANT_HOVERBOARD // Variant for using the side-boards connected to the Hoverboard mainboard
|
||||
#endif
|
||||
|
||||
/* ==================================== DO NOT TOUCH SETTINGS ==================================== */
|
||||
#define BYPASS_CUBEMX_DEFINES // [-] Use this flag to bypass the Ports definitions generated by CUBE MX in main.h and use the ones in defines.h
|
||||
#define MPU6050 // [-] Define IMU sensor type
|
||||
#define MPU_GYRO_FSR 2000 // [deg/s] Set Gyroscope Full Scale Range: 250 deg/s, 500 deg/s, 1000 deg/s, 2000 deg/s. !! DMP sensor fusion works only with 2000 deg/s !!
|
||||
#define MPU_ACCEL_FSR 2 // [g] Set Acceleromenter Full Scale Range: 2g, 4g, 8g, 16g. !! DMP sensor fusion works only with 2g !!
|
||||
#define MPU6050 // [-] Define IMU sensor type
|
||||
#define MPU_GYRO_FSR 2000 // [deg/s] Set Gyroscope Full Scale Range: 250 deg/s, 500 deg/s, 1000 deg/s, 2000 deg/s. !! DMP sensor fusion works only with 2000 deg/s !!
|
||||
#define MPU_ACCEL_FSR 2 // [g] Set Acceleromenter Full Scale Range: 2g, 4g, 8g, 16g. !! DMP sensor fusion works only with 2g !!
|
||||
#ifdef BYPASS_CUBEMX_DEFINES
|
||||
#define MPU_I2C_SPEED 400000 // [bit/s] Define I2C speed for communicating with the MPU6050
|
||||
#define MPU_I2C_SPEED 400000 // [bit/s] Define I2C speed for communicating with the MPU6050
|
||||
#endif
|
||||
#define DELAY_IN_MAIN_LOOP 1 // [ms] Delay in the main loop
|
||||
// #define PRINTF_FLOAT_SUPPORT // [-] Uncomment this for printf to support float on Serial Debug. It will increase code size! Better to avoid it!
|
||||
#define DELAY_IN_MAIN_LOOP 1 // [ms] Delay in the main loop
|
||||
// #define PRINTF_FLOAT_SUPPORT // [-] Uncomment this for printf to support float on Serial Debug. It will increase code size! Better to avoid it!
|
||||
/* =============================================================================================== */
|
||||
|
||||
|
||||
/* ==================================== SETTINGS MPU-6050 ==================================== */
|
||||
#define MPU_SENSOR_ENABLE // [-] Enable flag for MPU-6050 sensor. Comment-out this flag to Disable the MPU sensor and reduce code size.
|
||||
#define MPU_DMP_ENABLE // [-] Enable flag for MPU-6050 DMP (Digital Motion Processing) functionality.
|
||||
#define MPU_DEFAULT_HZ 20 // [Hz] Default MPU frequecy: must be between 1Hz and 200Hz.
|
||||
#define TEMP_READ_MS 500 // [ms] Temperature read time interval
|
||||
#define PEDO_READ_MS 1000 // [ms] Pedometer read time interval
|
||||
// #define USE_CAL_HW_REGISTERS // [-] Uncommnent this to SAVE the sensor calibration to the MPU-6050 registers after the Self-test was run
|
||||
#define MPU_DMP_ENABLE // [-] Enable flag for MPU-6050 DMP (Digital Motion Processing) functionality.
|
||||
#define MPU_DEFAULT_HZ 20 // [Hz] Default MPU frequecy: must be between 1Hz and 200Hz.
|
||||
#define TEMP_READ_MS 500 // [ms] Temperature read time interval
|
||||
#define PEDO_READ_MS 1000 // [ms] Pedometer read time interval
|
||||
// #define USE_CAL_HW_REGISTERS // [-] Uncommnent this to SAVE the sensor calibration to the MPU-6050 registers after the Self-test was run
|
||||
|
||||
// DMP Tap Detection Settings
|
||||
#define DMP_TAP_AXES TAP_XYZ // [-] Set which axes will register a tap: TAP_XYZ, TAP_X, TAP_Y, TAP_Z
|
||||
#define DMP_TAP_THRESH 250 // [mg/ms] Set tap threshold for the selected axis.
|
||||
#define DMP_TAP_COUNT 1 // [-] Set minimum number of taps needed for an interrupt. Minimum consecutive taps: 1 to 4
|
||||
#define DMP_TAP_TIME 100 // [ms] Set time length between valid taps.
|
||||
#define DMP_TAP_TIME_MULTI 500 // [ms] Set max time between taps to register as a multi-tap.
|
||||
#define DMP_SHAKE_REJECT_THRESH 200 // [deg/s] Set shake rejection threshold in degree per second (dps). If the DMP detects a gyro sample larger than the thresh, taps are rejected.
|
||||
#define DMP_SHAKE_REJECT_TIME 40 // [ms] Set shake rejection time. Sets the length of time that the gyro must be outside of the DMP_SHAKE_REJECT_THRESH before taps are rejected. A mandatory 60 ms is added to this parameter.
|
||||
#define DMP_SHAKE_REJECT_TIMEOUT 10 // [ms] Set shake rejection timeout. Sets the length of time after a shake rejection that the gyro must stay inside of the threshold before taps can be detected again. A mandatory 60 ms is added to this parameter.
|
||||
|
||||
#define DMP_TAP_AXES TAP_XYZ // [-] Set which axes will register a tap: TAP_XYZ, TAP_X, TAP_Y, TAP_Z
|
||||
#define DMP_TAP_THRESH 250 // [mg/ms] Set tap threshold for the selected axis.
|
||||
#define DMP_TAP_COUNT 1 // [-] Set minimum number of taps needed for an interrupt. Minimum consecutive taps: 1 to 4
|
||||
#define DMP_TAP_TIME 100 // [ms] Set time length between valid taps.
|
||||
#define DMP_TAP_TIME_MULTI 500 // [ms] Set max time between taps to register as a multi-tap.
|
||||
#define DMP_SHAKE_REJECT_THRESH 200 // [deg/s] Set shake rejection threshold in degree per second (dps). If the DMP detects a gyro sample larger than the thresh, taps are rejected.
|
||||
#define DMP_SHAKE_REJECT_TIME 40 // [ms] Set shake rejection time. Sets the length of time that the gyro must be outside of the DMP_SHAKE_REJECT_THRESH before taps are rejected. A mandatory 60 ms is added to this parameter.
|
||||
#define DMP_SHAKE_REJECT_TIMEOUT 10 // [ms] Set shake rejection timeout. Sets the length of time after a shake rejection that the gyro must stay inside of the threshold before taps can be detected again. A mandatory 60 ms is added to this parameter.
|
||||
|
||||
/* ==================================== SETTINGS USART ==================================== */
|
||||
#if defined(VARIANT_DEBUG)
|
||||
#define SERIAL_DEBUG // [-] Define for Serial Debug via the serial port
|
||||
#define SERIAL_DEBUG // [-] Define for Serial Debug via the serial port
|
||||
#elif defined(VARIANT_HOVERBOARD)
|
||||
#define SERIAL_CONTROL // [-] Define for Serial Control via the serial port
|
||||
#define SERIAL_FEEDBACK // [-] Define for Serial Feedback via the serial port
|
||||
#define SERIAL_CONTROL // [-] Define for Serial Control via the serial port
|
||||
#define SERIAL_FEEDBACK // [-] Define for Serial Feedback via the serial port
|
||||
#endif
|
||||
#ifdef BYPASS_CUBEMX_DEFINES
|
||||
#define USART_MAIN_BAUD 38400 // [bit/s] MAIN Serial Tx/Rx baud rate
|
||||
#define USART_MAIN_BAUD 38400 // [bit/s] MAIN Serial Tx/Rx baud rate
|
||||
#endif
|
||||
#define SERIAL_START_FRAME 0xABCD // [-] Start frame definition for reliable serial communication
|
||||
#define SERIAL_TIMEOUT 800 // [-] 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 // [-] Number of wrong received data for Serial timeout detection. Depends on DELAY_IN_MAIN_LOOP
|
||||
#define SERIAL_BUFFER_SIZE 64 // [bytes] Size of Serial Rx buffer. Make sure it is always larger than the 'Feedback' structure size
|
||||
|
||||
|
||||
/* ==================================== VALIDATE SETTINGS ==================================== */
|
||||
@ -82,6 +82,9 @@
|
||||
#error SERIAL_DEBUG and SERIAL_CONTROL not allowed. It is on the same cable.
|
||||
#endif
|
||||
|
||||
#if defined(SERIAL_DEBUG) && defined(SERIAL_FEEDBACK)
|
||||
#error SERIAL_DEBUG and SERIAL_FEEDBACK not allowed. It is on the same cable.
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -35,14 +35,15 @@
|
||||
#endif
|
||||
|
||||
/* =========================== Defines General =========================== */
|
||||
// #define _BV(bit) (1 << (bit))
|
||||
// #define ARRAYNUM(arr_nanme) (uint32_t)(sizeof(arr_nanme) / sizeof(*(arr_nanme)))
|
||||
#define min(a, b) (((a) < (b)) ? (a) : (b))
|
||||
#define max(a, b) (((a) > (b)) ? (a) : (b))
|
||||
#define i2c_write i2c_writeBytes
|
||||
#define i2c_read i2c_readBytes
|
||||
#define delay_ms HAL_Delay
|
||||
#define get_ms get_tick_count_ms
|
||||
// #define _BV(bit) (1 << (bit))
|
||||
#define ARRAY_LEN(x) (uint32_t)(sizeof(x) / sizeof(*(x)))
|
||||
#define min(a, b) (((a) < (b)) ? (a) : (b))
|
||||
#define max(a, b) (((a) > (b)) ? (a) : (b))
|
||||
#define i2c_write i2c_writeBytes
|
||||
#define i2c_read i2c_readBytes
|
||||
#define delay_ms HAL_Delay
|
||||
#define get_ms get_tick_count_ms
|
||||
#define log_i printf // redirect the log_i debug function to printf
|
||||
|
||||
#ifdef BYPASS_CUBEMX_DEFINES
|
||||
|
||||
@ -88,7 +89,6 @@
|
||||
#endif
|
||||
|
||||
/* =========================== 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)
|
||||
|
||||
35
Inc/util.h
35
Inc/util.h
@ -29,6 +29,41 @@ void consoleLog(char *message);
|
||||
void get_tick_count_ms(unsigned long *count);
|
||||
void intro_demo_led(uint32_t tDelay);
|
||||
|
||||
/* input initialization function */
|
||||
void input_init(void);
|
||||
|
||||
/* usart read functions */
|
||||
void usart_rx_check(void);
|
||||
|
||||
#ifdef SERIAL_DEBUG
|
||||
void usart_process_debug(uint8_t *userCommand, uint32_t len);
|
||||
#endif
|
||||
|
||||
#ifdef SERIAL_CONTROL
|
||||
typedef struct{
|
||||
uint16_t start;
|
||||
int16_t roll;
|
||||
int16_t pitch;
|
||||
int16_t yaw;
|
||||
uint16_t sensors;
|
||||
uint16_t checksum;
|
||||
} SerialSideboard;
|
||||
#endif
|
||||
#ifdef SERIAL_FEEDBACK
|
||||
typedef struct{
|
||||
uint16_t start;
|
||||
int16_t cmd1;
|
||||
int16_t cmd2;
|
||||
int16_t speedR_meas;
|
||||
int16_t speedL_meas;
|
||||
int16_t batVoltage;
|
||||
int16_t boardTemp;
|
||||
uint16_t cmdLed;
|
||||
uint16_t checksum;
|
||||
} SerialFeedback;
|
||||
void usart_process_data(SerialFeedback *Feedback_in, SerialFeedback *Feedback_out);
|
||||
#endif
|
||||
|
||||
/* i2c write/read functions */
|
||||
int8_t i2c_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
|
||||
int8_t i2c_writeByte (uint8_t slaveAddr, uint8_t regAddr, uint8_t data);
|
||||
|
||||
@ -17,7 +17,7 @@ The original sideboard hardware supports one 4-pin cable that originally was con
|
||||
The LED boards consist of colored LEDs (blue, red, green, orange) used for design and to inform the user about the current hoverboard state. Below the pinout of the LED boards is shown.
|
||||
|
||||
|
||||
The heart of the sideboard is a [STM32F103C8T6](/docs/stm32f103c8-Datasheet.pdf) with the pinout shown in the follwing figure:
|
||||
The sideboard brain is an [STM32F103C8T6](/docs/stm32f103c8-Datasheet.pdf) with the pinout shown in the follwing figure:
|
||||
|
||||
|
||||
A very important component of the sideboard is the IMU [MPU-6050](https://www.invensense.com/products/motion-tracking/6-axis/mpu-6050/) from Invensense. The [MPU-6050](https://www.invensense.com/products/motion-tracking/6-axis/mpu-6050/) determines the board orientation by combining a 3-axis gyroscope and a 3-axis accelerometer on the same silicon die, together with an onboard Digital Motion Processor™ (DMP™), which processes complex 6-axis MotionFusion algorithms. The DMP™ offers many features, such as:
|
||||
|
||||
139
Src/main.c
139
Src/main.c
@ -49,24 +49,7 @@
|
||||
|
||||
/* Private macro -------------------------------------------------------------*/
|
||||
/* USER CODE BEGIN PM */
|
||||
extern UART_HandleTypeDef huart2;
|
||||
uint8_t rxBuffer, userCommand = 0; // holds the user command input
|
||||
|
||||
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
|
||||
|
||||
#ifdef SERIAL_DEBUG
|
||||
if (rxBuffer != '\n' && rxBuffer != '\r') { // Do not accept 'new line' (ascii 10) and 'carriage return' (ascii 13) commands
|
||||
userCommand = rxBuffer;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
/* USER CODE END PM */
|
||||
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
@ -83,44 +66,25 @@ void SystemClock_Config(void);
|
||||
|
||||
/* Private user code ---------------------------------------------------------*/
|
||||
/* USER CODE BEGIN 0 */
|
||||
extern UART_HandleTypeDef huart2;
|
||||
|
||||
#ifdef SERIAL_CONTROL
|
||||
typedef struct{
|
||||
uint16_t start;
|
||||
int16_t roll;
|
||||
int16_t pitch;
|
||||
int16_t yaw;
|
||||
uint16_t sensors;
|
||||
uint16_t checksum;
|
||||
} SerialSideboard;
|
||||
SerialSideboard Sideboard;
|
||||
extern SerialSideboard Sideboard;
|
||||
#endif
|
||||
|
||||
#ifdef SERIAL_FEEDBACK
|
||||
typedef struct{
|
||||
uint16_t start;
|
||||
int16_t cmd1;
|
||||
int16_t cmd2;
|
||||
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;
|
||||
|
||||
static int16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial command
|
||||
static uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
|
||||
extern SerialFeedback Feedback;
|
||||
extern uint16_t timeoutCntSerial; // Timeout counter for Rx Serial command
|
||||
extern uint8_t timeoutFlagSerial; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
|
||||
#endif
|
||||
|
||||
extern MPU_Data mpu; // holds the MPU-6050 data
|
||||
ErrorStatus mpuStatus; // holds the MPU-6050 status: SUCCESS or ERROR
|
||||
extern MPU_Data mpu; // holds the MPU-6050 data
|
||||
extern ErrorStatus mpuStatus; // holds the MPU-6050 status: SUCCESS or ERROR
|
||||
|
||||
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
|
||||
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()
|
||||
static uint32_t main_loop_counter; // main loop counter to perform task squeduling inside main()
|
||||
/* USER CODE END 0 */
|
||||
|
||||
/**
|
||||
@ -155,36 +119,7 @@ int main(void)
|
||||
MX_USART2_UART_Init();
|
||||
MX_I2C1_Init();
|
||||
/* USER CODE BEGIN 2 */
|
||||
|
||||
#ifdef SERIAL_DEBUG
|
||||
__HAL_UART_FLUSH_DRREGISTER(&huart2);
|
||||
HAL_UART_Receive_DMA (&huart2, (uint8_t *)&rxBuffer, sizeof(rxBuffer));
|
||||
#endif
|
||||
#ifdef SERIAL_CONTROL
|
||||
__HAL_UART_FLUSH_DRREGISTER(&huart2);
|
||||
HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&Sideboard, sizeof(Sideboard));
|
||||
#endif
|
||||
#ifdef SERIAL_FEEDBACK
|
||||
__HAL_UART_FLUSH_DRREGISTER(&huart2);
|
||||
HAL_UART_Receive_DMA (&huart2, (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.
|
||||
|
||||
#ifdef MPU_SENSOR_ENABLE
|
||||
if(mpu_config()) { // IMU MPU-6050 config
|
||||
mpuStatus = ERROR;
|
||||
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
|
||||
#else
|
||||
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); // Turn on GREEN LED
|
||||
#endif
|
||||
|
||||
input_init(); // Input initialization
|
||||
/* USER CODE END 2 */
|
||||
|
||||
/* Infinite loop */
|
||||
@ -203,19 +138,9 @@ int main(void)
|
||||
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 ====================================
|
||||
#if defined(MPU_SENSOR_ENABLE) && defined(SERIAL_DEBUG)
|
||||
// Get the user Input as one character from Serial
|
||||
if (userCommand != 0) { // Check the availability of a user command set by the UART DMA
|
||||
log_i("Command = %c\n", userCommand);
|
||||
mpu_handle_input(userCommand);
|
||||
userCommand = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
// ==================================== MPU-6050 Handling ====================================
|
||||
#if defined(MPU_SENSOR_ENABLE) && defined(SERIAL_DEBUG)
|
||||
// Get MPU data. Because the MPU-6050 interrupt pin is not wired we have to check DMP data by pooling periodically
|
||||
@ -242,6 +167,7 @@ int main(void)
|
||||
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_SET);
|
||||
consoleLog("-- SENSOR 1 Active --\n");
|
||||
} else if(sensor1 == GPIO_PIN_SET && sensor1_read == GPIO_PIN_RESET) {
|
||||
// Sensor DEACTIVE: Do something here (one time task on deactivation)
|
||||
sensor1 = GPIO_PIN_RESET;
|
||||
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET);
|
||||
consoleLog("-- SENSOR 1 Deactive --\n");
|
||||
@ -254,6 +180,7 @@ int main(void)
|
||||
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_SET);
|
||||
consoleLog("-- SENSOR 2 Active --\n");
|
||||
} else if (sensor2 == GPIO_PIN_SET && sensor2_read == GPIO_PIN_RESET) {
|
||||
// Sensor DEACTIVE: Do something here (one time task on deactivation)
|
||||
sensor2 = GPIO_PIN_RESET;
|
||||
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET);
|
||||
consoleLog("-- SENSOR 2 Deactive --\n");
|
||||
@ -269,7 +196,7 @@ int main(void)
|
||||
|
||||
// ==================================== SERIAL Tx/Rx Handling ====================================
|
||||
#ifdef SERIAL_CONTROL
|
||||
if (main_loop_counter % 5 == 0) { // Transmit Tx data periodically using DMA
|
||||
if (main_loop_counter % 5 == 0 && __HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) { // Check if DMA channel counter is 0 (meaning all data has been transferred)
|
||||
Sideboard.start = (uint16_t)SERIAL_START_FRAME;
|
||||
Sideboard.roll = (int16_t)mpu.euler.roll;
|
||||
Sideboard.pitch = (int16_t)mpu.euler.pitch;
|
||||
@ -282,32 +209,12 @@ int main(void)
|
||||
#endif
|
||||
|
||||
#ifdef SERIAL_FEEDBACK
|
||||
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(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
|
||||
}
|
||||
// Most probably we are out-of-sync. Try to re-sync by reseting the DMA
|
||||
if (NewFeedback.start != SERIAL_START_FRAME && NewFeedback.start != 0xFFFF && main_loop_counter % 5 == 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
|
||||
if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification
|
||||
timeoutFlagSerial = 1; // Timeout detected
|
||||
timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value
|
||||
}
|
||||
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
|
||||
|
||||
|
||||
@ -3714,10 +3714,11 @@ void mpu_android_orient_func(unsigned char orientation)
|
||||
/* =========================== User Input Handling =========================== */
|
||||
void mpu_handle_input(char c)
|
||||
{
|
||||
#ifdef SERIAL_DEBUG
|
||||
switch (c) {
|
||||
/* This command prints the Help text. */
|
||||
case 'h':
|
||||
consoleLog("=================== HELP COMMANDS ===================\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");
|
||||
@ -3730,17 +3731,15 @@ void mpu_handle_input(char c)
|
||||
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("2: Set DMP/MPU frequency 50 Hz\n");
|
||||
consoleLog("3: 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("w: Test low-power accel mode\n");
|
||||
consoleLog("s: Run self-test (device must be facing up or down)\n");
|
||||
consoleLog("=====================================================\n");
|
||||
consoleLog("===========================\n");
|
||||
break;
|
||||
|
||||
/* These commands turn off individual sensors. */
|
||||
@ -3793,33 +3792,21 @@ void mpu_handle_input(char c)
|
||||
*/
|
||||
case '1':
|
||||
if (hal.dmp_on) {
|
||||
if (0 == dmp_set_fifo_rate(10)) {consoleLog("DMP set to 10 Hz.\n");}
|
||||
if (0 == dmp_set_fifo_rate(10)) {consoleLog("DMP: 10 Hz\n");}
|
||||
} else
|
||||
if (0 == mpu_set_sample_rate(10)) {consoleLog("MPU set to 10 Hz.\n");}
|
||||
if (0 == mpu_set_sample_rate(10)) {consoleLog("MPU: 10 Hz\n");}
|
||||
break;
|
||||
case '2':
|
||||
if (hal.dmp_on) {
|
||||
if (0 == dmp_set_fifo_rate(20)) {consoleLog("DMP set to 20 Hz.\n");}
|
||||
if (0 == dmp_set_fifo_rate(50)) {consoleLog("DMP: 50 Hz\n");}
|
||||
} else
|
||||
if (0 == mpu_set_sample_rate(20)) {consoleLog("MPU set to 20 Hz.\n");}
|
||||
if (0 == mpu_set_sample_rate(50)) {consoleLog("MPU: 50 Hz\n");}
|
||||
break;
|
||||
case '3':
|
||||
if (hal.dmp_on) {
|
||||
if (0 == dmp_set_fifo_rate(40)) {consoleLog("DMP set to 40 Hz.\n");}
|
||||
if (0 == dmp_set_fifo_rate(100)) {consoleLog("DMP: 100 Hz\n");}
|
||||
} else
|
||||
if (0 == mpu_set_sample_rate(40)) {consoleLog("MPU set to 40 Hz.\n");}
|
||||
break;
|
||||
case '4':
|
||||
if (hal.dmp_on) {
|
||||
if (0 == dmp_set_fifo_rate(50)) {consoleLog("DMP set to 50 Hz.\n");}
|
||||
} else
|
||||
if (0 == mpu_set_sample_rate(50)) {consoleLog("MPU set to 50 Hz.\n");}
|
||||
break;
|
||||
case '5':
|
||||
if (hal.dmp_on) {
|
||||
if (0 == dmp_set_fifo_rate(100)) {consoleLog("DMP set to 100 Hz.\n");}
|
||||
} else
|
||||
if (0 == mpu_set_sample_rate(100)) {consoleLog("MPU set to 100 Hz.\n");}
|
||||
if (0 == mpu_set_sample_rate(100)) {consoleLog("MPU: 100 Hz\n");}
|
||||
break;
|
||||
|
||||
/* Set hardware to interrupt on gesture event only. This feature is
|
||||
@ -3918,7 +3905,7 @@ void mpu_handle_input(char c)
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
#endif // SERIAL_DEBUG
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -23,7 +23,7 @@
|
||||
#include "stm32f1xx_it.h"
|
||||
/* Private includes ----------------------------------------------------------*/
|
||||
/* USER CODE BEGIN Includes */
|
||||
|
||||
#include "util.h"
|
||||
/* USER CODE END Includes */
|
||||
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
@ -267,7 +267,10 @@ void USART2_IRQHandler(void)
|
||||
/* USER CODE END USART2_IRQn 0 */
|
||||
HAL_UART_IRQHandler(&huart2);
|
||||
/* USER CODE BEGIN USART2_IRQn 1 */
|
||||
|
||||
if(RESET != __HAL_UART_GET_IT_SOURCE(&huart2, UART_IT_IDLE)) { // Check for IDLE line interrupt
|
||||
__HAL_UART_CLEAR_IDLEFLAG(&huart2); // Clear IDLE line flag (otherwise it will continue to enter interrupt)
|
||||
usart_rx_check(); // Check for data to process
|
||||
}
|
||||
/* USER CODE END USART2_IRQn 1 */
|
||||
}
|
||||
|
||||
|
||||
@ -112,9 +112,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
|
||||
HAL_NVIC_SetPriority(USART2_IRQn, 0, 0);
|
||||
HAL_NVIC_EnableIRQ(USART2_IRQn);
|
||||
/* USER CODE BEGIN USART2_MspInit 1 */
|
||||
// DMA1_Channel7->CPAR = (uint32_t) & (USART2->DR);
|
||||
// DMA1_Channel7->CNDTR = 0;
|
||||
// DMA1->IFCR = DMA_IFCR_CTCIF7 | DMA_IFCR_CHTIF7 | DMA_IFCR_CGIF7;
|
||||
__HAL_UART_ENABLE_IT (uartHandle, UART_IT_IDLE); // Enable the USART IDLE line detection interrupt
|
||||
/* USER CODE END USART2_MspInit 1 */
|
||||
}
|
||||
}
|
||||
|
||||
155
Src/util.c
155
Src/util.c
@ -26,10 +26,32 @@
|
||||
#include "defines.h"
|
||||
#include "config.h"
|
||||
#include "util.h"
|
||||
#include "mpu6050.h"
|
||||
|
||||
extern UART_HandleTypeDef huart2;
|
||||
extern I2C_HandleTypeDef hi2c1;
|
||||
|
||||
// USART variables
|
||||
#ifdef SERIAL_CONTROL
|
||||
SerialSideboard Sideboard;
|
||||
#endif
|
||||
|
||||
#if defined(SERIAL_DEBUG) || defined(SERIAL_FEEDBACK)
|
||||
static uint8_t rx_buffer[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer
|
||||
static uint32_t rx_buffer_len = ARRAY_LEN(rx_buffer);
|
||||
#endif
|
||||
|
||||
#ifdef SERIAL_FEEDBACK
|
||||
SerialFeedback Feedback;
|
||||
SerialFeedback FeedbackRaw;
|
||||
uint16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial command
|
||||
uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
|
||||
static uint32_t Feedback_len = sizeof(Feedback);
|
||||
#endif
|
||||
|
||||
// MPU variables
|
||||
ErrorStatus mpuStatus; // holds the MPU-6050 status: SUCCESS or ERROR
|
||||
|
||||
/* =========================== General Functions =========================== */
|
||||
|
||||
void consoleLog(char *message)
|
||||
@ -97,6 +119,139 @@ void intro_demo_led(uint32_t tDelay)
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* =========================== Input Initialization Function =========================== */
|
||||
void input_init(void) {
|
||||
#ifdef SERIAL_CONTROL
|
||||
HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&Sideboard, sizeof(Sideboard));
|
||||
#endif
|
||||
#if defined(SERIAL_DEBUG) || defined(SERIAL_FEEDBACK)
|
||||
HAL_UART_Receive_DMA (&huart2, (uint8_t *)rx_buffer, sizeof(rx_buffer));
|
||||
#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.
|
||||
|
||||
#ifdef MPU_SENSOR_ENABLE
|
||||
if(mpu_config()) { // IMU MPU-6050 config
|
||||
mpuStatus = ERROR;
|
||||
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_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
|
||||
#else
|
||||
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); // Turn on GREEN LED
|
||||
#endif
|
||||
}
|
||||
|
||||
/* =========================== USART READ Functions =========================== */
|
||||
|
||||
/*
|
||||
* Check for new data received on USART with DMA: refactored function from https://github.com/MaJerle/stm32-usart-uart-dma-rx-tx
|
||||
* - this function is called for every USART IDLE line detection, in the USART interrupt handler
|
||||
*/
|
||||
void usart_rx_check(void)
|
||||
{
|
||||
#ifdef SERIAL_DEBUG
|
||||
static uint32_t old_pos;
|
||||
uint32_t pos;
|
||||
|
||||
pos = rx_buffer_len - __HAL_DMA_GET_COUNTER(huart2.hdmarx); // Calculate current position in buffer, Rx: DMA1_Channel6->CNDTR, Tx: DMA1_Channel7
|
||||
if (pos != old_pos) { // Check change in received data
|
||||
if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one
|
||||
usart_process_debug(&rx_buffer[old_pos], pos - old_pos); // Process data
|
||||
} else { // "Overflow" buffer mode
|
||||
usart_process_debug(&rx_buffer[old_pos], rx_buffer_len - old_pos); // First Process data from the end of buffer
|
||||
if (pos > 0) { // Check and continue with beginning of buffer
|
||||
usart_process_debug(&rx_buffer[0], pos); // Process remaining data
|
||||
}
|
||||
}
|
||||
}
|
||||
old_pos = pos; // Updated old position
|
||||
|
||||
if (old_pos == rx_buffer_len) { // Check and manually update if we reached end of buffer
|
||||
old_pos = 0;
|
||||
}
|
||||
#endif // SERIAL_DEBUG
|
||||
|
||||
#ifdef SERIAL_FEEDBACK
|
||||
static uint32_t old_pos;
|
||||
uint32_t pos;
|
||||
uint8_t *ptr;
|
||||
pos = rx_buffer_len - __HAL_DMA_GET_COUNTER(huart2.hdmarx); // Calculate current position in buffer, Rx: DMA1_Channel6->CNDTR, Tx: DMA1_Channel7
|
||||
if (pos != old_pos) { // Check change in received data
|
||||
ptr = (uint8_t *)&FeedbackRaw; // Initialize the pointer with FeedbackRaw address
|
||||
if (pos > old_pos && (pos - old_pos) == Feedback_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
|
||||
memcpy(ptr, &rx_buffer[old_pos], Feedback_len); // Copy data. This is possible only if FeedbackRaw is contiguous! (meaning all the structure members have the same size)
|
||||
usart_process_data(&FeedbackRaw, &Feedback); // Process data
|
||||
} else if ((rx_buffer_len - old_pos + pos) == Feedback_len) { // "Overflow" buffer mode: check if data length equals expected length
|
||||
memcpy(ptr, &rx_buffer[old_pos], rx_buffer_len - old_pos); // First copy data from the end of buffer
|
||||
if (pos > 0) { // Check and continue with beginning of buffer
|
||||
ptr += rx_buffer_len - old_pos; // Move to correct position in FeedbackRaw
|
||||
memcpy(ptr, &rx_buffer[0], pos); // Copy remaining data
|
||||
}
|
||||
usart_process_data(&FeedbackRaw, &Feedback); // Process data
|
||||
}
|
||||
}
|
||||
old_pos = pos; // Update old position
|
||||
if (old_pos == rx_buffer_len) { // Check and manually update if we reached end of buffer
|
||||
old_pos = 0;
|
||||
}
|
||||
#endif // SERIAL_FEEDBACK
|
||||
}
|
||||
|
||||
/*
|
||||
* Process Rx debug user command input
|
||||
*/
|
||||
#ifdef SERIAL_DEBUG
|
||||
void usart_process_debug(uint8_t *userCommand, uint32_t len)
|
||||
{
|
||||
for (; len > 0; len--, userCommand++) {
|
||||
if (*userCommand != '\n' && *userCommand != '\r') { // Do not accept 'new line' and 'carriage return' commands
|
||||
log_i("Command = %c\n", *userCommand);
|
||||
mpu_handle_input(*userCommand);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // SERIAL_DEBUG
|
||||
|
||||
/*
|
||||
* Process Rx data
|
||||
* - if the Feedback_in data is valid (correct START_FRAME and checksum) copy the Feedback_in to Feedback_out
|
||||
*/
|
||||
#ifdef SERIAL_FEEDBACK
|
||||
void usart_process_data(SerialFeedback *Feedback_in, SerialFeedback *Feedback_out)
|
||||
{
|
||||
uint16_t checksum;
|
||||
if (Feedback_in->start == SERIAL_START_FRAME) {
|
||||
checksum = (uint16_t)(Feedback_in->start ^ Feedback_in->cmd1 ^ Feedback_in->cmd2 ^ Feedback_in->speedR_meas ^ Feedback_in->speedL_meas
|
||||
^ Feedback_in->batVoltage ^ Feedback_in->boardTemp ^ Feedback_in->cmdLed);
|
||||
if (Feedback_in->checksum == checksum) {
|
||||
*Feedback_out = *Feedback_in;
|
||||
timeoutCntSerial = 0; // Reset timeout counter
|
||||
timeoutFlagSerial = 0; // Clear timeout flag
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // SERIAL_FEEDBACK
|
||||
|
||||
/*
|
||||
* UART User Error Callback
|
||||
* - According to the STM documentation, when a DMA transfer error occurs during a DMA read or a write access,
|
||||
* the faulty channel is automatically disabled through a hardware clear of its EN bit
|
||||
* - For hoverboard applications, the UART communication can be unrealiable, disablind the DMA transfer
|
||||
* - therefore the DMA needs to be re-started
|
||||
*/
|
||||
void HAL_UART_ErrorCallback(UART_HandleTypeDef *uartHandle) {
|
||||
#if defined(SERIAL_DEBUG) || defined(SERIAL_FEEDBACK)
|
||||
if(uartHandle->Instance == USART2) {
|
||||
HAL_UART_Receive_DMA (uartHandle, (uint8_t *)rx_buffer, sizeof(rx_buffer));
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/* =========================== I2C WRITE Functions =========================== */
|
||||
|
||||
/*
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user