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

Dual-inputs implemented

Browse Source 
- implemented dual-inputs functionality
- the dual-inputs combinations mentioned in Readme are now supported
This commit is contained in:
EmanuelFeru
2020-12-20 10:16:31 +01:00
parent 5ca3fa4f85
commit df86ef44fd
8 changed files with 745 additions and 505 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
Src/control.c
View File

@@ -8,10 +8,11 @@
TIM_HandleTypeDef TimHandle;
TIM_HandleTypeDef TimHandle2;
uint8_t ppm_count = 0;
uint8_t pwm_count = 0;
uint32_t timeoutCnt = 0;
uint8_t nunchuk_data[6] = {0};
uint8_t ppm_count = 0;
uint8_t pwm_count = 0;
uint32_t timeoutCntGen = 0;
uint8_t timeoutFlgGen = 0;
uint8_t nunchuk_data[6] = {0};
uint8_t i2cBuffer[2];
@@ -34,7 +35,8 @@ void PPM_ISR_Callback(void) {
if (rc_delay > 3000) {
if (ppm_valid && ppm_count == PPM_NUM_CHANNELS) {
ppm_timeout = 0;
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
memcpy(ppm_captured_value, ppm_captured_value_buffer, sizeof(ppm_captured_value));
}
ppm_valid = true;
@@ -122,7 +124,8 @@ void PWM_ISR_CH1_Callback(void) {
} else { // Falling Edge interrupt -> measure pulse duration
uint16_t rc_signal = TIM2->CNT - pwm_CNT_prev_ch1;
if (IN_RANGE(rc_signal, 900, 2100)){
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
pwm_timeout_ch1 = 0;
pwm_captured_ch1_value = CLAMP(rc_signal, 1000, 2000) - 1000;
}
@@ -141,7 +144,8 @@ void PWM_ISR_CH2_Callback(void) {
} else { // Falling Edge interrupt -> measure pulse duration
uint16_t rc_signal = TIM2->CNT - pwm_CNT_prev_ch2;
if (IN_RANGE(rc_signal, 900, 2100)){
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
pwm_timeout_ch2 = 0;
pwm_captured_ch2_value = CLAMP(rc_signal, 1000, 2000) - 1000;
}
@@ -237,11 +241,12 @@ void Nunchuk_Read(void) {
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 10);
HAL_Delay(3);
if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuk_data, 6, 10) == HAL_OK) {
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
}
#ifndef TRANSPOTTER
if (timeoutCnt > 3) {
if (timeoutCntGen > 3) {
HAL_Delay(50);
Nunchuk_Init();
}

76
Src/main.c
View File

@@ -62,14 +62,16 @@ extern ExtY rtY_Left; /* External outputs */
extern ExtY rtY_Right; /* External outputs */
//---------------
extern InputStruct input1; // input structure
extern InputStruct input2; // input structure
extern uint8_t inIdx; // input index used for dual-inputs
extern InputStruct input1[]; // input structure
extern InputStruct input2[]; // input structure
extern int16_t speedAvg; // Average measured speed
extern int16_t speedAvgAbs; // Average measured speed in absolute
extern volatile uint32_t timeoutCnt; // Timeout counter for the General timeout (PPM, PWM, Nunchuck)
extern uint8_t timeoutFlagADC; // Timeout Flag for for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data)
extern uint8_t timeoutFlagSerial; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
extern volatile uint32_t timeoutCntGen; // Timeout counter for the General timeout (PPM, PWM, Nunchuk)
extern volatile uint8_t timeoutFlgGen; // Timeout Flag for the General timeout (PPM, PWM, Nunchuk)
extern uint8_t timeoutFlgADC; // Timeout Flag for for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data)
extern uint8_t timeoutFlgSerial; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
extern volatile int pwml; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
@@ -199,12 +201,12 @@ int main(void) {
while(1) {
HAL_Delay(DELAY_IN_MAIN_LOOP); // delay in ms
readCommand(); // Read Command: input1.cmd, input2.cmd
readCommand(); // Read Command: input1[inIdx].cmd, input2[inIdx].cmd
calcAvgSpeed(); // Calculate average measured speed: speedAvg, speedAvgAbs
#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) && (input1.cmd > -50 && input1.cmd < 50) && (input2.cmd > -50 && input2.cmd < 50)){
if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (input1[inIdx].cmd > -50 && input1[inIdx].cmd < 50) && (input2[inIdx].cmd > -50 && input2[inIdx].cmd < 50)){
beepShort(6); // make 2 beeps indicating the motor enable
beepShort(4); HAL_Delay(100);
steerFixdt = speedFixdt = 0; // reset filters
@@ -225,14 +227,16 @@ int main(void) {
#endif
#ifdef VARIANT_HOVERCAR
if (inIdx == CONTROL_ADC) { // Only use use implementation below if pedals are in use (ADC input)
if (speedAvgAbs < 60) { // Check if Hovercar is physically close to standstill to enable Double tap detection on Brake pedal for Reverse functionality
multipleTapDet(input1.cmd, HAL_GetTick(), &MultipleTapBrake); // Brake pedal in this case is "input1" variable
multipleTapDet(input1[inIdx].cmd, HAL_GetTick(), &MultipleTapBrake); // Brake pedal in this case is "input1" variable
}
if (input1.cmd > 30) { // If Brake pedal (input1) is pressed, bring to 0 also the Throttle pedal (input2) to avoid "Double pedal" driving
input2.cmd = (int16_t)((input2.cmd * speedBlend) >> 15);
if (input1[inIdx].cmd > 30) { // If Brake pedal (input1) is pressed, bring to 0 also the Throttle pedal (input2) to avoid "Double pedal" driving
input2[inIdx].cmd = (int16_t)((input2[inIdx].cmd * speedBlend) >> 15);
cruiseControl((uint8_t)rtP_Left.b_cruiseCtrlEna); // Cruise control deactivated by Brake pedal if it was active
}
}
#endif
#ifdef ELECTRIC_BRAKE_ENABLE
@@ -240,38 +244,43 @@ int main(void) {
#endif
#ifdef VARIANT_HOVERCAR
if (inIdx == CONTROL_ADC) { // Only use use implementation below if pedals are in use (ADC input)
if (speedAvg > 0) { // Make sure the Brake pedal is opposite to the direction of motion AND it goes to 0 as we reach standstill (to avoid Reverse driving by Brake pedal)
input1.cmd = (int16_t)((-input1.cmd * speedBlend) >> 15);
input1[inIdx].cmd = (int16_t)((-input1[inIdx].cmd * speedBlend) >> 15);
} else {
input1.cmd = (int16_t)(( input1.cmd * speedBlend) >> 15);
input1[inIdx].cmd = (int16_t)(( input1[inIdx].cmd * speedBlend) >> 15);
}
}
#endif
#ifdef VARIANT_SKATEBOARD
if (input2.cmd < 0) { // When Throttle is negative, it acts as brake. This condition is to make sure it goes to 0 as we reach standstill (to avoid Reverse driving)
if (input2[inIdx].cmd < 0) { // When Throttle is negative, it acts as brake. This condition is to make sure it goes to 0 as we reach standstill (to avoid Reverse driving)
if (speedAvg > 0) { // Make sure the braking is opposite to the direction of motion
input2.cmd = (int16_t)(( input2.cmd * speedBlend) >> 15);
input2[inIdx].cmd = (int16_t)(( input2[inIdx].cmd * speedBlend) >> 15);
} else {
input2.cmd = (int16_t)((-input2.cmd * speedBlend) >> 15);
input2[inIdx].cmd = (int16_t)((-input2[inIdx].cmd * speedBlend) >> 15);
}
}
#endif
// ####### LOW-PASS FILTER #######
rateLimiter16(input1.cmd , RATE, &steerRateFixdt);
rateLimiter16(input2.cmd , RATE, &speedRateFixdt);
rateLimiter16(input1[inIdx].cmd , RATE, &steerRateFixdt);
rateLimiter16(input2[inIdx].cmd , RATE, &speedRateFixdt);
filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt);
filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt);
steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer
speed = (int16_t)(speedFixdt >> 16); // convert fixed-point to integer
// ####### VARIANT_HOVERCAR #######
#ifdef VARIANT_HOVERCAR
#ifdef VARIANT_HOVERCAR
if (inIdx == CONTROL_ADC) { // Only use use implementation below if pedals are in use (ADC input)
if (!MultipleTapBrake.b_multipleTap) { // Check driving direction
speed = steer + speed; // Forward driving: in this case steer = Brake, speed = Throttle
} else {
speed = steer - speed; // Reverse driving: in this case steer = Brake, speed = Throttle
}
steer = 0; // Do not apply steering to avoid side effects if STEER_COEFFICIENT is NOT 0
}
#endif
// ####### MIXER #######
@@ -295,8 +304,8 @@ int main(void) {
#endif
#ifdef VARIANT_TRANSPOTTER
distance = CLAMP(input1.cmd - 180, 0, 4095);
steering = (input2.cmd - 2048) / 2048.0;
distance = CLAMP(input1[inIdx].cmd - 180, 0, 4095);
steering = (input2[inIdx].cmd - 2048) / 2048.0;
distanceErr = distance - (int)(setDistance * 1345);
if (nunchuk_connected == 0) {
@@ -329,10 +338,11 @@ int main(void) {
enable = 0;
}
}
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
}
if (timeoutCnt > TIMEOUT) {
if (timeoutFlgGen) {
pwml = 0;
pwmr = 0;
enable = 0;
@@ -366,7 +376,8 @@ int main(void) {
#ifdef SUPPORT_LCD
LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Nunchuk Control");
#endif
timeoutCnt = 0;
timeoutCntGen = 0;
timeoutFlgGen = 0;
HAL_Delay(1000);
nunchuk_connected = 1;
}
@@ -392,11 +403,11 @@ int main(void) {
#endif
// ####### SIDEBOARDS HANDLING #######
#if defined(SIDEBOARD_SERIAL_USART2)
#if defined(SIDEBOARD_SERIAL_USART2) && defined(FEEDBACK_SERIAL_USART2)
sideboardLeds(&sideboard_leds_L);
sideboardSensors((uint8_t)Sideboard_L.sensors);
#endif
#if defined(SIDEBOARD_SERIAL_USART3)
#if defined(SIDEBOARD_SERIAL_USART3) && defined(FEEDBACK_SERIAL_USART3)
sideboardLeds(&sideboard_leds_R);
sideboardSensors((uint8_t)Sideboard_R.sensors);
#endif
@@ -410,8 +421,8 @@ int main(void) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms
printf("in1:%i in2:%i cmdL:%i cmdR:%i BatADC:%i BatV:%i TempADC:%i Temp:%i\r\n",
input1.raw, // 1: INPUT1
input2.raw, // 2: INPUT2
input1[inIdx].raw, // 1: INPUT1
input2[inIdx].raw, // 2: INPUT2
cmdL, // 3: output command: [-1000, 1000]
cmdR, // 4: output command: [-1000, 1000]
adc_buffer.batt1, // 5: for battery voltage calibration
@@ -425,8 +436,8 @@ int main(void) {
#if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3)
if (main_loop_counter % 2 == 0) { // Send data periodically every 10 ms
Feedback.start = (uint16_t)SERIAL_START_FRAME;
Feedback.cmd1 = (int16_t)input1.cmd;
Feedback.cmd2 = (int16_t)input2.cmd;
Feedback.cmd1 = (int16_t)input1[inIdx].cmd;
Feedback.cmd2 = (int16_t)input2[inIdx].cmd;
Feedback.speedR_meas = (int16_t)rtY_Right.n_mot;
Feedback.speedL_meas = (int16_t)rtY_Left.n_mot;
Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC);
@@ -462,11 +473,11 @@ int main(void) {
} else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // 1 beep (low pitch): Motor error, disable motors
enable = 0;
beepCount(1, 24, 1);
} else if (timeoutFlagADC) { // 2 beeps (low pitch): ADC timeout
} else if (timeoutFlgADC) { // 2 beeps (low pitch): ADC timeout
beepCount(2, 24, 1);
} else if (timeoutFlagSerial) { // 3 beeps (low pitch): Serial timeout
} else if (timeoutFlgSerial) { // 3 beeps (low pitch): Serial timeout
beepCount(3, 24, 1);
} else if (timeoutCnt > TIMEOUT) { // 4 beeps (low pitch): General timeout (PPM, PWM, Nunchuck)
} else if (timeoutFlgGen) { // 4 beeps (low pitch): General timeout (PPM, PWM, Nunchuk)
beepCount(4, 24, 1);
} else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // 5 beeps (low pitch): Mainboard temperature warning
beepCount(5, 24, 1);
@@ -498,7 +509,6 @@ int main(void) {
cmdL_prev = cmdL;
cmdR_prev = cmdR;
main_loop_counter++;
timeoutCnt++;
}
}

661
Src/util.c
View File

@@ -53,7 +53,8 @@ extern uint8_t buzzerPattern; // global variable for the buzzer patter
extern uint8_t enable; // global variable for motor enable
extern uint8_t nunchuk_data[6];
extern volatile uint32_t timeoutCnt; // global variable for general timeout counter
extern volatile uint32_t timeoutCntGen; // global counter for general timeout counter
extern volatile uint8_t timeoutFlgGen; // global flag for general timeout counter
extern volatile uint32_t main_loop_counter;
#if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)
@@ -87,13 +88,19 @@ ExtU rtU_Right; /* External inputs */
ExtY rtY_Right; /* External outputs */
//---------------
InputStruct input1; // input structure
InputStruct input2; // input structure
uint8_t inIdx;
#if defined(PRI_INPUT1) && defined(PRI_INPUT2) && defined(AUX_INPUT1) && defined(AUX_INPUT2)
InputStruct input1[INPUTS_NR] = { {0, 0, 0, PRI_INPUT1}, {0, 0, 0, AUX_INPUT1} };
InputStruct input2[INPUTS_NR] = { {0, 0, 0, PRI_INPUT2}, {0, 0, 0, AUX_INPUT2} };
#else
InputStruct input1[INPUTS_NR] = { {0, 0, 0, PRI_INPUT1} };
InputStruct input2[INPUTS_NR] = { {0, 0, 0, PRI_INPUT2} };
#endif
int16_t speedAvg; // average measured speed
int16_t speedAvgAbs; // average measured speed in absolute
uint8_t timeoutFlagADC = 0; // Timeout Flag for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data)
uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
uint8_t timeoutFlgADC = 0; // Timeout Flag for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data)
uint8_t timeoutFlgSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
uint8_t ctrlModReqRaw = CTRL_MOD_REQ;
uint8_t ctrlModReq = CTRL_MOD_REQ; // Final control mode request
@@ -110,13 +117,14 @@ uint8_t nunchuk_connected = 0;
#ifdef VARIANT_TRANSPOTTER
float setDistance;
uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1337}; // Virtual address defined by the user: 0xFFFF value is prohibited
uint16_t VirtAddVarTab[NB_OF_VAR] = {1337}; // Virtual address defined by the user: 0xFFFF value is prohibited
static uint16_t saveValue = 0;
static uint8_t saveValue_valid = 0;
#elif !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300, 1301, 1302, 1303, 1304, 1305, 1306, 1307, 1308, 1309, 1310};
uint16_t VirtAddVarTab[NB_OF_VAR] = {1000, 1001, 1002, 1003, 1004, 1005, 1006, 1007, 1008, 1009,
1010, 1011, 1012, 1013, 1014, 1015, 1016, 1017, 1018};
#else
uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300}; // Dummy virtual address to avoid warnings
uint16_t VirtAddVarTab[NB_OF_VAR] = {1000}; // Dummy virtual address to avoid warnings
#endif
@@ -141,8 +149,8 @@ static uint8_t rx_buffer_L[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer
static uint32_t rx_buffer_L_len = ARRAY_LEN(rx_buffer_L);
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2)
static uint16_t timeoutCntSerial_L = 0; // Timeout counter for Rx Serial command
static uint8_t timeoutFlagSerial_L = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
static uint16_t timeoutCntSerial_L = 0; // Timeout counter for Rx Serial command
static uint8_t timeoutFlgSerial_L = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
#endif
#if defined(SIDEBOARD_SERIAL_USART2)
SerialSideboard Sideboard_L;
@@ -155,8 +163,8 @@ static uint8_t rx_buffer_R[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer
static uint32_t rx_buffer_R_len = ARRAY_LEN(rx_buffer_R);
#endif
#if defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3)
static uint16_t timeoutCntSerial_R = 0; // Timeout counter for Rx Serial command
static uint8_t timeoutFlagSerial_R = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
static uint16_t timeoutCntSerial_R = 0; // Timeout counter for Rx Serial command
static uint8_t timeoutFlgSerial_R = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
#endif
#if defined(SIDEBOARD_SERIAL_USART3)
SerialSideboard Sideboard_R;
@@ -164,13 +172,21 @@ SerialSideboard Sideboard_R_raw;
static uint32_t Sideboard_R_len = sizeof(Sideboard_R);
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
static SerialCommand command;
static SerialCommand command_raw;
static uint32_t command_len = sizeof(command);
#if defined(CONTROL_SERIAL_USART2)
static SerialCommand commandL;
static SerialCommand commandL_raw;
static uint32_t commandL_len = sizeof(commandL);
#ifdef CONTROL_IBUS
static uint16_t ibus_chksum;
static uint16_t ibus_captured_value[IBUS_NUM_CHANNELS];
static uint16_t ibusL_captured_value[IBUS_NUM_CHANNELS];
#endif
#endif
#if defined(CONTROL_SERIAL_USART3)
static SerialCommand commandR;
static SerialCommand commandR_raw;
static uint32_t commandR_len = sizeof(commandR);
#ifdef CONTROL_IBUS
static uint16_t ibusR_captured_value[IBUS_NUM_CHANNELS];
#endif
#endif
@@ -297,35 +313,39 @@ void Input_Init(void) {
EE_Init(); /* EEPROM Init */
EE_ReadVariable(VirtAddVarTab[0], &writeCheck);
if (writeCheck == FLASH_WRITE_KEY) {
EE_ReadVariable(VirtAddVarTab[1] , &readVal); input1.typ = (uint8_t)readVal;
EE_ReadVariable(VirtAddVarTab[2] , &readVal); input1.min = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[3] , &readVal); input1.mid = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[4] , &readVal); input1.max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[5] , &readVal); input2.typ = (uint8_t)readVal;
EE_ReadVariable(VirtAddVarTab[6] , &readVal); input2.min = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[7] , &readVal); input2.mid = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[8] , &readVal); input2.max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[9] , &readVal); rtP_Left.i_max = rtP_Right.i_max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[10], &readVal); rtP_Left.n_max = rtP_Right.n_max = (int16_t)readVal;
} else { // Else If Input type is 3 (auto), identify the input type based on the values from config.h
input1.typ = INPUT1_TYPE;
input1.min = INPUT1_MIN;
input1.mid = INPUT1_MID;
input1.max = INPUT1_MAX;
input2.typ = INPUT2_TYPE;
input2.min = INPUT2_MIN;
input2.mid = INPUT2_MID;
input2.max = INPUT2_MAX;
if (INPUT1_TYPE == 3) { input1.typ = checkInputType(INPUT1_MIN, INPUT1_MID, INPUT1_MAX); }
if (INPUT2_TYPE == 3) { input2.typ = checkInputType(INPUT2_MIN, INPUT2_MID, INPUT2_MAX); }
EE_ReadVariable(VirtAddVarTab[1] , &readVal); rtP_Left.i_max = rtP_Right.i_max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[2] , &readVal); rtP_Left.n_max = rtP_Right.n_max = (int16_t)readVal;
for (uint8_t i=0; i<INPUTS_NR; i++) {
EE_ReadVariable(VirtAddVarTab[ 3+8*i] , &readVal); input1[i].typ = (uint8_t)readVal;
EE_ReadVariable(VirtAddVarTab[ 4+8*i] , &readVal); input1[i].min = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[ 5+8*i] , &readVal); input1[i].mid = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[ 6+8*i] , &readVal); input1[i].max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[ 7+8*i] , &readVal); input2[i].typ = (uint8_t)readVal;
EE_ReadVariable(VirtAddVarTab[ 8+8*i] , &readVal); input2[i].min = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[ 9+8*i] , &readVal); input2[i].mid = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[10+8*i] , &readVal); input2[i].max = (int16_t)readVal;
}
} else {
for (uint8_t i=0; i<INPUTS_NR; i++) {
if (input1[i].typDef == 3) { // If Input type defined is 3 (auto), identify the input type based on the values from config.h
input1[i].typ = checkInputType(input1[i].min, input1[i].mid, input1[i].max);
} else {
input1[i].typ = input1[i].typDef;
}
if (input2[i].typDef == 3) {
input2[i].typ = checkInputType(input2[i].min, input2[i].mid, input2[i].max);
} else {
input2[i].typ = input2[i].typDef;
}
}
}
HAL_FLASH_Lock();
#endif
#ifdef VARIANT_TRANSPOTTER
enable = 1;
HAL_FLASH_Unlock();
HAL_FLASH_Unlock();
EE_Init(); /* EEPROM Init */
EE_ReadVariable(VirtAddVarTab[0], &saveValue);
HAL_FLASH_Lock();
@@ -474,21 +494,28 @@ void adcCalibLim(void) {
readInputRaw();
// Inititalization: MIN = a high value, MAX = a low value
int32_t input1_fixdt = input1.raw << 16;
int32_t input2_fixdt = input2.raw << 16;
int32_t input1_fixdt = input1[inIdx].raw << 16;
int32_t input2_fixdt = input2[inIdx].raw << 16;
int16_t INPUT1_MIN_temp = MAX_int16_T;
int16_t INPUT1_MID_temp = 0;
int16_t INPUT1_MAX_temp = MIN_int16_T;
int16_t INPUT2_MIN_temp = MAX_int16_T;
int16_t INPUT2_MID_temp = 0;
int16_t INPUT2_MAX_temp = MIN_int16_T;
int16_t input_margin = 0;
uint16_t input_cal_timeout = 0;
#ifdef CONTROL_ADC
if (inIdx == CONTROL_ADC) {
input_margin = ADC_MARGIN;
}
#endif
// Extract MIN, MAX and MID from ADC while the power button is not pressed
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && input_cal_timeout++ < 4000) { // 20 sec timeout
readInputRaw();
filtLowPass32(input1.raw, FILTER, &input1_fixdt);
filtLowPass32(input2.raw, FILTER, &input2_fixdt);
filtLowPass32(input1[inIdx].raw, FILTER, &input1_fixdt);
filtLowPass32(input2[inIdx].raw, FILTER, &input2_fixdt);
INPUT1_MID_temp = (int16_t)(input1_fixdt >> 16);// CLAMP(input1_fixdt >> 16, INPUT1_MIN, INPUT1_MAX); // convert fixed-point to integer
INPUT2_MID_temp = (int16_t)(input2_fixdt >> 16);// CLAMP(input2_fixdt >> 16, INPUT2_MIN, INPUT2_MAX);
@@ -502,16 +529,16 @@ void adcCalibLim(void) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Input1 is ");
#endif
input1.typ = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp);
if (input1.typ == INPUT1_TYPE || INPUT1_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
input1.min = INPUT1_MIN_temp + INPUT_MARGIN;
input1.mid = INPUT1_MID_temp;
input1.max = INPUT1_MAX_temp - INPUT_MARGIN;
input1[inIdx].typ = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp);
if (input1[inIdx].typ == input1[inIdx].typDef || input1[inIdx].typDef == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
input1[inIdx].min = INPUT1_MIN_temp + input_margin;
input1[inIdx].mid = INPUT1_MID_temp;
input1[inIdx].max = INPUT1_MAX_temp - input_margin;
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..OK\r\n");
#endif
} else {
input1.typ = 0; // Disable input
input1[inIdx].typ = 0; // Disable input
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..NOK\r\n");
#endif
@@ -520,16 +547,16 @@ void adcCalibLim(void) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Input2 is ");
#endif
input2.typ = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp);
if (input2.typ == INPUT2_TYPE || INPUT2_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
input2.min = INPUT2_MIN_temp + INPUT_MARGIN;
input2.mid = INPUT2_MID_temp;
input2.max = INPUT2_MAX_temp - INPUT_MARGIN;
input2[inIdx].typ = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp);
if (input2[inIdx].typ == input2[inIdx].typDef || input2[inIdx].typDef == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto)
input2[inIdx].min = INPUT2_MIN_temp + input_margin;
input2[inIdx].mid = INPUT2_MID_temp;
input2[inIdx].max = INPUT2_MAX_temp - input_margin;
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..OK\r\n");
#endif
} else {
input2.typ = 0; // Disable input
input2[inIdx].typ = 0; // Disable input
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..NOK\r\n");
#endif
@@ -537,8 +564,8 @@ void adcCalibLim(void) {
inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Limits Input1: TYP:%i MIN:%i MID:%i MAX:%i\r\nLimits Input2: TYP:%i MIN:%i MID:%i MAX:%i\r\n",
input1.typ, input1.min, input1.mid, input1.max,
input2.typ, input2.min, input2.mid, input2.max);
input1[inIdx].typ, input1[inIdx].min, input1[inIdx].mid, input1[inIdx].max,
input2[inIdx].typ, input2[inIdx].min, input2[inIdx].mid, input2[inIdx].max);
#endif
#endif
@@ -561,8 +588,8 @@ void updateCurSpdLim(void) {
printf("Torque and Speed limits update started...\r\n");
#endif
int32_t input1_fixdt = input1.raw << 16;
int32_t input2_fixdt = input2.raw << 16;
int32_t input1_fixdt = input1[inIdx].raw << 16;
int32_t input2_fixdt = input2[inIdx].raw << 16;
uint16_t cur_factor; // fixdt(0,16,16)
uint16_t spd_factor; // fixdt(0,16,16)
uint16_t cur_spd_timeout = 0;
@@ -571,21 +598,21 @@ void updateCurSpdLim(void) {
// Wait for the power button press
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout
readInputRaw();
filtLowPass32(input1.raw, FILTER, &input1_fixdt);
filtLowPass32(input2.raw, FILTER, &input2_fixdt);
filtLowPass32(input1[inIdx].raw, FILTER, &input1_fixdt);
filtLowPass32(input2[inIdx].raw, FILTER, &input2_fixdt);
HAL_Delay(5);
}
// Calculate scaling factors
cur_factor = CLAMP((input1_fixdt - (input1.min << 16)) / (input1.max - input1.min), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A
spd_factor = CLAMP((input2_fixdt - (input2.min << 16)) / (input2.max - input2.min), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm
cur_factor = CLAMP((input1_fixdt - (input1[inIdx].min << 16)) / (input1[inIdx].max - input1[inIdx].min), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A
spd_factor = CLAMP((input2_fixdt - (input2[inIdx].min << 16)) / (input2[inIdx].max - input2[inIdx].min), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm
if (input1.typ != 0){
if (input1[inIdx].typ != 0){
// Update current limit
rtP_Left.i_max = rtP_Right.i_max = (int16_t)((I_MOT_MAX * A2BIT_CONV * cur_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
cur_spd_valid = 1; // Mark update to be saved in Flash at shutdown
}
if (input2.typ != 0){
if (input2[inIdx].typ != 0){
// Update speed limit
rtP_Left.n_max = rtP_Right.n_max = (int16_t)((N_MOT_MAX * spd_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
cur_spd_valid += 2; // Mark update to be saved in Flash at shutdown
@@ -611,8 +638,8 @@ void updateCurSpdLim(void) {
void standstillHold(void) {
#if defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE)
if (!rtP_Left.b_cruiseCtrlEna) { // If Stanstill in NOT Active -> try Activation
if (((input1.cmd > 50 || input2.cmd < -50) && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small
|| (input2.cmd < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small
if (((input1[inIdx].cmd > 50 || input2[inIdx].cmd < -50) && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small
|| (input2[inIdx].cmd < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small
rtP_Left.n_cruiseMotTgt = 0;
rtP_Right.n_cruiseMotTgt = 0;
rtP_Left.b_cruiseCtrlEna = 1;
@@ -621,7 +648,7 @@ void standstillHold(void) {
}
}
else { // If Stanstill is Active -> try Deactivation
if (input1.cmd < 20 && input2.cmd > 50 && !cruiseCtrlAcv) { // Check if Brake is released AND Throttle is pressed AND no Cruise Control
if (input1[inIdx].cmd < 20 && input2[inIdx].cmd > 50 && !cruiseCtrlAcv) { // Check if Brake is released AND Throttle is pressed AND no Cruise Control
rtP_Left.b_cruiseCtrlEna = 0;
rtP_Right.b_cruiseCtrlEna = 0;
standstillAcv = 0;
@@ -646,7 +673,7 @@ void electricBrake(uint16_t speedBlend, uint8_t reverseDir) {
if (speedAvg > 0) {
brakeVal = (int16_t)((-ELECTRIC_BRAKE_MAX * speedBlend) >> 15);
} else {
brakeVal = (int16_t)(( ELECTRIC_BRAKE_MAX * speedBlend) >> 15);
brakeVal = (int16_t)(( ELECTRIC_BRAKE_MAX * speedBlend) >> 15);
}
// Check if direction is reversed
@@ -655,14 +682,14 @@ void electricBrake(uint16_t speedBlend, uint8_t reverseDir) {
}
// Calculate the new input2.cmd with brake component included
if (input2.cmd >= 0 && input2.cmd < ELECTRIC_BRAKE_THRES) {
input2.cmd = MAX(brakeVal, ((ELECTRIC_BRAKE_THRES - input2.cmd) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (input2.cmd >= -ELECTRIC_BRAKE_THRES && input2.cmd < 0) {
input2.cmd = MIN(brakeVal, ((ELECTRIC_BRAKE_THRES + input2.cmd) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (input2.cmd >= ELECTRIC_BRAKE_THRES) {
input2.cmd = MAX(brakeVal, ((input2.cmd - ELECTRIC_BRAKE_THRES) * INPUT_MAX) / (INPUT_MAX - ELECTRIC_BRAKE_THRES));
if (input2[inIdx].cmd >= 0 && input2[inIdx].cmd < ELECTRIC_BRAKE_THRES) {
input2[inIdx].cmd = MAX(brakeVal, ((ELECTRIC_BRAKE_THRES - input2[inIdx].cmd) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (input2[inIdx].cmd >= -ELECTRIC_BRAKE_THRES && input2[inIdx].cmd < 0) {
input2[inIdx].cmd = MIN(brakeVal, ((ELECTRIC_BRAKE_THRES + input2[inIdx].cmd) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (input2[inIdx].cmd >= ELECTRIC_BRAKE_THRES) {
input2[inIdx].cmd = MAX(brakeVal, ((input2[inIdx].cmd - ELECTRIC_BRAKE_THRES) * INPUT_MAX) / (INPUT_MAX - ELECTRIC_BRAKE_THRES));
} else { // when (input2.cmd < -ELECTRIC_BRAKE_THRES)
input2.cmd = MIN(brakeVal, ((input2.cmd + ELECTRIC_BRAKE_THRES) * INPUT_MIN) / (INPUT_MIN + ELECTRIC_BRAKE_THRES));
input2[inIdx].cmd = MIN(brakeVal, ((input2[inIdx].cmd + ELECTRIC_BRAKE_THRES) * INPUT_MIN) / (INPUT_MIN + ELECTRIC_BRAKE_THRES));
}
#endif
}
@@ -724,7 +751,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) {
if ((min + ADC_MARGIN - ADC_PROTECT_THRESH) > 0 && (max - ADC_MARGIN + ADC_PROTECT_THRESH) < 4095) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf(" AND protected");
#endif
@@ -738,97 +765,253 @@ int checkInputType(int16_t min, int16_t mid, int16_t max){
/* =========================== Read Functions =========================== */
/* =========================== Input Functions =========================== */
/*
* Calculate Input Command
* This function realizes dead-band around 0 and scales the input between [out_min, out_max]
*/
void calcInputCmd(InputStruct *in, int16_t out_min, int16_t out_max) {
switch (in->typ){
case 1: // Input is a normal pot
in->cmd = CLAMP(MAP(in->raw, in->min, in->max, 0, out_max), 0, out_max);
break;
case 2: // Input is a mid resting pot
if( in->raw > in->mid - in->dband && in->raw < in->mid + in->dband ) {
in->cmd = 0;
} else if(in->raw > in->mid) {
in->cmd = CLAMP(MAP(in->raw, in->mid + in->dband, in->max, 0, out_max), 0, out_max);
} else {
in->cmd = CLAMP(MAP(in->raw, in->mid - in->dband, in->min, 0, out_min), out_min, 0);
}
break;
default: // Input is ignored
in->cmd = 0;
break;
}
}
/*
* Function to read the Input Raw values from various input devices
*/
void readInputRaw(void) {
#ifdef CONTROL_ADC
// ADC values range: 0-4095, see ADC-calibration in config.h
input1.raw = adc_buffer.l_tx2;
input2.raw = adc_buffer.l_rx2;
timeoutCnt = 0;
if (inIdx == CONTROL_ADC) {
input1[inIdx].raw = adc_buffer.l_tx2;
input2[inIdx].raw = adc_buffer.l_rx2;
}
#endif
#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK)
if (inIdx == CONTROL_NUNCHUK) {
if (nunchuk_connected != 0) {
Nunchuk_Read();
input1.raw = (nunchuk_data[0] - 127) * 8; // X axis 0-255
input2.raw = (nunchuk_data[1] - 128) * 8; // Y axis 0-255
input1[inIdx].raw = (nunchuk_data[0] - 127) * 8; // X axis 0-255
input2[inIdx].raw = (nunchuk_data[1] - 128) * 8; // Y axis 0-255
#ifdef SUPPORT_BUTTONS
button1 = (uint8_t)nunchuk_data[5] & 1;
button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1;
#endif
}
}
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
// Handle received data validity, timeout and fix out-of-sync if necessary
#if defined(CONTROL_SERIAL_USART2)
if (inIdx == CONTROL_SERIAL_USART2) {
#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
ibusL_captured_value[(i/2)] = CLAMP(commandL.channels[i] + (commandL.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000
}
input1.raw = (ibus_captured_value[0] - 500) * 2;
input2.raw = (ibus_captured_value[1] - 500) * 2;
input1[inIdx].raw = (ibusL_captured_value[0] - 500) * 2;
input2[inIdx].raw = (ibusL_captured_value[1] - 500) * 2;
#else
input1.raw = command.steer;
input2.raw = command.speed;
input1[inIdx].raw = commandL.steer;
input2[inIdx].raw = commandL.speed;
#endif
timeoutCnt = 0;
}
#endif
#if defined(CONTROL_SERIAL_USART3)
if (inIdx == CONTROL_SERIAL_USART3) {
#ifdef CONTROL_IBUS
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) {
ibusR_captured_value[(i/2)] = CLAMP(commandR.channels[i] + (commandR.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000
}
input1[inIdx].raw = (ibusR_captured_value[0] - 500) * 2;
input2[inIdx].raw = (ibusR_captured_value[1] - 500) * 2;
#else
input1[inIdx].raw = commandR.steer;
input2[inIdx].raw = commandR.speed;
#endif
}
#endif
#if defined(SIDEBOARD_SERIAL_USART2)
if (inIdx == SIDEBOARD_SERIAL_USART2) {
input1[inIdx].raw = Sideboard_L.cmd1;
input2[inIdx].raw = Sideboard_L.cmd2;
}
#endif
#if defined(SIDEBOARD_SERIAL_USART3)
if (!timeoutFlagSerial_R && Sideboard_R.sensors & SW1_SET) { // If no Timeout and SW1 is set, switch to Sideboard control
input1.raw = Sideboard_R.cmd1;
input2.raw = Sideboard_R.cmd2;
} else {
Sideboard_R.sensors &= ~SW1_SET; // Clear SW1 bit, to switch to default control input
}
#endif
#if defined(SIDEBOARD_SERIAL_USART2) // Priority on the Left sideboard
if (!timeoutFlagSerial_L && Sideboard_L.sensors & SW1_SET) {
input1.raw = Sideboard_L.cmd1;
input2.raw = Sideboard_L.cmd2;
} else {
Sideboard_L.sensors &= ~SW1_SET;
}
if (inIdx == SIDEBOARD_SERIAL_USART3) {
input1[inIdx].raw = Sideboard_R.cmd1;
input2[inIdx].raw = Sideboard_R.cmd2;
}
#endif
#if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)
input1.raw = (ppm_captured_value[0] - 500) * 2;
input2.raw = (ppm_captured_value[1] - 500) * 2;
#ifdef SUPPORT_BUTTONS
button1 = ppm_captured_value[5] > 500;
button2 = 0;
#if defined(CONTROL_PPM_LEFT)
if (inIdx == CONTROL_PPM_LEFT) {
input1[inIdx].raw = (ppm_captured_value[0] - 500) * 2;
input2[inIdx].raw = (ppm_captured_value[1] - 500) * 2;
}
#endif
#if defined(CONTROL_PPM_RIGHT)
if (inIdx == CONTROL_PPM_RIGHT) {
input1[inIdx].raw = (ppm_captured_value[0] - 500) * 2;
input2[inIdx].raw = (ppm_captured_value[1] - 500) * 2;
}
#endif
#if (defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)) && defined(SUPPORT_BUTTONS)
button1 = ppm_captured_value[5] > 500;
button2 = 0;
#endif
#if defined(CONTROL_PWM_LEFT)
if (inIdx == CONTROL_PWM_LEFT) {
input1[inIdx].raw = (pwm_captured_ch1_value - 500) * 2;
input2[inIdx].raw = (pwm_captured_ch2_value - 500) * 2;
}
#endif
#if defined(CONTROL_PWM_RIGHT)
if (inIdx == CONTROL_PWM_RIGHT) {
input1[inIdx].raw = (pwm_captured_ch1_value - 500) * 2;
input2[inIdx].raw = (pwm_captured_ch2_value - 500) * 2;
}
#endif
#ifdef VARIANT_TRANSPOTTER
#ifdef GAMETRAK_CONNECTION_NORMAL
input1[inIdx].cmd = adc_buffer.l_rx2;
input2[inIdx].cmd = adc_buffer.l_tx2;
#endif
#ifdef GAMETRAK_CONNECTION_ALTERNATE
input1[inIdx].cmd = adc_buffer.l_tx2;
input2[inIdx].cmd = adc_buffer.l_rx2;
#endif
#endif
#if defined(CONTROL_PWM_LEFT) || defined(CONTROL_PWM_RIGHT)
input1.raw = (pwm_captured_ch1_value - 500) * 2;
input2.raw = (pwm_captured_ch2_value - 500) * 2;
#endif
}
/*
* Add Dead-band to a signal
* This function realizes a dead-band around 0 and scales the input between [out_min, out_max]
* Function to handle the ADC, UART and General timeout (Nunchuk, PPM, PWM)
*/
void readInputCmd(InputStruct *in, int16_t out_min, int16_t out_max) {
switch (in->typ){
case 1: // Input is a normal pot
in->cmd = CLAMP(MAP(in->raw, in->min, in->max, 0, out_max), 0, out_max);
case 2: // Input is a mid resting pot
if( in->raw > in->mid - in->deadband && in->raw < in->mid + in->deadband ) {
in->cmd = 0;
} else if(in->raw > in->mid) {
in->cmd = CLAMP(MAP(in->raw, in->mid + in->deadband, in->max, 0, out_max), 0, out_max);
void handleTimeout(void) {
#ifdef CONTROL_ADC
if (inIdx == CONTROL_ADC) {
// If input1 or Input2 is either below MIN - Threshold or above MAX + Threshold, ADC protection timeout
if (IN_RANGE(input1[inIdx].raw, input1[inIdx].min - ADC_PROTECT_THRESH, input1[inIdx].max + ADC_PROTECT_THRESH) &&
IN_RANGE(input2[inIdx].raw, input2[inIdx].min - ADC_PROTECT_THRESH, input2[inIdx].max + ADC_PROTECT_THRESH)) {
timeoutFlgADC = 0; // Reset the timeout flag
timeoutCntADC = 0; // Reset the timeout counter
} else {
in->cmd = CLAMP(MAP(in->raw, in->mid - in->deadband, in->min, 0, out_min), out_min, 0);
if (timeoutCntADC++ >= ADC_PROTECT_TIMEOUT) { // Timeout qualification
timeoutFlgADC = 1; // Timeout detected
timeoutCntADC = ADC_PROTECT_TIMEOUT; // Limit timout counter value
}
}
default: // Input is ignored
in->cmd = 0;
}
}
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2)
if (timeoutCntSerial_L++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlgSerial_L = 1; // Timeout detected
timeoutCntSerial_L = SERIAL_TIMEOUT; // Limit timout counter value
#if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART2) && (SIDEBOARD_SERIAL_USART2 == 1) // Switch to Primary input in case of Timeout on Auxiliary input
inIdx = !SIDEBOARD_SERIAL_USART2;
#elif defined(DUAL_INPUTS) && (CONTROL_SERIAL_USART2 == 1)
inIdx = !CONTROL_SERIAL_USART2;
#endif
} else { // No Timeout
#if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART2)
if (Sideboard_L.sensors & SW1_SET) { // If SW1 is set, switch to Sideboard control
inIdx = SIDEBOARD_SERIAL_USART2;
} else {
inIdx = !SIDEBOARD_SERIAL_USART2;
}
#elif defined(DUAL_INPUTS) && (CONTROL_SERIAL_USART2 == 1)
inIdx = CONTROL_SERIAL_USART2;
#endif
}
#if (defined(CONTROL_SERIAL_USART2) && CONTROL_SERIAL_USART2 == 0) || (defined(SIDEBOARD_SERIAL_USART2) && SIDEBOARD_SERIAL_USART2 == 0 && !defined(VARIANT_HOVERBOARD))
timeoutFlgSerial = timeoutFlgSerial_L; // Report Timeout only on the Primary Input
#endif
#endif
#if defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3)
if (timeoutCntSerial_R++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlgSerial_R = 1; // Timeout detected
timeoutCntSerial_R = SERIAL_TIMEOUT; // Limit timout counter value
#if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART3) && (SIDEBOARD_SERIAL_USART3 == 1) // Switch to Primary input in case of Timeout on Auxiliary input
inIdx = !SIDEBOARD_SERIAL_USART3;
#elif defined(DUAL_INPUTS) && (CONTROL_SERIAL_USART3 == 1)
inIdx = !CONTROL_SERIAL_USART3;
#endif
} else { // No Timeout
#if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART3)
if (Sideboard_R.sensors & SW1_SET) { // If SW1 is set, switch to Sideboard control
inIdx = SIDEBOARD_SERIAL_USART3;
} else {
inIdx = !SIDEBOARD_SERIAL_USART3;
}
#elif defined(DUAL_INPUTS) && (CONTROL_SERIAL_USART3 == 1)
inIdx = CONTROL_SERIAL_USART3;
#endif
}
#if (defined(CONTROL_SERIAL_USART3) && CONTROL_SERIAL_USART3 == 0) || (defined(SIDEBOARD_SERIAL_USART3) && SIDEBOARD_SERIAL_USART3 == 0 && !defined(VARIANT_HOVERBOARD))
timeoutFlgSerial = timeoutFlgSerial_R; // Report Timeout only on the Primary Input
#endif
#endif
#if defined(SIDEBOARD_SERIAL_USART2) && defined(SIDEBOARD_SERIAL_USART3)
timeoutFlgSerial = timeoutFlgSerial_L || timeoutFlgSerial_R;
#endif
#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) || defined(VARIANT_TRANSPOTTER) || \
defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT) || defined(CONTROL_PWM_LEFT) || defined(CONTROL_PWM_RIGHT)
if (timeoutCntGen++ >= TIMEOUT) { // Timeout qualification
#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) || defined(VARIANT_TRANSPOTTER) || \
(defined(CONTROL_PPM_LEFT) && CONTROL_PPM_LEFT == 0) || (defined(CONTROL_PPM_RIGHT) && CONTROL_PPM_RIGHT == 0) || \
(defined(CONTROL_PWM_LEFT) && CONTROL_PWM_LEFT == 0) || (defined(CONTROL_PWM_RIGHT) && CONTROL_PWM_RIGHT == 0)
timeoutFlgGen = 1; // Report Timeout only on the Primary Input
timeoutCntGen = TIMEOUT;
#endif
#if defined(DUAL_INPUTS) && defined(CONTROL_PPM_LEFT)
inIdx = !CONTROL_PPM_LEFT;
#elif defined(DUAL_INPUTS) && defined(CONTROL_PPM_RIGHT)
inIdx = !CONTROL_PPM_RIGHT;
#elif defined(DUAL_INPUTS) && defined(CONTROL_PWM_LEFT)
inIdx = !CONTROL_PWM_LEFT;
#elif defined(DUAL_INPUTS) && defined(CONTROL_PWM_RIGHT)
inIdx = !CONTROL_PWM_RIGHT;
#endif
} else {
#if defined(DUAL_INPUTS) && defined(CONTROL_PPM_LEFT)
inIdx = CONTROL_PPM_LEFT;
#elif defined(DUAL_INPUTS) && defined(CONTROL_PPM_RIGHT)
inIdx = CONTROL_PPM_RIGHT;
#elif defined(DUAL_INPUTS) && defined(CONTROL_PWM_LEFT)
inIdx = CONTROL_PWM_LEFT;
#elif defined(DUAL_INPUTS) && defined(CONTROL_PWM_RIGHT)
inIdx = CONTROL_PWM_RIGHT;
#endif
}
#endif
if (timeoutFlgADC || timeoutFlgSerial || timeoutFlgGen) { // In case of timeout bring the system to a Safe State
ctrlModReq = OPEN_MODE; // Request OPEN_MODE. This will bring the motor power to 0 in a controlled way
input1[inIdx].cmd = 0;
input2[inIdx].cmd = 0;
} else {
ctrlModReq = ctrlModReqRaw; // Follow the Mode request
}
}
/*
@@ -838,68 +1021,26 @@ void readInputCmd(InputStruct *in, int16_t out_min, int16_t out_max) {
*/
void readCommand(void) {
readInputRaw();
#ifdef CONTROL_ADC
// If input1 or Input2 is either below MIN - Threshold or above MAX + Threshold, ADC protection timeout
if (IN_RANGE(input1.raw, input1.min - ADC_PROTECT_THRESH, input1.max + ADC_PROTECT_THRESH) &&
IN_RANGE(input2.raw, input2.min - ADC_PROTECT_THRESH, input2.max + ADC_PROTECT_THRESH)) {
timeoutCntADC = 0; // Reset the timeout counter
} else {
if (timeoutCntADC++ >= ADC_PROTECT_TIMEOUT) { // Timeout qualification
timeoutFlagADC = 1; // Timeout detected
timeoutCntADC = ADC_PROTECT_TIMEOUT; // Limit timout counter value
}
}
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2)
if (timeoutCntSerial_L++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial_L = 1; // Timeout detected
timeoutCntSerial_L = SERIAL_TIMEOUT; // Limit timout counter value
}
timeoutFlagSerial = timeoutFlagSerial_L;
#endif
#if defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3)
if (timeoutCntSerial_R++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial_R = 1; // Timeout detected
timeoutCntSerial_R = SERIAL_TIMEOUT; // Limit timout counter value
}
timeoutFlagSerial = timeoutFlagSerial_R;
#endif
#if defined(SIDEBOARD_SERIAL_USART2) && defined(SIDEBOARD_SERIAL_USART3)
timeoutFlagSerial = timeoutFlagSerial_L || timeoutFlagSerial_R;
#endif
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
readInputCmd(&input1, INPUT_MIN, INPUT_MAX);
calcInputCmd(&input1[inIdx], INPUT_MIN, INPUT_MAX);
#if !defined(VARIANT_SKATEBOARD)
readInputCmd(&input2, INPUT_MIN, INPUT_MAX);
calcInputCmd(&input2[inIdx], INPUT_MIN, INPUT_MAX);
#else
readInputCmd(&input2, INPUT2_BRAKE, INPUT_MAX);
calcInputCmd(&input2[inIdx], INPUT_BRK, INPUT_MAX);
#endif
#endif
#ifdef VARIANT_TRANSPOTTER
#ifdef GAMETRAK_CONNECTION_NORMAL
input1.cmd = adc_buffer.l_rx2;
input2.cmd = adc_buffer.l_tx2;
#endif
#ifdef GAMETRAK_CONNECTION_ALTERNATE
input1.cmd = adc_buffer.l_tx2;
inputc.cmd = adc_buffer.l_rx2;
#endif
#endif
handleTimeout();
#ifdef VARIANT_HOVERCAR
brakePressed = (uint8_t)(input1.cmd > 50);
#endif
if (timeoutFlagADC || timeoutFlagSerial || timeoutCnt > TIMEOUT) { // In case of timeout bring the system to a Safe State
ctrlModReq = OPEN_MODE; // Request OPEN_MODE. This will bring the motor power to 0 in a controlled way
input1.cmd = 0;
input2.cmd = 0;
} else {
ctrlModReq = ctrlModReqRaw; // Follow the Mode request
if (inIdx == CONTROL_ADC) {
brakePressed = (uint8_t)(input1[inIdx].cmd > 50);
}
else {
brakePressed = (uint8_t)(input2[inIdx].cmd < -50);
}
#endif
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
@@ -940,17 +1081,17 @@ void usart2_rx_check(void)
#ifdef CONTROL_SERIAL_USART2
uint8_t *ptr;
if (pos != old_pos) { // Check change in received data
ptr = (uint8_t *)&command_raw; // Initialize the pointer with command_raw address
if (pos > old_pos && (pos - old_pos) == command_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
memcpy(ptr, &rx_buffer_L[old_pos], command_len); // Copy data. This is possible only if command_raw is contiguous! (meaning all the structure members have the same size)
usart_process_command(&command_raw, &command, 2); // Process data
} else if ((rx_buffer_L_len - old_pos + pos) == command_len) { // "Overflow" buffer mode: check if data length equals expected length
ptr = (uint8_t *)&commandL_raw; // Initialize the pointer with command_raw address
if (pos > old_pos && (pos - old_pos) == commandL_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
memcpy(ptr, &rx_buffer_L[old_pos], commandL_len); // Copy data. This is possible only if command_raw is contiguous! (meaning all the structure members have the same size)
usart_process_command(&commandL_raw, &commandL, 2); // Process data
} else if ((rx_buffer_L_len - old_pos + pos) == commandL_len) { // "Overflow" buffer mode: check if data length equals expected length
memcpy(ptr, &rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx_buffer_L_len - old_pos; // Move to correct position in command_raw
memcpy(ptr, &rx_buffer_L[0], pos); // Copy remaining data
}
usart_process_command(&command_raw, &command, 2); // Process data
usart_process_command(&commandL_raw, &commandL, 2); // Process data
}
}
#endif // CONTROL_SERIAL_USART2
@@ -1010,17 +1151,17 @@ void usart3_rx_check(void)
#ifdef CONTROL_SERIAL_USART3
uint8_t *ptr;
if (pos != old_pos) { // Check change in received data
ptr = (uint8_t *)&command_raw; // Initialize the pointer with command_raw address
if (pos > old_pos && (pos - old_pos) == command_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
memcpy(ptr, &rx_buffer_R[old_pos], command_len); // Copy data. This is possible only if command_raw is contiguous! (meaning all the structure members have the same size)
usart_process_command(&command_raw, &command, 3); // Process data
} else if ((rx_buffer_R_len - old_pos + pos) == command_len) { // "Overflow" buffer mode: check if data length equals expected length
ptr = (uint8_t *)&commandR_raw; // Initialize the pointer with command_raw address
if (pos > old_pos && (pos - old_pos) == commandR_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
memcpy(ptr, &rx_buffer_R[old_pos], commandR_len); // Copy data. This is possible only if command_raw is contiguous! (meaning all the structure members have the same size)
usart_process_command(&commandR_raw, &commandR, 3); // Process data
} else if ((rx_buffer_R_len - old_pos + pos) == commandR_len) { // "Overflow" buffer mode: check if data length equals expected length
memcpy(ptr, &rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx_buffer_R_len - old_pos; // Move to correct position in command_raw
memcpy(ptr, &rx_buffer_R[0], pos); // Copy remaining data
}
usart_process_command(&command_raw, &command, 3); // Process data
usart_process_command(&commandR_raw, &commandR, 3); // Process data
}
}
#endif // CONTROL_SERIAL_USART3
@@ -1074,6 +1215,7 @@ void usart_process_debug(uint8_t *userCommand, uint32_t len)
void usart_process_command(SerialCommand *command_in, SerialCommand *command_out, uint8_t usart_idx)
{
#ifdef CONTROL_IBUS
uint16_t ibus_chksum;
if (command_in->start == IBUS_LENGTH && command_in->type == IBUS_COMMAND) {
ibus_chksum = 0xFFFF - IBUS_LENGTH - IBUS_COMMAND;
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i++) {
@@ -1083,13 +1225,13 @@ void usart_process_command(SerialCommand *command_in, SerialCommand *command_out
*command_out = *command_in;
if (usart_idx == 2) { // Sideboard USART2
#ifdef CONTROL_SERIAL_USART2
timeoutCntSerial_L = 0; // Reset timeout counter
timeoutFlagSerial_L = 0; // Clear timeout flag
timeoutFlgSerial_L = 0; // Clear timeout flag
timeoutCntSerial_L = 0; // Reset timeout counter
#endif
} else if (usart_idx == 3) { // Sideboard USART3
#ifdef CONTROL_SERIAL_USART3
timeoutCntSerial_R = 0; // Reset timeout counter
timeoutFlagSerial_R = 0; // Clear timeout flag
timeoutFlgSerial_R = 0; // Clear timeout flag
timeoutCntSerial_R = 0; // Reset timeout counter
#endif
}
}
@@ -1102,13 +1244,13 @@ void usart_process_command(SerialCommand *command_in, SerialCommand *command_out
*command_out = *command_in;
if (usart_idx == 2) { // Sideboard USART2
#ifdef CONTROL_SERIAL_USART2
timeoutCntSerial_L = 0; // Reset timeout counter
timeoutFlagSerial_L = 0; // Clear timeout flag
timeoutFlgSerial_L = 0; // Clear timeout flag
timeoutCntSerial_L = 0; // Reset timeout counter
#endif
} else if (usart_idx == 3) { // Sideboard USART3
#ifdef CONTROL_SERIAL_USART3
timeoutCntSerial_R = 0; // Reset timeout counter
timeoutFlagSerial_R = 0; // Clear timeout flag
timeoutFlgSerial_R = 0; // Clear timeout flag
timeoutCntSerial_R = 0; // Reset timeout counter
#endif
}
}
@@ -1132,12 +1274,12 @@ void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sid
if (usart_idx == 2) { // Sideboard USART2
#ifdef SIDEBOARD_SERIAL_USART2
timeoutCntSerial_L = 0; // Reset timeout counter
timeoutFlagSerial_L = 0; // Clear timeout flag
timeoutFlgSerial_L = 0; // Clear timeout flag
#endif
} else if (usart_idx == 3) { // Sideboard USART3
#ifdef SIDEBOARD_SERIAL_USART3
timeoutCntSerial_R = 0; // Reset timeout counter
timeoutFlagSerial_R = 0; // Clear timeout flag
timeoutCntSerial_R = 0; // Reset timeout counter
timeoutFlgSerial_R = 0; // Clear timeout flag
#endif
}
}
@@ -1184,7 +1326,7 @@ void sideboardLeds(uint8_t *leds) {
// Battery Level Indicator: use LED1, LED2, LED3
if (main_loop_counter % BAT_BLINK_INTERVAL == 0) { // | RED (LED1) | YELLOW (LED3) | GREEN (LED2) |
if (batVoltage < BAT_DEAD) { // | 0 | 0 | 0 |
*leds &= ~LED1_SET & ~LED3_SET & ~LED2_SET;
*leds &= ~LED1_SET & ~LED3_SET & ~LED2_SET;
} else if (batVoltage < BAT_LVL1) { // | B | 0 | 0 |
*leds ^= LED1_SET;
*leds &= ~LED3_SET & ~LED2_SET;
@@ -1213,7 +1355,7 @@ void sideboardLeds(uint8_t *leds) {
*leds |= LED1_SET;
*leds &= ~LED3_SET & ~LED2_SET;
}
if (timeoutFlagADC || timeoutFlagSerial) {
if (timeoutFlgADC || timeoutFlgSerial) {
*leds |= LED3_SET;
*leds &= ~LED1_SET & ~LED2_SET;
}
@@ -1227,18 +1369,32 @@ void sideboardLeds(uint8_t *leds) {
*/
void sideboardSensors(uint8_t sensors) {
#if !defined(VARIANT_HOVERBOARD) && (defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3))
static uint8_t sensor1_prev, sensor2_prev;
uint8_t sensor1_rising_edge, sensor2_rising_edge;
sensor1_rising_edge = (sensors & SENSOR1_SET) && !sensor1_prev;
sensor2_rising_edge = (sensors & SENSOR2_SET) && !sensor2_prev;
sensor1_prev = sensors & SENSOR1_SET;
sensor2_prev = sensors & SENSOR2_SET;
static uint8_t sensor1_prev, sensor2_prev;
static uint8_t sensor1_index; // holds the press index number for sensor1, when used as a button
uint8_t sensor1_trig, sensor2_trig;
sensor1_trig = (sensors & SENSOR1_SET) && !sensor1_prev; // rising edge detection
sensor2_trig = (sensors & SENSOR2_SET) && !sensor2_prev; // rising edge detection
sensor1_prev = sensors & SENSOR1_SET;
sensor2_prev = sensors & SENSOR2_SET;
// Control MODE and Control Type Handling: use Sensor1 as push button
static uint8_t sensor1_index; // holds the press index number for sensor1, when used as a button
if (sensor1_rising_edge) {
sensor1_index++;
if (sensor1_index > 4) { sensor1_index = 0; }
// Override in case the Sideboard control is Active
#if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART2)
if (inIdx == SIDEBOARD_SERIAL_USART2) {
sensor1_index = (Sideboard_L.sensors & SW3_SET) >> 11; // SW3 on RC transmitter is used to change Control Mode
sensor1_trig = sensor1_index != sensor1_prev; // rising or falling edge detection
sensor1_prev = sensor1_index;
}
#endif
#if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART3)
if (inIdx == SIDEBOARD_SERIAL_USART3) {
sensor1_index = (Sideboard_R.sensors & SW3_SET) >> 11; // SW3 on RC transmitter is used to change Control Mode
sensor1_trig = sensor1_index != sensor1_prev; // rising or falling edge change detection
sensor1_prev = sensor1_index;
}
#endif
// Control MODE and Control Type Handling
if (sensor1_trig) {
switch (sensor1_index) {
case 0: // FOC VOLTAGE
rtP_Left.z_ctrlTypSel = FOC_CTRL;
@@ -1258,21 +1414,37 @@ void sideboardSensors(uint8_t sensors) {
case 4: // COMMUTATION
rtP_Left.z_ctrlTypSel = COM_CTRL;
rtP_Right.z_ctrlTypSel = COM_CTRL;
break;
break;
}
beepShortMany(sensor1_index + 1, 1);
if (++sensor1_index > 4) { sensor1_index = 0; }
}
// Field Weakening: use Sensor2 as push button
// Field Weakening Activation/Deactivation
#ifdef CRUISE_CONTROL_SUPPORT
if (sensor2_rising_edge) {
cruiseControl(sensor2_rising_edge);
if (sensor2_trig) {
cruiseControl(sensor2_trig);
}
#else
static uint8_t sensor2_index; // holds the press index number for sensor2, when used as a button
if (sensor2_rising_edge) {
sensor2_index++;
if (sensor2_index > 1) { sensor2_index = 0; }
static uint8_t sensor2_index = 1; // holds the press index number for sensor2, when used as a button
// Override in case the Sideboard control is Active
#if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART2)
if (inIdx == SIDEBOARD_SERIAL_USART2) {
sensor2_index = (Sideboard_L.sensors & SW4_SET) >> 13; // SW4 on RC transmitter is used to Activate/Deactivate Field Weakening
sensor2_trig = sensor2_index != sensor2_prev; // rising or falling edge change detection
sensor2_prev = sensor2_index;
}
#endif
#if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART3)
if (inIdx == SIDEBOARD_SERIAL_USART3) {
sensor2_index = (Sideboard_R.sensors & SW4_SET) >> 13; // SW4 on RC transmitter is used to Activate/Deactivate Field Weakening
sensor2_trig = sensor2_index != sensor2_prev; // rising or falling edge change detection
sensor2_prev = sensor2_index;
}
#endif
if (sensor2_trig) {
switch (sensor2_index) {
case 0: // FW Disabled
rtP_Left.b_fieldWeakEna = 0;
@@ -1285,7 +1457,8 @@ void sideboardSensors(uint8_t sensors) {
Input_Lim_Init();
break;
}
beepShortMany(sensor2_index + 1, 1);
beepShortMany(sensor2_index + 1, 1);
if (++sensor2_index > 1) { sensor2_index = 0; }
}
#endif // CRUISE_CONTROL_SUPPORT
#endif
@@ -1311,16 +1484,18 @@ void saveConfig() {
if (inp_cal_valid || cur_spd_valid) {
HAL_FLASH_Unlock();
EE_WriteVariable(VirtAddVarTab[0] , (uint16_t)FLASH_WRITE_KEY);
EE_WriteVariable(VirtAddVarTab[1] , (uint16_t)input1.typ);
EE_WriteVariable(VirtAddVarTab[2] , (uint16_t)input1.min);
EE_WriteVariable(VirtAddVarTab[3] , (uint16_t)input1.mid);
EE_WriteVariable(VirtAddVarTab[4] , (uint16_t)input1.max);
EE_WriteVariable(VirtAddVarTab[5] , (uint16_t)input2.typ);
EE_WriteVariable(VirtAddVarTab[6] , (uint16_t)input2.min);
EE_WriteVariable(VirtAddVarTab[7] , (uint16_t)input2.mid);
EE_WriteVariable(VirtAddVarTab[8] , (uint16_t)input2.max);
EE_WriteVariable(VirtAddVarTab[9] , (uint16_t)rtP_Left.i_max);
EE_WriteVariable(VirtAddVarTab[10], (uint16_t)rtP_Left.n_max);
EE_WriteVariable(VirtAddVarTab[1] , (uint16_t)rtP_Left.i_max);
EE_WriteVariable(VirtAddVarTab[2] , (uint16_t)rtP_Left.n_max);
for (uint8_t i=0; i<INPUTS_NR; i++) {
EE_WriteVariable(VirtAddVarTab[ 3+8*i] , (uint16_t)input1[i].typ);
EE_WriteVariable(VirtAddVarTab[ 4+8*i] , (uint16_t)input1[i].min);
EE_WriteVariable(VirtAddVarTab[ 5+8*i] , (uint16_t)input1[i].mid);
EE_WriteVariable(VirtAddVarTab[ 6+8*i] , (uint16_t)input1[i].max);
EE_WriteVariable(VirtAddVarTab[ 7+8*i] , (uint16_t)input2[i].typ);
EE_WriteVariable(VirtAddVarTab[ 8+8*i] , (uint16_t)input2[i].min);
EE_WriteVariable(VirtAddVarTab[ 9+8*i] , (uint16_t)input2[i].mid);
EE_WriteVariable(VirtAddVarTab[10+8*i] , (uint16_t)input2[i].max);
}
HAL_FLASH_Lock();
}
#endif
@@ -1345,7 +1520,7 @@ void poweroff(void) {
void poweroffPressCheck(void) {
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0;
uint16_t cnt_press = 0;
@@ -1365,7 +1540,7 @@ void poweroffPressCheck(void) {
adcCalibLim();
beepShort(5);
}
} else { // Short press: power off
} else if (cnt_press > 8) { // Short press: power off (80 ms debounce)
poweroff();
}
}