From f302ff80417fcf72c9fbe2671864332199252091 Mon Sep 17 00:00:00 2001 From: Fisch Date: Fri, 6 Dec 2019 22:08:17 +0100 Subject: [PATCH] change back to HAL_Delay --- Inc/config.h | 2 +- Src/main.c | 423 +++++++++++++++++++++++++-------------------------- 2 files changed, 210 insertions(+), 215 deletions(-) diff --git a/Inc/config.h b/Inc/config.h index f3c46a6..be3a49c 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -89,7 +89,7 @@ #define START_FRAME 0xAAAA // [-] Start frame definition for serial commands #define SERIAL_TIMEOUT 160 // [-] Serial timeout duration for the received data. 160 ~= 0.8 sec. Calculation: 0.8 sec / 0.005 sec -#define USART2_BAUD 38400 // UART2 baud rate (long wired cable, soldered next to buzzer) +#define USART2_BAUD 38400 // UART2 baud rate (long wired cable, soldered next to buzzer) #define USART2_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! diff --git a/Src/main.c b/Src/main.c index bcfab78..0dc3868 100644 --- a/Src/main.c +++ b/Src/main.c @@ -284,239 +284,234 @@ int main(void) { int16_t board_temp_adcFilt = adc_buffer.temp; int16_t board_temp_deg_c; - uint32_t timePrev=0; //for loop timing - while(1) { - //HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms - if ((HAL_GetTick()-timePrev) > DELAY_IN_MAIN_LOOP) { // try with DELAY_IN_MAIN_LOOP = 5 ms and then with DELAY_IN_MAIN_LOOP = 10 ms + HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms - #ifdef CONTROL_NUNCHUCK - Nunchuck_Read(); - cmd1 = CLAMP((nunchuck_data[0] - 127) * 8, -1000, 1000); // x - axis. Nunchuck joystick readings range 30 - 230 - cmd2 = CLAMP((nunchuck_data[1] - 128) * 8, -1000, 1000); // y - axis + #ifdef CONTROL_NUNCHUCK + Nunchuck_Read(); + cmd1 = CLAMP((nunchuck_data[0] - 127) * 8, -1000, 1000); // x - axis. Nunchuck joystick readings range 30 - 230 + cmd2 = CLAMP((nunchuck_data[1] - 128) * 8, -1000, 1000); // y - axis - button1 = (uint8_t)nunchuck_data[5] & 1; - button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1; + button1 = (uint8_t)nunchuck_data[5] & 1; + button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1; + #endif + + #ifdef CONTROL_PPM + cmd1 = CLAMP((ppm_captured_value[0] - 500) * 2, -1000, 1000); + cmd2 = CLAMP((ppm_captured_value[1] - 500) * 2, -1000, 1000); + button1 = ppm_captured_value[5] > 500; + float scale = ppm_captured_value[2] / 1000.0f; + #endif + + #ifdef CONTROL_ADC + // ADC values range: 0-4095, see ADC-calibration in config.h + #ifdef ADC1_MID_POT + cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MID) * 1000 / (ADC1_MAX - ADC1_MID), 0, 1000) + -CLAMP((ADC1_MID - adc_buffer.l_tx2) * 1000 / (ADC1_MID - ADC1_MIN), 0, 1000); // ADC1 + #else + cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN) * 1000 / (ADC1_MAX - ADC1_MIN), 0, 1000); // ADC1 #endif - #ifdef CONTROL_PPM - cmd1 = CLAMP((ppm_captured_value[0] - 500) * 2, -1000, 1000); - cmd2 = CLAMP((ppm_captured_value[1] - 500) * 2, -1000, 1000); - button1 = ppm_captured_value[5] > 500; - float scale = ppm_captured_value[2] / 1000.0f; + #ifdef ADC2_MID_POT + cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MID) * 1000 / (ADC2_MAX - ADC2_MID), 0, 1000) + -CLAMP((ADC2_MID - adc_buffer.l_rx2) * 1000 / (ADC2_MID - ADC2_MIN), 0, 1000); // ADC2 + #else + cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN) * 1000 / (ADC2_MAX - ADC2_MIN), 0, 1000); // ADC2 #endif - #ifdef CONTROL_ADC - // ADC values range: 0-4095, see ADC-calibration in config.h - #ifdef ADC1_MID_POT - cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MID) * 1000 / (ADC1_MAX - ADC1_MID), 0, 1000) - -CLAMP((ADC1_MID - adc_buffer.l_tx2) * 1000 / (ADC1_MID - ADC1_MIN), 0, 1000); // ADC1 - #else - cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN) * 1000 / (ADC1_MAX - ADC1_MIN), 0, 1000); // ADC1 - #endif + // use ADCs as button inputs: + button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1 + button2 = (uint8_t)(adc_buffer.l_rx2 > 2000); // ADC2 - #ifdef ADC2_MID_POT - cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MID) * 1000 / (ADC2_MAX - ADC2_MID), 0, 1000) - -CLAMP((ADC2_MID - adc_buffer.l_rx2) * 1000 / (ADC2_MID - ADC2_MIN), 0, 1000); // ADC2 - #else - cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN) * 1000 / (ADC2_MAX - ADC2_MIN), 0, 1000); // ADC2 - #endif + timeout = 0; + #endif - // use ADCs as button inputs: - button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1 - button2 = (uint8_t)(adc_buffer.l_rx2 > 2000); // ADC2 - - timeout = 0; - #endif - - #if defined CONTROL_SERIAL_USART2 || defined CONTROL_SERIAL_USART3 - - // Handle received data validity, timeout and fix out-of-sync if necessary - if (command.start == START_FRAME && command.checksum == (uint16_t)(command.start ^ command.speedLeft ^ command.speedRight)) { - if (timeoutFlag) { // Check for previous timeout flag - if (timeoutCnt-- <= 0) // Timeout de-qualification - timeoutFlag = 0; // Timeout flag cleared - } else { - //cmd1 = CLAMP((int16_t)command.steer, -1000, 1000); - cmd1 = CLAMP((int16_t)command.speedLeft, -1000, 1000); - //cmd2 = CLAMP((int16_t)command.speed, -1000, 1000); - cmd2 = CLAMP((int16_t)command.speedRight, -1000, 1000); - command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle - timeoutCnt = 0; // Reset the timeout counter - } - - // ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) ####### - if (enable == 0 && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){ - buzzerPattern = 0; - buzzerFreq = 6; HAL_Delay(100); // make 2 beeps indicating the motor enable - buzzerFreq = 4; HAL_Delay(200); - buzzerFreq = 0; - enable = 1; // enable motors - consoleLog("-- Motors enabled --\r\n"); - } + #if defined CONTROL_SERIAL_USART2 || defined CONTROL_SERIAL_USART3 + // Handle received data validity, timeout and fix out-of-sync if necessary + if (command.start == START_FRAME && command.checksum == (uint16_t)(command.start ^ command.speedLeft ^ command.speedRight)) { + if (timeoutFlag) { // Check for previous timeout flag + if (timeoutCnt-- <= 0) // Timeout de-qualification + timeoutFlag = 0; // Timeout flag cleared } else { - if (timeoutCnt++ >= SERIAL_TIMEOUT) { // Timeout qualification - timeoutFlag = 1; // Timeout detected - timeoutCnt = SERIAL_TIMEOUT; // Limit timout counter value - } - // Check 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 (command.start != START_FRAME && command.start != 0xFFFF) { - HAL_UART_DMAStop(&huart); - HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command)); - } + //cmd1 = CLAMP((int16_t)command.steer, -1000, 1000); + cmd1 = CLAMP((int16_t)command.speedLeft, -1000, 1000); + //cmd2 = CLAMP((int16_t)command.speed, -1000, 1000); + cmd2 = CLAMP((int16_t)command.speedRight, -1000, 1000); + command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle + timeoutCnt = 0; // Reset the timeout counter } - if (timeoutFlag) { // In case of timeout bring the system to a Safe State - ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way - cmd1 = 0; - cmd2 = 0; - } else { - ctrlModReq = ctrlModReqRaw; // Follow the Mode request + // ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) ####### + if (enable == 0 && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){ + buzzerPattern = 0; + buzzerFreq = 6; HAL_Delay(100); // make 2 beeps indicating the motor enable + buzzerFreq = 4; HAL_Delay(200); + buzzerFreq = 0; + enable = 1; // enable motors + consoleLog("-- Motors enabled --\r\n"); } - timeout = 0; - #endif - - - - - // ####### LOW-PASS FILTER ####### - /* - rateLimiter16(cmd1, RATE, &steerRateFixdt); - rateLimiter16(cmd2, RATE, &speedRateFixdt); - filtLowPass16(steerRateFixdt >> 4, FILTER, &steerFixdt); - filtLowPass16(speedRateFixdt >> 4, FILTER, &speedFixdt); - steer = steerFixdt >> 4; // convert fixed-point to integer - speed = speedFixdt >> 4; // convert fixed-point to integer - */ - rateLimiter16(cmd1, RATE, &speedLeftRateFixdt); - rateLimiter16(cmd2, RATE, &speedRightRateFixdt); - filtLowPass16(speedLeftRateFixdt >> 4, FILTER, &speedLeftFixdt); - filtLowPass16(speedRightRateFixdt >> 4, FILTER, &speedRightFixdt); - speedL = speedLeftFixdt >> 4; // convert fixed-point to integer - speedR = speedRightFixdt >> 4; // convert fixed-point to integer - - speed = (speedL+speedR)/2; - - // ####### MIXER ####### - // speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000); - // speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000); - //mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above - - // ####### SET OUTPUTS (if the target change is less than +/- 50) ####### - if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) { - #ifdef INVERT_R_DIRECTION - pwmr = speedR; - #else - pwmr = -speedR; - #endif - #ifdef INVERT_L_DIRECTION - pwml = -speedL; - #else - pwml = speedL; - #endif - } - - lastSpeedL = speedL; - lastSpeedR = speedR; - - - // ####### CALC BOARD TEMPERATURE ####### - filtLowPass16(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt); - board_temp_adcFilt = board_temp_adcFixdt >> 4; // convert fixed-point to integer - board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C; - - serialSendCounter++; // Increment the counter - if (serialSendCounter > 20) { // Send data every 100 ms = 20 * 5 ms, where 5 ms is approximately the main loop duration - serialSendCounter = 0; // Reset the counter - - // ####### DEBUG SERIAL OUT ####### - #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) - #ifdef CONTROL_ADC - setScopeChannel(0, (int16_t)adc_buffer.l_tx2); // 1: ADC1 - setScopeChannel(1, (int16_t)adc_buffer.l_rx2); // 2: ADC2 - #endif - setScopeChannel(2, (int16_t)speedR); // 1: output command: [-1000, 1000] - setScopeChannel(3, (int16_t)speedL); // 2: output command: [-1000, 1000] - setScopeChannel(4, (int16_t)adc_buffer.batt1); // 5: for battery voltage calibration - setScopeChannel(5, (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC)); // 6: for verifying battery voltage calibration - setScopeChannel(6, (int16_t)board_temp_adcFilt); // 7: for board temperature calibration - setScopeChannel(7, (int16_t)board_temp_deg_c); // 8: for verifying board temperature calibration - consoleScope(); - - // ####### FEEDBACK SERIAL OUT ####### - #elif defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3) - if(UART_DMA_CHANNEL->CNDTR == 0) { - Feedback.start = (uint16_t)START_FRAME; - Feedback.cmd1 = (int16_t)cmd1; - Feedback.cmd2 = (int16_t)cmd2; - Feedback.speedR = (int16_t)speedR; - Feedback.speedL = (int16_t)speedL; - Feedback.speedR_meas = (int16_t)rtY_Left.n_mot; - Feedback.speedL_meas = (int16_t)rtY_Right.n_mot; - Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); - Feedback.boardTemp = (int16_t)board_temp_deg_c; - Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR ^ Feedback.speedL - ^ Feedback.speedR_meas ^ Feedback.speedL_meas ^ Feedback.batVoltage ^ Feedback.boardTemp); - - UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN; - UART_DMA_CHANNEL->CNDTR = sizeof(Feedback); - UART_DMA_CHANNEL->CMAR = (uint32_t)&Feedback; - UART_DMA_CHANNEL->CCR |= DMA_CCR_EN; - } - #endif - } - - HAL_GPIO_TogglePin(LED_PORT, LED_PIN); - // ####### POWEROFF BY POWER-BUTTON ####### - if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - enable = 0; // disable motors - while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released - if(__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) { // do not power off after software reset (from a programmer/debugger) - __HAL_RCC_CLEAR_RESET_FLAGS(); // clear reset flags - } else { - poweroff(); // release power-latch - } - } - - - // ####### BEEP AND EMERGENCY POWEROFF ####### - if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && abs(speed) < 20) || (batVoltage < BAT_LOW_DEAD && abs(speed) < 20)) { // poweroff before mainboard burns OR low bat 3 - poweroff(); - } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot - buzzerFreq = 4; - buzzerPattern = 1; - } else if (batVoltage < BAT_LOW_LVL1 && batVoltage >= BAT_LOW_LVL2 && BAT_LOW_LVL1_ENABLE) { // low bat 1: slow beep - buzzerFreq = 5; - buzzerPattern = 42; - } else if (batVoltage < BAT_LOW_LVL2 && batVoltage >= BAT_LOW_DEAD && BAT_LOW_LVL2_ENABLE) { // low bat 2: fast beep - buzzerFreq = 5; - buzzerPattern = 6; - } else if (errCode_Left || errCode_Right || timeoutFlag) { // beep in case of Motor error or serial timeout - fast beep - buzzerFreq = 12; - buzzerPattern = 1; - } else if (BEEPS_BACKWARD && speed < -50) { // backward beep - buzzerFreq = 5; - buzzerPattern = 1; - } else { // do not beep - buzzerFreq = 0; - buzzerPattern = 0; - } - - - // ####### INACTIVITY TIMEOUT ####### - if (abs(speedL) > 50 || abs(speedR) > 50) { - inactivity_timeout_counter = 0; } else { - inactivity_timeout_counter ++; + if (timeoutCnt++ >= SERIAL_TIMEOUT) { // Timeout qualification + timeoutFlag = 1; // Timeout detected + timeoutCnt = SERIAL_TIMEOUT; // Limit timout counter value + } + // Check 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 (command.start != START_FRAME && command.start != 0xFFFF) { + HAL_UART_DMAStop(&huart); + HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command)); + } } - if (inactivity_timeout_counter > (INACTIVITY_TIMEOUT * 60 * 1000) / (DELAY_IN_MAIN_LOOP + 1)) { // rest of main loop needs maybe 1ms - poweroff(); + + if (timeoutFlag) { // In case of timeout bring the system to a Safe State + ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way + cmd1 = 0; + cmd2 = 0; + } else { + ctrlModReq = ctrlModReqRaw; // Follow the Mode request + } + timeout = 0; + + #endif + + + + + // ####### LOW-PASS FILTER ####### + /* + rateLimiter16(cmd1, RATE, &steerRateFixdt); + rateLimiter16(cmd2, RATE, &speedRateFixdt); + filtLowPass16(steerRateFixdt >> 4, FILTER, &steerFixdt); + filtLowPass16(speedRateFixdt >> 4, FILTER, &speedFixdt); + steer = steerFixdt >> 4; // convert fixed-point to integer + speed = speedFixdt >> 4; // convert fixed-point to integer + */ + rateLimiter16(cmd1, RATE, &speedLeftRateFixdt); + rateLimiter16(cmd2, RATE, &speedRightRateFixdt); + filtLowPass16(speedLeftRateFixdt >> 4, FILTER, &speedLeftFixdt); + filtLowPass16(speedRightRateFixdt >> 4, FILTER, &speedRightFixdt); + speedL = speedLeftFixdt >> 4; // convert fixed-point to integer + speedR = speedRightFixdt >> 4; // convert fixed-point to integer + + speed = (speedL+speedR)/2; + + // ####### MIXER ####### + // speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000); + // speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000); + //mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above + + // ####### SET OUTPUTS (if the target change is less than +/- 50) ####### + if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) { + #ifdef INVERT_R_DIRECTION + pwmr = speedR; + #else + pwmr = -speedR; + #endif + #ifdef INVERT_L_DIRECTION + pwml = -speedL; + #else + pwml = speedL; + #endif + } + + lastSpeedL = speedL; + lastSpeedR = speedR; + + + // ####### CALC BOARD TEMPERATURE ####### + filtLowPass16(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt); + board_temp_adcFilt = board_temp_adcFixdt >> 4; // convert fixed-point to integer + board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C; + + serialSendCounter++; // Increment the counter + if (serialSendCounter > 20) { // Send data every 100 ms = 20 * 5 ms, where 5 ms is approximately the main loop duration + serialSendCounter = 0; // Reset the counter + + // ####### DEBUG SERIAL OUT ####### + #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) + #ifdef CONTROL_ADC + setScopeChannel(0, (int16_t)adc_buffer.l_tx2); // 1: ADC1 + setScopeChannel(1, (int16_t)adc_buffer.l_rx2); // 2: ADC2 + #endif + setScopeChannel(2, (int16_t)speedR); // 1: output command: [-1000, 1000] + setScopeChannel(3, (int16_t)speedL); // 2: output command: [-1000, 1000] + setScopeChannel(4, (int16_t)adc_buffer.batt1); // 5: for battery voltage calibration + setScopeChannel(5, (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC)); // 6: for verifying battery voltage calibration + setScopeChannel(6, (int16_t)board_temp_adcFilt); // 7: for board temperature calibration + setScopeChannel(7, (int16_t)board_temp_deg_c); // 8: for verifying board temperature calibration + consoleScope(); + + // ####### FEEDBACK SERIAL OUT ####### + #elif defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3) + if(UART_DMA_CHANNEL->CNDTR == 0) { + Feedback.start = (uint16_t)START_FRAME; + Feedback.cmd1 = (int16_t)cmd1; + Feedback.cmd2 = (int16_t)cmd2; + Feedback.speedR = (int16_t)speedR; + Feedback.speedL = (int16_t)speedL; + Feedback.speedR_meas = (int16_t)rtY_Left.n_mot; + Feedback.speedL_meas = (int16_t)rtY_Right.n_mot; + Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); + Feedback.boardTemp = (int16_t)board_temp_deg_c; + Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR ^ Feedback.speedL + ^ Feedback.speedR_meas ^ Feedback.speedL_meas ^ Feedback.batVoltage ^ Feedback.boardTemp); + + UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN; + UART_DMA_CHANNEL->CNDTR = sizeof(Feedback); + UART_DMA_CHANNEL->CMAR = (uint32_t)&Feedback; + UART_DMA_CHANNEL->CCR |= DMA_CCR_EN; + } + #endif + } + + HAL_GPIO_TogglePin(LED_PORT, LED_PIN); + // ####### POWEROFF BY POWER-BUTTON ####### + if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { + enable = 0; // disable motors + while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released + if(__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) { // do not power off after software reset (from a programmer/debugger) + __HAL_RCC_CLEAR_RESET_FLAGS(); // clear reset flags + } else { + poweroff(); // release power-latch } } - timePrev = HAL_GetTick(); + + + // ####### BEEP AND EMERGENCY POWEROFF ####### + if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && abs(speed) < 20) || (batVoltage < BAT_LOW_DEAD && abs(speed) < 20)) { // poweroff before mainboard burns OR low bat 3 + poweroff(); + } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot + buzzerFreq = 4; + buzzerPattern = 1; + } else if (batVoltage < BAT_LOW_LVL1 && batVoltage >= BAT_LOW_LVL2 && BAT_LOW_LVL1_ENABLE) { // low bat 1: slow beep + buzzerFreq = 5; + buzzerPattern = 42; + } else if (batVoltage < BAT_LOW_LVL2 && batVoltage >= BAT_LOW_DEAD && BAT_LOW_LVL2_ENABLE) { // low bat 2: fast beep + buzzerFreq = 5; + buzzerPattern = 6; + } else if (errCode_Left || errCode_Right || timeoutFlag) { // beep in case of Motor error or serial timeout - fast beep + buzzerFreq = 12; + buzzerPattern = 1; + } else if (BEEPS_BACKWARD && speed < -50) { // backward beep + buzzerFreq = 5; + buzzerPattern = 1; + } else { // do not beep + buzzerFreq = 0; + buzzerPattern = 0; + } + + + // ####### INACTIVITY TIMEOUT ####### + if (abs(speedL) > 50 || abs(speedR) > 50) { + inactivity_timeout_counter = 0; + } else { + inactivity_timeout_counter ++; + } + if (inactivity_timeout_counter > (INACTIVITY_TIMEOUT * 60 * 1000) / (DELAY_IN_MAIN_LOOP + 1)) { // rest of main loop needs maybe 1ms + poweroff(); + } } }