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

Dual-inputs implemented 

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

427
Inc/config.h
View File

@ -120,7 +120,7 @@
* 3. If you re-calibrate the Field Weakening please take all the safety measures! The motors can spin very fast! * 3. If you re-calibrate the Field Weakening please take all the safety measures! The motors can spin very fast!
Inputs: 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 - 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 - adc_buffer.l_tx2 and adc_buffer.l_rx2: unfiltered ADC values (you do not need them). 0 to 4095
Outputs: Outputs:
@ -169,7 +169,9 @@
// Default settings will be applied at the end of this config file if not set before // 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 INACTIVITY_TIMEOUT 8 // Minutes of not driving until poweroff. it is not very precise.
#define BEEPS_BACKWARD 1 // 0 or 1 #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 /* 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) * 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 SETTINGS ############################
/* Cruise Control info: /* Cruise Control info:
* enable CRUISE_CONTROL_SUPPORT and (SUPPORT_BUTTONS_LEFT or SUPPORT_BUTTONS_RIGHT depending on which cable is the button installed) * 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: * DEBUG_SERIAL_ASCII output is:
* // "in1:345 in2:1337 cmdL:0 cmdR:0 BatADC:0 BatV:0 TempADC:0 Temp:0\r\n" * // "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 * in1: (int16_t)input1[inIdx].raw); raw input1: ADC1, UART, PWM, PPM, iBUS
* in2: (int16_t)input2); raw input2: ADC2, UART, PWM, PPM, iBUS * in2: (int16_t)input2[inIdx].raw); raw input2: ADC2, UART, PWM, PPM, iBUS
* cmdL: (int16_t)cmdL); output command: [-1000, 1000] * cmdL: (int16_t)cmdL); output command Left: [-1000, 1000]
* cmdR: (int16_t)cmdR); output command: [-1000, 1000] * cmdR: (int16_t)cmdR); output command Right: [-1000, 1000]
* BatADC: (int16_t)adc_buffer.batt1); Battery adc-value measured by mainboard * 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 * 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 * TempADC: (int16_t)board_temp_adcFilt); for board temperature calibration
@ -250,26 +270,27 @@
* Procedure: * 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!) * - 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 * - 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 minimum position, write value in1 to PRI_INPUT1 MIN and value in2 to PRI_INPUT2 MIN
* - turn the potis to maximum position, write value 1 to INPUT1_MAX and value 2 to INPUT2_MAX * - 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 1 to INPUT1_MID and value 2 to INPUT2_MID * - 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 CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-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 DUAL_INPUTS // ADC*(Primary) + UART(Auxiliary). Uncomment this to use Dual-inputs
#define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT1_MIN 0 // min ADC1-value while poti at min-position (0 - 4095) #define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT1_MID 0 // mid ADC1-value while poti at mid-position (INPUT1_MIN - INPUT1_MAX) #ifdef DUAL_INPUTS
#define INPUT1_MAX 4095 // max ADC1-value while poti at max-position (0 - 4095) #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 INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) // #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 INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
#define INPUT2_MIN 0 // min ADC2-value while poti at min-position (0 - 4095) #define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT2_MID 0 // mid ADC2-value while poti at mid-position (INPUT2_MIN - INPUT2_MAX) #define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT2_MAX 4095 // max ADC2-value while poti at max-position (0 - 4095) #else
#define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #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_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_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
#endif #endif
@ -279,25 +300,27 @@
// ############################ VARIANT_USART SETTINGS ############################ // ############################ VARIANT_USART SETTINGS ############################
#ifdef VARIANT_USART #ifdef VARIANT_USART
// #define SIDEBOARD_SERIAL_USART2 // #define SIDEBOARD_SERIAL_USART2 0
// #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino #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 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_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_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3!
#endif #endif
@ -313,25 +336,28 @@
* use original nunchuk. most clones does not work very well. * use original nunchuk. most clones does not work very well.
* Recommendation: Nunchuk Breakout Board https://github.com/Jan--Henrik/hoverboard-breakout * 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! #define CONTROL_NUNCHUK 0 // use nunchuk as input. Number indicates priority for dual-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 DUAL_INPUTS // Nunchuk*(Primary) + UART(Auxiliary). Uncomment this to use Dual-inputs
#define INPUT1_MIN -1024 // (-1024 - 0) #define PRI_INPUT1 2, -1024, 0, 1024, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT1_MID 0 #define PRI_INPUT2 2, -1024, 0, 1024, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT1_MAX 1024 // (0 - 1024) #ifdef DUAL_INPUTS
#define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #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 INPUT2_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define CONTROL_SERIAL_USART2 1 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino
#define INPUT2_MIN -1024 // (-1024 - 0) #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
#define INPUT2_MID 0 #define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT2_MAX 1024 // (0 - 1024) #define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #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 # // # maybe good for ARMCHAIR #
#define FILTER 3276 // 0.05f #define FILTER 3276 // 0.05f
#define SPEED_COEFFICIENT 8192 // 0.5f #define SPEED_COEFFICIENT 8192 // 0.5f
#define STEER_COEFFICIENT 62259 // -0.2f #define STEER_COEFFICIENT 62259 // -0.2f
#define DEBUG_SERIAL_USART2 // left sensor cable debug // #define SUPPORT_BUTTONS // Define for Nunchuk buttons support
// #define SUPPORT_BUTTONS // Define for Nunchuck buttons support
#endif #endif
// ############################# END OF VARIANT_NUNCHUK SETTINGS ######################### // ############################# END OF VARIANT_NUNCHUK SETTINGS #########################
@ -343,29 +369,33 @@
* Right sensor board cable. Channel 1: steering, Channel 2: speed. * Right sensor board cable. Channel 1: steering, Channel 2: speed.
* https://gist.github.com/peterpoetzi/1b63a4a844162196613871767189bd05 * https://gist.github.com/peterpoetzi/1b63a4a844162196613871767189bd05
*/ */
// #define CONTROL_PPM_LEFT // use PPM-Sum as input on the LEFT cable . disable CONTROL_SERIAL_USART2! // #define DUAL_INPUTS // ADC*(Primary) + PPM(Auxiliary). Uncomment this to use Dual-inputs
#define CONTROL_PPM_RIGHT // use PPM-Sum as input on the RIGHT cable. disable CONTROL_SERIAL_USART3! #ifdef DUAL_INPUTS
#ifdef CONTROL_PPM_RIGHT #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 DEBUG_SERIAL_USART2 // left sensor cable debug #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 #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 #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 #endif
// ############################# END OF VARIANT_PPM SETTINGS ############################ // ############################# 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. * Right sensor board cable. Connect PA2 to channel 1 and PA3 to channel 2 on receiver.
* Channel 1: steering, Channel 2: speed. * Channel 1: steering, Channel 2: speed.
*/ */
// #define CONTROL_PWM_LEFT // use RC PWM as input on the LEFT cable. disable DEBUG_SERIAL_USART2! // #define DUAL_INPUTS // ADC*(Primary) + PWM(Auxiliary). Uncomment this to use Dual-inputs
#define CONTROL_PWM_RIGHT // use RC PWM as input on the RIGHT cable. disable DEBUG_SERIAL_USART3! #ifdef DUAL_INPUTS
#ifdef CONTROL_PWM_RIGHT #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 DEBUG_SERIAL_USART2 // left sensor cable debug #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 #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 #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 FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0].
#define INPUT1_MIN -1000 // (-1000 - 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 INPUT1_MID 0 #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 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 INVERT_R_DIRECTION // #define INVERT_R_DIRECTION
// #define INVERT_L_DIRECTION // #define INVERT_L_DIRECTION
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! // #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_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 #endif
// ############################# END OF VARIANT_PWM SETTINGS ############################ // ############################# END OF VARIANT_PWM SETTINGS ############################
@ -413,33 +446,35 @@
/* CONTROL VIA RC REMOTE WITH FLYSKY IBUS PROTOCOL /* CONTROL VIA RC REMOTE WITH FLYSKY IBUS PROTOCOL
* Connected to Right sensor board cable. Channel 1: steering, Channel 2: speed. * Connected to Right sensor board cable. Channel 1: steering, Channel 2: speed.
*/ */
#define CONTROL_IBUS // use IBUS as input #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_NUM_CHANNELS 14 // total number of IBUS channels to receive, even if they are not used.
#define IBUS_LENGTH 0x20 #define IBUS_LENGTH 0x20
#define IBUS_COMMAND 0x40 #define IBUS_COMMAND 0x40
#define USART3_BAUD 115200
#undef USART3_BAUD // #define DUAL_INPUTS // ADC*(Primary) + iBUS(Auxiliary). Uncomment this to use Dual-inputs
#define USART3_BAUD 115200 #ifdef DUAL_INPUTS
#define CONTROL_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used! #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 FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used! #define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
#ifdef CONTROL_SERIAL_USART3 #define CONTROL_SERIAL_USART3 1 // use RC iBUS input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3!
#define DEBUG_SERIAL_USART2 // left sensor cable debug #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 #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 #endif
// Min / Max values of each channel (use DEBUG to determine these values) #if defined(CONTROL_SERIAL_USART3) && !defined(DUAL_INPUTS)
#define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define DEBUG_SERIAL_USART2 // left sensor cable debug
#define INPUT1_MIN -1000 // (-1000 - 0) #elif defined(DEBUG_SERIAL_USART2) && !defined(DUAL_INPUTS)
#define INPUT1_MID 0 #define DEBUG_SERIAL_USART3 // right sensor cable debug
#define INPUT1_MAX 1000 // (0 - 1000) #endif
#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)
#endif #endif
// ############################# END OF VARIANT_IBUS SETTINGS ############################ // ############################# END OF VARIANT_IBUS SETTINGS ############################
@ -447,45 +482,38 @@
// ############################ VARIANT_HOVERCAR SETTINGS ############################ // ############################ VARIANT_HOVERCAR SETTINGS ############################
#ifdef VARIANT_HOVERCAR #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 #undef CTRL_MOD_REQ
#define CTRL_MOD_REQ TRQ_MODE // HOVERCAR works best in TORQUE Mode #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 CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-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 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 ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values #define FEEDBACK_SERIAL_USART3 // Tx to right sensor board: for LED battery indication. Comment-out if sideboard is not used!
#define INPUT1_TYPE 1 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define DUAL_INPUTS // ADC*(Primary) + Sideboard_R(Auxiliary). Uncomment this to use Dual-inputs
#define INPUT1_MIN 1000 // min ADC1-value while poti at minimum-position (0 - 4095) #define PRI_INPUT1 1, 1000, 0, 2500, 0 // Pedal Brake TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT1_MID 0 #define PRI_INPUT2 1, 500, 0, 2200, 0 // Pedal Accel TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT1_MAX 2500 // max ADC1-value while poti at maximum-position (0 - 4095) #define AUX_INPUT1 2, -1000, 0, 1000, 0 // Sideboard Steer TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) #define AUX_INPUT2 2, -1000, 0, 1000, 0 // Sideboard Speed TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define INPUT2_TYPE 1 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #define SPEED_COEFFICIENT 16384 // 1.0f
#define INPUT2_MIN 500 // min ADC2-value while poti at minimum-position (0 - 4095) #define STEER_COEFFICIENT 8192 // 0.5f Only active in Sideboard input
#define INPUT2_MID 0 // #define INVERT_R_DIRECTION // Invert rotation of right motor
#define INPUT2_MAX 2200 // max ADC2-value while poti at maximum-position (0 - 4095) // #define INVERT_L_DIRECTION // Invert rotation of left motor
#define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) // #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used!
#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!
// Extra functionality // Extra functionality
// #define CRUISE_CONTROL_SUPPORT // [-] Flag to enable Cruise Control support. Activation/Deactivation is done by sideboard button or Brake pedal press. // #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 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_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_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 ELECTRIC_BRAKE_THRES 120 // (0, 500) Threshold below at which the electric brake starts engaging.
#endif #endif
// Multiple tap detection: default DOUBLE Tap on Brake pedal (4 pulses) // 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_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_HI 600 // [-] Multiple tap detection High hysteresis threshold
#define MULTIPLE_TAP_LO 200 // [-] Multiple tap detection Low 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_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 ######################### // ######################## END OF VARIANT_HOVERCAR SETTINGS #########################
@ -494,10 +522,18 @@
// Communication: [DONE] // Communication: [DONE]
// Balancing controller: [TODO] // Balancing controller: [TODO]
#ifdef VARIANT_HOVERBOARD #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 FEEDBACK_SERIAL_USART2
#define SIDEBOARD_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! #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 #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 #endif
// ######################## END OF VARIANT_HOVERBOARD SETTINGS ######################### // ######################## END OF VARIANT_HOVERBOARD SETTINGS #########################
@ -506,17 +542,20 @@
// ################################# VARIANT_TRANSPOTTER SETTINGS ############################ // ################################# VARIANT_TRANSPOTTER SETTINGS ############################
//TODO ADD VALIDATION //TODO ADD VALIDATION
#ifdef VARIANT_TRANSPOTTER #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 CONTROL_GAMETRAK
#define SUPPORT_LCD #define SUPPORT_LCD
// #define SUPPORT_NUNCHUK // #define SUPPORT_NUNCHUK
#define GAMETRAK_CONNECTION_NORMAL // for normal wiring according to the wiki instructions #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. #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) // during nunchuk control (only relevant when activated)
#define SPEED_COEFFICIENT 14746 // 0.9f - higher value == stronger. 0.0 to ~2.0? #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 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_R_DIRECTION // Invert right motor
#define INVERT_L_DIRECTION // Invert left 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 #endif
// ############################# END OF VARIANT_TRANSPOTTER SETTINGS ######################## // ############################# 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. * right sensor board cable. Connect PB10 to channel 1 and PB11 to channel 2 on receiver.
* Channel 1: steering, Channel 2: speed. * 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 #undef CTRL_MOD_REQ
#define CTRL_MOD_REQ TRQ_MODE // SKATEBOARD works best in TORQUE Mode #define CTRL_MOD_REQ TRQ_MODE // SKATEBOARD works best in TORQUE Mode
//#define CONTROL_PWM_LEFT // use RC PWM as input on the LEFT cable. disable DEBUG_SERIAL_USART2! // #define CONTROL_PWM_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 // use RC PWM as input on the RIGHT cable. disable DEBUG_SERIAL_USART3! #define CONTROL_PWM_RIGHT 0 // use RC PWM as input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3!
#ifdef CONTROL_PWM_RIGHT
#define DEBUG_SERIAL_USART2 // left sensor cable debug #define PRI_INPUT1 0, -1000, 0, 1000, 0 // Disabled. TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#else #define PRI_INPUT2 2, -800, 0, 700, 100 // Active. TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section
#define DEBUG_SERIAL_USART3 // right sensor cable debug #define INPUT_BRK -400 // (-1000 - 0) Change this value to adjust the braking amount
#endif
// Min / Max values of each channel (use DEBUG to determine these values) #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0].
#define INPUT1_TYPE 0 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect #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 INPUT1_MIN -1000 // (-1000 - 0) #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 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 INVERT_R_DIRECTION #define INVERT_R_DIRECTION
#define INVERT_L_DIRECTION #define INVERT_L_DIRECTION
// #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! // #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_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 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 #endif
// ############################# END OF VARIANT_SKATEBOARD SETTINGS ############################ // ############################# END OF VARIANT_SKATEBOARD SETTINGS ############################
@ -599,10 +631,10 @@
#ifndef STEER_COEFFICIENT #ifndef STEER_COEFFICIENT
#define STEER_COEFFICIENT DEFAULT_STEER_COEFFICIENT #define STEER_COEFFICIENT DEFAULT_STEER_COEFFICIENT
#endif #endif
#ifdef CONTROL_ADC #if defined(PRI_INPUT1) && defined(PRI_INPUT2) && defined(AUX_INPUT1) && defined(AUX_INPUT2)
#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 #define INPUTS_NR 2
#else #else
#define INPUT_MARGIN 0 #define INPUTS_NR 1
#endif #endif
// ########################### END OF APPLY DEFAULT SETTING ############################ // ########################### END OF APPLY DEFAULT SETTING ############################
@ -616,11 +648,6 @@
// General checks // 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) #if defined(CONTROL_SERIAL_USART2) && defined(SIDEBOARD_SERIAL_USART2)
#error CONTROL_SERIAL_USART2 and SIDEBOARD_SERIAL_USART2 not allowed, choose one. #error CONTROL_SERIAL_USART2 and SIDEBOARD_SERIAL_USART2 not allowed, choose one.
#endif #endif

6
Inc/defines.h
View File

@ -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 // 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 SW1_SET (0x0100) // 0000 0001 0000 0000
#define SW2_SET1 (0x0200) // 0000 0010 0000 0000 #define SW2_SET (0x0600) // 0000 0110 0000 0000
#define SW2_SET2 (0x0400) // 0000 0100 0000 0000 #define SW3_SET (0x1800) // 0001 1000 0000 0000
#define SW3_SET1 (0x0800) // 0000 1000 0000 0000
#define SW3_SET2 (0x1000) // 0001 0000 0000 0000
#define SW4_SET (0x2000) // 0010 0000 0000 0000 #define SW4_SET (0x2000) // 0010 0000 0000 0000

2
Inc/eeprom.h
View File

@ -209,7 +209,7 @@
#define PAGE_FULL ((uint8_t)0x80) #define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */ /* Variables' number */
#define NB_OF_VAR ((uint8_t)0x0B) #define NB_OF_VAR ((uint8_t)0x13) /* 19 Variables */
/* Exported types ------------------------------------------------------------*/ /* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/

19
Inc/util.h
View File

@ -55,14 +55,16 @@
} SerialSideboard; } SerialSideboard;
#endif #endif
// Input Structure
typedef struct { typedef struct {
int16_t raw; // raw input int16_t raw; // raw input
int16_t cmd; // command (scaled) int16_t cmd; // command
uint8_t typ; uint8_t typ; // type
int16_t min; uint8_t typDef; // type Defined
int16_t mid; int16_t min; // minimum
int16_t max; int16_t mid; // middle
int16_t deadband; int16_t max; // maximum
int16_t dband; // deadband
} InputStruct; } InputStruct;
// Initialization Functions // Initialization Functions
@ -85,9 +87,10 @@ void electricBrake(uint16_t speedBlend, uint8_t reverseDir);
void cruiseControl(uint8_t button); void cruiseControl(uint8_t button);
int checkInputType(int16_t min, int16_t mid, int16_t max); 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 readInputRaw(void);
void readInputCmd(InputStruct *in, int16_t out_min, int16_t out_max); void handleTimeout(void);
void readCommand(void); void readCommand(void);
void usart2_rx_check(void); void usart2_rx_check(void);
void usart3_rx_check(void); void usart3_rx_check(void);

36
README.md
View File

@ -14,7 +14,8 @@ Table of Contents
* [Hardware](#hardware) * [Hardware](#hardware)
* [FOC Firmware](#foc-firmware) * [FOC Firmware](#foc-firmware)
* [Example Variants ](#example-variants) * [Example Variants](#example-variants)
* [Dual Inputs](#dual-inputs)
* [Flashing](#flashing) * [Flashing](#flashing)
* [Troubleshooting](#troubleshooting) * [Troubleshooting](#troubleshooting)
* [Diagnostics](#diagnostics) * [Diagnostics](#diagnostics)
@ -76,11 +77,9 @@ In this firmware 3 control types are available:
|FOC SPEED| +++ | +++ | + | ++ | n.a. | +++ | |FOC SPEED| +++ | +++ | + | ++ | n.a. | +++ |
|FOC TORQUE| +++ | +++ | +++ | ++ | +++<sup>(1)</sup> | n.a<sup>(2)</sup> | |FOC TORQUE| +++ | +++ | +++ | ++ | +++<sup>(1)</sup> | n.a<sup>(2)</sup> |
<sup>(1)</sup> By enabling `ELECTRIC_BRAKE_ENABLE` in `config.h`, the freewheeling amount can be adjusted using the `ELECTRIC_BRAKE_MAX` parameter. <sup>(1)</sup> By enabling `ELECTRIC_BRAKE_ENABLE` in `config.h`, the freewheeling amount can be adjusted using the `ELECTRIC_BRAKE_MAX` parameter.<br/>
<sup>(2)</sup> The standstill hold functionality can be forced by enabling `STANDSTILL_HOLD_ENABLE` in `config.h`. <sup>(2)</sup> 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. 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! - 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 - 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 - 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 - 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)): 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) - **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. 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<sup>(0)</sup> | UART<sup>(1)</sup> | VARIANT_ADC |
| ADC<sup>(0)</sup> | {PPM,PWM,iBUS}<sup>(1)</sup> | VARIANT_{PPM,PWM,IBUS} |
| ADC<sup>(0)</sup> | Sideboard<sup></sup><sup>(1*)</sup> | VARIANT_HOVERCAR |
| UART<sup>(0)</sup> | Sideboard<sup>(1)</sup> | VARIANT_UART |
| UART<sup>(1)</sup> | Nunchuk<sup>(0)</sup> | VARIANT_NUNCHUK |
<sup>(0)</sup> Primary input: this input is used when the Auxilliary input is not available or not connected.<br/>
<sup>(1)</sup> Auxilliary input: this inputs is used when connected or enabled by a switch<sup>(*)</sup>. 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<sup>(1)</sup> | UART<sup>(0)</sup> | VARIANT_UART |
| {PPM,PWM,iBUS}<sup>(1)</sup> | UART<sup>(0)</sup> | VARIANT_{PPM,PWM,IBUS} |
| {PPM,PWM,iBUS}<sup>(1)</sup> | Nunchuk<sup>(0)</sup> | VARIANT_{PPM,PWM,IBUS} |
--- ---
## Flashing ## 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)) - **1 beep (low pitch)**: Motor error (see [possible causes](https://github.com/EmanuelFeru/bldc-motor-control-FOC#diagnostics))
- **2 beeps (low pitch)**: ADC timeout - **2 beeps (low pitch)**: ADC timeout
- **3 beeps (low pitch)**: Serial communication 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 - **5 beeps (low pitch)**: Mainboard temperature warning
- **1 beep slow (medium pitch)**: Low battery voltage < 36V - **1 beep slow (medium pitch)**: Low battery voltage < 36V
- **1 beep fast (medium pitch)**: Low battery voltage < 35V - **1 beep fast (medium pitch)**: Low battery voltage < 35V

23
Src/control.c
View File

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

76
Src/main.c
View File

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

661
Src/util.c
View File

@ -53,7 +53,8 @@ extern uint8_t buzzerPattern; // global variable for the buzzer patter
extern uint8_t enable; // global variable for motor enable extern uint8_t enable; // global variable for motor enable
extern uint8_t nunchuk_data[6]; 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; extern volatile uint32_t main_loop_counter;
#if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT) #if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)
@ -87,13 +88,19 @@ ExtU rtU_Right; /* External inputs */
ExtY rtY_Right; /* External outputs */ ExtY rtY_Right; /* External outputs */
//--------------- //---------------
InputStruct input1; // input structure uint8_t inIdx;
InputStruct input2; // input structure #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 speedAvg; // average measured speed
int16_t speedAvgAbs; // average measured speed in absolute 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 timeoutFlgADC = 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 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 ctrlModReqRaw = CTRL_MOD_REQ;
uint8_t ctrlModReq = CTRL_MOD_REQ; // Final control mode request uint8_t ctrlModReq = CTRL_MOD_REQ; // Final control mode request
@ -110,13 +117,14 @@ uint8_t nunchuk_connected = 0;
#ifdef VARIANT_TRANSPOTTER #ifdef VARIANT_TRANSPOTTER
float setDistance; 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 uint16_t saveValue = 0;
static uint8_t saveValue_valid = 0; static uint8_t saveValue_valid = 0;
#elif !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) #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 #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 #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); static uint32_t rx_buffer_L_len = ARRAY_LEN(rx_buffer_L);
#endif #endif
#if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) #if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2)
static uint16_t timeoutCntSerial_L = 0; // Timeout counter for Rx Serial command 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 uint8_t timeoutFlgSerial_L = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
#endif #endif
#if defined(SIDEBOARD_SERIAL_USART2) #if defined(SIDEBOARD_SERIAL_USART2)
SerialSideboard Sideboard_L; 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); static uint32_t rx_buffer_R_len = ARRAY_LEN(rx_buffer_R);
#endif #endif
#if defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) #if defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3)
static uint16_t timeoutCntSerial_R = 0; // Timeout counter for Rx Serial command 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 uint8_t timeoutFlgSerial_R = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
#endif #endif
#if defined(SIDEBOARD_SERIAL_USART3) #if defined(SIDEBOARD_SERIAL_USART3)
SerialSideboard Sideboard_R; SerialSideboard Sideboard_R;
@ -164,13 +172,21 @@ SerialSideboard Sideboard_R_raw;
static uint32_t Sideboard_R_len = sizeof(Sideboard_R); static uint32_t Sideboard_R_len = sizeof(Sideboard_R);
#endif #endif
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) #if defined(CONTROL_SERIAL_USART2)
static SerialCommand command; static SerialCommand commandL;
static SerialCommand command_raw; static SerialCommand commandL_raw;
static uint32_t command_len = sizeof(command); static uint32_t commandL_len = sizeof(commandL);
#ifdef CONTROL_IBUS #ifdef CONTROL_IBUS
static uint16_t ibus_chksum; static uint16_t ibusL_captured_value[IBUS_NUM_CHANNELS];
static uint16_t ibus_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
#endif #endif
@ -297,35 +313,39 @@ void Input_Init(void) {
EE_Init(); /* EEPROM Init */ EE_Init(); /* EEPROM Init */
EE_ReadVariable(VirtAddVarTab[0], &writeCheck); EE_ReadVariable(VirtAddVarTab[0], &writeCheck);
if (writeCheck == FLASH_WRITE_KEY) { if (writeCheck == FLASH_WRITE_KEY) {
EE_ReadVariable(VirtAddVarTab[1] , &readVal); input1.typ = (uint8_t)readVal; EE_ReadVariable(VirtAddVarTab[1] , &readVal); rtP_Left.i_max = rtP_Right.i_max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[2] , &readVal); input1.min = (int16_t)readVal; EE_ReadVariable(VirtAddVarTab[2] , &readVal); rtP_Left.n_max = rtP_Right.n_max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[3] , &readVal); input1.mid = (int16_t)readVal; for (uint8_t i=0; i<INPUTS_NR; i++) {
EE_ReadVariable(VirtAddVarTab[4] , &readVal); input1.max = (int16_t)readVal; EE_ReadVariable(VirtAddVarTab[ 3+8*i] , &readVal); input1[i].typ = (uint8_t)readVal;
EE_ReadVariable(VirtAddVarTab[5] , &readVal); input2.typ = (uint8_t)readVal; EE_ReadVariable(VirtAddVarTab[ 4+8*i] , &readVal); input1[i].min = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[6] , &readVal); input2.min = (int16_t)readVal; EE_ReadVariable(VirtAddVarTab[ 5+8*i] , &readVal); input1[i].mid = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[7] , &readVal); input2.mid = (int16_t)readVal; EE_ReadVariable(VirtAddVarTab[ 6+8*i] , &readVal); input1[i].max = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[8] , &readVal); input2.max = (int16_t)readVal; EE_ReadVariable(VirtAddVarTab[ 7+8*i] , &readVal); input2[i].typ = (uint8_t)readVal;
EE_ReadVariable(VirtAddVarTab[9] , &readVal); rtP_Left.i_max = rtP_Right.i_max = (int16_t)readVal; EE_ReadVariable(VirtAddVarTab[ 8+8*i] , &readVal); input2[i].min = (int16_t)readVal;
EE_ReadVariable(VirtAddVarTab[10], &readVal); rtP_Left.n_max = rtP_Right.n_max = (int16_t)readVal; EE_ReadVariable(VirtAddVarTab[ 9+8*i] , &readVal); input2[i].mid = (int16_t)readVal;
} else { // Else If Input type is 3 (auto), identify the input type based on the values from config.h EE_ReadVariable(VirtAddVarTab[10+8*i] , &readVal); input2[i].max = (int16_t)readVal;
input1.typ = INPUT1_TYPE; }
input1.min = INPUT1_MIN; } else {
input1.mid = INPUT1_MID; for (uint8_t i=0; i<INPUTS_NR; i++) {
input1.max = INPUT1_MAX; if (input1[i].typDef == 3) { // If Input type defined is 3 (auto), identify the input type based on the values from config.h
input2.typ = INPUT2_TYPE; input1[i].typ = checkInputType(input1[i].min, input1[i].mid, input1[i].max);
input2.min = INPUT2_MIN; } else {
input2.mid = INPUT2_MID; input1[i].typ = input1[i].typDef;
input2.max = INPUT2_MAX; }
if (INPUT1_TYPE == 3) { input1.typ = checkInputType(INPUT1_MIN, INPUT1_MID, INPUT1_MAX); } if (input2[i].typDef == 3) {
if (INPUT2_TYPE == 3) { input2.typ = checkInputType(INPUT2_MIN, INPUT2_MID, INPUT2_MAX); } input2[i].typ = checkInputType(input2[i].min, input2[i].mid, input2[i].max);
} else {
input2[i].typ = input2[i].typDef;
}
}
} }
HAL_FLASH_Lock(); HAL_FLASH_Lock();
#endif #endif
#ifdef VARIANT_TRANSPOTTER #ifdef VARIANT_TRANSPOTTER
enable = 1; enable = 1;
HAL_FLASH_Unlock(); HAL_FLASH_Unlock();
EE_Init(); /* EEPROM Init */ EE_Init(); /* EEPROM Init */
EE_ReadVariable(VirtAddVarTab[0], &saveValue); EE_ReadVariable(VirtAddVarTab[0], &saveValue);
HAL_FLASH_Lock(); HAL_FLASH_Lock();
@ -474,21 +494,28 @@ void adcCalibLim(void) {
readInputRaw(); readInputRaw();
// Inititalization: MIN = a high value, MAX = a low value // Inititalization: MIN = a high value, MAX = a low value
int32_t input1_fixdt = input1.raw << 16; int32_t input1_fixdt = input1[inIdx].raw << 16;
int32_t input2_fixdt = input2.raw << 16; int32_t input2_fixdt = input2[inIdx].raw << 16;
int16_t INPUT1_MIN_temp = MAX_int16_T; int16_t INPUT1_MIN_temp = MAX_int16_T;
int16_t INPUT1_MID_temp = 0; int16_t INPUT1_MID_temp = 0;
int16_t INPUT1_MAX_temp = MIN_int16_T; int16_t INPUT1_MAX_temp = MIN_int16_T;
int16_t INPUT2_MIN_temp = MAX_int16_T; int16_t INPUT2_MIN_temp = MAX_int16_T;
int16_t INPUT2_MID_temp = 0; int16_t INPUT2_MID_temp = 0;
int16_t INPUT2_MAX_temp = MIN_int16_T; int16_t INPUT2_MAX_temp = MIN_int16_T;
int16_t input_margin = 0;
uint16_t input_cal_timeout = 0; uint16_t input_cal_timeout = 0;
#ifdef CONTROL_ADC
if (inIdx == CONTROL_ADC) {
input_margin = ADC_MARGIN;
}
#endif
// Extract MIN, MAX and MID from ADC while the power button is not pressed // Extract MIN, MAX and MID from ADC while the power button is not pressed
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && input_cal_timeout++ < 4000) { // 20 sec timeout while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && input_cal_timeout++ < 4000) { // 20 sec timeout
readInputRaw(); readInputRaw();
filtLowPass32(input1.raw, FILTER, &input1_fixdt); filtLowPass32(input1[inIdx].raw, FILTER, &input1_fixdt);
filtLowPass32(input2.raw, FILTER, &input2_fixdt); filtLowPass32(input2[inIdx].raw, FILTER, &input2_fixdt);
INPUT1_MID_temp = (int16_t)(input1_fixdt >> 16);// CLAMP(input1_fixdt >> 16, INPUT1_MIN, INPUT1_MAX); // convert fixed-point to integer INPUT1_MID_temp = (int16_t)(input1_fixdt >> 16);// CLAMP(input1_fixdt >> 16, INPUT1_MIN, INPUT1_MAX); // convert fixed-point to integer
INPUT2_MID_temp = (int16_t)(input2_fixdt >> 16);// CLAMP(input2_fixdt >> 16, INPUT2_MIN, INPUT2_MAX); 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) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Input1 is "); printf("Input1 is ");
#endif #endif
input1.typ = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp); input1[inIdx].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) 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.min = INPUT1_MIN_temp + INPUT_MARGIN; input1[inIdx].min = INPUT1_MIN_temp + input_margin;
input1.mid = INPUT1_MID_temp; input1[inIdx].mid = INPUT1_MID_temp;
input1.max = INPUT1_MAX_temp - INPUT_MARGIN; input1[inIdx].max = INPUT1_MAX_temp - input_margin;
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..OK\r\n"); printf("..OK\r\n");
#endif #endif
} else { } else {
input1.typ = 0; // Disable input input1[inIdx].typ = 0; // Disable input
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..NOK\r\n"); printf("..NOK\r\n");
#endif #endif
@ -520,16 +547,16 @@ void adcCalibLim(void) {
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("Input2 is "); printf("Input2 is ");
#endif #endif
input2.typ = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp); input2[inIdx].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) 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.min = INPUT2_MIN_temp + INPUT_MARGIN; input2[inIdx].min = INPUT2_MIN_temp + input_margin;
input2.mid = INPUT2_MID_temp; input2[inIdx].mid = INPUT2_MID_temp;
input2.max = INPUT2_MAX_temp - INPUT_MARGIN; input2[inIdx].max = INPUT2_MAX_temp - input_margin;
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..OK\r\n"); printf("..OK\r\n");
#endif #endif
} else { } else {
input2.typ = 0; // Disable input input2[inIdx].typ = 0; // Disable input
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf("..NOK\r\n"); printf("..NOK\r\n");
#endif #endif
@ -537,8 +564,8 @@ void adcCalibLim(void) {
inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) #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", 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, input1[inIdx].typ, input1[inIdx].min, input1[inIdx].mid, input1[inIdx].max,
input2.typ, input2.min, input2.mid, input2.max); input2[inIdx].typ, input2[inIdx].min, input2[inIdx].mid, input2[inIdx].max);
#endif #endif
#endif #endif
@ -561,8 +588,8 @@ void updateCurSpdLim(void) {
printf("Torque and Speed limits update started...\r\n"); printf("Torque and Speed limits update started...\r\n");
#endif #endif
int32_t input1_fixdt = input1.raw << 16; int32_t input1_fixdt = input1[inIdx].raw << 16;
int32_t input2_fixdt = input2.raw << 16; int32_t input2_fixdt = input2[inIdx].raw << 16;
uint16_t cur_factor; // fixdt(0,16,16) uint16_t cur_factor; // fixdt(0,16,16)
uint16_t spd_factor; // fixdt(0,16,16) uint16_t spd_factor; // fixdt(0,16,16)
uint16_t cur_spd_timeout = 0; uint16_t cur_spd_timeout = 0;
@ -571,21 +598,21 @@ void updateCurSpdLim(void) {
// Wait for the power button press // Wait for the power button press
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout
readInputRaw(); readInputRaw();
filtLowPass32(input1.raw, FILTER, &input1_fixdt); filtLowPass32(input1[inIdx].raw, FILTER, &input1_fixdt);
filtLowPass32(input2.raw, FILTER, &input2_fixdt); filtLowPass32(input2[inIdx].raw, FILTER, &input2_fixdt);
HAL_Delay(5); HAL_Delay(5);
} }
// Calculate scaling factors // Calculate scaling factors
cur_factor = CLAMP((input1_fixdt - (input1.min << 16)) / (input1.max - input1.min), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A 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.min << 16)) / (input2.max - input2.min), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm 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 // 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) 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 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 // 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) 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 cur_spd_valid += 2; // Mark update to be saved in Flash at shutdown
@ -611,8 +638,8 @@ void updateCurSpdLim(void) {
void standstillHold(void) { void standstillHold(void) {
#if defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE) #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 (!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 if (((input1[inIdx].cmd > 50 || input2[inIdx].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 || (input2[inIdx].cmd < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small
rtP_Left.n_cruiseMotTgt = 0; rtP_Left.n_cruiseMotTgt = 0;
rtP_Right.n_cruiseMotTgt = 0; rtP_Right.n_cruiseMotTgt = 0;
rtP_Left.b_cruiseCtrlEna = 1; rtP_Left.b_cruiseCtrlEna = 1;
@ -621,7 +648,7 @@ void standstillHold(void) {
} }
} }
else { // If Stanstill is Active -> try Deactivation 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_Left.b_cruiseCtrlEna = 0;
rtP_Right.b_cruiseCtrlEna = 0; rtP_Right.b_cruiseCtrlEna = 0;
standstillAcv = 0; standstillAcv = 0;
@ -646,7 +673,7 @@ void electricBrake(uint16_t speedBlend, uint8_t reverseDir) {
if (speedAvg > 0) { if (speedAvg > 0) {
brakeVal = (int16_t)((-ELECTRIC_BRAKE_MAX * speedBlend) >> 15); brakeVal = (int16_t)((-ELECTRIC_BRAKE_MAX * speedBlend) >> 15);
} else { } else {
brakeVal = (int16_t)(( ELECTRIC_BRAKE_MAX * speedBlend) >> 15); brakeVal = (int16_t)(( ELECTRIC_BRAKE_MAX * speedBlend) >> 15);
} }
// Check if direction is reversed // 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 // Calculate the new input2.cmd with brake component included
if (input2.cmd >= 0 && input2.cmd < ELECTRIC_BRAKE_THRES) { if (input2[inIdx].cmd >= 0 && input2[inIdx].cmd < ELECTRIC_BRAKE_THRES) {
input2.cmd = MAX(brakeVal, ((ELECTRIC_BRAKE_THRES - input2.cmd) * brakeVal) / ELECTRIC_BRAKE_THRES); input2[inIdx].cmd = MAX(brakeVal, ((ELECTRIC_BRAKE_THRES - input2[inIdx].cmd) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (input2.cmd >= -ELECTRIC_BRAKE_THRES && input2.cmd < 0) { } else if (input2[inIdx].cmd >= -ELECTRIC_BRAKE_THRES && input2[inIdx].cmd < 0) {
input2.cmd = MIN(brakeVal, ((ELECTRIC_BRAKE_THRES + input2.cmd) * brakeVal) / ELECTRIC_BRAKE_THRES); input2[inIdx].cmd = MIN(brakeVal, ((ELECTRIC_BRAKE_THRES + input2[inIdx].cmd) * brakeVal) / ELECTRIC_BRAKE_THRES);
} else if (input2.cmd >= ELECTRIC_BRAKE_THRES) { } else if (input2[inIdx].cmd >= ELECTRIC_BRAKE_THRES) {
input2.cmd = MAX(brakeVal, ((input2.cmd - ELECTRIC_BRAKE_THRES) * INPUT_MAX) / (INPUT_MAX - 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) } 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 #endif
} }
@ -724,7 +751,7 @@ int checkInputType(int16_t min, int16_t mid, int16_t max){
} }
#ifdef CONTROL_ADC #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) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
printf(" AND protected"); printf(" AND protected");
#endif #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 * Function to read the Input Raw values from various input devices
*/ */
void readInputRaw(void) { void readInputRaw(void) {
#ifdef CONTROL_ADC #ifdef CONTROL_ADC
// ADC values range: 0-4095, see ADC-calibration in config.h if (inIdx == CONTROL_ADC) {
input1.raw = adc_buffer.l_tx2; input1[inIdx].raw = adc_buffer.l_tx2;
input2.raw = adc_buffer.l_rx2; input2[inIdx].raw = adc_buffer.l_rx2;
timeoutCnt = 0; }
#endif #endif
#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) #if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK)
if (inIdx == CONTROL_NUNCHUK) {
if (nunchuk_connected != 0) { if (nunchuk_connected != 0) {
Nunchuk_Read(); Nunchuk_Read();
input1.raw = (nunchuk_data[0] - 127) * 8; // X axis 0-255 input1[inIdx].raw = (nunchuk_data[0] - 127) * 8; // X axis 0-255
input2.raw = (nunchuk_data[1] - 128) * 8; // Y axis 0-255 input2[inIdx].raw = (nunchuk_data[1] - 128) * 8; // Y axis 0-255
#ifdef SUPPORT_BUTTONS #ifdef SUPPORT_BUTTONS
button1 = (uint8_t)nunchuk_data[5] & 1; button1 = (uint8_t)nunchuk_data[5] & 1;
button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1; button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1;
#endif #endif
} }
}
#endif #endif
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) #if defined(CONTROL_SERIAL_USART2)
// Handle received data validity, timeout and fix out-of-sync if necessary if (inIdx == CONTROL_SERIAL_USART2) {
#ifdef CONTROL_IBUS #ifdef CONTROL_IBUS
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) { 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; input1[inIdx].raw = (ibusL_captured_value[0] - 500) * 2;
input2.raw = (ibus_captured_value[1] - 500) * 2; input2[inIdx].raw = (ibusL_captured_value[1] - 500) * 2;
#else #else
input1.raw = command.steer; input1[inIdx].raw = commandL.steer;
input2.raw = command.speed; input2[inIdx].raw = commandL.speed;
#endif #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 #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 defined(SIDEBOARD_SERIAL_USART3)
if (!timeoutFlagSerial_R && Sideboard_R.sensors & SW1_SET) { // If no Timeout and SW1 is set, switch to Sideboard control if (inIdx == SIDEBOARD_SERIAL_USART3) {
input1.raw = Sideboard_R.cmd1; input1[inIdx].raw = Sideboard_R.cmd1;
input2.raw = Sideboard_R.cmd2; input2[inIdx].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;
}
#endif #endif
#if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT) #if defined(CONTROL_PPM_LEFT)
input1.raw = (ppm_captured_value[0] - 500) * 2; if (inIdx == CONTROL_PPM_LEFT) {
input2.raw = (ppm_captured_value[1] - 500) * 2; input1[inIdx].raw = (ppm_captured_value[0] - 500) * 2;
#ifdef SUPPORT_BUTTONS input2[inIdx].raw = (ppm_captured_value[1] - 500) * 2;
button1 = ppm_captured_value[5] > 500; }
button2 = 0; #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
#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 #endif
} }
/* /*
* Add Dead-band to a signal * Function to handle the ADC, UART and General timeout (Nunchuk, PPM, PWM)
* This function realizes a dead-band around 0 and scales the input between [out_min, out_max]
*/ */
void readInputCmd(InputStruct *in, int16_t out_min, int16_t out_max) { void handleTimeout(void) {
switch (in->typ){ #ifdef CONTROL_ADC
case 1: // Input is a normal pot if (inIdx == CONTROL_ADC) {
in->cmd = CLAMP(MAP(in->raw, in->min, in->max, 0, out_max), 0, out_max); // If input1 or Input2 is either below MIN - Threshold or above MAX + Threshold, ADC protection timeout
case 2: // Input is a mid resting pot if (IN_RANGE(input1[inIdx].raw, input1[inIdx].min - ADC_PROTECT_THRESH, input1[inIdx].max + ADC_PROTECT_THRESH) &&
if( in->raw > in->mid - in->deadband && in->raw < in->mid + in->deadband ) { IN_RANGE(input2[inIdx].raw, input2[inIdx].min - ADC_PROTECT_THRESH, input2[inIdx].max + ADC_PROTECT_THRESH)) {
in->cmd = 0; timeoutFlgADC = 0; // Reset the timeout flag
} else if(in->raw > in->mid) { timeoutCntADC = 0; // Reset the timeout counter
in->cmd = CLAMP(MAP(in->raw, in->mid + in->deadband, in->max, 0, out_max), 0, out_max);
} else { } 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) { void readCommand(void) {
readInputRaw(); 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) #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
readInputCmd(&input1, INPUT_MIN, INPUT_MAX); calcInputCmd(&input1[inIdx], INPUT_MIN, INPUT_MAX);
#if !defined(VARIANT_SKATEBOARD) #if !defined(VARIANT_SKATEBOARD)
readInputCmd(&input2, INPUT_MIN, INPUT_MAX); calcInputCmd(&input2[inIdx], INPUT_MIN, INPUT_MAX);
#else #else
readInputCmd(&input2, INPUT2_BRAKE, INPUT_MAX); calcInputCmd(&input2[inIdx], INPUT_BRK, INPUT_MAX);
#endif #endif
#endif #endif
#ifdef VARIANT_TRANSPOTTER handleTimeout();
#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
#ifdef VARIANT_HOVERCAR #ifdef VARIANT_HOVERCAR
brakePressed = (uint8_t)(input1.cmd > 50); if (inIdx == CONTROL_ADC) {
#endif brakePressed = (uint8_t)(input1[inIdx].cmd > 50);
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
} }
else {
brakePressed = (uint8_t)(input2[inIdx].cmd < -50);
}
#endif
#if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT) #if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)
button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN); button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN);
@ -940,17 +1081,17 @@ void usart2_rx_check(void)
#ifdef CONTROL_SERIAL_USART2 #ifdef CONTROL_SERIAL_USART2
uint8_t *ptr; uint8_t *ptr;
if (pos != old_pos) { // Check change in received data if (pos != old_pos) { // Check change in received data
ptr = (uint8_t *)&command_raw; // Initialize the pointer with command_raw address ptr = (uint8_t *)&commandL_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 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], command_len); // Copy data. This is possible only if command_raw is contiguous! (meaning all the structure members have the same size) 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(&command_raw, &command, 2); // Process data usart_process_command(&commandL_raw, &commandL, 2); // Process data
} else if ((rx_buffer_L_len - old_pos + pos) == command_len) { // "Overflow" buffer mode: check if data length equals expected length } 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 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 if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx_buffer_L_len - old_pos; // Move to correct position in command_raw ptr += rx_buffer_L_len - old_pos; // Move to correct position in command_raw
memcpy(ptr, &rx_buffer_L[0], pos); // Copy remaining data 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 #endif // CONTROL_SERIAL_USART2
@ -1010,17 +1151,17 @@ void usart3_rx_check(void)
#ifdef CONTROL_SERIAL_USART3 #ifdef CONTROL_SERIAL_USART3
uint8_t *ptr; uint8_t *ptr;
if (pos != old_pos) { // Check change in received data if (pos != old_pos) { // Check change in received data
ptr = (uint8_t *)&command_raw; // Initialize the pointer with command_raw address ptr = (uint8_t *)&commandR_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 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], command_len); // Copy data. This is possible only if command_raw is contiguous! (meaning all the structure members have the same size) 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(&command_raw, &command, 3); // Process data usart_process_command(&commandR_raw, &commandR, 3); // Process data
} else if ((rx_buffer_R_len - old_pos + pos) == command_len) { // "Overflow" buffer mode: check if data length equals expected length } 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 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 if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx_buffer_R_len - old_pos; // Move to correct position in command_raw ptr += rx_buffer_R_len - old_pos; // Move to correct position in command_raw
memcpy(ptr, &rx_buffer_R[0], pos); // Copy remaining data 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 #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) void usart_process_command(SerialCommand *command_in, SerialCommand *command_out, uint8_t usart_idx)
{ {
#ifdef CONTROL_IBUS #ifdef CONTROL_IBUS
uint16_t ibus_chksum;
if (command_in->start == IBUS_LENGTH && command_in->type == IBUS_COMMAND) { if (command_in->start == IBUS_LENGTH && command_in->type == IBUS_COMMAND) {
ibus_chksum = 0xFFFF - IBUS_LENGTH - IBUS_COMMAND; ibus_chksum = 0xFFFF - IBUS_LENGTH - IBUS_COMMAND;
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i++) { 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; *command_out = *command_in;
if (usart_idx == 2) { // Sideboard USART2 if (usart_idx == 2) { // Sideboard USART2
#ifdef CONTROL_SERIAL_USART2 #ifdef CONTROL_SERIAL_USART2
timeoutCntSerial_L = 0; // Reset timeout counter timeoutFlgSerial_L = 0; // Clear timeout flag
timeoutFlagSerial_L = 0; // Clear timeout flag timeoutCntSerial_L = 0; // Reset timeout counter
#endif #endif
} else if (usart_idx == 3) { // Sideboard USART3 } else if (usart_idx == 3) { // Sideboard USART3
#ifdef CONTROL_SERIAL_USART3 #ifdef CONTROL_SERIAL_USART3
timeoutCntSerial_R = 0; // Reset timeout counter timeoutFlgSerial_R = 0; // Clear timeout flag
timeoutFlagSerial_R = 0; // Clear timeout flag timeoutCntSerial_R = 0; // Reset timeout counter
#endif #endif
} }
} }
@ -1102,13 +1244,13 @@ void usart_process_command(SerialCommand *command_in, SerialCommand *command_out
*command_out = *command_in; *command_out = *command_in;
if (usart_idx == 2) { // Sideboard USART2 if (usart_idx == 2) { // Sideboard USART2
#ifdef CONTROL_SERIAL_USART2 #ifdef CONTROL_SERIAL_USART2
timeoutCntSerial_L = 0; // Reset timeout counter timeoutFlgSerial_L = 0; // Clear timeout flag
timeoutFlagSerial_L = 0; // Clear timeout flag timeoutCntSerial_L = 0; // Reset timeout counter
#endif #endif
} else if (usart_idx == 3) { // Sideboard USART3 } else if (usart_idx == 3) { // Sideboard USART3
#ifdef CONTROL_SERIAL_USART3 #ifdef CONTROL_SERIAL_USART3
timeoutCntSerial_R = 0; // Reset timeout counter timeoutFlgSerial_R = 0; // Clear timeout flag
timeoutFlagSerial_R = 0; // Clear timeout flag timeoutCntSerial_R = 0; // Reset timeout counter
#endif #endif
} }
} }
@ -1132,12 +1274,12 @@ void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sid
if (usart_idx == 2) { // Sideboard USART2 if (usart_idx == 2) { // Sideboard USART2
#ifdef SIDEBOARD_SERIAL_USART2 #ifdef SIDEBOARD_SERIAL_USART2
timeoutCntSerial_L = 0; // Reset timeout counter timeoutCntSerial_L = 0; // Reset timeout counter
timeoutFlagSerial_L = 0; // Clear timeout flag timeoutFlgSerial_L = 0; // Clear timeout flag
#endif #endif
} else if (usart_idx == 3) { // Sideboard USART3 } else if (usart_idx == 3) { // Sideboard USART3
#ifdef SIDEBOARD_SERIAL_USART3 #ifdef SIDEBOARD_SERIAL_USART3
timeoutCntSerial_R = 0; // Reset timeout counter timeoutCntSerial_R = 0; // Reset timeout counter
timeoutFlagSerial_R = 0; // Clear timeout flag timeoutFlgSerial_R = 0; // Clear timeout flag
#endif #endif
} }
} }
@ -1184,7 +1326,7 @@ void sideboardLeds(uint8_t *leds) {
// Battery Level Indicator: use LED1, LED2, LED3 // Battery Level Indicator: use LED1, LED2, LED3
if (main_loop_counter % BAT_BLINK_INTERVAL == 0) { // | RED (LED1) | YELLOW (LED3) | GREEN (LED2) | if (main_loop_counter % BAT_BLINK_INTERVAL == 0) { // | RED (LED1) | YELLOW (LED3) | GREEN (LED2) |
if (batVoltage < BAT_DEAD) { // | 0 | 0 | 0 | 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 | } else if (batVoltage < BAT_LVL1) { // | B | 0 | 0 |
*leds ^= LED1_SET; *leds ^= LED1_SET;
*leds &= ~LED3_SET & ~LED2_SET; *leds &= ~LED3_SET & ~LED2_SET;
@ -1213,7 +1355,7 @@ void sideboardLeds(uint8_t *leds) {
*leds |= LED1_SET; *leds |= LED1_SET;
*leds &= ~LED3_SET & ~LED2_SET; *leds &= ~LED3_SET & ~LED2_SET;
} }
if (timeoutFlagADC || timeoutFlagSerial) { if (timeoutFlgADC || timeoutFlgSerial) {
*leds |= LED3_SET; *leds |= LED3_SET;
*leds &= ~LED1_SET & ~LED2_SET; *leds &= ~LED1_SET & ~LED2_SET;
} }
@ -1227,18 +1369,32 @@ void sideboardLeds(uint8_t *leds) {
*/ */
void sideboardSensors(uint8_t sensors) { void sideboardSensors(uint8_t sensors) {
#if !defined(VARIANT_HOVERBOARD) && (defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3)) #if !defined(VARIANT_HOVERBOARD) && (defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3))
static uint8_t sensor1_prev, sensor2_prev; static uint8_t sensor1_prev, sensor2_prev;
uint8_t sensor1_rising_edge, sensor2_rising_edge; static uint8_t sensor1_index; // holds the press index number for sensor1, when used as a button
sensor1_rising_edge = (sensors & SENSOR1_SET) && !sensor1_prev; uint8_t sensor1_trig, sensor2_trig;
sensor2_rising_edge = (sensors & SENSOR2_SET) && !sensor2_prev; sensor1_trig = (sensors & SENSOR1_SET) && !sensor1_prev; // rising edge detection
sensor1_prev = sensors & SENSOR1_SET; sensor2_trig = (sensors & SENSOR2_SET) && !sensor2_prev; // rising edge detection
sensor2_prev = sensors & SENSOR2_SET; sensor1_prev = sensors & SENSOR1_SET;
sensor2_prev = sensors & SENSOR2_SET;
// Control MODE and Control Type Handling: use Sensor1 as push button // Override in case the Sideboard control is Active
static uint8_t sensor1_index; // holds the press index number for sensor1, when used as a button #if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART2)
if (sensor1_rising_edge) { if (inIdx == SIDEBOARD_SERIAL_USART2) {
sensor1_index++; sensor1_index = (Sideboard_L.sensors & SW3_SET) >> 11; // SW3 on RC transmitter is used to change Control Mode
if (sensor1_index > 4) { sensor1_index = 0; } 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) { switch (sensor1_index) {
case 0: // FOC VOLTAGE case 0: // FOC VOLTAGE
rtP_Left.z_ctrlTypSel = FOC_CTRL; rtP_Left.z_ctrlTypSel = FOC_CTRL;
@ -1258,21 +1414,37 @@ void sideboardSensors(uint8_t sensors) {
case 4: // COMMUTATION case 4: // COMMUTATION
rtP_Left.z_ctrlTypSel = COM_CTRL; rtP_Left.z_ctrlTypSel = COM_CTRL;
rtP_Right.z_ctrlTypSel = COM_CTRL; rtP_Right.z_ctrlTypSel = COM_CTRL;
break; break;
} }
beepShortMany(sensor1_index + 1, 1); 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 #ifdef CRUISE_CONTROL_SUPPORT
if (sensor2_rising_edge) { if (sensor2_trig) {
cruiseControl(sensor2_rising_edge); cruiseControl(sensor2_trig);
} }
#else #else
static uint8_t sensor2_index; // holds the press index number for sensor2, when used as a button static uint8_t sensor2_index = 1; // holds the press index number for sensor2, when used as a button
if (sensor2_rising_edge) {
sensor2_index++; // Override in case the Sideboard control is Active
if (sensor2_index > 1) { sensor2_index = 0; } #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) { switch (sensor2_index) {
case 0: // FW Disabled case 0: // FW Disabled
rtP_Left.b_fieldWeakEna = 0; rtP_Left.b_fieldWeakEna = 0;
@ -1285,7 +1457,8 @@ void sideboardSensors(uint8_t sensors) {
Input_Lim_Init(); Input_Lim_Init();
break; break;
} }
beepShortMany(sensor2_index + 1, 1); beepShortMany(sensor2_index + 1, 1);
if (++sensor2_index > 1) { sensor2_index = 0; }
} }
#endif // CRUISE_CONTROL_SUPPORT #endif // CRUISE_CONTROL_SUPPORT
#endif #endif
@ -1311,16 +1484,18 @@ void saveConfig() {
if (inp_cal_valid || cur_spd_valid) { if (inp_cal_valid || cur_spd_valid) {
HAL_FLASH_Unlock(); HAL_FLASH_Unlock();
EE_WriteVariable(VirtAddVarTab[0] , (uint16_t)FLASH_WRITE_KEY); EE_WriteVariable(VirtAddVarTab[0] , (uint16_t)FLASH_WRITE_KEY);
EE_WriteVariable(VirtAddVarTab[1] , (uint16_t)input1.typ); EE_WriteVariable(VirtAddVarTab[1] , (uint16_t)rtP_Left.i_max);
EE_WriteVariable(VirtAddVarTab[2] , (uint16_t)input1.min); EE_WriteVariable(VirtAddVarTab[2] , (uint16_t)rtP_Left.n_max);
EE_WriteVariable(VirtAddVarTab[3] , (uint16_t)input1.mid); for (uint8_t i=0; i<INPUTS_NR; i++) {
EE_WriteVariable(VirtAddVarTab[4] , (uint16_t)input1.max); EE_WriteVariable(VirtAddVarTab[ 3+8*i] , (uint16_t)input1[i].typ);
EE_WriteVariable(VirtAddVarTab[5] , (uint16_t)input2.typ); EE_WriteVariable(VirtAddVarTab[ 4+8*i] , (uint16_t)input1[i].min);
EE_WriteVariable(VirtAddVarTab[6] , (uint16_t)input2.min); EE_WriteVariable(VirtAddVarTab[ 5+8*i] , (uint16_t)input1[i].mid);
EE_WriteVariable(VirtAddVarTab[7] , (uint16_t)input2.mid); EE_WriteVariable(VirtAddVarTab[ 6+8*i] , (uint16_t)input1[i].max);
EE_WriteVariable(VirtAddVarTab[8] , (uint16_t)input2.max); EE_WriteVariable(VirtAddVarTab[ 7+8*i] , (uint16_t)input2[i].typ);
EE_WriteVariable(VirtAddVarTab[9] , (uint16_t)rtP_Left.i_max); EE_WriteVariable(VirtAddVarTab[ 8+8*i] , (uint16_t)input2[i].min);
EE_WriteVariable(VirtAddVarTab[10], (uint16_t)rtP_Left.n_max); EE_WriteVariable(VirtAddVarTab[ 9+8*i] , (uint16_t)input2[i].mid);
EE_WriteVariable(VirtAddVarTab[10+8*i] , (uint16_t)input2[i].max);
}
HAL_FLASH_Lock(); HAL_FLASH_Lock();
} }
#endif #endif
@ -1345,7 +1520,7 @@ void poweroff(void) {
void poweroffPressCheck(void) { void poweroffPressCheck(void) {
#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER)
if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0; enable = 0;
uint16_t cnt_press = 0; uint16_t cnt_press = 0;
@ -1365,7 +1540,7 @@ void poweroffPressCheck(void) {
adcCalibLim(); adcCalibLim();
beepShort(5); beepShort(5);
} }
} else { // Short press: power off } else if (cnt_press > 8) { // Short press: power off (80 ms debounce)
poweroff(); poweroff();
} }
} }