From df86ef44fdf773f6ac34d7a3db411dc035f2cea4 Mon Sep 17 00:00:00 2001 From: EmanuelFeru Date: Sun, 20 Dec 2020 10:16:31 +0100 Subject: [PATCH] Dual-inputs implemented - implemented dual-inputs functionality - the dual-inputs combinations mentioned in Readme are now supported --- Inc/config.h | 427 +++++++++++++++++--------------- Inc/defines.h | 6 +- Inc/eeprom.h | 2 +- Inc/util.h | 19 +- README.md | 36 ++- Src/control.c | 23 +- Src/main.c | 76 +++--- Src/util.c | 661 +++++++++++++++++++++++++++++++------------------- 8 files changed, 745 insertions(+), 505 deletions(-) diff --git a/Inc/config.h b/Inc/config.h index 22b45b1..45b2a9d 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -120,7 +120,7 @@ * 3. If you re-calibrate the Field Weakening please take all the safety measures! The motors can spin very fast! Inputs: - - input1.cmd and input2.cmd: analog normalized input values. INPUT_MIN to INPUT_MAX + - input1[inIdx].cmd and input2[inIdx].cmd: normalized input values. INPUT_MIN to INPUT_MAX - button1 and button2: digital input values. 0 or 1 - adc_buffer.l_tx2 and adc_buffer.l_rx2: unfiltered ADC values (you do not need them). 0 to 4095 Outputs: @@ -169,7 +169,9 @@ // 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 0x1233 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h +#define ADC_MARGIN 100 // ADC input margin applied on the raw ADC min and max to make sure the MIN and MAX values are reached even in the presence of noise +#define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken +#define ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values /* 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) @@ -185,6 +187,24 @@ +// ############################## INPUT FORMAT ############################ +/* ***_INPUT: TYPE, MIN, MID, MAX, DEADBAND + * ----------------------------------------- + * TYPE: 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect + * MIN: min ADC1-value while poti at minimum-position (0 - 4095) + * MID: mid ADC1-value while poti at mid-position (INPUT_MIN - INPUT_MAX) + * MAX: max ADC2-value while poti at maximum-position (0 - 4095) + * DEADBAND: how much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) + * + * Dual-inputs + * PRI_INPUT: Primary Input. These limits will be used for the input with priority 0 + * AUX_INPUT: Auxiliary Input. These limits will be used for the input with priority 1 + * ----------------------------------------- +*/ + // ############################## END OF INPUT FORMAT ############################ + + + // ############################## CRUISE CONTROL SETTINGS ############################ /* Cruise Control info: * enable CRUISE_CONTROL_SUPPORT and (SUPPORT_BUTTONS_LEFT or SUPPORT_BUTTONS_RIGHT depending on which cable is the button installed) @@ -210,10 +230,10 @@ * DEBUG_SERIAL_ASCII output is: * // "in1:345 in2:1337 cmdL:0 cmdR:0 BatADC:0 BatV:0 TempADC:0 Temp:0\r\n" * - * in1: (int16_t)input1); raw input1: ADC1, UART, PWM, PPM, iBUS - * in2: (int16_t)input2); raw input2: ADC2, UART, PWM, PPM, iBUS - * cmdL: (int16_t)cmdL); output command: [-1000, 1000] - * cmdR: (int16_t)cmdR); output command: [-1000, 1000] + * in1: (int16_t)input1[inIdx].raw); raw input1: ADC1, UART, PWM, PPM, iBUS + * in2: (int16_t)input2[inIdx].raw); raw input2: ADC2, UART, PWM, PPM, iBUS + * cmdL: (int16_t)cmdL); output command Left: [-1000, 1000] + * cmdR: (int16_t)cmdR); output command Right: [-1000, 1000] * BatADC: (int16_t)adc_buffer.batt1); Battery adc-value measured by mainboard * BatV: (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC)); Battery calibrated voltage multiplied by 100 for verifying battery voltage calibration * TempADC: (int16_t)board_temp_adcFilt); for board temperature calibration @@ -250,26 +270,27 @@ * Procedure: * - connect gnd, rx and tx of a usb-uart converter in 3.3V mode to the right sensor board cable (do NOT use the red 15V wire!) * - readout values using a serial terminal in 115200 baud rate - * - turn the potis to minimum position, write value 1 to INPUT1_MIN and value 2 to INPUT2_MIN - * - turn the potis to maximum position, write value 1 to INPUT1_MAX and value 2 to INPUT2_MAX - * - for middle resting potis: Let the potis in the middle resting position, write value 1 to INPUT1_MID and value 2 to INPUT2_MID + * - turn the potis to minimum position, write value in1 to PRI_INPUT1 MIN and value in2 to PRI_INPUT2 MIN + * - turn the potis to maximum position, write value in1 to PRI_INPUT1 MAX and value in2 to PRI_INPUT2 MAX + * - for middle resting potis: Let the potis in the middle resting position, write value in1 to PRI_INPUT1 MID and value in2 to PRI_INPUT2 MID */ - #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 200 // ADC Protection threshold below/above the MIN/MAX ADC values - #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT1_MIN 0 // min ADC1-value while poti at min-position (0 - 4095) - #define INPUT1_MID 0 // mid ADC1-value while poti at mid-position (INPUT1_MIN - INPUT1_MAX) - #define INPUT1_MAX 4095 // max ADC1-value while poti at max-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 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT2_MIN 0 // min ADC2-value while poti at min-position (0 - 4095) - #define INPUT2_MID 0 // mid ADC2-value while poti at mid-position (INPUT2_MIN - INPUT2_MAX) - #define INPUT2_MAX 4095 // max ADC2-value while poti at max-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 CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! + + // #define DUAL_INPUTS // ADC*(Primary) + UART(Auxiliary). Uncomment this to use Dual-inputs + #define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1101 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + // #define SIDEBOARD_SERIAL_USART3 1 + #define CONTROL_SERIAL_USART3 1 // right sensor board cable. Number indicates priority for dual-input. Disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino + #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + #define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #else + #define FLASH_WRITE_KEY 0x1001 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + #endif - #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! // #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 @@ -279,25 +300,27 @@ // ############################ VARIANT_USART SETTINGS ############################ #ifdef VARIANT_USART - // #define SIDEBOARD_SERIAL_USART2 - // #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino - // #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + // #define SIDEBOARD_SERIAL_USART2 0 + #define CONTROL_SERIAL_USART2 0 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino + #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + + // #define SIDEBOARD_SERIAL_USART3 0 + // #define CONTROL_SERIAL_USART3 0 // right sensor board cable. Number indicates priority for dual-input. Disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino + // #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + + // #define DUAL_INPUTS // UART*(Primary) + SIDEBOARD(Auxiliary). Uncomment this to use Dual-inputs + #define PRI_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1102 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + // #define SIDEBOARD_SERIAL_USART2 1 // left sideboard + #define SIDEBOARD_SERIAL_USART3 1 // right sideboard + #define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #else + #define FLASH_WRITE_KEY 0x1002 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #endif - // #define SIDEBOARD_SERIAL_USART3 - #define CONTROL_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino - #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 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #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 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #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) // #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 @@ -313,25 +336,28 @@ * use original nunchuk. most clones does not work very well. * Recommendation: Nunchuk Breakout Board https://github.com/Jan--Henrik/hoverboard-breakout */ - #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, 3:Auto-detect - #define INPUT1_MIN -1024 // (-1024 - 0) - #define INPUT1_MID 0 - #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, 3:Auto-detect - #define INPUT2_MIN -1024 // (-1024 - 0) - #define INPUT2_MID 0 - #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) + #define CONTROL_NUNCHUK 0 // use nunchuk as input. Number indicates priority for dual-input. Disable FEEDBACK_SERIAL_USART3, DEBUG_SERIAL_USART3! + + // #define DUAL_INPUTS // Nunchuk*(Primary) + UART(Auxiliary). Uncomment this to use Dual-inputs + #define PRI_INPUT1 2, -1024, 0, 1024, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 2, -1024, 0, 1024, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1103 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + // #define SIDEBOARD_SERIAL_USART2 1 + #define CONTROL_SERIAL_USART2 1 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino + #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + #define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #else + #define FLASH_WRITE_KEY 0x1003 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define DEBUG_SERIAL_USART2 // left sensor cable debug + #endif + // # maybe good for ARMCHAIR # - #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 FILTER 3276 // 0.05f + #define SPEED_COEFFICIENT 8192 // 0.5f + #define STEER_COEFFICIENT 62259 // -0.2f + // #define SUPPORT_BUTTONS // Define for Nunchuk buttons support #endif // ############################# END OF VARIANT_NUNCHUK SETTINGS ######################### @@ -343,29 +369,33 @@ * Right sensor board cable. Channel 1: steering, Channel 2: speed. * 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! - #ifdef CONTROL_PPM_RIGHT - #define DEBUG_SERIAL_USART2 // left sensor cable debug + // #define DUAL_INPUTS // ADC*(Primary) + PPM(Auxiliary). Uncomment this to use Dual-inputs + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1104 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! + #define CONTROL_PPM_RIGHT 1 // use PPM-Sum as input on the RIGHT cable. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART3! + #define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #else - #define DEBUG_SERIAL_USART3 // right sensor cable debug + #define FLASH_WRITE_KEY 0x1004 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + // #define CONTROL_PPM_LEFT 0 // use PPM-Sum as input on the LEFT cable. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2! + #define CONTROL_PPM_RIGHT 0 // use PPM-Sum as input on the RIGHT cable. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART3! + #define PRI_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #endif + #define PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used. + + // #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! + + #if defined(CONTROL_PPM_RIGHT) && !defined(DUAL_INPUTS) + #define DEBUG_SERIAL_USART2 // left sensor cable debug + #elif defined(CONTROL_PPM_LEFT) && !defined(DUAL_INPUTS) + #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. - // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT1_MIN -1000 // (-1000 - 0) - #define INPUT1_MID 0 - #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 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT2_MIN -1000 // (-1000 - 0) - #define INPUT2_MID 0 - #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! #endif // ############################# END OF VARIANT_PPM SETTINGS ############################ @@ -376,33 +406,36 @@ * Right sensor board cable. Connect PA2 to channel 1 and PA3 to channel 2 on receiver. * Channel 1: steering, Channel 2: speed. */ - // #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 - #define DEBUG_SERIAL_USART2 // left sensor cable debug + // #define DUAL_INPUTS // ADC*(Primary) + PWM(Auxiliary). Uncomment this to use Dual-inputs + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1105 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! + #define CONTROL_PWM_RIGHT 1 // use RC PWM as input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3! + #define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #else - #define DEBUG_SERIAL_USART3 // right sensor cable debug + #define FLASH_WRITE_KEY 0x1005 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + // #define CONTROL_PWM_LEFT 0 // use RC PWM as input on the LEFT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART2! + #define CONTROL_PWM_RIGHT 0 // use RC PWM as input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3! + #define PRI_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #endif - // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT1_MIN -1000 // (-1000 - 0) - #define INPUT1_MID 0 - #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 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT2_MIN -1000 // (-1000 - 0) - #define INPUT2_MID 0 - #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 - #define STEER_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. If you do not want any steering, set it to 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 + #define STEER_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. If you do not want any steering, set it to 0. // #define INVERT_R_DIRECTION // #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! + // #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! + + #if defined(CONTROL_PWM_RIGHT) && !defined(DUAL_INPUTS) + #define DEBUG_SERIAL_USART2 // left sensor cable debug + #elif defined(CONTROL_PWM_LEFT) && !defined(DUAL_INPUTS) + #define DEBUG_SERIAL_USART3 // right sensor cable debug + #endif #endif // ############################# END OF VARIANT_PWM SETTINGS ############################ @@ -413,33 +446,35 @@ /* CONTROL VIA RC REMOTE WITH FLYSKY IBUS PROTOCOL * Connected to Right 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 IBUS_LENGTH 0x20 - #define IBUS_COMMAND 0x40 + #define CONTROL_IBUS // use IBUS as input. Number indicates priority for dual-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 + #define USART3_BAUD 115200 - #undef USART3_BAUD - #define USART3_BAUD 115200 - #define CONTROL_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used! - #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used! - #ifdef CONTROL_SERIAL_USART3 - #define DEBUG_SERIAL_USART2 // left sensor cable debug + // #define DUAL_INPUTS // ADC*(Primary) + iBUS(Auxiliary). Uncomment this to use Dual-inputs + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1106 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! + #define CONTROL_SERIAL_USART3 1 // use RC iBUS input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3! + #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used! + #define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #else - #define DEBUG_SERIAL_USART3 // right sensor cable debug + #define FLASH_WRITE_KEY 0x1006 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define CONTROL_SERIAL_USART3 0 // use RC iBUS input on the RIGHT cable, disable if ADC or PPM is used! + #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used! + #define PRI_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #endif - // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #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 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #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) + #if defined(CONTROL_SERIAL_USART3) && !defined(DUAL_INPUTS) + #define DEBUG_SERIAL_USART2 // left sensor cable debug + #elif defined(DEBUG_SERIAL_USART2) && !defined(DUAL_INPUTS) + #define DEBUG_SERIAL_USART3 // right sensor cable debug + #endif #endif // ############################# END OF VARIANT_IBUS SETTINGS ############################ @@ -447,45 +482,38 @@ // ############################ VARIANT_HOVERCAR SETTINGS ############################ #ifdef VARIANT_HOVERCAR + #define FLASH_WRITE_KEY 0x1107 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h #undef CTRL_MOD_REQ - #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 200 // ADC Protection threshold below/above the MIN/MAX ADC values - - #define INPUT1_TYPE 1 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #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, 3:Auto-detect - #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 - // #define INVERT_R_DIRECTION // Invert rotation of right motor - // #define INVERT_L_DIRECTION // Invert rotation of left motor - #define SIDEBOARD_SERIAL_USART3 // Tx -> Rx of right sensor board: for LED battery indication. Comment-out if sideboard is not used! - #define FEEDBACK_SERIAL_USART3 // Rx <- Tx of right sensor board: to use photosensors as buttons. Comment-out if sideboard is not used! - // #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + #define CTRL_MOD_REQ TRQ_MODE // HOVERCAR works best in TORQUE Mode + #define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! + #define SIDEBOARD_SERIAL_USART3 1 // Rx from right sensor board: to use photosensors as buttons. Number indicates priority for dual-input. Comment-out if sideboard is not used! + #define FEEDBACK_SERIAL_USART3 // Tx to right sensor board: for LED battery indication. Comment-out if sideboard is not used! + + #define DUAL_INPUTS // ADC*(Primary) + Sideboard_R(Auxiliary). Uncomment this to use Dual-inputs + #define PRI_INPUT1 1, 1000, 0, 2500, 0 // Pedal Brake TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 1, 500, 0, 2200, 0 // Pedal Accel TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT1 2, -1000, 0, 1000, 0 // Sideboard Steer TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 2, -1000, 0, 1000, 0 // Sideboard Speed TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + + #define SPEED_COEFFICIENT 16384 // 1.0f + #define STEER_COEFFICIENT 8192 // 0.5f Only active in Sideboard input + // #define INVERT_R_DIRECTION // Invert rotation of right motor + // #define INVERT_L_DIRECTION // Invert rotation of left motor + // #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! // Extra functionality - // #define CRUISE_CONTROL_SUPPORT // [-] Flag to enable Cruise Control support. Activation/Deactivation is done by sideboard button or Brake pedal press. - // #define STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode. - // #define ELECTRIC_BRAKE_ENABLE // [-] Flag to enable electric brake and replace the motor "freewheel" with a constant braking when the input torque request is 0. Only available and makes sense for TORQUE mode. - // #define ELECTRIC_BRAKE_MAX 100 // (0, 500) Maximum electric brake to be applied when input torque request is 0 (pedal fully released). - // #define ELECTRIC_BRAKE_THRES 120 // (0, 500) Threshold below at which the electric brake starts engaging. + // #define CRUISE_CONTROL_SUPPORT // [-] Flag to enable Cruise Control support. Activation/Deactivation is done by sideboard button or Brake pedal press. + // #define STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode. + // #define ELECTRIC_BRAKE_ENABLE // [-] Flag to enable electric brake and replace the motor "freewheel" with a constant braking when the input torque request is 0. Only available and makes sense for TORQUE mode. + // #define ELECTRIC_BRAKE_MAX 100 // (0, 500) Maximum electric brake to be applied when input torque request is 0 (pedal fully released). + // #define ELECTRIC_BRAKE_THRES 120 // (0, 500) Threshold below at which the electric brake starts engaging. #endif // Multiple tap detection: default DOUBLE Tap on Brake pedal (4 pulses) -#define MULTIPLE_TAP_NR 2 * 2 // [-] Define tap number: MULTIPLE_TAP_NR = number_of_taps * 2, number_of_taps = 1 (for single taping), 2 (for double tapping), 3 (for triple tapping), etc... -#define MULTIPLE_TAP_HI 600 // [-] Multiple tap detection High hysteresis threshold -#define MULTIPLE_TAP_LO 200 // [-] Multiple tap detection Low hysteresis threshold -#define MULTIPLE_TAP_TIMEOUT 2000 // [ms] Multiple tap detection Timeout period. The taps need to happen within this time window to be accepted. +#define MULTIPLE_TAP_NR 2 * 2 // [-] Define tap number: MULTIPLE_TAP_NR = number_of_taps * 2, number_of_taps = 1 (for single taping), 2 (for double tapping), 3 (for triple tapping), etc... +#define MULTIPLE_TAP_HI 600 // [-] Multiple tap detection High hysteresis threshold +#define MULTIPLE_TAP_LO 200 // [-] Multiple tap detection Low hysteresis threshold +#define MULTIPLE_TAP_TIMEOUT 2000 // [ms] Multiple tap detection Timeout period. The taps need to happen within this time window to be accepted. // ######################## END OF VARIANT_HOVERCAR SETTINGS ######################### @@ -494,10 +522,18 @@ // Communication: [DONE] // Balancing controller: [TODO] #ifdef VARIANT_HOVERBOARD - #define SIDEBOARD_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + #define FLASH_WRITE_KEY 0x1008 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define SIDEBOARD_SERIAL_USART2 1 // left sensor board cable. Number indicates priority for dual-input. Disable if ADC or PPM is used! #define FEEDBACK_SERIAL_USART2 - #define SIDEBOARD_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! - #define FEEDBACK_SERIAL_USART3 + #define SIDEBOARD_SERIAL_USART3 0 // right sensor board cable. Number indicates priority for dual-input. Disable if I2C (nunchuk or lcd) is used! + #define FEEDBACK_SERIAL_USART3 + + // If an iBUS RC receiver is connected to either Left Sideboard (AUX_INPUT) or Right Sideboard (PRI_INPUT) + // PRIMARY INPUT: TYPE, MIN, MID, MAX, DEADBAND /* TYPE: 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect */ + #define PRI_INPUT1 3, -1000, 0, 1000, 0 // Priority Sideboard can be used to send commands via an iBUS Receiver connected to the sideboard + #define PRI_INPUT2 3, -1000, 0, 1000, 0 // Priority Sideboard can be used to send commands via an iBUS Receiver connected to the sideboard + #define AUX_INPUT1 3, -1000, 0, 1000, 0 // not used + #define AUX_INPUT2 3, -1000, 0, 1000, 0 // not used #endif // ######################## END OF VARIANT_HOVERBOARD SETTINGS ######################### @@ -506,17 +542,20 @@ // ################################# VARIANT_TRANSPOTTER SETTINGS ############################ //TODO ADD VALIDATION #ifdef VARIANT_TRANSPOTTER + #define FLASH_WRITE_KEY 0x1009 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h #define CONTROL_GAMETRAK #define SUPPORT_LCD // #define SUPPORT_NUNCHUK #define GAMETRAK_CONNECTION_NORMAL // for normal wiring according to the wiki instructions - //#define GAMETRAK_CONNECTION_ALTERNATE // use this define instead if you messed up the gametrak ADC wiring (steering is speed, and length of the wire is steering) + // #define GAMETRAK_CONNECTION_ALTERNATE // use this define instead if you messed up the gametrak ADC wiring (steering is speed, and length of the wire is steering) #define ROT_P 1.2 // P coefficient for the direction controller. Positive / Negative values to invert gametrak steering direction. // during nunchuk control (only relevant when activated) #define SPEED_COEFFICIENT 14746 // 0.9f - higher value == stronger. 0.0 to ~2.0? #define STEER_COEFFICIENT 8192 // 0.5f - higher value == stronger. if you do not want any steering, set it to 0.0; 0.0 to 1.0 #define INVERT_R_DIRECTION // Invert right motor #define INVERT_L_DIRECTION // Invert left motor + #define PRI_INPUT1 2, -1000, 0, 1000, 0 // dummy input, TRANSPOTTER does not use input limitations + #define PRI_INPUT2 2, -1000, 0, 1000, 0 // dummy input, TRANSPOTTER does not use input limitations #endif // ############################# END OF VARIANT_TRANSPOTTER SETTINGS ######################## @@ -527,37 +566,30 @@ * right sensor board cable. Connect PB10 to channel 1 and PB11 to channel 2 on receiver. * Channel 1: steering, Channel 2: speed. */ + #define FLASH_WRITE_KEY 0x1010 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h #undef CTRL_MOD_REQ - #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 - #define DEBUG_SERIAL_USART2 // left sensor cable debug - #else - #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, 3:Auto-detect - #define INPUT1_MIN -1000 // (-1000 - 0) - #define INPUT1_MID 0 - #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, 3:Auto-detect - #define INPUT2_MIN -800 // (-1000 - 0) - #define INPUT2_MID 0 - #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 - #define STEER_COEFFICIENT 0 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14. If you do not want any steering, set it to 0. + #define CTRL_MOD_REQ TRQ_MODE // SKATEBOARD works best in TORQUE Mode + // #define CONTROL_PWM_LEFT 0 // use RC PWM as input on the LEFT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART2! + #define CONTROL_PWM_RIGHT 0 // use RC PWM as input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3! + + #define PRI_INPUT1 0, -1000, 0, 1000, 0 // Disabled. TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 2, -800, 0, 700, 100 // Active. TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define INPUT_BRK -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 + #define STEER_COEFFICIENT 0 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14. If you do not want any steering, set it to 0. #define INVERT_R_DIRECTION #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! - // #define STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode. + // #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 STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode. + + #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_SKATEBOARD SETTINGS ############################ @@ -599,10 +631,10 @@ #ifndef STEER_COEFFICIENT #define STEER_COEFFICIENT DEFAULT_STEER_COEFFICIENT #endif -#ifdef CONTROL_ADC - #define INPUT_MARGIN 100 // Input margin applied on the raw ADC min and max to make sure the motor MIN and MAX values are reached even in the presence of noise +#if defined(PRI_INPUT1) && defined(PRI_INPUT2) && defined(AUX_INPUT1) && defined(AUX_INPUT2) + #define INPUTS_NR 2 #else - #define INPUT_MARGIN 0 + #define INPUTS_NR 1 #endif // ########################### END OF APPLY DEFAULT SETTING ############################ @@ -616,11 +648,6 @@ // General checks -#if (defined(CONTROL_ADC) || defined(CONTROL_SERIAL_USART2) || defined(CONTROL_PPM_LEFT) || defined(CONTROL_PWM_LEFT)) && \ - (defined(CONTROL_NUNCHUK) || defined(CONTROL_SERIAL_USART3) || defined(CONTROL_PPM_RIGHT) || defined(CONTROL_PWM_RIGHT)) - #warning !! Multiple control input sources defined !! If NOT handled correctly, it can lead to undesired behavior! -#endif - #if defined(CONTROL_SERIAL_USART2) && defined(SIDEBOARD_SERIAL_USART2) #error CONTROL_SERIAL_USART2 and SIDEBOARD_SERIAL_USART2 not allowed, choose one. #endif diff --git a/Inc/defines.h b/Inc/defines.h index 6f0892d..7e02176 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -224,10 +224,8 @@ void PWM_ISR_CH2_Callback(void); // RC iBUS switch definitions. Flysky FS-i6S has SW1, SW4 - 2 positions; SW2, SW3 - 3 positions #define SW1_SET (0x0100) // 0000 0001 0000 0000 -#define SW2_SET1 (0x0200) // 0000 0010 0000 0000 -#define SW2_SET2 (0x0400) // 0000 0100 0000 0000 -#define SW3_SET1 (0x0800) // 0000 1000 0000 0000 -#define SW3_SET2 (0x1000) // 0001 0000 0000 0000 +#define SW2_SET (0x0600) // 0000 0110 0000 0000 +#define SW3_SET (0x1800) // 0001 1000 0000 0000 #define SW4_SET (0x2000) // 0010 0000 0000 0000 diff --git a/Inc/eeprom.h b/Inc/eeprom.h index cc7b1b9..e78b37b 100644 --- a/Inc/eeprom.h +++ b/Inc/eeprom.h @@ -209,7 +209,7 @@ #define PAGE_FULL ((uint8_t)0x80) /* Variables' number */ -#define NB_OF_VAR ((uint8_t)0x0B) +#define NB_OF_VAR ((uint8_t)0x13) /* 19 Variables */ /* Exported types ------------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ diff --git a/Inc/util.h b/Inc/util.h index ee15f67..714ef86 100644 --- a/Inc/util.h +++ b/Inc/util.h @@ -55,14 +55,16 @@ } SerialSideboard; #endif +// Input Structure typedef struct { int16_t raw; // raw input - int16_t cmd; // command (scaled) - uint8_t typ; - int16_t min; - int16_t mid; - int16_t max; - int16_t deadband; + int16_t cmd; // command + uint8_t typ; // type + uint8_t typDef; // type Defined + int16_t min; // minimum + int16_t mid; // middle + int16_t max; // maximum + int16_t dband; // deadband } InputStruct; // Initialization Functions @@ -85,9 +87,10 @@ void electricBrake(uint16_t speedBlend, uint8_t reverseDir); void cruiseControl(uint8_t button); int checkInputType(int16_t min, int16_t mid, int16_t max); -// Read Functions +// Input Functions +void calcInputCmd(InputStruct *in, int16_t out_min, int16_t out_max); void readInputRaw(void); -void readInputCmd(InputStruct *in, int16_t out_min, int16_t out_max); +void handleTimeout(void); void readCommand(void); void usart2_rx_check(void); void usart3_rx_check(void); diff --git a/README.md b/README.md index c061f2a..79d1bc0 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,8 @@ Table of Contents * [Hardware](#hardware) * [FOC Firmware](#foc-firmware) -* [Example Variants ](#example-variants) +* [Example Variants](#example-variants) +* [Dual Inputs](#dual-inputs) * [Flashing](#flashing) * [Troubleshooting](#troubleshooting) * [Diagnostics](#diagnostics) @@ -76,11 +77,9 @@ In this firmware 3 control types are available: |FOC SPEED| +++ | +++ | + | ++ | n.a. | +++ | |FOC TORQUE| +++ | +++ | +++ | ++ | +++(1) | n.a(2) | -(1) By enabling `ELECTRIC_BRAKE_ENABLE` in `config.h`, the freewheeling amount can be adjusted using the `ELECTRIC_BRAKE_MAX` parameter. - +(1) By enabling `ELECTRIC_BRAKE_ENABLE` in `config.h`, the freewheeling amount can be adjusted using the `ELECTRIC_BRAKE_MAX` parameter.
(2) The standstill hold functionality can be forced by enabling `STANDSTILL_HOLD_ENABLE` in `config.h`. - In all FOC control modes, the controller features maximum motor speed and maximum motor current protection. This brings great advantages to fulfil the needs of many robotic applications while maintaining safe operation. @@ -94,7 +93,7 @@ In all FOC control modes, the controller features maximum motor speed and maximu - If you re-calibrate the Field Weakening please take all the safety measures! The motors can spin very fast! -### Parameters +### Parameters - All the calibratable motor parameters can be found in the 'BLDC_controller_data.c'. I provided you with an already calibrated controller, but if you feel like fine tuning it feel free to do so - The parameters are represented in Fixed-point data type for a more efficient code execution - For calibrating the fixed-point parameters use the [Fixed-Point Viewer](https://github.com/EmanuelFeru/FixedPointViewer) tool @@ -102,7 +101,7 @@ In all FOC control modes, the controller features maximum motor speed and maximu --- -## Example Variants +## Example Variants This firmware offers currently these variants (selectable in [platformio.ini](/platformio.ini) or [config.h](/Inc/config.h)): - **VARIANT_ADC**: The motors are controlled by two potentiometers connected to the Left sensor cable (long wired) @@ -119,6 +118,29 @@ This firmware offers currently these variants (selectable in [platformio.ini](/p Of course the firmware can be further customized for other needs or projects. +--- +## Dual Inputs + +The firmware supports the input to be provided from two different sources connected to the Left and Right cable, respectively. To enable dual-inputs functionality uncomment `#define DUAL_INPUTS` in config.h for the respective variant. Various dual-inputs combinations can be realized as illustrated in the following table: +| Left | Right | Availability | +| --- | --- | --- | +| ADC(0) | UART(1) | VARIANT_ADC | +| ADC(0) | {PPM,PWM,iBUS}(1) | VARIANT_{PPM,PWM,IBUS} | +| ADC(0) | Sideboard(1*) | VARIANT_HOVERCAR | +| UART(0) | Sideboard(1) | VARIANT_UART | +| UART(1) | Nunchuk(0) | VARIANT_NUNCHUK | + +(0) Primary input: this input is used when the Auxilliary input is not available or not connected.
+(1) Auxilliary input: this inputs is used when connected or enabled by a switch(*). If the Auxilliary input is disconnected, the firmware will automatically switch to the Primary input. Timeout is reported **only** on the Primary input. + +With slight modifications in config.h, other dual-inputs combinations can be realized as: +| Left | Right | Possibility | +| --- | --- | --- | +| Sideboard(1) | UART(0) | VARIANT_UART | +| {PPM,PWM,iBUS}(1) | UART(0) | VARIANT_{PPM,PWM,IBUS} | +| {PPM,PWM,iBUS}(1) | Nunchuk(0) | VARIANT_{PPM,PWM,IBUS} | + + --- ## Flashing @@ -223,7 +245,7 @@ The errors reported by the board are in the form of audible beeps: - **1 beep (low pitch)**: Motor error (see [possible causes](https://github.com/EmanuelFeru/bldc-motor-control-FOC#diagnostics)) - **2 beeps (low pitch)**: ADC timeout - **3 beeps (low pitch)**: Serial communication timeout -- **4 beeps (low pitch)**: General timeout (PPM, PWM, Nunchuck) +- **4 beeps (low pitch)**: General timeout (PPM, PWM, Nunchuk) - **5 beeps (low pitch)**: Mainboard temperature warning - **1 beep slow (medium pitch)**: Low battery voltage < 36V - **1 beep fast (medium pitch)**: Low battery voltage < 35V diff --git a/Src/control.c b/Src/control.c index 3a82eb1..a1bb91a 100644 --- a/Src/control.c +++ b/Src/control.c @@ -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(); } diff --git a/Src/main.c b/Src/main.c index 93bf2a2..fca8cab 100644 --- a/Src/main.c +++ b/Src/main.c @@ -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++; } } diff --git a/Src/util.c b/Src/util.c index b4af78c..a9c7071 100644 --- a/Src/util.c +++ b/Src/util.c @@ -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> 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 8) { // Short press: power off (80 ms debounce) poweroff(); } }