From 44befc480a650bca5099642d24bba29abb791721 Mon Sep 17 00:00:00 2001 From: EmanuelFeru Date: Wed, 18 Nov 2020 19:16:56 +0100 Subject: [PATCH] Input Auto-calibration - mainly rearangement of function - updated initialization in the auto-calibration function - added beep for confirmation of Input protection --- Inc/config.h | 167 +++++++++--------- Inc/util.h | 8 +- Src/control.c | 3 +- Src/main.c | 1 + Src/util.c | 455 +++++++++++++++++++++++++------------------------- 5 files changed, 307 insertions(+), 327 deletions(-) diff --git a/Inc/config.h b/Inc/config.h index 6327998..c752b0f 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -166,9 +166,9 @@ // ############################## DEFAULT SETTINGS ############################ // Default settings will be applied at the end of this config file if not set before -#define INACTIVITY_TIMEOUT 8 // Minutes of not driving until poweroff. it is not very precise. -#define BEEPS_BACKWARD 1 // 0 or 1 -#define FLASH_WRITE_KEY 0x1234 // Flash writing key, used when writing data to flash memory +#define INACTIVITY_TIMEOUT 8 // Minutes of not driving until poweroff. it is not very precise. +#define BEEPS_BACKWARD 1 // 0 or 1 +#define FLASH_WRITE_KEY 0x1233 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h /* FILTER is in fixdt(0,16,16): VAL_fixedPoint = VAL_floatingPoint * 2^16. In this case 6553 = 0.1 * 2^16 * Value of COEFFICIENT is in fixdt(1,16,14) @@ -201,7 +201,7 @@ // ############################### DEBUG SERIAL ############################### /* Connect GND and RX of a 3.3v uart-usb adapter to the left (USART2) or right sensor board cable (USART3) - * Be careful not to use the red wire of the cable. 15v will destroye evrything. + * Be careful not to use the red wire of the cable. 15v will destroy everything. * If you are using VARIANT_NUNCHUK, disable it temporarily. * enable DEBUG_SERIAL_USART3 or DEBUG_SERIAL_USART2 * and DEBUG_SERIAL_ASCII use asearial terminal. @@ -210,8 +210,8 @@ * DEBUG_SERIAL_ASCII output is: * // "1:345 2:1337 3:0 4:0 5:0 6:0 7:0 8:0\r\n" * - * 1: (int16_t)adc_buffer.l_tx2); ADC1 - * 2: (int16_t)adc_buffer.l_rx2); ADC2 + * 1: (int16_t)input1); raw input1: ADC1, UART, PWM, PPM, iBUS + * 2: (int16_t)input2); raw input2: ADC2, UART, PWM, PPM, iBUS * 3: (int16_t)speedR); output command: [-1000, 1000] * 4: (int16_t)speedL); output command: [-1000, 1000] * 5: (int16_t)adc_buffer.batt1); Battery adc-value measured by mainboard @@ -252,18 +252,18 @@ */ #define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! #define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken - #define ADC_PROTECT_THRESH 300 // ADC Protection threshold below/above the MIN/MAX ADC values + #define ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define INPUT1_MIN 0 // min ADC1-value while poti at minimum-position (0 - 4095) #define INPUT1_MID 2048 // mid ADC1-value while poti at minimum-position (ADC1_MIN - ADC1_MAX) #define INPUT1_MAX 4095 // max ADC1-value while poti at maximum-position (0 - 4095) + #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define INPUT2_MIN 0 // min ADC2-value while poti at minimum-position (0 - 4095) #define INPUT2_MID 2048 // mid ADC2-value while poti at minimum-position (ADC2_MIN - ADC2_MAX) #define INPUT2_MAX 4095 // max ADC2-value while poti at maximum-position (0 - 4095) + #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) // #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! // #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! #endif @@ -282,18 +282,18 @@ #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! // Min / Max values of each channel (use DEBUG to determine these values) #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define INPUT1_MIN -1000 // (-1000 - 0) #define INPUT1_MID 0 #define INPUT1_MAX 1000 // (0 - 1000) + #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define INPUT2_MIN -1000 // (-1000 - 0) #define INPUT2_MID 0 #define INPUT2_MAX 1000 // (0 - 1000) - // #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! - // #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! + #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) + // #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! + // #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! #endif // ######################## END OF VARIANT_USART SETTINGS ######################### @@ -301,7 +301,7 @@ // ################################# VARIANT_NUNCHUK SETTINGS ############################ #ifdef VARIANT_NUNCHUK - /* left sensor board cable. USART3 + /* on Right sensor cable * keep cable short, use shielded cable, use ferrits, stabalize voltage in nunchuk, * use the right one of the 2 types of nunchuks, add i2c pullups. * use original nunchuk. most clones does not work very well. @@ -309,23 +309,23 @@ */ #define CONTROL_NUNCHUK // use nunchuk as input. disable FEEDBACK_SERIAL_USART3, DEBUG_SERIAL_USART3! // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT1_MIN -1024 // (-1024 - 0) + #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_MIN -1024 // (-1024 - 0) #define INPUT1_MID 0 - #define INPUT1_MAX 1024 // (0 - 1024) + #define INPUT1_MAX 1024 // (0 - 1024) + #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_MIN -1024 // (-1024 - 0) + #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_MIN -1024 // (-1024 - 0) #define INPUT2_MID 0 - #define INPUT2_MAX 1024 // (0 - 1024) + #define INPUT2_MAX 1024 // (0 - 1024) + #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) // # maybe good for ARMCHAIR # - #define FILTER 3276 // 0.05f - #define SPEED_COEFFICIENT 8192 // 0.5f - #define STEER_COEFFICIENT 62259 // -0.2f + #define FILTER 3276 // 0.05f + #define SPEED_COEFFICIENT 8192 // 0.5f + #define STEER_COEFFICIENT 62259 // -0.2f #define DEBUG_SERIAL_USART2 // left sensor cable debug - // #define SUPPORT_BUTTONS // Define for Nunchuck buttons support + // #define SUPPORT_BUTTONS // Define for Nunchuck buttons support #endif // ############################# END OF VARIANT_NUNCHUK SETTINGS ######################### @@ -338,25 +338,25 @@ * https://gist.github.com/peterpoetzi/1b63a4a844162196613871767189bd05 */ #define CONTROL_PPM_LEFT // use PPM-Sum as input on the LEFT cable . disable CONTROL_SERIAL_USART2! - //#define CONTROL_PPM_RIGHT // use PPM-Sum as input on the RIGHT cable. disable CONTROL_SERIAL_USART3! + // #define CONTROL_PPM_RIGHT // use PPM-Sum as input on the RIGHT cable. disable CONTROL_SERIAL_USART3! #ifdef CONTROL_PPM_RIGHT #define DEBUG_SERIAL_USART2 // left sensor cable debug #else #define DEBUG_SERIAL_USART3 // right sensor cable debug #endif - #define PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used. + #define PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used. // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT1_MIN -1000 // (-1000 - 0) + #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_MIN -1000 // (-1000 - 0) #define INPUT1_MID 0 - #define INPUT1_MAX 1000 // (0 - 1000) + #define INPUT1_MAX 1000 // (0 - 1000) + #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT2_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_MIN -1000 // (-1000 - 0) + #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_MIN -1000 // (-1000 - 0) #define INPUT2_MID 0 - #define INPUT2_MAX 1000 // (0 - 1000) + #define INPUT2_MAX 1000 // (0 - 1000) + #define INPUT2_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) // #define SUPPORT_BUTTONS // Define for PPM buttons support // #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! // #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! @@ -378,17 +378,17 @@ #define DEBUG_SERIAL_USART3 // right sensor cable debug #endif // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT1_MIN -1000 // (-1000 - 0) + #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_MIN -1000 // (-1000 - 0) #define INPUT1_MID 0 - #define INPUT1_MAX 1000 // (0 - 1000) + #define INPUT1_MAX 1000 // (0 - 1000) + #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT2_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_MIN -1000 // (-1000 - 0) + #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_MIN -1000 // (-1000 - 0) #define INPUT2_MID 0 - #define INPUT2_MAX 1000 // (0 - 1000) + #define INPUT2_MAX 1000 // (0 - 1000) + #define INPUT2_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. #define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 @@ -397,11 +397,6 @@ // #define INVERT_L_DIRECTION // #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! // #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! - #ifdef CONTROL_PWM_RIGHT - #define DEBUG_SERIAL_USART2 // left sensor cable debug - #else - #define DEBUG_SERIAL_USART3 // right sensor cable debug - #endif #endif // ############################# END OF VARIANT_PWM SETTINGS ############################ @@ -412,29 +407,29 @@ /* CONTROL VIA RC REMOTE WITH FLYSKY IBUS PROTOCOL * Connected to Left sensor board cable. Channel 1: steering, Channel 2: speed. */ - #define CONTROL_IBUS // use IBUS as input - #define IBUS_NUM_CHANNELS 14 // total number of IBUS channels to receive, even if they are not used. + #define CONTROL_IBUS // use IBUS as input + #define IBUS_NUM_CHANNELS 14 // total number of IBUS channels to receive, even if they are not used. #define IBUS_LENGTH 0x20 #define IBUS_COMMAND 0x40 - // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT1_MIN -1000 // (-1000 - 0) - #define INPUT1_MID 0 - #define INPUT1_MAX 1000 // (0 - 1000) - - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_MIN -1000 // (-1000 - 0) - #define INPUT2_MID 0 - #define INPUT2_MAX 1000 // (0 - 1000) - #undef USART2_BAUD - #define USART2_BAUD 115200 - #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! - #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! - // #define DEBUG_SERIAL_USART3 // right sensor cable debug + #define USART2_BAUD 115200 + #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + // #define DEBUG_SERIAL_USART3 // right sensor cable debug + + // Min / Max values of each channel (use DEBUG to determine these values) + #define INPUT1_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_MIN -1000 // (-1000 - 0) + #define INPUT1_MID 0 + #define INPUT1_MAX 1000 // (0 - 1000) + #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) + + #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_MIN -1000 // (-1000 - 0) + #define INPUT2_MID 0 + #define INPUT2_MAX 1000 // (0 - 1000) + #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #endif // ############################# END OF VARIANT_IBUS SETTINGS ############################ @@ -446,19 +441,19 @@ #define CTRL_MOD_REQ TRQ_MODE // HOVERCAR works best in TORQUE Mode #define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! #define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken - #define ADC_PROTECT_THRESH 300 // ADC Protection threshold below/above the MIN/MAX ADC values + #define ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values #define INPUT1_TYPE 1 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define INPUT1_MIN 1000 // min ADC1-value while poti at minimum-position (0 - 4095) #define INPUT1_MID 0 #define INPUT1_MAX 2500 // max ADC1-value while poti at maximum-position (0 - 4095) + #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define INPUT2_TYPE 1 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define INPUT2_MIN 500 // min ADC2-value while poti at minimum-position (0 - 4095) #define INPUT2_MID 0 #define INPUT2_MAX 2200 // max ADC2-value while poti at maximum-position (0 - 4095) + #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define SPEED_COEFFICIENT 16384 // 1.0f #define STEER_COEFFICIENT 0 // 0.0f @@ -523,7 +518,7 @@ * Channel 1: steering, Channel 2: speed. */ #undef CTRL_MOD_REQ - #define CTRL_MOD_REQ TRQ_MODE // SKATEBOARD works best in TORQUE Mode + #define CTRL_MOD_REQ TRQ_MODE // SKATEBOARD works best in TORQUE Mode //#define CONTROL_PWM_LEFT // use RC PWM as input on the LEFT cable. disable DEBUG_SERIAL_USART2! #define CONTROL_PWM_RIGHT // use RC PWM as input on the RIGHT cable. disable DEBUG_SERIAL_USART3! #ifdef CONTROL_PWM_RIGHT @@ -532,18 +527,18 @@ #define DEBUG_SERIAL_USART3 // right sensor cable debug #endif // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 0 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT1_MIN -1000 // (-1000 - 0) + #define INPUT1_TYPE 0 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT1_MIN -1000 // (-1000 - 0) #define INPUT1_MID 0 - #define INPUT1_MAX 1000 // (0 - 1000) + #define INPUT1_MAX 1000 // (0 - 1000) + #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot - #define INPUT2_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define INPUT2_MIN -800 // (-1000 - 0) + #define INPUT2_TYPE 2 // 0:Disabled 1:Normal POT 2:Middle Resting Pot + #define INPUT2_MIN -800 // (-1000 - 0) #define INPUT2_MID 0 - #define INPUT2_MAX 700 // (0 - 1000) - #define INPUT2_OUT_MIN -400 // (-1000 - 0) Change this value to adjust the braking amount + #define INPUT2_MAX 700 // (0 - 1000) + #define INPUT2_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) + #define INPUT2_BRAKE -400 // (-1000 - 0) Change this value to adjust the braking amount #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. #define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 @@ -711,16 +706,6 @@ // Functional checks -#if defined(ADC_PROTECT_ENA) && ((ADC1_MIN - ADC_PROTECT_THRESH) <= 0 || (ADC1_MAX + ADC_PROTECT_THRESH) >= 4095) - #warning ADC1 Protection NOT possible! Adjust the ADC thresholds. - #undef ADC_PROTECT_ENA -#endif - -#if defined(ADC_PROTECT_ENA) && ((ADC2_MIN - ADC_PROTECT_THRESH) <= 0 || (ADC2_MAX + ADC_PROTECT_THRESH) >= 4095) - #warning ADC2 Protection NOT possible! Adjust the ADC thresholds. - #undef ADC_PROTECT_ENA -#endif - #if (defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)) && !defined(PPM_NUM_CHANNELS) #error Total number of PPM channels needs to be set #endif diff --git a/Inc/util.h b/Inc/util.h index 8f34635..54e0fe6 100644 --- a/Inc/util.h +++ b/Inc/util.h @@ -66,11 +66,11 @@ void shortBeep(uint8_t freq); void shortBeepMany(uint8_t cnt, int8_t dir); void longBeep(uint8_t freq); void calcAvgSpeed(void); +int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max); +int checkInputType(int16_t min, int16_t mid, int16_t max); void adcCalibLim(void); -int checkInputType(int16_t min, int16_t mid, int16_t max); void updateCurSpdLim(void); void saveConfig(void); -int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max); void standstillHold(void); void electricBrake(uint16_t speedBlend, uint8_t reverseDir); void cruiseControl(uint8_t button); @@ -79,9 +79,9 @@ void cruiseControl(uint8_t button); void poweroff(void); void poweroffPressCheck(void); -// Read Command Function -void readCommand(void); +// Read Functions void readInput(void); +void readCommand(void); void usart2_rx_check(void); void usart3_rx_check(void); #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) diff --git a/Src/control.c b/Src/control.c index a6c53be..3a82eb1 100644 --- a/Src/control.c +++ b/Src/control.c @@ -34,14 +34,13 @@ void PPM_ISR_Callback(void) { if (rc_delay > 3000) { if (ppm_valid && ppm_count == PPM_NUM_CHANNELS) { ppm_timeout = 0; - timeoutCnt = 0; // added this + timeoutCnt = 0; memcpy(ppm_captured_value, ppm_captured_value_buffer, sizeof(ppm_captured_value)); } ppm_valid = true; ppm_count = 0; } else if (ppm_count < PPM_NUM_CHANNELS && IN_RANGE(rc_delay, 900, 2100)){ - //timeoutCnt = 0; ppm_captured_value_buffer[ppm_count++] = CLAMP(rc_delay, 1000, 2000) - 1000; } else { ppm_valid = false; diff --git a/Src/main.c b/Src/main.c index abd59d9..90afd7e 100644 --- a/Src/main.c +++ b/Src/main.c @@ -210,6 +210,7 @@ int main(void) { 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); + steerFixdt = speedFixdt = 0; // reset filters enable = 1; // enable motors consoleLog("-- Motors enabled --\r\n"); } diff --git a/Src/util.c b/Src/util.c index 0deb562..926f83e 100644 --- a/Src/util.c +++ b/Src/util.c @@ -129,29 +129,29 @@ static int16_t INPUT_MIN; // [-] Input target minimum limitation #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - static uint8_t cur_spd_valid = 0; - static uint8_t input_cal_valid = 0; + static uint8_t cur_spd_valid = 0; + static uint8_t inp_cal_valid = 0; + static uint16_t INPUT1_TYP_CAL = INPUT1_TYPE; static uint16_t INPUT1_MIN_CAL = INPUT1_MIN; static uint16_t INPUT1_MID_CAL = INPUT1_MID; static uint16_t INPUT1_MAX_CAL = INPUT1_MAX; - static uint16_t INPUT1_TYPE_CAL = INPUT1_TYPE; + static uint16_t INPUT2_TYP_CAL = INPUT2_TYPE; static uint16_t INPUT2_MIN_CAL = INPUT2_MIN; static uint16_t INPUT2_MID_CAL = INPUT2_MID; static uint16_t INPUT2_MAX_CAL = INPUT2_MAX; - static uint16_t INPUT2_TYPE_CAL = INPUT2_TYPE; #endif #if defined(CONTROL_ADC) -static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection +static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection #endif #if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) -static uint8_t rx_buffer_L[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer +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 timeoutFlagSerial_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; @@ -160,12 +160,12 @@ static uint32_t Sideboard_L_len = sizeof(Sideboard_L); #endif #if defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) -static uint8_t rx_buffer_R[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer +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 timeoutFlagSerial_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; @@ -273,32 +273,30 @@ void Input_Init(void) { #endif #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - uint16_t writeCheck, i_max, n_max; - HAL_FLASH_Unlock(); - EE_Init(); /* EEPROM Init */ + HAL_FLASH_Unlock(); + EE_Init(); /* EEPROM Init */ EE_ReadVariable(VirtAddVarTab[0], &writeCheck); if (writeCheck == FLASH_WRITE_KEY) { - EE_ReadVariable(VirtAddVarTab[1] , &INPUT1_MIN_CAL); - EE_ReadVariable(VirtAddVarTab[2] , &INPUT1_MAX_CAL); - EE_ReadVariable(VirtAddVarTab[3] , &INPUT1_MID_CAL); - EE_ReadVariable(VirtAddVarTab[4] , &INPUT2_MIN_CAL); - EE_ReadVariable(VirtAddVarTab[5] , &INPUT2_MAX_CAL); - EE_ReadVariable(VirtAddVarTab[6] , &INPUT2_MID_CAL); - EE_ReadVariable(VirtAddVarTab[7] , &i_max); - EE_ReadVariable(VirtAddVarTab[8] , &n_max); - EE_ReadVariable(VirtAddVarTab[9] , &INPUT1_TYPE_CAL); - EE_ReadVariable(VirtAddVarTab[10], &INPUT2_TYPE_CAL); + EE_ReadVariable(VirtAddVarTab[1] , &INPUT1_TYP_CAL); + EE_ReadVariable(VirtAddVarTab[2] , &INPUT1_MIN_CAL); + EE_ReadVariable(VirtAddVarTab[3] , &INPUT1_MAX_CAL); + EE_ReadVariable(VirtAddVarTab[4] , &INPUT1_MID_CAL); + EE_ReadVariable(VirtAddVarTab[5] , &INPUT2_TYP_CAL); + EE_ReadVariable(VirtAddVarTab[6] , &INPUT2_MIN_CAL); + EE_ReadVariable(VirtAddVarTab[7] , &INPUT2_MAX_CAL); + EE_ReadVariable(VirtAddVarTab[8] , &INPUT2_MID_CAL); + EE_ReadVariable(VirtAddVarTab[9] , &i_max); + EE_ReadVariable(VirtAddVarTab[10], &n_max); rtP_Left.i_max = i_max; rtP_Left.n_max = n_max; rtP_Right.i_max = i_max; rtP_Right.n_max = n_max; } HAL_FLASH_Lock(); - #endif - #ifdef VARIANT_TRANSPOTTER + #ifdef VARIANT_TRANSPOTTER enable = 1; HAL_FLASH_Unlock(); @@ -358,12 +356,9 @@ void Input_Init(void) { #if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) || \ defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) void UART_DisableRxErrors(UART_HandleTypeDef *huart) -{ - /* Disable PE (Parity Error) interrupts */ - CLEAR_BIT(huart->Instance->CR1, USART_CR1_PEIE); - - /* Disable EIE (Frame error, noise error, overrun error) interrupts */ - CLEAR_BIT(huart->Instance->CR3, USART_CR3_EIE); +{ + CLEAR_BIT(huart->Instance->CR1, USART_CR1_PEIE); /* Disable PE (Parity Error) interrupts */ + CLEAR_BIT(huart->Instance->CR3, USART_CR3_EIE); /* Disable EIE (Frame error, noise error, overrun error) interrupts */ } #endif @@ -421,6 +416,70 @@ void calcAvgSpeed(void) { speedAvgAbs = abs(speedAvg); } + /* + * Add Dead-band to a signal + * This function realizes a dead-band around 0 and scales the input between [out_min, out_max] + */ +int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max) { + switch (type){ + case 0: // Input is ignored + return 0; + case 1: // Input is a normal pot + return CLAMP(MAP(u, in_min, in_max, 0, out_max), 0, out_max); + case 2: // Input is a mid resting pot + if( u > in_mid - deadBand && u < in_mid + deadBand ) { + return 0; + } else if(u > in_mid) { + return CLAMP(MAP(u, in_mid + deadBand, in_max, 0, out_max), 0, out_max); + } else { + return CLAMP(MAP(u, in_mid - deadBand, in_min, 0, out_min), out_min, 0); + } + default: + return 0; + } +} + + /* + * Check Input Type + * This function identifies the input type: 0: Disabled, 1: Normal Pot, 2: Middle Resting Pot + */ +int checkInputType(int16_t min, int16_t mid, int16_t max){ + + int type = 0; + #ifdef CONTROL_ADC + int16_t threshold = 400; // Threshold to define if values are too close + #else + int16_t threshold = 200; + #endif + + HAL_Delay(10); + if ((min / threshold) == (max / threshold)) { + consoleLog("Input is ignored"); // MIN and MAX are close, disable input + type = 0; + } else { + if ((min / threshold) == (mid / threshold)){ + consoleLog("Input is a normal pot"); // MIN and MID are close, it's a normal pot + type = 1; + } else { + consoleLog("Input is a mid-resting pot"); // it's a mid resting pot + type = 2; + } + HAL_Delay(10); + #ifdef CONTROL_ADC + if ((min - ADC_PROTECT_THRESH) > 0 && (max + ADC_PROTECT_THRESH) < 4095) { + consoleLog(" and protected"); + shortBeep(2); // Indicate protection by a beep + } + #endif + } + + HAL_Delay(10); + consoleLog("\n"); + HAL_Delay(10); + + return type; +} + /* * Auto-calibration of the ADC Limits * This function finds the Minimum, Maximum, and Middle for the ADC input @@ -435,23 +494,20 @@ void adcCalibLim(void) { return; } - #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - + #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) consoleLog("Input calibration started...\n"); - readInput(); - // Inititalization: MIN = a high values, MAX = a low value, - int32_t input1_fixdt = input1 << 16; - int32_t input2_fixdt = input2 << 16; + readInput(); + // Inititalization: MIN = a high value, MAX = a low value + int32_t input1_fixdt = input1 << 16; + int32_t input2_fixdt = input2 << 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; uint16_t input_cal_timeout = 0; - int16_t INPUT1_MIN_temp = INPUT1_MAX; - int16_t INPUT1_MID_temp = 0; - int16_t INPUT1_MAX_temp = INPUT1_MIN; - int16_t INPUT2_MIN_temp = INPUT2_MAX; - int16_t INPUT2_MID_temp = 0; - int16_t INPUT2_MAX_temp = INPUT2_MIN; - - input_cal_valid = 1; // 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 @@ -459,90 +515,48 @@ void adcCalibLim(void) { filtLowPass32(input1, FILTER, &input1_fixdt); filtLowPass32(input2, FILTER, &input2_fixdt); - INPUT1_MID_temp = (int16_t)(input1_fixdt >> 16);// CLAMP(input1_fixdt >> 16, INPUT1_MIN, INPUT1_MAX); // convert fixed-point to integer + 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); INPUT1_MIN_temp = MIN(INPUT1_MIN_temp, INPUT1_MID_temp); - INPUT1_MAX_temp = MAX(INPUT1_MAX_temp, INPUT1_MID_temp); + INPUT1_MAX_temp = MAX(INPUT1_MAX_temp, INPUT1_MID_temp); INPUT2_MIN_temp = MIN(INPUT2_MIN_temp, INPUT2_MID_temp); INPUT2_MAX_temp = MAX(INPUT2_MAX_temp, INPUT2_MID_temp); HAL_Delay(5); } - - uint16_t input_margin = 0; + #ifdef CONTROL_ADC - input_margin = 100; + int16_t input_margin = 100; + #else + int16_t input_margin = 0; #endif INPUT1_MIN_CAL = INPUT1_MIN_temp + input_margin; INPUT1_MID_CAL = INPUT1_MID_temp; INPUT1_MAX_CAL = INPUT1_MAX_temp - input_margin; - INPUT1_TYPE_CAL = checkInputType(INPUT1_MIN_CAL,INPUT1_MID_CAL,INPUT1_MAX_CAL); + INPUT1_TYP_CAL = checkInputType(INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL); INPUT2_MIN_CAL = INPUT2_MIN_temp + input_margin; INPUT2_MID_CAL = INPUT2_MID_temp; INPUT2_MAX_CAL = INPUT2_MAX_temp - input_margin; - INPUT2_TYPE_CAL = checkInputType(INPUT2_MIN_CAL,INPUT2_MID_CAL,INPUT2_MAX_CAL); + INPUT2_TYP_CAL = checkInputType(INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL); + inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown consoleLog("Saved limits\n"); HAL_Delay(10); - setScopeChannel(0, (int16_t)INPUT1_MIN_CAL); - setScopeChannel(1, (uint16_t)INPUT1_MID_CAL); - setScopeChannel(2, (int16_t)INPUT1_MAX_CAL); - setScopeChannel(3, (int16_t)0); - setScopeChannel(4, (int16_t)INPUT2_MIN_CAL); - setScopeChannel(5, (uint16_t)INPUT2_MID_CAL); - setScopeChannel(6, (int16_t)INPUT2_MAX_CAL); - setScopeChannel(7, (int16_t)0); + setScopeChannel(0, (int16_t)INPUT1_TYP_CAL); + setScopeChannel(1, (int16_t)INPUT1_MIN_CAL); + setScopeChannel(2, (int16_t)INPUT1_MID_CAL); + setScopeChannel(3, (int16_t)INPUT1_MAX_CAL); + setScopeChannel(4, (int16_t)INPUT2_TYP_CAL); + setScopeChannel(5, (int16_t)INPUT2_MIN_CAL); + setScopeChannel(6, (int16_t)INPUT2_MID_CAL); + setScopeChannel(7, (int16_t)INPUT2_MAX_CAL); consoleScope(); HAL_Delay(20); consoleLog("OK\n"); #endif } - -int checkInputType(int16_t min, int16_t mid, int16_t max){ - - HAL_Delay(10); - - int type = 0; - - // Threshold to define if values are too close - int16_t threshold = 200; - #ifdef CONTROL_ADC - threshold = 400; - #endif - - if ( (min/threshold) == (max/threshold) ){ - // MIN MID and MAX are close, disable input - consoleLog("Input is ignored"); - type = 0; - } else { - if ( (min/threshold) == (mid/threshold) ){ - // MIN and MID are close, it's a normal pot - consoleLog("Input is a normal pot"); - type = 1; - }else { - // it's a mid resting pot - consoleLog("Input is a mid-resting pot"); - type = 2; - } - - HAL_Delay(10); - #ifdef CONTROL_ADC - if ( (min - ADC_PROTECT_THRESH) > 0 && (max + ADC_PROTECT_THRESH) < 4095){ - consoleLog(" and protected"); - } - #endif - } - - HAL_Delay(10); - consoleLog("\n"); - HAL_Delay(10); - - return type; -} - - /* * Update Maximum Motor Current Limit (via ADC1) and Maximum Speed Limit (via ADC2) * Procedure: @@ -556,14 +570,14 @@ void updateCurSpdLim(void) { } #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - consoleLog("Torque and Speed limits update started...\n"); - int32_t input1_fixdt = input1 << 16; - int32_t input2_fixdt = input2 << 16; - uint16_t cur_spd_timeout = 0; + int32_t input1_fixdt = input1 << 16; + int32_t input2_fixdt = input2 << 16; uint16_t cur_factor; // fixdt(0,16,16) uint16_t spd_factor; // fixdt(0,16,16) + uint16_t cur_spd_timeout = 0; + cur_spd_valid = 0; // Wait for the power button press while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout @@ -573,31 +587,31 @@ void updateCurSpdLim(void) { HAL_Delay(5); } // Calculate scaling factors - cur_factor = CLAMP((input1_fixdt - ((int16_t)INPUT1_MIN_CAL << 16)) / ((int16_t)INPUT1_MAX_CAL - (int16_t)INPUT1_MIN_CAL), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A - spd_factor = CLAMP((input2_fixdt - ((int16_t)INPUT2_MIN_CAL << 16)) / ((int16_t)INPUT2_MAX_CAL - (int16_t)INPUT2_MIN_CAL), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm + cur_factor = CLAMP((input1_fixdt - ((int16_t)INPUT1_MIN_CAL << 16)) / ((int16_t)INPUT1_MAX_CAL - (int16_t)INPUT1_MIN_CAL), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A + spd_factor = CLAMP((input2_fixdt - ((int16_t)INPUT2_MIN_CAL << 16)) / ((int16_t)INPUT2_MAX_CAL - (int16_t)INPUT2_MIN_CAL), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm - if (INPUT1_TYPE_CAL != 0){ + if (INPUT1_TYP_CAL != 0){ // Update current limit - rtP_Right.i_max = rtP_Left.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; + 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_TYPE_CAL != 0){ + if (INPUT2_TYP_CAL != 0){ // Update speed limit - rtP_Right.n_max = rtP_Left.n_max = (int16_t)((N_MOT_MAX * spd_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4) - cur_spd_valid = 1; + 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 } consoleLog("Saved limits\n"); HAL_Delay(10); - setScopeChannel(0, (int16_t)input1_fixdt); - setScopeChannel(1, (uint16_t)cur_factor); - setScopeChannel(2, (int16_t)rtP_Right.i_max); - setScopeChannel(3, (int16_t)0); - setScopeChannel(4, (int16_t)input2_fixdt); - setScopeChannel(5, (uint16_t)spd_factor); - setScopeChannel(6, (int16_t)rtP_Right.n_max); - setScopeChannel(7, (int16_t)0); + setScopeChannel(0, (int16_t)cur_spd_valid); // 0 = No limit changed, 1 = Current limit changed, 2 = Speed limit changed, 3 = Both limits changed + setScopeChannel(1, (int16_t)input1_fixdt); + setScopeChannel(2, (int16_t)cur_factor); + setScopeChannel(3, (int16_t)rtP_Left.i_max); + setScopeChannel(4, (int16_t)0); + setScopeChannel(5, (int16_t)input2_fixdt); + setScopeChannel(6, (int16_t)spd_factor); + setScopeChannel(7, (int16_t)rtP_Left.n_max); consoleScope(); HAL_Delay(20); consoleLog("OK\n"); @@ -618,47 +632,24 @@ void saveConfig() { } #endif #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - if (input_cal_valid || cur_spd_valid) { + if (inp_cal_valid || cur_spd_valid) { HAL_FLASH_Unlock(); EE_WriteVariable(VirtAddVarTab[0] , FLASH_WRITE_KEY); - EE_WriteVariable(VirtAddVarTab[1] , INPUT1_MIN_CAL); - EE_WriteVariable(VirtAddVarTab[2] , INPUT1_MAX_CAL); - EE_WriteVariable(VirtAddVarTab[3] , INPUT1_MID_CAL); - EE_WriteVariable(VirtAddVarTab[4] , INPUT2_MIN_CAL); - EE_WriteVariable(VirtAddVarTab[5] , INPUT2_MAX_CAL); - EE_WriteVariable(VirtAddVarTab[6] , INPUT2_MID_CAL); - EE_WriteVariable(VirtAddVarTab[7] , rtP_Left.i_max); - EE_WriteVariable(VirtAddVarTab[8] , rtP_Left.n_max); - EE_WriteVariable(VirtAddVarTab[9] , INPUT1_TYPE_CAL); - EE_WriteVariable(VirtAddVarTab[10], INPUT2_TYPE_CAL); + EE_WriteVariable(VirtAddVarTab[1] , INPUT1_TYP_CAL); + EE_WriteVariable(VirtAddVarTab[2] , INPUT1_MIN_CAL); + EE_WriteVariable(VirtAddVarTab[3] , INPUT1_MAX_CAL); + EE_WriteVariable(VirtAddVarTab[4] , INPUT1_MID_CAL); + EE_WriteVariable(VirtAddVarTab[5] , INPUT2_TYP_CAL); + EE_WriteVariable(VirtAddVarTab[6] , INPUT2_MIN_CAL); + EE_WriteVariable(VirtAddVarTab[7] , INPUT2_MAX_CAL); + EE_WriteVariable(VirtAddVarTab[8] , INPUT2_MID_CAL); + EE_WriteVariable(VirtAddVarTab[9] , rtP_Left.i_max); + EE_WriteVariable(VirtAddVarTab[10], rtP_Left.n_max); HAL_FLASH_Lock(); } #endif } - /* - * Add Dead-band to a signal - * This function realizes a dead-band around 0 and scales the input between [out_min, out_max] - */ -int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max) { - switch (type){ - case 0: // Input is ignored - return 0; - case 1: // Input is a normal pot - return CLAMP(MAP( u , in_min, in_max, 0, out_max ), 0, out_max); - case 2: // Input is a mid resting pot - if( u > in_mid - deadBand && u < in_mid + deadBand ) { - return 0; - } else if(u > in_mid) { - return CLAMP( MAP(u, in_mid + deadBand, in_max, 0, out_max), 0 , out_max); - } else { - return CLAMP( MAP(u, in_mid - deadBand, in_min, 0, out_min), out_min, 0); - } - default: - return 0; - } -} - /* * Standstill Hold Function * This function uses Cruise Control to provide an anti-roll functionality at standstill. @@ -824,13 +815,18 @@ void poweroffPressCheck(void) { #endif } + +/* =========================== Read Functions =========================== */ + + /* + * Function to read the raw Input values from various input devices + */ void readInput(void) { - #if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) + #if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) if (nunchuk_connected != 0) { Nunchuk_Read(); - input1 = (nunchuk_data[0] - 127) * 8; // X axis 0-255 - input2 = (nunchuk_data[1] - 128) * 8; // Y axis 0-255 - + input1 = (nunchuk_data[0] - 127) * 8; // X axis 0-255 + input2 = (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; @@ -842,8 +838,8 @@ void readInput(void) { input1 = (ppm_captured_value[0] - 500) * 2; input2 = (ppm_captured_value[1] - 500) * 2; #ifdef SUPPORT_BUTTONS - button1 = ppm_captured_value[5] > 500; - button2 = 0; + button1 = ppm_captured_value[5] > 500; + button2 = 0; #endif #endif @@ -854,8 +850,9 @@ void readInput(void) { #ifdef CONTROL_ADC // ADC values range: 0-4095, see ADC-calibration in config.h - input1 = adc_buffer.l_tx2; - input2 = adc_buffer.l_rx2; + input1 = adc_buffer.l_tx2; + input2 = adc_buffer.l_rx2; + timeoutCnt = 0; #endif #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) @@ -866,38 +863,37 @@ void readInput(void) { } input1 = (ibus_captured_value[0] - 500) * 2; input2 = (ibus_captured_value[1] - 500) * 2; - #else - if (IN_RANGE(command.steer, INPUT_MIN, INPUT_MAX) && IN_RANGE(command.speed, INPUT_MIN, INPUT_MAX)) { + #else input1 = command.steer; input2 = command.speed; - } #endif timeoutCnt = 0; - #endif - + #endif } -/* =========================== Read Command Function =========================== */ - + /* + * Function to calculate the command to the motors. This function also manages: + * - timeout detection + * - MIN/MAX limitations and deadband + */ void readCommand(void) { readInput(); #ifdef CONTROL_ADC // If input1 or Input2 is either below MIN - Threshold or above MAX + Threshold, ADC protection timeout - if ((IN_RANGE(input1,(int16_t)INPUT1_MIN_CAL - ADC_PROTECT_THRESH,(int16_t)INPUT1_MAX_CAL + ADC_PROTECT_THRESH)) && - (IN_RANGE(input2,(int16_t)INPUT2_MIN_CAL - ADC_PROTECT_THRESH,(int16_t)INPUT2_MAX_CAL + ADC_PROTECT_THRESH))){ - if (timeoutFlagADC) { // Check for previous timeout flag - if (timeoutCntADC-- <= 0) // Timeout de-qualification - timeoutFlagADC = 0; // Timeout flag cleared + if (IN_RANGE(input1, (int16_t)INPUT1_MIN_CAL - ADC_PROTECT_THRESH, (int16_t)INPUT1_MAX_CAL + ADC_PROTECT_THRESH) && + IN_RANGE(input2, (int16_t)INPUT2_MIN_CAL - ADC_PROTECT_THRESH, (int16_t)INPUT2_MAX_CAL + ADC_PROTECT_THRESH)){ + if (timeoutFlagADC) { // Check for previous timeout flag + if (timeoutCntADC-- <= 0) // Timeout de-qualification + timeoutFlagADC = 0; // Timeout flag cleared } else { - timeoutCntADC = 0; // Reset the timeout counter + 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 + if (timeoutCntADC++ >= ADC_PROTECT_TIMEOUT) { // Timeout qualification + timeoutFlagADC = 1; // Timeout detected + timeoutCntADC = ADC_PROTECT_TIMEOUT; // Limit timout counter value } - } - timeoutCnt = 0; + } #endif #if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) @@ -919,23 +915,13 @@ void readCommand(void) { #endif #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - cmd1 = addDeadBand(input1, INPUT1_TYPE_CAL, INPUT1_DEADBAND, INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL, INPUT_MIN, INPUT_MAX); + cmd1 = addDeadBand(input1, INPUT1_TYP_CAL, INPUT1_DEADBAND, INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL, INPUT_MIN, INPUT_MAX); #if !defined(VARIANT_SKATEBOARD) - cmd2 = addDeadBand(input2, INPUT2_TYPE_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT_MIN, INPUT_MAX); + cmd2 = addDeadBand(input2, INPUT2_TYP_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT_MIN, INPUT_MAX); #else - cmd2 = addDeadBand(input2, INPUT2_TYPE_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT2_OUT_MIN, INPUT_MAX); + cmd2 = addDeadBand(input2, INPUT2_TYP_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT2_BRAKE, INPUT_MAX); #endif #endif - - - #ifdef VARIANT_HOVERCAR - brakePressed = (uint8_t)(cmd1 > 50); - #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 GAMETRAK_CONNECTION_NORMAL @@ -948,6 +934,10 @@ void readCommand(void) { #endif #endif + #ifdef VARIANT_HOVERCAR + brakePressed = (uint8_t)(cmd1 > 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 cmd1 = 0; @@ -956,6 +946,11 @@ void readCommand(void) { ctrlModReq = ctrlModReqRaw; // Follow the Mode request } + #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 + #if defined(CRUISE_CONTROL_SUPPORT) && (defined(SUPPORT_BUTTONS) || defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)) cruiseControl(button1); // Cruise control activation/deactivation #endif @@ -975,17 +970,17 @@ void usart2_rx_check(void) #endif #if defined(DEBUG_SERIAL_USART2) - if (pos != old_pos) { // Check change in received data + if (pos != old_pos) { // Check change in received data if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one usart_process_debug(&rx_buffer_L[old_pos], pos - old_pos); // Process data } else { // "Overflow" buffer mode - usart_process_debug(&rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First Process data from the end of buffer - if (pos > 0) { // Check and continue with beginning of buffer - usart_process_debug(&rx_buffer_L[0], pos); // Process remaining data + usart_process_debug(&rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First Process data from the end of buffer + if (pos > 0) { // Check and continue with beginning of buffer + usart_process_debug(&rx_buffer_L[0], pos); // Process remaining data } } } - #endif // DEBUG_SERIAL_USART2 + #endif // DEBUG_SERIAL_USART2 #ifdef CONTROL_SERIAL_USART2 uint8_t *ptr; @@ -997,7 +992,7 @@ void usart2_rx_check(void) } else if ((rx_buffer_L_len - old_pos + pos) == command_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 + 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 @@ -1005,7 +1000,7 @@ void usart2_rx_check(void) } #endif // CONTROL_SERIAL_USART2 - #ifdef SIDEBOARD_SERIAL_USART2 + #ifdef SIDEBOARD_SERIAL_USART2 uint8_t *ptr; if (pos != old_pos) { // Check change in received data ptr = (uint8_t *)&Sideboard_L_raw; // Initialize the pointer with Sideboard_raw address @@ -1015,7 +1010,7 @@ void usart2_rx_check(void) } else if ((rx_buffer_L_len - old_pos + pos) == Sideboard_L_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 Sideboard_raw + ptr += rx_buffer_L_len - old_pos; // Move to correct position in Sideboard_raw memcpy(ptr, &rx_buffer_L[0], pos); // Copy remaining data } usart_process_sideboard(&Sideboard_L_raw, &Sideboard_L, 2); // Process data @@ -1045,17 +1040,17 @@ void usart3_rx_check(void) #endif #if defined(DEBUG_SERIAL_USART3) - if (pos != old_pos) { // Check change in received data + if (pos != old_pos) { // Check change in received data if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one usart_process_debug(&rx_buffer_R[old_pos], pos - old_pos); // Process data } else { // "Overflow" buffer mode - usart_process_debug(&rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First Process data from the end of buffer - if (pos > 0) { // Check and continue with beginning of buffer - usart_process_debug(&rx_buffer_R[0], pos); // Process remaining data + usart_process_debug(&rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First Process data from the end of buffer + if (pos > 0) { // Check and continue with beginning of buffer + usart_process_debug(&rx_buffer_R[0], pos); // Process remaining data } } } - #endif // DEBUG_SERIAL_USART3 + #endif // DEBUG_SERIAL_USART3 #ifdef CONTROL_SERIAL_USART3 uint8_t *ptr; @@ -1067,7 +1062,7 @@ void usart3_rx_check(void) } else if ((rx_buffer_R_len - old_pos + pos) == command_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 + 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 @@ -1075,7 +1070,7 @@ void usart3_rx_check(void) } #endif // CONTROL_SERIAL_USART3 - #ifdef SIDEBOARD_SERIAL_USART3 + #ifdef SIDEBOARD_SERIAL_USART3 uint8_t *ptr; if (pos != old_pos) { // Check change in received data ptr = (uint8_t *)&Sideboard_R_raw; // Initialize the pointer with Sideboard_raw address @@ -1085,7 +1080,7 @@ void usart3_rx_check(void) } else if ((rx_buffer_R_len - old_pos + pos) == Sideboard_R_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 Sideboard_raw + ptr += rx_buffer_R_len - old_pos; // Move to correct position in Sideboard_raw memcpy(ptr, &rx_buffer_R[0], pos); // Copy remaining data } usart_process_sideboard(&Sideboard_R_raw, &Sideboard_R, 3); // Process data @@ -1098,7 +1093,7 @@ void usart3_rx_check(void) if (old_pos == rx_buffer_R_len) { // Check and manually update if we reached end of buffer old_pos = 0; } - #endif + #endif } /* @@ -1107,11 +1102,11 @@ void usart3_rx_check(void) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) void usart_process_debug(uint8_t *userCommand, uint32_t len) { - for (; len > 0; len--, userCommand++) { - if (*userCommand != '\n' && *userCommand != '\r') { // Do not accept 'new line' and 'carriage return' commands - //consoleLog("-- Command received --\r\n"); - // handle_input(*userCommand); // -> Create this function to handle the user commands - } + for (; len > 0; len--, userCommand++) { + if (*userCommand != '\n' && *userCommand != '\r') { // Do not accept 'new line' and 'carriage return' commands + consoleLog("-- Command received --\r\n"); + // handle_input(*userCommand); // -> Create this function to handle the user commands + } } } #endif // SERIAL_DEBUG @@ -1146,10 +1141,10 @@ void usart_process_command(SerialCommand *command_in, SerialCommand *command_out } #else uint16_t checksum; - if (command_in->start == SERIAL_START_FRAME) { - checksum = (uint16_t)(command_in->start ^ command_in->steer ^ command_in->speed); - if (command_in->checksum == checksum) { - *command_out = *command_in; + if (command_in->start == SERIAL_START_FRAME) { + checksum = (uint16_t)(command_in->start ^ command_in->steer ^ command_in->speed); + if (command_in->checksum == checksum) { + *command_out = *command_in; if (usart_idx == 2) { // Sideboard USART2 #ifdef CONTROL_SERIAL_USART2 timeoutCntSerial_L = 0; // Reset timeout counter @@ -1173,12 +1168,12 @@ void usart_process_command(SerialCommand *command_in, SerialCommand *command_out */ #if defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3) void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sideboard_out, uint8_t usart_idx) -{ +{ 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); - if (Sideboard_in->checksum == checksum) { - *Sideboard_out = *Sideboard_in; + 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); + if (Sideboard_in->checksum == checksum) { + *Sideboard_out = *Sideboard_in; if (usart_idx == 2) { // Sideboard USART2 #ifdef SIDEBOARD_SERIAL_USART2 timeoutCntSerial_L = 0; // Reset timeout counter @@ -1191,7 +1186,7 @@ void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sid #endif } } - } + } } #endif