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

UART with mainboard works

Browse Source 
- the sideboard can now send and receive Serial data from the mainboard
- fixed Processing sketch
This commit is contained in:
EmanuelFeru
2020-03-01 09:43:14 +01:00
parent e21f61ec2e
commit 5d9e79afaf
13 changed files with 105 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
Src/main.c
View File

@@ -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++;

32
Src/mpu6050.c
View File

@@ -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 <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)

2
Src/util.c
View File

@@ -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);