interfisch
/
hoverboard-firmware-hack-foc-serial-esc
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Update util.c

This commit is contained in:
Candas1 2020-10-28 23:34:29 +01:00 committed by GitHub
parent 78246f4e53
commit dd09115e95
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 126 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

260
Src/util.c
View File

@ -88,6 +88,8 @@ ExtY rtY_Right; /* External outputs */
int16_t cmd1; // normalized input value. -1000 to 1000 int16_t cmd1; // normalized input value. -1000 to 1000
int16_t cmd2; // normalized input value. -1000 to 1000 int16_t cmd2; // normalized input value. -1000 to 1000
int16_t cmd1_in; // normalized input value. -1000 to 1000
int16_t cmd2_in; // normalized input value. -1000 to 1000
int16_t speedAvg; // average measured speed int16_t speedAvg; // average measured speed
int16_t speedAvgAbs; // average measured speed in absolute int16_t speedAvgAbs; // average measured speed in absolute
@ -125,24 +127,24 @@ uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300}; // Dummy virtual address to av
static int16_t INPUT_MAX; // [-] Input target maximum limitation static int16_t INPUT_MAX; // [-] Input target maximum limitation
static int16_t INPUT_MIN; // [-] Input target minimum limitation static int16_t INPUT_MIN; // [-] Input target minimum limitation
#ifdef CONTROL_ADC
static uint8_t cur_spd_valid = 0; static uint8_t cur_spd_valid = 0;
static uint8_t adc_cal_valid = 0; static uint8_t adc_cal_valid = 0;
static uint16_t ADC1_MIN_CAL = ADC1_MIN; static uint16_t INPUT1_MIN_CAL = INPUT1_MIN;
static uint16_t ADC1_MAX_CAL = ADC1_MAX; static uint16_t INPUT1_MAX_CAL = INPUT1_MAX;
static uint16_t ADC2_MIN_CAL = ADC2_MIN; static uint16_t INPUT2_MIN_CAL = INPUT2_MIN;
static uint16_t ADC2_MAX_CAL = ADC2_MAX; static uint16_t INPUT2_MAX_CAL = INPUT2_MAX;
#ifdef ADC1_MID_POT #ifdef INPUT1_MID_POT
static uint16_t ADC1_MID_CAL = ADC1_MID; static uint16_t INPUT1_MID_CAL = INPUT1_MID;
#else #else
static uint16_t ADC1_MID_CAL = 0; static uint16_t INPUT1_MID_CAL = 0;
#endif
#ifdef ADC1_MID_POT
static uint16_t ADC2_MID_CAL = ADC2_MID;
#else
static uint16_t ADC2_MID_CAL = 0;
#endif
#endif #endif
#ifdef INPUT1_MID_POT
static uint16_t INPUT2_MID_CAL = INPUT2_MID;
#else
static uint16_t INPUT2_MID_CAL = 0;
#endif
#if defined(CONTROL_ADC) && defined(ADC_PROTECT_ENA) #if defined(CONTROL_ADC) && defined(ADC_PROTECT_ENA)
static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection
@ -278,12 +280,12 @@ void Input_Init(void) {
EE_Init(); /* EEPROM Init */ EE_Init(); /* EEPROM Init */
EE_ReadVariable(VirtAddVarTab[0], &writeCheck); EE_ReadVariable(VirtAddVarTab[0], &writeCheck);
if (writeCheck == FLASH_WRITE_KEY) { if (writeCheck == FLASH_WRITE_KEY) {
EE_ReadVariable(VirtAddVarTab[1], &ADC1_MIN_CAL); EE_ReadVariable(VirtAddVarTab[1], &INPUT1_MIN_CAL);
EE_ReadVariable(VirtAddVarTab[2], &ADC1_MAX_CAL); EE_ReadVariable(VirtAddVarTab[2], &INPUT1_MAX_CAL);
EE_ReadVariable(VirtAddVarTab[3], &ADC1_MID_CAL); EE_ReadVariable(VirtAddVarTab[3], &INPUT1_MID_CAL);
EE_ReadVariable(VirtAddVarTab[4], &ADC2_MIN_CAL); EE_ReadVariable(VirtAddVarTab[4], &INPUT2_MIN_CAL);
EE_ReadVariable(VirtAddVarTab[5], &ADC2_MAX_CAL); EE_ReadVariable(VirtAddVarTab[5], &INPUT2_MAX_CAL);
EE_ReadVariable(VirtAddVarTab[6], &ADC2_MID_CAL); EE_ReadVariable(VirtAddVarTab[6], &INPUT2_MID_CAL);
EE_ReadVariable(VirtAddVarTab[7], &i_max); EE_ReadVariable(VirtAddVarTab[7], &i_max);
EE_ReadVariable(VirtAddVarTab[8], &n_max); EE_ReadVariable(VirtAddVarTab[8], &n_max);
rtP_Left.i_max = i_max; rtP_Left.i_max = i_max;
@ -427,65 +429,63 @@ void calcAvgSpeed(void) {
* - release potentiometers to the resting postion * - release potentiometers to the resting postion
* - press the power button to confirm or wait for the 20 sec timeout * - press the power button to confirm or wait for the 20 sec timeout
*/ */
void adcCalibLim(void) { void inputCalibLim(void) {
if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning
return; return;
} }
#ifdef CONTROL_ADC
consoleLog("ADC calibration started... ");
// Inititalization: MIN = a high values, MAX = a low value, consoleLog("Input calibration started... ");
int32_t adc1_fixdt = adc_buffer.l_tx2 << 16;
int32_t adc2_fixdt = adc_buffer.l_rx2 << 16;
uint16_t adc_cal_timeout = 0;
uint16_t ADC1_MIN_temp = 4095;
uint16_t ADC1_MID_temp = 0;
uint16_t ADC1_MAX_temp = 0;
uint16_t ADC2_MIN_temp = 4095;
uint16_t ADC2_MID_temp = 0;
uint16_t ADC2_MAX_temp = 0;
adc_cal_valid = 1; // Inititalization: MIN = a high values, MAX = a low value,
int32_t input1_fixdt = adc_buffer.l_tx2 << 16;
int32_t input2_fixdt = adc_buffer.l_rx2 << 16;
uint16_t input_cal_timeout = 0;
uint16_t INPUT1_MIN_temp = 4095;
uint16_t INPUT1_MID_temp = 0;
uint16_t INPUT1_MAX_temp = 0;
uint16_t INPUT2_MIN_temp = 4095;
uint16_t INPUT2_MID_temp = 0;
uint16_t INPUT2_MAX_temp = 0;
// Extract MIN, MAX and MID from ADC while the power button is not pressed input_cal_valid = 1;
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && adc_cal_timeout++ < 4000) { // 20 sec timeout
filtLowPass32(adc_buffer.l_tx2, FILTER, &adc1_fixdt);
filtLowPass32(adc_buffer.l_rx2, FILTER, &adc2_fixdt);
ADC1_MID_temp = (uint16_t)CLAMP(adc1_fixdt >> 16, 0, 4095); // convert fixed-point to integer
ADC2_MID_temp = (uint16_t)CLAMP(adc2_fixdt >> 16, 0, 4095);
ADC1_MIN_temp = MIN(ADC1_MIN_temp, ADC1_MID_temp);
ADC1_MAX_temp = MAX(ADC1_MAX_temp, ADC1_MID_temp);
ADC2_MIN_temp = MIN(ADC2_MIN_temp, ADC2_MID_temp);
ADC2_MAX_temp = MAX(ADC2_MAX_temp, ADC2_MID_temp);
HAL_Delay(5);
}
// ADC calibration checks // Extract MIN, MAX and MID from ADC while the power button is not pressed
#ifdef ADC_PROTECT_ENA while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && input_cal_timeout++ < 4000) { // 20 sec timeout
if ((ADC1_MIN_temp + 100 - ADC_PROTECT_THRESH) > 0 && (ADC1_MAX_temp - 100 + ADC_PROTECT_THRESH) < 4095 && filtLowPass32(cmd1_in, FILTER, &input1_fixdt);
(ADC2_MIN_temp + 100 - ADC_PROTECT_THRESH) > 0 && (ADC2_MAX_temp - 100 + ADC_PROTECT_THRESH) < 4095) { filtLowPass32(cmd2_in, FILTER, &input2_fixdt);
adc_cal_valid = 1; INPUT1_MID_temp = (uint16_t)CLAMP(input1_fixdt >> 16, 0, 4095); // convert fixed-point to integer
} else { INPUT2_MID_temp = (uint16_t)CLAMP(input2_fixdt >> 16, 0, 4095);
adc_cal_valid = 0; INPUT1_MIN_temp = MIN(INPUT1_MIN_temp, INPUT1_MID_temp);
consoleLog("FAIL (ADC out-of-range protection not possible)\n"); INPUT1_MAX_temp = MAX(INPUT1_MAX_temp, INPUT1_MID_temp);
} INPUT2_MIN_temp = MIN(INPUT2_MIN_temp, INPUT2_MID_temp);
#endif INPUT2_MAX_temp = MAX(INPUT2_MAX_temp, INPUT2_MID_temp);
HAL_Delay(5);
// Add final ADC margin to have exact 0 and MAX at the minimum and maximum ADC value }
if (adc_cal_valid && (ADC1_MAX_temp - ADC1_MIN_temp) > 500 && (ADC2_MAX_temp - ADC2_MIN_temp) > 500) {
ADC1_MIN_CAL = ADC1_MIN_temp + 100;
ADC1_MID_CAL = ADC1_MID_temp;
ADC1_MAX_CAL = ADC1_MAX_temp - 100;
ADC2_MIN_CAL = ADC2_MIN_temp + 100;
ADC2_MID_CAL = ADC2_MID_temp;
ADC2_MAX_CAL = ADC2_MAX_temp - 100;
consoleLog("OK\n");
} else {
adc_cal_valid = 0;
consoleLog("FAIL (Pots travel too short)\n");
}
// ADC calibration checks
#ifdef ADC_PROTECT_ENA
if ((INPUT1_MIN_temp + 100 - ADC_PROTECT_THRESH) > 0 && (INPUT1_MAX_temp - 100 + ADC_PROTECT_THRESH) < 4095 &&
(INPUT2_MIN_temp + 100 - ADC_PROTECT_THRESH) > 0 && (INPUT2_MAX_temp - 100 + ADC_PROTECT_THRESH) < 4095) {
input_cal_valid = 1;
} else {
input_cal_valid = 0;
consoleLog("FAIL (ADC out-of-range protection not possible)\n");
}
#endif #endif
// Add final ADC margin to have exact 0 and MAX at the minimum and maximum ADC value
if (input_cal_valid && (INPUT1_MAX_temp - INPUT1_MIN_temp) > 500 && (INPUT2_MAX_temp - INPUT2_MIN_temp) > 500) {
INPUT1_MIN_CAL = INPUT1_MIN_temp + 100;
INPUT1_MID_CAL = INPUT1_MID_temp;
INPUT1_MAX_CAL = INPUT1_MAX_temp - 100;
INPUT2_MIN_CAL = INPUT2_MIN_temp + 100;
INPUT2_MID_CAL = INPUT2_MID_temp;
INPUT2_MAX_CAL = INPUT2_MAX_temp - 100;
consoleLog("OK\n");
} else {
input_cal_valid = 0;
consoleLog("FAIL (Pots travel too short)\n");
}
} }
@ -548,12 +548,12 @@ void saveConfig() {
if (adc_cal_valid || cur_spd_valid) { if (adc_cal_valid || cur_spd_valid) {
HAL_FLASH_Unlock(); HAL_FLASH_Unlock();
EE_WriteVariable(VirtAddVarTab[0], FLASH_WRITE_KEY); EE_WriteVariable(VirtAddVarTab[0], FLASH_WRITE_KEY);
EE_WriteVariable(VirtAddVarTab[1], ADC1_MIN_CAL); EE_WriteVariable(VirtAddVarTab[1], INPUT1_MIN_CAL);
EE_WriteVariable(VirtAddVarTab[2], ADC1_MAX_CAL); EE_WriteVariable(VirtAddVarTab[2], INPUT1_MAX_CAL);
EE_WriteVariable(VirtAddVarTab[3], ADC1_MID_CAL); EE_WriteVariable(VirtAddVarTab[3], INPUT1_MID_CAL);
EE_WriteVariable(VirtAddVarTab[4], ADC2_MIN_CAL); EE_WriteVariable(VirtAddVarTab[4], INPUT2_MIN_CAL);
EE_WriteVariable(VirtAddVarTab[5], ADC2_MAX_CAL); EE_WriteVariable(VirtAddVarTab[5], INPUT2_MAX_CAL);
EE_WriteVariable(VirtAddVarTab[6], ADC2_MID_CAL); EE_WriteVariable(VirtAddVarTab[6], INPUT2_MID_CAL);
EE_WriteVariable(VirtAddVarTab[7], rtP_Left.i_max); EE_WriteVariable(VirtAddVarTab[7], rtP_Left.i_max);
EE_WriteVariable(VirtAddVarTab[8], rtP_Left.n_max); EE_WriteVariable(VirtAddVarTab[8], rtP_Left.n_max);
HAL_FLASH_Lock(); HAL_FLASH_Lock();
@ -731,21 +731,16 @@ void poweroffPressCheck(void) {
#endif #endif
} }
void readInput(void) {
#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK)
/* =========================== Read Command Function =========================== */
void readCommand(void) {
#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK)
if (nunchuk_connected != 0) { if (nunchuk_connected != 0) {
Nunchuk_Read(); Nunchuk_Read();
cmd1_in = (nunchuk_data[0] - 127) * 8; // X axis 0-255 cmd1_in = (nunchuk_data[0] - 127) * 8; // X axis 0-255
cmd2_in = (nunchuk_data[1] - 128) * 8; // Y axis 0-255 cmd2_in = (nunchuk_data[1] - 128) * 8; // Y axis 0-255
#ifdef SUPPORT_BUTTONS #ifdef SUPPORT_BUTTONS
button1 = (uint8_t)nunchuk_data[5] & 1; button1 = (uint8_t)nunchuk_data[5] & 1;
button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1; button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1;
#endif #endif
} }
#endif #endif
@ -756,28 +751,44 @@ void readCommand(void) {
#ifdef SUPPORT_BUTTONS #ifdef SUPPORT_BUTTONS
button1 = ppm_captured_value[5] > 500; button1 = ppm_captured_value[5] > 500;
button2 = 0; button2 = 0;
#elif defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
#endif #endif
// float scale = ppm_captured_value[2] / 1000.0f; // not used for now, uncomment if needed
#endif #endif
#if defined(CONTROL_PWM_LEFT) || defined(CONTROL_PWM_RIGHT) #if defined(CONTROL_PWM_LEFT) || defined(CONTROL_PWM_RIGHT)
cmd1_in = (pwm_captured_ch1_value - 500) * 2; cmd1_in = (pwm_captured_ch1_value - 500) * 2;
cmd2_in = (pwm_captured_ch2_value - 500) * 2; cmd2_in = (pwm_captured_ch2_value - 500) * 2;
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
#endif
#endif #endif
#ifdef CONTROL_ADC #ifdef CONTROL_ADC
// ADC values range: 0-4095, see ADC-calibration in config.h // ADC values range: 0-4095, see ADC-calibration in config.h
cmd1_in = adc_buffer.l_tx2; cmd1_in = adc_buffer.l_tx2;
cmd2_in = adc_buffer.l_rx2; cmd2_in = adc_buffer.l_rx2;
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
// Handle received data validity, timeout and fix out-of-sync if necessary
#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
}
cmd1_in = (ibus_captured_value[0] - 500) * 2;
cmd2_in = (ibus_captured_value[1] - 500) * 2;
#else
if (IN_RANGE(command.steer, INPUT_MIN, INPUT_MAX) && IN_RANGE(command.speed, INPUT_MIN, INPUT_MAX)) {
cmd1_in = command.steer;
cmd2_in = command.speed;
}
#endif
timeoutCnt = 0;
#endif
}
/* =========================== Read Command Function =========================== */
void readCommand(void) {
readInput();
#ifdef CONTROL_ADC
#ifdef ADC_PROTECT_ENA #ifdef ADC_PROTECT_ENA
if (adc_buffer.l_tx2 >= (ADC1_MIN_CAL - ADC_PROTECT_THRESH) && adc_buffer.l_tx2 <= (ADC1_MAX_CAL + ADC_PROTECT_THRESH) && if (adc_buffer.l_tx2 >= (ADC1_MIN_CAL - ADC_PROTECT_THRESH) && adc_buffer.l_tx2 <= (ADC1_MAX_CAL + ADC_PROTECT_THRESH) &&
adc_buffer.l_rx2 >= (ADC2_MIN_CAL - ADC_PROTECT_THRESH) && adc_buffer.l_rx2 <= (ADC2_MAX_CAL + ADC_PROTECT_THRESH)) { adc_buffer.l_rx2 >= (ADC2_MIN_CAL - ADC_PROTECT_THRESH) && adc_buffer.l_rx2 <= (ADC2_MAX_CAL + ADC_PROTECT_THRESH)) {
@ -795,32 +806,6 @@ void readCommand(void) {
} }
#endif #endif
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
#endif
timeoutCnt = 0;
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
// Handle received data validity, timeout and fix out-of-sync if necessary
#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
}
cmd1_in = (ibus_captured_value[0] - 500) * 2;
cmd2_in = (ibus_captured_value[1] - 500) * 2;
#else
if (IN_RANGE(command.steer, INPUT_MIN, INPUT_MAX) && IN_RANGE(command.speed, INPUT_MIN, INPUT_MAX)) {
cmd1 = command.steer;
cmd2 = command.speed;
}
#endif
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
#endif
timeoutCnt = 0; timeoutCnt = 0;
#endif #endif
@ -842,29 +827,32 @@ void readCommand(void) {
timeoutFlagSerial = timeoutFlagSerial_L || timeoutFlagSerial_R; timeoutFlagSerial = timeoutFlagSerial_L || timeoutFlagSerial_R;
#endif #endif
cmd1 = addDeadBand(cmd1_in, INPUT1_DEADBAND, INPUT1_MIN, INPUT1_MID, INPUT1_MAX, INPUT_MIN, INPUT_MAX); #ifdef INPUT1_MID_POT
cmd1 = addDeadBand(cmd1_in, INPUT1_DEADBAND, INPUT1_MIN, INPUT1_MID, INPUT1_MAX, INPUT_MIN, INPUT_MAX);
#else
cmd1 = MAP( cmd1_in , INPUT1_MIN_CAL, INPUT1_MAX_CAL, 0, INPUT_MAX ); // ADC1
#endif
#if !defined(VARIANT_SKATEBOARD) #if !defined(VARIANT_SKATEBOARD)
cmd2 = addDeadBand(cmd2_in, INPUT2_DEADBAND, INPUT2_MIN, INPUT2_MID, INPUT2_MAX, INPUT_MIN, INPUT_MAX); #ifdef INPUT2_MID_POT
cmd2 = addDeadBand(cmd2_in, INPUT2_DEADBAND, INPUT2_MIN, INPUT2_MID, INPUT2_MAX, INPUT_MIN, INPUT_MAX);
#else
cmd2 = MAP( cmd2_in , INPUT2_MIN_CAL, INPUT2_MAX_CAL, 0, INPUT_MAX ); // ADC2
#endif
#else #else
cmd2 = addDeadBand(cmd2_in, INPUT2_DEADBAND, INPUT2_MIN, INPUT2_MID, INPUT2_MAX, INPUT_OUT_MIN, INPUT_MAX); cmd2 = addDeadBand(cmd2_in, INPUT2_DEADBAND, INPUT2_MIN, INPUT2_MID, INPUT2_MAX, INPUT_OUT_MIN, INPUT_MAX);
#endif #endif
#ifdef ADC1_MID_POT
#else
cmd1 = MAP( adc_buffer.l_tx2 , ADC1_MIN_CAL, ADC1_MAX_CAL, 0, INPUT_MAX ); // ADC1
#endif
#ifdef ADC2_MID_POT
#else
cmd2 = MAP( adc_buffer.l_rx2 , ADC2_MIN_CAL, ADC2_MAX_CAL, 0, INPUT_MAX ); // ADC2
#endif
#ifdef VARIANT_HOVERCAR #ifdef VARIANT_HOVERCAR
brakePressed = (uint8_t)(cmd1 > 50); brakePressed = (uint8_t)(cmd1 > 50);
#endif #endif
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
button2 = !HAL_GPIO_ReadPin(BUTTON2_PORT, BUTTON2_PIN);
#endif
#ifdef VARIANT_TRANSPOTTER #ifdef VARIANT_TRANSPOTTER
#ifdef GAMETRAK_CONNECTION_NORMAL #ifdef GAMETRAK_CONNECTION_NORMAL
cmd1 = adc_buffer.l_rx2; cmd1 = adc_buffer.l_rx2;