EFeru/hoverboard-firmware-hack-FOC
1
0
Fork 1
mirror of https://github.com/EFeru/hoverboard-firmware-hack-FOC.git synced 2025-08-05 10:34:28 +02:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into master

Browse Source
This commit is contained in:
EmanuelFeru
2020-12-09 18:44:37 +01:00
committed by GitHub
parent 18ac2bc224 3fcae4fc8b
commit 31f0f915cf
14 changed files with 236 additions and 203 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
Src/bldc.c
View File

@@ -59,7 +59,10 @@ extern volatile adc_buf_t adc_buffer;
uint8_t buzzerFreq = 0;
uint8_t buzzerPattern = 0;
uint8_t buzzerCount = 0;
static uint32_t buzzerTimer = 0;
static uint8_t buzzerPrev = 0;
static uint8_t buzzerIdx = 0;
uint8_t enable = 0; // initially motors are disabled for SAFETY
static uint8_t enableFin = 0;
@@ -97,7 +100,7 @@ void DMA1_Channel1_IRQHandler(void) {
return;
}
if (buzzerTimer % 1000 == 0) { // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point
if (buzzerTimer % 1000 == 0) { // Filter battery voltage at a slower sampling rate
filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt);
batVoltage = (int16_t)(batVoltageFixdt >> 16); // convert fixed-point to integer
}
@@ -126,14 +129,21 @@ void DMA1_Channel1_IRQHandler(void) {
RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
}
//create square wave for buzzer
// Create square wave for buzzer
buzzerTimer++;
if (buzzerFreq != 0 && (buzzerTimer / 5000) % (buzzerPattern + 1) == 0) {
if (buzzerTimer % buzzerFreq == 0) {
if (buzzerPrev == 0) {
buzzerPrev = 1;
if (++buzzerIdx > (buzzerCount + 2)) { // pause 2 periods
buzzerIdx = 1;
}
}
if (buzzerTimer % buzzerFreq == 0 && (buzzerIdx <= buzzerCount || buzzerCount == 0)) {
HAL_GPIO_TogglePin(BUZZER_PORT, BUZZER_PIN);
}
} else {
} else if (buzzerPrev) {
HAL_GPIO_WritePin(BUZZER_PORT, BUZZER_PIN, GPIO_PIN_RESET);
buzzerPrev = 0;
}
// ############################### MOTOR CONTROL ###############################

71
Src/main.c
View File

@@ -75,9 +75,6 @@ extern uint8_t timeoutFlagSerial; // Timeout Flag for Rx Serial command: 0
extern volatile int pwml; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
extern uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t enable; // global variable for motor enable
extern int16_t batVoltage; // global variable for battery voltage
@@ -115,7 +112,7 @@ typedef struct{
int16_t speedL_meas;
int16_t batVoltage;
int16_t boardTemp;
uint16_t cmdLed;
uint16_t cmdLed;
uint16_t checksum;
} SerialFeedback;
static SerialFeedback Feedback;
@@ -181,13 +178,13 @@ int main(void) {
MX_ADC1_Init();
MX_ADC2_Init();
BLDC_Init(); // BLDC Controller Init
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET); // Activate Latch
Input_Lim_Init(); // Input Limitations Init
Input_Init(); // Input Init
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET);
HAL_ADC_Start(&hadc1);
HAL_ADC_Start(&hadc2);
HAL_ADC_Start(&hadc2);
poweronMelody();
HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET);
@@ -213,8 +210,8 @@ int main(void) {
#ifndef VARIANT_TRANSPOTTER
// ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){
shortBeep(6); // make 2 beeps indicating the motor enable
shortBeep(4); HAL_Delay(100);
beepShort(6); // make 2 beeps indicating the motor enable
beepShort(4); HAL_Delay(100);
steerFixdt = speedFixdt = 0; // reset filters
enable = 1; // enable motors
printf("# -- Motors enabled --\n");
@@ -353,7 +350,7 @@ int main(void) {
if ((distance / 1345.0) - setDistance > 0.5 && (lastDistance / 1345.0) - setDistance > 0.5) { // Error, robot too far away!
enable = 0;
longBeep(5);
beepLong(5);
#ifdef SUPPORT_LCD
LCD_ClearDisplay(&lcd);
HAL_Delay(5);
@@ -467,37 +464,33 @@ int main(void) {
if (board_temp_deg_c >= TEMP_POWEROFF) printf("# Error: STM32 overtemp: %4.1f°C: power off\n", board_temp_deg_c / 10.0);
if (batVoltage < BAT_DEAD) printf("# Battery empty: %4.2fV: power off\n", batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC / 100.0);
poweroff();
} else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // disable motors and beep in case of Motor error - fast beep
enable = 0;
if (rtY_Left.z_errCode && main_loop_counter % 50 == 0) printf("# Warning: rtY_Left.z_errCode: %i\n", rtY_Left.z_errCode);
if (rtY_Right.z_errCode && main_loop_counter % 50 == 0) printf("# Warning: rtY_Right.z_errCode: %i\n", rtY_Right.z_errCode);
buzzerFreq = 8;
buzzerPattern = 1;
} else if (timeoutFlagADC || timeoutFlagSerial || timeoutCnt > TIMEOUT) { // beep in case of ADC timeout, Serial timeout or General timeout - fast beep
if (timeoutFlagADC && main_loop_counter % 50 == 0) printf("# Warning: ADC timeout\n");
if (timeoutFlagSerial && main_loop_counter % 50 == 0) printf("# Warning: Serial timeout\n");
if (timeoutCnt > TIMEOUT && main_loop_counter % 50 == 0) printf("# Warning: General timeout\n");
buzzerFreq = 24;
buzzerPattern = 1;
} else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot
if (main_loop_counter % 50 == 0) printf("# Warning: STM32 is getting hot: %4.1f°C\n", board_temp_deg_c / 10.0);
buzzerFreq = 4;
buzzerPattern = 1;
} else if (BAT_LVL1_ENABLE && batVoltage < BAT_LVL1) { // low bat 1: fast beep
if (main_loop_counter % 50 == 0) printf("# Warning: Battery is getting empty 1: %4.2fV\n", batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC / 100.0);
buzzerFreq = 5;
buzzerPattern = 6;
} else if (BAT_LVL2_ENABLE && batVoltage < BAT_LVL2) { // low bat 2: slow beep
if (main_loop_counter % 50 == 0) printf("# Warning: Battery is getting empty 2: %4.2fV\n", batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC / 100.0);
buzzerFreq = 5;
buzzerPattern = 42;
} else if (BEEPS_BACKWARD && ((speed < -50 && speedAvg < 0) || MultipleTapBrake.b_multipleTap)) { // backward beep
buzzerFreq = 5;
buzzerPattern = 1;
} else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // 1 beep (low pitch): Motor error, disable motors
enable = 0;
beepCount(1, 24, 1);
printf("# Warning: Left_err: %i Right_err: %i\n", rtY_Left.z_errCode);
} else if (timeoutFlagADC) { // 2 beeps (low pitch): ADC timeout
beepCount(2, 24, 1);
printf("# Warning: ADC timeout\n");
} else if (timeoutFlagSerial) { // 3 beeps (low pitch): Serial timeout
beepCount(3, 24, 1);
printf("# Warning: Serial timeout\n");
} else if (timeoutCnt > TIMEOUT) { // 4 beeps (low pitch): General timeout (PPM, PWM, Nunchuck)
beepCount(4, 24, 1);
printf("# Warning: General timeout\n");
} else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // 5 beeps (low pitch): Mainboard temperature warning
beepCount(5, 24, 1);
printf("# Warning: STM32 is getting hot: %4.1f°C\n", board_temp_deg_c / 10.0);
} else if (BAT_LVL1_ENABLE && batVoltage < BAT_LVL1) { // 1 beep fast (medium pitch): Low bat 1
beepCount(0, 10, 6);
printf("# Warning: Battery is getting empty 1: %4.2fV\n", batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC / 100.0);
} else if (BAT_LVL2_ENABLE && batVoltage < BAT_LVL2) { // 1 beep slow (medium pitch): Low bat 2
beepCount(0, 10, 30);
printf("# Warning: Battery is getting empty 2: %4.2fV\n", batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC / 100.0);
} else if (BEEPS_BACKWARD && ((speed < -50 && speedAvg < 0) || MultipleTapBrake.b_multipleTap)) { // 1 beep fast (high pitch): Backward spinning motors
beepCount(0, 5, 1);
backwardDrive = 1;
} else { // do not beep
buzzerFreq = 0;
buzzerPattern = 0;
beepCount(0, 0, 0);
backwardDrive = 0;
}

100
Src/util.c
View File

@@ -45,6 +45,7 @@ extern UART_HandleTypeDef huart3;
extern int16_t batVoltage;
extern uint8_t backwardDrive;
extern uint8_t buzzerCount; // global variable for the buzzer counts. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7...
@@ -368,37 +369,46 @@ void UART_DisableRxErrors(UART_HandleTypeDef *huart)
/* =========================== General Functions =========================== */
void poweronMelody(void) {
for (int i = 8; i >= 0; i--) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
}
buzzerFreq = 0;
buzzerCount = 0; // prevent interraction with beep counter
for (int i = 8; i >= 0; i--) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
}
buzzerFreq = 0;
}
void shortBeep(uint8_t freq) {
void beepCount(uint8_t cnt, uint8_t freq, uint8_t pattern) {
buzzerCount = cnt;
buzzerFreq = freq;
buzzerPattern = pattern;
}
void beepLong(uint8_t freq) {
buzzerCount = 0; // prevent interraction with beep counter
buzzerFreq = freq;
HAL_Delay(500);
buzzerFreq = 0;
}
void beepShort(uint8_t freq) {
buzzerCount = 0; // prevent interraction with beep counter
buzzerFreq = freq;
HAL_Delay(100);
buzzerFreq = 0;
}
void shortBeepMany(uint8_t cnt, int8_t dir) {
void beepShortMany(uint8_t cnt, int8_t dir) {
if (dir >= 0) { // increasing tone
for(uint8_t i = 2*cnt; i >= 2; i=i-2) {
shortBeep(i + 3);
beepShort(i + 3);
}
} else { // decreasing tone
for(uint8_t i = 2; i <= 2*cnt; i=i+2) {
shortBeep(i + 3);
beepShort(i + 3);
}
}
}
void longBeep(uint8_t freq) {
buzzerFreq = freq;
HAL_Delay(500);
buzzerFreq = 0;
}
void calcAvgSpeed(void) {
// Calculate measured average speed. The minus sign (-) is because motors spin in opposite directions
#if !defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION)
@@ -470,7 +480,7 @@ int checkInputType(int16_t min, int16_t mid, int16_t max){
#ifdef CONTROL_ADC
if ((min + INPUT_MARGIN - ADC_PROTECT_THRESH) > 0 && (max - INPUT_MARGIN + ADC_PROTECT_THRESH) < 4095) {
printf(" and protected");
longBeep(2); // Indicate protection by a beep
beepLong(2); // Indicate protection by a beep
}
#endif
}
@@ -498,10 +508,7 @@ void adcCalibLim(void) {
}
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
printf("# Input calibration started...\n");
printf("# move the potentiometers freely to the min and max limits repeatedly\n");
printf("# release potentiometers to the resting postion\n");
printf("# press the power button to confirm or wait for the 20 sec timeout\n");
printf("Input calibration started...\r\n");
readInput();
// Inititalization: MIN = a high value, MAX = a low value
@@ -535,10 +542,10 @@ void adcCalibLim(void) {
INPUT1_MIN_CAL = INPUT1_MIN_temp + INPUT_MARGIN;
INPUT1_MID_CAL = INPUT1_MID_temp;
INPUT1_MAX_CAL = INPUT1_MAX_temp - INPUT_MARGIN;
printf("# Input1 OK\n"); HAL_Delay(10);
printf("# Input1 OK\r\n"); HAL_Delay(10);
} else {
INPUT1_TYP_CAL = 0; // Disable input
printf("# Input1 Fail\n"); HAL_Delay(10);
printf("# Input1 Fail\r\n"); HAL_Delay(10);
}
INPUT2_TYP_CAL = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp);
@@ -546,10 +553,10 @@ void adcCalibLim(void) {
INPUT2_MIN_CAL = INPUT2_MIN_temp + INPUT_MARGIN;
INPUT2_MID_CAL = INPUT2_MID_temp;
INPUT2_MAX_CAL = INPUT2_MAX_temp - INPUT_MARGIN;
printf("# Input2 OK\n"); HAL_Delay(10);
printf("# Input2 OK\r\n"); HAL_Delay(10);
} else {
INPUT2_TYP_CAL = 0; // Disable input
printf("# Input2 Fail\n"); HAL_Delay(10);
printf("# Input2 Fail\r\n"); HAL_Delay(10);
}
inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown
printf("# Limits: INPUT1_TYP_CAL:%i INPUT1_MIN_CAL:%i INPUT1_MID_CAL:%i INPUT1_MAX_CAL:%i INPUT2_TYP_CAL:%i INPUT2_MIN_CAL:%i INPUT2_MID_CAL:%i INPUT2_MAX_CAL:%i\n",
@@ -569,9 +576,7 @@ void updateCurSpdLim(void) {
}
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
printf("# Torque and Speed limits update started...\n");
printf("# move and hold the pots to a desired limit position for Current and Speed\n");
printf("# press the power button to confirm or wait for the 10 sec timeout\n");
printf("Torque and Speed limits update started...\r\n");
int32_t input1_fixdt = input1 << 16;
int32_t input2_fixdt = input2 << 16;
@@ -737,16 +742,17 @@ void cruiseControl(uint8_t button) {
/* =========================== Poweroff Functions =========================== */
void poweroff(void) {
buzzerPattern = 0;
enable = 0;
printf("# -- Motors disabled --\n");
enable = 0;
printf("-- Motors disabled --\r\n");
buzzerCount = 0; // prevent interraction with beep counter
buzzerPattern = 0;
for (int i = 0; i < 8; i++) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
}
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
}
saveConfig();
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET);
while(1) {}
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET);
while(1) {}
}
@@ -757,19 +763,19 @@ void poweroffPressCheck(void) {
uint16_t cnt_press = 0;
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
HAL_Delay(10);
if (cnt_press++ == 5 * 100) { shortBeep(5); }
if (cnt_press++ == 5 * 100) { beepShort(5); }
}
if (cnt_press >= 5 * 100) { // Check if press is more than 5 sec
HAL_Delay(1000);
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { // Double press: Adjust Max Current, Max Speed
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
longBeep(8);
beepLong(8);
updateCurSpdLim();
shortBeep(5);
beepShort(5);
} else { // Long press: Calibrate ADC Limits
longBeep(16);
beepLong(16);
adcCalibLim();
shortBeep(5);
beepShort(5);
}
} else { // Short press: power off
poweroff();
@@ -779,11 +785,11 @@ void poweroffPressCheck(void) {
if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0;
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
shortBeep(5);
beepShort(5);
HAL_Delay(300);
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); }
longBeep(5);
beepLong(5);
HAL_Delay(350);
poweroff();
} else {
@@ -791,7 +797,7 @@ void poweroffPressCheck(void) {
if (setDistance > 2.6) {
setDistance = 0.5;
}
shortBeep(setDistance / 0.25);
beepShort(setDistance / 0.25);
saveValue = setDistance * 1000;
saveValue_valid = 1;
}
@@ -847,7 +853,7 @@ void readInput(void) {
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
// Handle received data validity, timeout and fix out-of-sync if necessary
#ifdef CONTROL_IBUS
#ifdef CONTROL_IBUS
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) {
ibus_captured_value[(i/2)] = CLAMP(command.channels[i] + (command.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000
}
@@ -1161,7 +1167,7 @@ void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sid
{
uint16_t checksum;
if (Sideboard_in->start == SERIAL_START_FRAME) {
checksum = (uint16_t)(Sideboard_in->start ^ Sideboard_in->roll ^ Sideboard_in->pitch ^ Sideboard_in->yaw ^ Sideboard_in->sensors);
checksum = (uint16_t)(Sideboard_in->start ^ Sideboard_in->pitch ^ Sideboard_in->dPitch ^ Sideboard_in->cmd1 ^ Sideboard_in->cmd2 ^ Sideboard_in->sensors);
if (Sideboard_in->checksum == checksum) {
*Sideboard_out = *Sideboard_in;
if (usart_idx == 2) { // Sideboard USART2
@@ -1295,7 +1301,7 @@ void sideboardSensors(uint8_t sensors) {
rtP_Right.z_ctrlTypSel = COM_CTRL;
break;
}
shortBeepMany(sensor1_index + 1, 1);
beepShortMany(sensor1_index + 1, 1);
}
// Field Weakening: use Sensor2 as push button
@@ -1320,7 +1326,7 @@ void sideboardSensors(uint8_t sensors) {
Input_Lim_Init();
break;
}
shortBeepMany(sensor2_index + 1, 1);
beepShortMany(sensor2_index + 1, 1);
}
#endif // CRUISE_CONTROL_SUPPORT
#endif