diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000..6806b48 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,21 @@ +--- +name: Bug report +about: Create a report to help us improve +title: '' +labels: bug +assignees: '' + +--- + +### Your System +- VARIANT: +- CONTROL TYPE: +- CONTROL MODE: +- What is your application? + - Hovercar, Skateboard, RC platform, etc. + +Describe the bug and how we can reproduce it. + +```c +// a code sample may improve communication +``` diff --git a/.github/ISSUE_TEMPLATE/idea---feature-request.md b/.github/ISSUE_TEMPLATE/idea---feature-request.md new file mode 100644 index 0000000..b1259ea --- /dev/null +++ b/.github/ISSUE_TEMPLATE/idea---feature-request.md @@ -0,0 +1,14 @@ +--- +name: Idea / Feature request +about: Suggest an idea for this project +title: '' +labels: enhancement +assignees: '' + +--- + +**What can we do to make the firmware better?** +Consider if code examples or images would help communicate your request. + +**Describe suggestions or alternatives you have considered** +A clear and concise description of any alternative solutions or features you've considered. diff --git a/.github/ISSUE_TEMPLATE/question-about-the-firmware.md b/.github/ISSUE_TEMPLATE/question-about-the-firmware.md new file mode 100644 index 0000000..2443656 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/question-about-the-firmware.md @@ -0,0 +1,26 @@ +--- +name: Question about the firmware +about: How to use the firmware to... +title: '' +labels: question +assignees: '' + +--- + +### Your System +- VARIANT: +- CONTROL TYPE: +- CONTROL MODE: +- What is your application? + - Hovercar, Skateboard, RC platform, etc. + +**Before asking a question** see if it is already answered in: +* [Wiki pages](https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC/wiki) + +**Indicate what you have tried:** +If applicable, indicate what you tried that _doesn't_ work: + +```c +input1[inIdx].raw = adc_buffer.l_rx2; +input2[inIdx].raw = adc_buffer.l_tx2 +``` diff --git a/.travis.yml b/.travis.yml index 29957e7..b0ebc5d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -48,8 +48,6 @@ jobs: - name: platformio script: platformio run language: python - python: - - "2.7" install: - pip install -U platformio - platformio update diff --git a/Inc/comms.h b/Inc/comms.h new file mode 100644 index 0000000..c15d5ad --- /dev/null +++ b/Inc/comms.h @@ -0,0 +1,113 @@ +/** + * This file is part of the hoverboard-firmware-hack project. + * + * Copyright (C) 2020-2021 Emanuel FERU + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . +*/ + +// Define to prevent recursive inclusion +#ifndef COMMS_H +#define COMMS_H + +#include "stm32f1xx_hal.h" + +#if defined(DEBUG_SERIAL_PROTOCOL) + +enum types {UINT8_T,UINT16_T,UINT32_T,INT8_T,INT16_T,INT32_T,INT,FLOAT}; +#define typename(x) _Generic((x), \ + uint8_t: UINT8_T, \ + uint16_t: UINT16_T, \ + uint32_t: UINT32_T, \ + int8_t: INT8_T, \ + int16_t: INT16_T, \ + int32_t: INT32_T, \ + int: INT, \ + float: FLOAT) + +#define PARAM_SIZE(param) sizeof(param) / sizeof(parameter_entry) +#define COMMAND_SIZE(command) sizeof(command) / sizeof(command_entry) + +#define SIZEP(x) ((char*)(&(x) + 1) - (char*)&(x)) +#define ADD_PARAM(var) typename(var),&var + + +int32_t extToInt(uint8_t index,int32_t value); +int8_t setParamValInt(uint8_t index, int32_t newValue); +int8_t setParamValExt(uint8_t index, int32_t newValue); +int32_t intToExt(uint8_t index,int32_t value); +int32_t getParamValInt(uint8_t index); +int32_t getParamValExt(uint8_t index); + +int8_t initParamVal(uint8_t index); +int8_t incrParamVal(uint8_t index); + +int8_t saveAllParamVal(); +int16_t getParamInitInt(uint8_t index); +int32_t getParamInitExt(uint8_t index); +int8_t printCommandHelp(uint8_t index); +int8_t printParamHelp(uint8_t index); +int8_t printAllParamHelp(); +int8_t printParamVal(); +int8_t printParamDef(uint8_t index); +int8_t printAllParamDef(); +void printError(uint8_t errornum ); +int8_t watchParamVal(uint8_t index); + +int8_t findCommand(uint8_t *userCommand, uint32_t len); +int8_t findParam(uint8_t *userCommand, uint32_t len); +void handle_input(uint8_t *userCommand, uint32_t len); +void process_debug(); + + +typedef struct debug_command_struct debug_command; +struct debug_command_struct { + uint8_t semaphore; + uint8_t error; + int8_t command_index; + int8_t param_index; + int32_t param_value; +}; + +typedef struct command_entry_struct command_entry; +struct command_entry_struct { + const uint8_t type; + const char *name; + int8_t (*callback_function0)(); + int8_t (*callback_function1)(uint8_t index); + int8_t (*callback_function2)(uint8_t index,int32_t value); + const char *help; +}; + +typedef struct parameter_entry_struct parameter_entry; +struct parameter_entry_struct { + const uint8_t type; + const char *name; + const uint8_t datatype; + void *valueL; + void *valueR; + const uint16_t addr; + const int32_t init; + const uint8_t initFormat; + const int32_t min; + const int32_t max; + const uint8_t div; + const uint8_t mul; + const uint8_t fix; + void (*callback_function)(); + const char *help; +}; + +#endif // DEBUG_SERIAL_PROTOCOL +#endif // COMMS_H diff --git a/Inc/config.h b/Inc/config.h index fbfe7d3..29b1dd4 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -59,7 +59,13 @@ #define ADC_TOTAL_CONV_TIME (ADC_CLOCK_DIV * ADC_CONV_CLOCK_CYCLES) // = ((SystemCoreClock / ADC_CLOCK_HZ) * ADC_CONV_CLOCK_CYCLES), where ADC_CLOCK_HZ = SystemCoreClock/ADC_CLOCK_DIV // ########################### END OF DO-NOT-TOUCH SETTINGS ############################ - +// ############################### BOARD VARIANT ############################### +/* Board Variant + * 0 - Default board type + * 1 - Alternate board type with different pin mapping for DCLINK, Buzzer and ON/OFF, Button and Charger +*/ +#define BOARD_VARIANT 0 // change if board with alternate pin mapping +// ######################## END OF BOARD VARIANT ############################### // ############################### BATTERY ############################### /* Battery voltage calibration: connect power source. @@ -121,7 +127,7 @@ * 3. If you re-calibrate the Field Weakening please take all the safety measures! The motors can spin very fast! Inputs: - - cmd1 and cmd2: analog normalized input values. INPUT_MIN to INPUT_MAX + - input1[inIdx].cmd and input2[inIdx].cmd: normalized input values. INPUT_MIN to INPUT_MAX - button1 and button2: digital input values. 0 or 1 - adc_buffer.l_tx2 and adc_buffer.l_rx2: unfiltered ADC values (you do not need them). 0 to 4095 Outputs: @@ -170,7 +176,10 @@ // Default settings will be applied at the end of this config file if not set before #define INACTIVITY_TIMEOUT 8 // Minutes of not driving until poweroff. it is not very precise. #define BEEPS_BACKWARD 1 // 0 or 1 -#define FLASH_WRITE_KEY 0x1233 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h +#define ADC_MARGIN 100 // ADC input margin applied on the raw ADC min and max to make sure the MIN and MAX values are reached even in the presence of noise +#define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken +#define ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values +#define AUTO_CALIBRATION_ENA // Enable/Disable input auto-calibration by holding power button pressed. Un-comment this if auto-calibration is not needed. /* 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) @@ -186,6 +195,24 @@ +// ############################## INPUT FORMAT ############################ +/* ***_INPUT: TYPE, MIN, MID, MAX, DEADBAND + * ----------------------------------------- + * TYPE: 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect + * MIN: min ADC1-value while poti at minimum-position (0 - 4095) + * MID: mid ADC1-value while poti at mid-position (INPUT_MIN - INPUT_MAX) + * MAX: max ADC2-value while poti at maximum-position (0 - 4095) + * DEADBAND: how much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) + * + * Dual-inputs + * PRI_INPUT: Primary Input. These limits will be used for the input with priority 0 + * AUX_INPUT: Auxiliary Input. These limits will be used for the input with priority 1 + * ----------------------------------------- +*/ + // ############################## END OF INPUT FORMAT ############################ + + + // ############################## CRUISE CONTROL SETTINGS ############################ /* Cruise Control info: * enable CRUISE_CONTROL_SUPPORT and (SUPPORT_BUTTONS_LEFT or SUPPORT_BUTTONS_RIGHT depending on which cable is the button installed) @@ -208,13 +235,13 @@ * enable DEBUG_SERIAL_USART3 or DEBUG_SERIAL_USART2 * * - * DEBUG_SERIAL_ASCII output is: + * DEBUG ASCII output is: * // "in1:345 in2:1337 cmdL:0 cmdR:0 BatADC:0 BatV:0 TempADC:0 Temp:0\r\n" * - * in1: (int16_t)input1); raw input1: ADC1, UART, PWM, PPM, iBUS - * in2: (int16_t)input2); raw input2: ADC2, UART, PWM, PPM, iBUS - * cmdL: (int16_t)speedL); output command: [-1000, 1000] - * cmdR: (int16_t)speedR); output command: [-1000, 1000] + * in1: (int16_t)input1[inIdx].raw); raw input1: ADC1, UART, PWM, PPM, iBUS + * in2: (int16_t)input2[inIdx].raw); raw input2: ADC2, UART, PWM, PPM, iBUS + * cmdL: (int16_t)cmdL); output command Left: [-1000, 1000] + * cmdR: (int16_t)cmdR); output command Right: [-1000, 1000] * BatADC: (int16_t)adc_buffer.batt1); Battery adc-value measured by mainboard * BatV: (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC)); Battery calibrated voltage multiplied by 100 for verifying battery voltage calibration * TempADC: (int16_t)board_temp_adcFilt); for board temperature calibration @@ -224,6 +251,7 @@ // #define DEBUG_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! // #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! +// #define DEBUG_SERIAL_PROTOCOL // uncomment this to send user commands to the board, change parameters and print specific signals (see comms.c for the user commands) // ########################### END OF DEBUG SERIAL ############################ @@ -251,26 +279,29 @@ * Procedure: * - connect gnd, rx and tx of a usb-uart converter in 3.3V mode to the right sensor board cable (do NOT use the red 15V wire!) * - readout values using a serial terminal in 115200 baud rate - * - turn the potis to minimum position, write value 1 to INPUT1_MIN and value 2 to INPUT2_MIN - * - turn the potis to maximum position, write value 1 to INPUT1_MAX and value 2 to INPUT2_MAX - * - for middle resting potis: Let the potis in the middle resting position, write value 1 to INPUT1_MID and value 2 to INPUT2_MID + * - turn the potis to minimum position, write value in1 to PRI_INPUT1 MIN and value in2 to PRI_INPUT2 MIN + * - turn the potis to maximum position, write value in1 to PRI_INPUT1 MAX and value in2 to PRI_INPUT2 MAX + * - for middle resting potis: Let the potis in the middle resting position, write value in1 to PRI_INPUT1 MID and value in2 to PRI_INPUT2 MID */ - #define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! - #define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken - #define ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values - #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT1_MIN 0 // min ADC1-value while poti at min-position (0 - 4095) - #define INPUT1_MID 0 // mid ADC1-value while poti at mid-position (INPUT1_MIN - INPUT1_MAX) - #define INPUT1_MAX 4095 // max ADC1-value while poti at max-position (0 - 4095) - #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - - #define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT2_MIN 0 // min ADC2-value while poti at min-position (0 - 4095) - #define INPUT2_MID 0 // mid ADC2-value while poti at mid-position (INPUT2_MIN - INPUT2_MAX) - #define INPUT2_MAX 4095 // max ADC2-value while poti at max-position (0 - 4095) - #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - - #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) 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! + + // #define DUAL_INPUTS // ADC*(Primary) + UART(Auxiliary). Uncomment this to use Dual-inputs + #define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1101 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + // #define SIDEBOARD_SERIAL_USART3 1 + #define CONTROL_SERIAL_USART3 1 // right sensor board cable. Number indicates priority for dual-input. Disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino + #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + #define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #else + #define FLASH_WRITE_KEY 0x1001 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + #endif + + // #define ADC_ALTERNATE_CONNECT // use to swap ADC inputs // #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 @@ -280,25 +311,28 @@ // ############################ VARIANT_USART SETTINGS ############################ #ifdef VARIANT_USART - // #define SIDEBOARD_SERIAL_USART2 - // #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino - // #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! - // #define SIDEBOARD_SERIAL_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 SIDEBOARD_SERIAL_USART2 0 + #define CONTROL_SERIAL_USART2 0 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino + #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + + // #define SIDEBOARD_SERIAL_USART3 0 + // #define CONTROL_SERIAL_USART3 0 // right sensor board cable. Number indicates priority for dual-input. Disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino + // #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + + // #define DUAL_INPUTS // UART*(Primary) + SIDEBOARD(Auxiliary). Uncomment this to use Dual-inputs + #define PRI_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1102 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + // #define SIDEBOARD_SERIAL_USART2 1 // left sideboard + #define SIDEBOARD_SERIAL_USART3 1 // right sideboard + #define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #else + #define FLASH_WRITE_KEY 0x1002 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #endif - #define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT2_MIN -1000 // (-1000 - 0) - #define INPUT2_MID 0 - #define INPUT2_MAX 1000 // (0 - 1000) - #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) // #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! // #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! #endif @@ -344,25 +378,28 @@ * use original nunchuk. most clones does not work very well. * Recommendation: Nunchuk Breakout Board https://github.com/Jan--Henrik/hoverboard-breakout */ - #define CONTROL_NUNCHUK // use nunchuk as input. disable FEEDBACK_SERIAL_USART3, DEBUG_SERIAL_USART3! - // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT1_MIN -1024 // (-1024 - 0) - #define INPUT1_MID 0 - #define INPUT1_MAX 1024 // (0 - 1024) - #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) + #define CONTROL_NUNCHUK 0 // use nunchuk as input. Number indicates priority for dual-input. Disable FEEDBACK_SERIAL_USART3, DEBUG_SERIAL_USART3! + + // #define DUAL_INPUTS // Nunchuk*(Primary) + UART(Auxiliary). Uncomment this to use Dual-inputs + #define PRI_INPUT1 2, -1024, 0, 1024, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 2, -1024, 0, 1024, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1103 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + // #define SIDEBOARD_SERIAL_USART2 1 + #define CONTROL_SERIAL_USART2 1 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino + #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + #define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #else + #define FLASH_WRITE_KEY 0x1003 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define DEBUG_SERIAL_USART2 // left sensor cable debug + #endif - #define INPUT2_TYPE 2 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT2_MIN -1024 // (-1024 - 0) - #define INPUT2_MID 0 - #define INPUT2_MAX 1024 // (0 - 1024) - #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) // # maybe good for ARMCHAIR # - #define FILTER 3276 // 0.05f - #define SPEED_COEFFICIENT 8192 // 0.5f - #define STEER_COEFFICIENT 62259 // -0.2f - #define DEBUG_SERIAL_USART2 // left sensor cable debug - // #define SUPPORT_BUTTONS // Define for Nunchuck buttons support + #define FILTER 3276 // 0.05f + #define SPEED_COEFFICIENT 8192 // 0.5f + #define STEER_COEFFICIENT 62259 // -0.2f + // #define SUPPORT_BUTTONS // Define for Nunchuk buttons support #endif // ############################# END OF VARIANT_NUNCHUK SETTINGS ######################### @@ -374,29 +411,33 @@ * Right sensor board cable. Channel 1: steering, Channel 2: speed. * https://gist.github.com/peterpoetzi/1b63a4a844162196613871767189bd05 */ - // #define CONTROL_PPM_LEFT // use PPM-Sum as input on the LEFT cable . disable CONTROL_SERIAL_USART2! - #define CONTROL_PPM_RIGHT // use PPM-Sum as input on the RIGHT cable. disable CONTROL_SERIAL_USART3! - #ifdef CONTROL_PPM_RIGHT - #define DEBUG_SERIAL_USART2 // left sensor cable debug + // #define DUAL_INPUTS // ADC*(Primary) + PPM(Auxiliary). Uncomment this to use Dual-inputs + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1104 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! + #define CONTROL_PPM_RIGHT 1 // use PPM-Sum as input on the RIGHT cable. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART3! + #define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #else - #define DEBUG_SERIAL_USART3 // right sensor cable debug + #define FLASH_WRITE_KEY 0x1004 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + // #define CONTROL_PPM_LEFT 0 // use PPM-Sum as input on the LEFT cable. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2! + #define CONTROL_PPM_RIGHT 0 // use PPM-Sum as input on the RIGHT cable. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART3! + #define PRI_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #endif - #define PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used. - // 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 PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used. - #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! + // #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 // ############################# END OF VARIANT_PPM SETTINGS ############################ @@ -407,33 +448,36 @@ * Right sensor board cable. Connect PA2 to channel 1 and PA3 to channel 2 on receiver. * Channel 1: steering, Channel 2: speed. */ - // #define CONTROL_PWM_LEFT // use RC PWM as input on the LEFT cable. disable DEBUG_SERIAL_USART2! - #define CONTROL_PWM_RIGHT // use RC PWM as input on the RIGHT cable. disable DEBUG_SERIAL_USART3! - #ifdef CONTROL_PWM_RIGHT - #define DEBUG_SERIAL_USART2 // left sensor cable debug + // #define DUAL_INPUTS // ADC*(Primary) + PWM(Auxiliary). Uncomment this to use Dual-inputs + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1105 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! + #define CONTROL_PWM_RIGHT 1 // use RC PWM as input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3! + #define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #else - #define DEBUG_SERIAL_USART3 // right sensor cable debug + #define FLASH_WRITE_KEY 0x1005 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + // #define CONTROL_PWM_LEFT 0 // use RC PWM as input on the LEFT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART2! + #define CONTROL_PWM_RIGHT 0 // use RC PWM as input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3! + #define PRI_INPUT1 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, -1000, 0, 1000, 100 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #endif - // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT1_MIN -1000 // (-1000 - 0) - #define INPUT1_MID 0 - #define INPUT1_MAX 1000 // (0 - 1000) - #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - - #define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT2_MIN -1000 // (-1000 - 0) - #define INPUT2_MID 0 - #define INPUT2_MAX 1000 // (0 - 1000) - #define INPUT2_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. - #define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 - #define STEER_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14. If you do not want any steering, set it to 0. + #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. + #define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 + #define STEER_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14. If you do not want any steering, set it to 0. // #define INVERT_R_DIRECTION // #define INVERT_L_DIRECTION - // #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! - // #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! + // #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! + // #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! + + #if defined(CONTROL_PWM_RIGHT) && !defined(DUAL_INPUTS) + #define DEBUG_SERIAL_USART2 // left sensor cable debug + #elif defined(CONTROL_PWM_LEFT) && !defined(DUAL_INPUTS) + #define DEBUG_SERIAL_USART3 // right sensor cable debug + #endif #endif // ############################# END OF VARIANT_PWM SETTINGS ############################ @@ -444,33 +488,35 @@ /* CONTROL VIA RC REMOTE WITH FLYSKY IBUS PROTOCOL * Connected to Right sensor board cable. Channel 1: steering, Channel 2: speed. */ - #define CONTROL_IBUS // use IBUS as input - #define IBUS_NUM_CHANNELS 14 // total number of IBUS channels to receive, even if they are not used. - #define IBUS_LENGTH 0x20 - #define IBUS_COMMAND 0x40 - - #undef USART3_BAUD - #define USART3_BAUD 115200 - #define CONTROL_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used! - #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used! - #ifdef CONTROL_SERIAL_USART3 - #define DEBUG_SERIAL_USART2 // left sensor cable debug + #define CONTROL_IBUS // use IBUS as input. Number indicates priority for dual-input. + #define IBUS_NUM_CHANNELS 14 // total number of IBUS channels to receive, even if they are not used. + #define IBUS_LENGTH 0x20 + #define IBUS_COMMAND 0x40 + #define USART3_BAUD 115200 + + // #define DUAL_INPUTS // ADC*(Primary) + iBUS(Auxiliary). Uncomment this to use Dual-inputs + #ifdef DUAL_INPUTS + #define FLASH_WRITE_KEY 0x1106 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! + #define CONTROL_SERIAL_USART3 1 // use RC iBUS input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3! + #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used! + #define PRI_INPUT1 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, 0, 0, 4095, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #else - #define DEBUG_SERIAL_USART3 // right sensor cable debug + #define FLASH_WRITE_KEY 0x1006 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define CONTROL_SERIAL_USART3 0 // use RC iBUS input on the RIGHT cable, disable if ADC or PPM is used! + #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if ADC or PPM is used! + #define PRI_INPUT1 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 3, -1000, 0, 1000, 0 // TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section #endif - // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT1_MIN -1000 // (-1000 - 0) - #define INPUT1_MID 0 - #define INPUT1_MAX 1000 // (0 - 1000) - #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - - #define INPUT2_TYPE 3 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT2_MIN -1000 // (-1000 - 0) - #define INPUT2_MID 0 - #define INPUT2_MAX 1000 // (0 - 1000) - #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) + #if defined(CONTROL_SERIAL_USART3) && !defined(DUAL_INPUTS) + #define DEBUG_SERIAL_USART2 // left sensor cable debug + #elif defined(DEBUG_SERIAL_USART2) && !defined(DUAL_INPUTS) + #define DEBUG_SERIAL_USART3 // right sensor cable debug + #endif #endif // ############################# END OF VARIANT_IBUS SETTINGS ############################ @@ -478,45 +524,39 @@ // ############################ VARIANT_HOVERCAR SETTINGS ############################ #ifdef VARIANT_HOVERCAR + #define FLASH_WRITE_KEY 0x1107 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h #undef CTRL_MOD_REQ - #define CTRL_MOD_REQ TRQ_MODE // HOVERCAR works best in TORQUE Mode - #define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! - #define ADC_PROTECT_TIMEOUT 100 // ADC Protection: number of wrong / missing input commands before safety state is taken - #define ADC_PROTECT_THRESH 200 // ADC Protection threshold below/above the MIN/MAX ADC values - - #define INPUT1_TYPE 1 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT1_MIN 1000 // min ADC1-value while poti at minimum-position (0 - 4095) - #define INPUT1_MID 0 - #define INPUT1_MAX 2500 // max ADC1-value while poti at maximum-position (0 - 4095) - #define INPUT1_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - - #define INPUT2_TYPE 1 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT2_MIN 500 // min ADC2-value while poti at minimum-position (0 - 4095) - #define INPUT2_MID 0 - #define INPUT2_MAX 2200 // max ADC2-value while poti at maximum-position (0 - 4095) - #define INPUT2_DEADBAND 0 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) - - #define SPEED_COEFFICIENT 16384 // 1.0f - #define STEER_COEFFICIENT 0 // 0.0f - // #define INVERT_R_DIRECTION // Invert rotation of right motor - // #define INVERT_L_DIRECTION // Invert rotation of left motor - #define SIDEBOARD_SERIAL_USART3 // Tx -> Rx of right sensor board: for LED battery indication. Comment-out if sideboard is not used! - #define FEEDBACK_SERIAL_USART3 // Rx <- Tx of right sensor board: to use photosensors as buttons. Comment-out if sideboard is not used! - // #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + #define CTRL_MOD_REQ TRQ_MODE // HOVERCAR works best in TORQUE Mode + #define CONTROL_ADC 0 // use ADC as input. Number indicates priority for dual-input. Disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! + #define SIDEBOARD_SERIAL_USART3 1 // Rx from right sensor board: to use photosensors as buttons. Number indicates priority for dual-input. Comment-out if sideboard is not used! + #define FEEDBACK_SERIAL_USART3 // Tx to right sensor board: for LED battery indication. Comment-out if sideboard is not used! + + #define DUAL_INPUTS // ADC*(Primary) + Sideboard_R(Auxiliary). Uncomment this to use Dual-inputs + #define PRI_INPUT1 1, 1000, 0, 2500, 0 // Pedal Brake TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 1, 500, 0, 2200, 0 // Pedal Accel TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT1 2, -1000, 0, 1000, 0 // Sideboard Steer TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define AUX_INPUT2 2, -1000, 0, 1000, 0 // Sideboard Speed TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + + #define SPEED_COEFFICIENT 16384 // 1.0f + #define STEER_COEFFICIENT 8192 // 0.5f Only active in Sideboard input + // #define ADC_ALTERNATE_CONNECT // use to swap ADC inputs + // #define INVERT_R_DIRECTION // Invert rotation of right motor + // #define INVERT_L_DIRECTION // Invert rotation of left motor + // #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! // Extra functionality - // #define CRUISE_CONTROL_SUPPORT // [-] Flag to enable Cruise Control support. Activation/Deactivation is done by sideboard button or Brake pedal press. - // #define STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode. - // #define ELECTRIC_BRAKE_ENABLE // [-] Flag to enable electric brake and replace the motor "freewheel" with a constant braking when the input torque request is 0. Only available and makes sense for TORQUE mode. - // #define ELECTRIC_BRAKE_MAX 100 // (0, 500) Maximum electric brake to be applied when input torque request is 0 (pedal fully released). - // #define ELECTRIC_BRAKE_THRES 120 // (0, 500) Threshold below at which the electric brake starts engaging. + // #define CRUISE_CONTROL_SUPPORT // [-] Flag to enable Cruise Control support. Activation/Deactivation is done by sideboard button or Brake pedal press. + // #define STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode. + // #define ELECTRIC_BRAKE_ENABLE // [-] Flag to enable electric brake and replace the motor "freewheel" with a constant braking when the input torque request is 0. Only available and makes sense for TORQUE mode. + // #define ELECTRIC_BRAKE_MAX 100 // (0, 500) Maximum electric brake to be applied when input torque request is 0 (pedal fully released). + // #define ELECTRIC_BRAKE_THRES 120 // (0, 500) Threshold below at which the electric brake starts engaging. #endif // Multiple tap detection: default DOUBLE Tap on Brake pedal (4 pulses) -#define MULTIPLE_TAP_NR 2 * 2 // [-] Define tap number: MULTIPLE_TAP_NR = number_of_taps * 2, number_of_taps = 1 (for single taping), 2 (for double tapping), 3 (for triple tapping), etc... -#define MULTIPLE_TAP_HI 600 // [-] Multiple tap detection High hysteresis threshold -#define MULTIPLE_TAP_LO 200 // [-] Multiple tap detection Low hysteresis threshold -#define MULTIPLE_TAP_TIMEOUT 2000 // [ms] Multiple tap detection Timeout period. The taps need to happen within this time window to be accepted. +#define MULTIPLE_TAP_NR 2 * 2 // [-] Define tap number: MULTIPLE_TAP_NR = number_of_taps * 2, number_of_taps = 1 (for single taping), 2 (for double tapping), 3 (for triple tapping), etc... +#define MULTIPLE_TAP_HI 600 // [-] Multiple tap detection High hysteresis threshold +#define MULTIPLE_TAP_LO 200 // [-] Multiple tap detection Low hysteresis threshold +#define MULTIPLE_TAP_TIMEOUT 2000 // [ms] Multiple tap detection Timeout period. The taps need to happen within this time window to be accepted. // ######################## END OF VARIANT_HOVERCAR SETTINGS ######################### @@ -525,10 +565,18 @@ // Communication: [DONE] // Balancing controller: [TODO] #ifdef VARIANT_HOVERBOARD - #define SIDEBOARD_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + #define FLASH_WRITE_KEY 0x1008 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h + #define SIDEBOARD_SERIAL_USART2 1 // left sensor board cable. Number indicates priority for dual-input. Disable if ADC or PPM is used! #define FEEDBACK_SERIAL_USART2 - #define SIDEBOARD_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + #define SIDEBOARD_SERIAL_USART3 0 // right sensor board cable. Number indicates priority for dual-input. Disable if I2C (nunchuk or lcd) is used! #define FEEDBACK_SERIAL_USART3 + + // If an iBUS RC receiver is connected to either Left Sideboard (AUX_INPUT) or Right Sideboard (PRI_INPUT) + // PRIMARY INPUT: TYPE, MIN, MID, MAX, DEADBAND /* TYPE: 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect */ + #define PRI_INPUT1 3, -1000, 0, 1000, 0 // Priority Sideboard can be used to send commands via an iBUS Receiver connected to the sideboard + #define PRI_INPUT2 3, -1000, 0, 1000, 0 // Priority Sideboard can be used to send commands via an iBUS Receiver connected to the sideboard + #define AUX_INPUT1 3, -1000, 0, 1000, 0 // not used + #define AUX_INPUT2 3, -1000, 0, 1000, 0 // not used #endif // ######################## END OF VARIANT_HOVERBOARD SETTINGS ######################### @@ -537,17 +585,20 @@ // ################################# VARIANT_TRANSPOTTER SETTINGS ############################ //TODO ADD VALIDATION #ifdef VARIANT_TRANSPOTTER + #define FLASH_WRITE_KEY 0x1009 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h #define CONTROL_GAMETRAK #define SUPPORT_LCD // #define SUPPORT_NUNCHUK #define GAMETRAK_CONNECTION_NORMAL // for normal wiring according to the wiki instructions - //#define GAMETRAK_CONNECTION_ALTERNATE // use this define instead if you messed up the gametrak ADC wiring (steering is speed, and length of the wire is steering) + // #define GAMETRAK_CONNECTION_ALTERNATE // use this define instead if you messed up the gametrak ADC wiring (steering is speed, and length of the wire is steering) #define ROT_P 1.2 // P coefficient for the direction controller. Positive / Negative values to invert gametrak steering direction. // during nunchuk control (only relevant when activated) #define SPEED_COEFFICIENT 14746 // 0.9f - higher value == stronger. 0.0 to ~2.0? #define STEER_COEFFICIENT 8192 // 0.5f - higher value == stronger. if you do not want any steering, set it to 0.0; 0.0 to 1.0 #define INVERT_R_DIRECTION // Invert right motor #define INVERT_L_DIRECTION // Invert left motor + #define PRI_INPUT1 2, -1000, 0, 1000, 0 // dummy input, TRANSPOTTER does not use input limitations + #define PRI_INPUT2 2, -1000, 0, 1000, 0 // dummy input, TRANSPOTTER does not use input limitations #endif // ############################# END OF VARIANT_TRANSPOTTER SETTINGS ######################## @@ -558,37 +609,30 @@ * right sensor board cable. Connect PB10 to channel 1 and PB11 to channel 2 on receiver. * Channel 1: steering, Channel 2: speed. */ + #define FLASH_WRITE_KEY 0x1010 // Flash memory writing key. Change this key to ignore the input calibrations from the flash memory and use the ones in config.h #undef CTRL_MOD_REQ - #define CTRL_MOD_REQ TRQ_MODE // SKATEBOARD works best in TORQUE Mode - //#define CONTROL_PWM_LEFT // use RC PWM as input on the LEFT cable. disable DEBUG_SERIAL_USART2! - #define CONTROL_PWM_RIGHT // use RC PWM as input on the RIGHT cable. disable DEBUG_SERIAL_USART3! - #ifdef CONTROL_PWM_RIGHT - #define DEBUG_SERIAL_USART2 // left sensor cable debug - #else - #define DEBUG_SERIAL_USART3 // right sensor cable debug - #endif - // Min / Max values of each channel (use DEBUG to determine these values) - #define INPUT1_TYPE 0 // 0:Disabled, 1:Normal Pot, 2:Middle Resting Pot, 3:Auto-detect - #define INPUT1_MIN -1000 // (-1000 - 0) - #define INPUT1_MID 0 - #define INPUT1_MAX 1000 // (0 - 1000) - #define INPUT1_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) + #define CTRL_MOD_REQ TRQ_MODE // SKATEBOARD works best in TORQUE Mode + // #define CONTROL_PWM_LEFT 0 // use RC PWM as input on the LEFT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART2! + #define CONTROL_PWM_RIGHT 0 // use RC PWM as input on the RIGHT cable. Number indicates priority for dual-input. Disable DEBUG_SERIAL_USART3! - #define 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 PRI_INPUT1 0, -1000, 0, 1000, 0 // Disabled. TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define PRI_INPUT2 2, -800, 0, 700, 100 // Active. TYPE, MIN, MID, MAX, DEADBAND. See INPUT FORMAT section + #define INPUT_BRK -400 // (-1000 - 0) Change this value to adjust the braking amount - #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. - #define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 - #define STEER_COEFFICIENT 0 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14. If you do not want any steering, set it to 0. + #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. + #define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 + #define STEER_COEFFICIENT 0 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14. If you do not want any steering, set it to 0. #define INVERT_R_DIRECTION #define INVERT_L_DIRECTION - // #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! - // #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! - // #define STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode. + // #define SUPPORT_BUTTONS_LEFT // use left sensor board cable for button inputs. Disable DEBUG_SERIAL_USART2! + // #define SUPPORT_BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! + // #define STANDSTILL_HOLD_ENABLE // [-] Flag to hold the position when standtill is reached. Only available and makes sense for VOLTAGE or TORQUE mode. + + #ifdef CONTROL_PWM_RIGHT + #define DEBUG_SERIAL_USART2 // left sensor cable debug + #else + #define DEBUG_SERIAL_USART3 // right sensor cable debug + #endif #endif // ############################# END OF VARIANT_SKATEBOARD SETTINGS ############################ @@ -630,10 +674,10 @@ #ifndef STEER_COEFFICIENT #define STEER_COEFFICIENT DEFAULT_STEER_COEFFICIENT #endif -#ifdef CONTROL_ADC - #define INPUT_MARGIN 100 // Input margin applied on the raw ADC min and max to make sure the motor MIN and MAX values are reached even in the presence of noise +#if defined(PRI_INPUT1) && defined(PRI_INPUT2) && defined(AUX_INPUT1) && defined(AUX_INPUT2) + #define INPUTS_NR 2 #else - #define INPUT_MARGIN 0 + #define INPUTS_NR 1 #endif // ########################### END OF APPLY DEFAULT SETTING ############################ @@ -647,11 +691,6 @@ // General checks -#if (defined(CONTROL_ADC) || defined(CONTROL_SERIAL_USART2) || defined(CONTROL_PPM_LEFT) || defined(CONTROL_PWM_LEFT)) && \ - (defined(CONTROL_NUNCHUK) || defined(CONTROL_SERIAL_USART3) || defined(CONTROL_PPM_RIGHT) || defined(CONTROL_PWM_RIGHT)) - #warning !! Multiple control input sources defined !! If NOT handled correctly, it can lead to undesired behavior! -#endif - #if defined(CONTROL_SERIAL_USART2) && defined(SIDEBOARD_SERIAL_USART2) #error CONTROL_SERIAL_USART2 and SIDEBOARD_SERIAL_USART2 not allowed, choose one. #endif diff --git a/Inc/defines.h b/Inc/defines.h index 6f0892d..a9b28e3 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -102,28 +102,56 @@ // #define DCLINK_ADC ADC3 // #define DCLINK_CHANNEL + +#if BOARD_VARIANT == 0 #define DCLINK_PIN GPIO_PIN_2 #define DCLINK_PORT GPIOC +#elif BOARD_VARIANT == 1 +#define DCLINK_PIN GPIO_PIN_1 +#define DCLINK_PORT GPIOA +#endif + // #define DCLINK_PULLUP 30000 // #define DCLINK_PULLDOWN 1000 #define LED_PIN GPIO_PIN_2 #define LED_PORT GPIOB +#if BOARD_VARIANT == 0 #define BUZZER_PIN GPIO_PIN_4 #define BUZZER_PORT GPIOA +#elif BOARD_VARIANT == 1 +#define BUZZER_PIN GPIO_PIN_13 +#define BUZZER_PORT GPIOC +#endif -#define SWITCH_PIN GPIO_PIN_1 -#define SWITCH_PORT GPIOA +// UNUSED/REDUNDANT +//#define SWITCH_PIN GPIO_PIN_1 +//#define SWITCH_PORT GPIOA +#if BOARD_VARIANT == 0 #define OFF_PIN GPIO_PIN_5 #define OFF_PORT GPIOA +#elif BOARD_VARIANT == 1 +#define OFF_PIN GPIO_PIN_15 +#define OFF_PORT GPIOC +#endif +#if BOARD_VARIANT == 0 #define BUTTON_PIN GPIO_PIN_1 #define BUTTON_PORT GPIOA +#elif BOARD_VARIANT == 1 +#define BUTTON_PIN GPIO_PIN_9 +#define BUTTON_PORT GPIOB +#endif +#if BOARD_VARIANT == 0 #define CHARGER_PIN GPIO_PIN_12 #define CHARGER_PORT GPIOA +#elif BOARD_VARIANT == 1 +#define CHARGER_PIN GPIO_PIN_11 +#define CHARGER_PORT GPIOA +#endif #if defined(CONTROL_PPM_LEFT) #define PPM_PIN GPIO_PIN_3 @@ -222,13 +250,11 @@ void PWM_ISR_CH2_Callback(void); #define SENSOR2_SET (0x02) #define SENSOR_MPU (0x04) -// RC iBUS switch definitions. Flysky FS-i6S has SW1, SW4 - 2 positions; SW2, SW3 - 3 positions -#define SW1_SET (0x0100) // 0000 0001 0000 0000 -#define SW2_SET1 (0x0200) // 0000 0010 0000 0000 -#define SW2_SET2 (0x0400) // 0000 0100 0000 0000 -#define SW3_SET1 (0x0800) // 0000 1000 0000 0000 -#define SW3_SET2 (0x1000) // 0001 0000 0000 0000 -#define SW4_SET (0x2000) // 0010 0000 0000 0000 +// RC iBUS switch definitions. Flysky FS-i6S has [SWA, SWB, SWC, SWD] = [2, 3, 3, 2] positions switch +#define SWA_SET (0x0100) // 0000 0001 0000 0000 +#define SWB_SET (0x0600) // 0000 0110 0000 0000 +#define SWC_SET (0x1800) // 0001 1000 0000 0000 +#define SWD_SET (0x2000) // 0010 0000 0000 0000 #endif // DEFINES_H diff --git a/Inc/eeprom.h b/Inc/eeprom.h index cc7b1b9..e78b37b 100644 --- a/Inc/eeprom.h +++ b/Inc/eeprom.h @@ -209,7 +209,7 @@ #define PAGE_FULL ((uint8_t)0x80) /* Variables' number */ -#define NB_OF_VAR ((uint8_t)0x0B) +#define NB_OF_VAR ((uint8_t)0x13) /* 19 Variables */ /* Exported types ------------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ diff --git a/Inc/util.h b/Inc/util.h index 3f737c1..714ef86 100644 --- a/Inc/util.h +++ b/Inc/util.h @@ -55,6 +55,18 @@ } SerialSideboard; #endif +// Input Structure +typedef struct { + int16_t raw; // raw input + int16_t cmd; // command + uint8_t typ; // type + uint8_t typDef; // type Defined + int16_t min; // minimum + int16_t mid; // middle + int16_t max; // maximum + int16_t dband; // deadband +} InputStruct; + // Initialization Functions void BLDC_Init(void); void Input_Lim_Init(void); @@ -68,21 +80,17 @@ void beepLong(uint8_t freq); void beepShort(uint8_t freq); void beepShortMany(uint8_t cnt, int8_t dir); void calcAvgSpeed(void); -int addDeadBand(int16_t u, int16_t type, int16_t deadBand, int16_t in_min, int16_t in_mid, int16_t in_max, int16_t out_min, int16_t out_max); -int checkInputType(int16_t min, int16_t mid, int16_t max); void adcCalibLim(void); void updateCurSpdLim(void); -void saveConfig(void); void standstillHold(void); void electricBrake(uint16_t speedBlend, uint8_t reverseDir); void cruiseControl(uint8_t button); +int checkInputType(int16_t min, int16_t mid, int16_t max); -// Poweroff Functions -void poweroff(void); -void poweroffPressCheck(void); - -// Read Functions -void readInput(void); +// Input Functions +void calcInputCmd(InputStruct *in, int16_t out_min, int16_t out_max); +void readInputRaw(void); +void handleTimeout(void); void readCommand(void); void usart2_rx_check(void); void usart3_rx_check(void); @@ -100,6 +108,11 @@ void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sid void sideboardLeds(uint8_t *leds); void sideboardSensors(uint8_t sensors); +// Poweroff Functions +void saveConfig(void); +void poweroff(void); +void poweroffPressCheck(void); + // Filtering Functions void filtLowPass32(int32_t u, uint16_t coef, int32_t *y); void rateLimiter16(int16_t u, int16_t rate, int16_t *y); diff --git a/MDK-ARM/mainboard-hack.uvoptx b/MDK-ARM/mainboard-hack.uvoptx index 7f8ed9c..93099a6 100644 --- a/MDK-ARM/mainboard-hack.uvoptx +++ b/MDK-ARM/mainboard-hack.uvoptx @@ -75,7 +75,7 @@ 1 0 - 0 + 1 18 @@ -1452,7 +1452,7 @@ 1 0 - 1 + 0 18 @@ -1721,6 +1721,18 @@ 0 0 + + 2 + 14 + 1 + 0 + 0 + 0 + ..\Src\comms.c + comms.c + 0 + 0 + @@ -1731,7 +1743,7 @@ 0 3 - 14 + 15 1 0 0 @@ -1743,7 +1755,7 @@ 3 - 15 + 16 1 0 0 @@ -1755,7 +1767,7 @@ 3 - 16 + 17 1 0 0 @@ -1767,7 +1779,7 @@ 3 - 17 + 18 1 0 0 @@ -1779,7 +1791,7 @@ 3 - 18 + 19 1 0 0 @@ -1791,7 +1803,7 @@ 3 - 19 + 20 1 0 0 @@ -1803,7 +1815,7 @@ 3 - 20 + 21 1 0 0 @@ -1815,7 +1827,7 @@ 3 - 21 + 22 1 0 0 @@ -1827,7 +1839,7 @@ 3 - 22 + 23 1 0 0 @@ -1839,7 +1851,7 @@ 3 - 23 + 24 1 0 0 @@ -1851,7 +1863,7 @@ 3 - 24 + 25 1 0 0 @@ -1863,7 +1875,7 @@ 3 - 25 + 26 1 0 0 @@ -1875,7 +1887,7 @@ 3 - 26 + 27 1 0 0 @@ -1887,7 +1899,7 @@ 3 - 27 + 28 1 0 0 @@ -1899,7 +1911,7 @@ 3 - 28 + 29 1 0 0 @@ -1911,7 +1923,7 @@ 3 - 29 + 30 1 0 0 @@ -1931,7 +1943,7 @@ 0 4 - 30 + 31 1 0 0 diff --git a/MDK-ARM/mainboard-hack.uvprojx b/MDK-ARM/mainboard-hack.uvprojx index 58bf2f9..326b741 100644 --- a/MDK-ARM/mainboard-hack.uvprojx +++ b/MDK-ARM/mainboard-hack.uvprojx @@ -449,6 +449,11 @@ 5 ..\Inc\config.h + + comms.c + 1 + ..\Src\comms.c + @@ -994,6 +999,11 @@ 5 ..\Inc\config.h + + comms.c + 1 + ..\Src\comms.c + @@ -1607,6 +1617,11 @@ 5 ..\Inc\config.h + + comms.c + 1 + ..\Src\comms.c + @@ -2220,6 +2235,11 @@ 5 ..\Inc\config.h + + comms.c + 1 + ..\Src\comms.c + @@ -2833,6 +2853,11 @@ 5 ..\Inc\config.h + + comms.c + 1 + ..\Src\comms.c + @@ -3446,6 +3471,11 @@ 5 ..\Inc\config.h + + comms.c + 1 + ..\Src\comms.c + @@ -4059,6 +4089,11 @@ 5 ..\Inc\config.h + + comms.c + 1 + ..\Src\comms.c + @@ -4672,6 +4707,11 @@ 5 ..\Inc\config.h + + comms.c + 1 + ..\Src\comms.c + @@ -5217,6 +5257,11 @@ 5 ..\Inc\config.h + + comms.c + 1 + ..\Src\comms.c + @@ -5762,6 +5807,11 @@ 5 ..\Inc\config.h + + comms.c + 1 + ..\Src\comms.c + diff --git a/Makefile b/Makefile index 3d4cad7..e8e0f3d 100644 --- a/Makefile +++ b/Makefile @@ -38,6 +38,7 @@ Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_dma.c \ Src/system_stm32f1xx.c \ Src/setup.c \ Src/control.c \ +Src/comms.c \ Src/util.c \ Src/main.c \ Src/bldc.c \ diff --git a/README.md b/README.md index 6b81f3c..ca7618b 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,8 @@ Table of Contents * [Hardware](#hardware) * [FOC Firmware](#foc-firmware) -* [Example Variants ](#example-variants) +* [Example Variants](#example-variants) +* [Dual Inputs](#dual-inputs) * [Flashing](#flashing) * [Troubleshooting](#troubleshooting) * [Diagnostics](#diagnostics) @@ -59,22 +60,12 @@ Typically, the mainboard brain is an [STM32F103RCT6](/docs/literature/[10]_STM32 In this firmware 3 control types are available: - Commutation - SIN (Sinusoidal) -<<<<<<< HEAD -- FOC (Field Oriented Control) - -![Schematic representation of the available control methods](https://mygit.jblb.net/jerome/hoverboard-firmware-hack-FOC/raw/branch/master/01_Matlab/02_Figures/control_methods.png) - -Demo videos: - -[â–ºVideo: Commutation vs Advanced control (constant speed)](https://drive.google.com/open?id=1vC_kEkp2LE2lAaMCJcmK4z2m3jrPUoBD) -======= - FOC (Field Oriented Control) with the following 3 control modes: - **VOLTAGE MODE**: in this mode the controller applies a constant Voltage to the motors. Recommended for robotics applications or applications where a fast motor response is required. - **SPEED MODE**: in this mode a closed-loop controller realizes the input speed target by rejecting any of the disturbance (resistive load) applied to the motor. Recommended for robotics applications or constant speed applications. - **TORQUE MODE**: in this mode the input torque target is realized. This mode enables motor "freewheeling" when the torque target is `0`. Recommended for most applications with a sitting human driver. #### Comparison between different control methods ->>>>>>> 3fcae4fc8bb60bc6d26a7466a681183c1965cad1 |Control method| Complexity | Efficiency | Smoothness | Field Weakening | Freewheeling | Standstill hold | |--|--|--|--|--|--|--| @@ -84,11 +75,9 @@ Demo videos: |FOC SPEED| +++ | +++ | + | ++ | n.a. | +++ | |FOC TORQUE| +++ | +++ | +++ | ++ | +++(1) | n.a(2) | -(1) By enabling `ELECTRIC_BRAKE_ENABLE` in `config.h`, the freewheeling amount can be adjusted using the `ELECTRIC_BRAKE_MAX` parameter. - +(1) By enabling `ELECTRIC_BRAKE_ENABLE` in `config.h`, the freewheeling amount can be adjusted using the `ELECTRIC_BRAKE_MAX` parameter.
(2) The standstill hold functionality can be forced by enabling `STANDSTILL_HOLD_ENABLE` in `config.h`. - In all FOC control modes, the controller features maximum motor speed and maximum motor current protection. This brings great advantages to fulfil the needs of many robotic applications while maintaining safe operation. @@ -120,7 +109,7 @@ This firmware offers currently these variants (selectable in [platformio.ini](/p - **VARIANT_PPM**: RC remote control with PPM Sum signal. - **VARIANT_PWM**: RC remote control with PWM signal. - **VARIANT_IBUS**: RC remote control with Flysky iBUS protocol connected to the Left sensor cable. -- **VARIANT_HOVERCAR**: The motors are controlled by two pedals brake and throttle. Reverse is engaged by double tapping on the brake pedal at standstill. See [HOVERCAR video](https://www.youtube.com/watch?v=IgHCcj0NgWQ&t=). +- **VARIANT_HOVERCAR**: The motors are controlled by two pedals brake and throttle. Reverse is engaged by double tapping on the brake pedal at standstill. See [HOVERCAR wiki](https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC/wiki/Variant-HOVERCAR). - **VARIANT_HOVERBOARD**: The mainboard reads the two sideboards data. The sideboards need to be flashed with the hacked version. The balancing controller is **not** yet implemented. - **VARIANT_TRANSPOTTER**: This is for transpotter build, which is a hoverboard based transportation system. For more details on how to build it check [here](https://github.com/NiklasFauth/hoverboard-firmware-hack/wiki/Build-Instruction:-TranspOtter) and [here](https://hackaday.io/project/161891-transpotter-ng). - **VARIANT_SKATEBOARD**: This is for skateboard build, controlled using an RC remote with PWM signal connected to the right sensor cable. @@ -128,6 +117,29 @@ This firmware offers currently these variants (selectable in [platformio.ini](/p Of course the firmware can be further customized for other needs or projects. +--- +## Dual Inputs + +The firmware supports the input to be provided from two different sources connected to the Left and Right cable, respectively. To enable dual-inputs functionality uncomment `#define DUAL_INPUTS` in config.h for the respective variant. Various dual-inputs combinations can be realized as illustrated in the following table: +| Left | Right | Availability | +| --- | --- | --- | +| ADC(0) | UART(1) | VARIANT_ADC | +| ADC(0) | {PPM,PWM,iBUS}(1) | VARIANT_{PPM,PWM,IBUS} | +| ADC(0) | Sideboard(1*) | VARIANT_HOVERCAR | +| UART(0) | Sideboard(1*) | VARIANT_UART | +| UART(1) | Nunchuk(0) | VARIANT_NUNCHUK | + +(0) Primary input: this input is used when the Auxilliary input is not available or not connected.
+(1) Auxilliary input: this inputs is used when connected or enabled by a switch(*). If the Auxilliary input is disconnected, the firmware will automatically switch to the Primary input. Timeout is reported **only** on the Primary input. + +With slight modifications in config.h, other dual-inputs combinations can be realized as: +| Left | Right | Possibility | +| --- | --- | --- | +| Sideboard(1*) | UART(0) | VARIANT_UART | +| UART(0) | {PPM,PWM,iBUS}(1) | VARIANT_{PPM,PWM,IBUS} | +| {PPM,PWM,iBUS}(1) | Nunchuk(0) | VARIANT_{PPM,PWM,IBUS} | + + --- ## Flashing @@ -237,7 +249,7 @@ The errors reported by the board are in the form of audible beeps: - **1 beep (low pitch)**: Motor error (see [possible causes](https://github.com/EmanuelFeru/bldc-motor-control-FOC#diagnostics)) - **2 beeps (low pitch)**: ADC timeout - **3 beeps (low pitch)**: Serial communication timeout -- **4 beeps (low pitch)**: General timeout (PPM, PWM, Nunchuck) +- **4 beeps (low pitch)**: General timeout (PPM, PWM, Nunchuk) - **5 beeps (low pitch)**: Mainboard temperature warning - **1 beep slow (medium pitch)**: Low battery voltage < 36V - **1 beep fast (medium pitch)**: Low battery voltage < 35V @@ -249,7 +261,8 @@ For a more detailed troubleshooting connect an [FTDI Serial adapter](https://s.c ## Projects and Links - **Original firmware:** [https://github.com/NiklasFauth/hoverboard-firmware-hack](https://github.com/NiklasFauth/hoverboard-firmware-hack) -- **[Candas](https://github.com/Candas1/) Hoverboard Web Serial Control:** [https://candas1.github.io/Hoverboard-Web-Serial-Control/](https://candas1.github.io/Hoverboard-Web-Serial-Control/) + +- **[Candas](https://github.com/Candas1/) Hoverboard Web Serial Control:** [https://github.com/Candas1/Hoverboard-Web-Serial-Control](https://github.com/Candas1/Hoverboard-Web-Serial-Control) - **[RoboDurden's](https://github.com/RoboDurden) online compiler:** [https://pionierland.de/hoverhack/](https://pionierland.de/hoverhack/) - **Hoverboard hack for AT32F403RCT6 mainboards:** [https://github.com/cloidnerux/hoverboard-firmware-hack](https://github.com/cloidnerux/hoverboard-firmware-hack) - **Hoverboard hack for split mainboards:** [https://github.com/flo199213/Hoverboard-Firmware-Hack-Gen2](https://github.com/flo199213/Hoverboard-Firmware-Hack-Gen2) @@ -258,7 +271,7 @@ For a more detailed troubleshooting connect an [FTDI Serial adapter](https://s.c -- **Bobbycar** [https://github.com/larsmm/hoverboard-firmware-hack-bbcar](https://github.com/larsmm/hoverboard-firmware-hack-bbcar) +- **Bobbycar** [https://github.com/larsmm/hoverboard-firmware-hack-FOC-bbcar](https://github.com/larsmm/hoverboard-firmware-hack-FOC-bbcar) - **Wheel chair:** [https://github.com/Lahorde/steer_speed_ctrl](https://github.com/Lahorde/steer_speed_ctrl) - **TranspOtterNG:** [https://github.com/Jan--Henrik/transpOtterNG](https://github.com/Jan--Henrik/transpOtterNG) - **ST Community:** [Custom FOC motor control](https://community.st.com/s/question/0D50X0000B28qTDSQY/custom-foc-control-current-measurement-dma-timer-interrupt-needs-review) @@ -267,6 +280,10 @@ For a more detailed troubleshooting connect an [FTDI Serial adapter](https://s.c - **Telegram Community:** If you are an enthusiast join our [Hooover Telegram Group](https://t.me/joinchat/BHWO_RKu2LT5ZxEkvUB8uw) +--- +## Stargazers + +[![Stargazers over time](https://starchart.cc/EmanuelFeru/hoverboard-firmware-hack-FOC.svg)](https://starchart.cc/EmanuelFeru/hoverboard-firmware-hack-FOC) --- ## Contributions @@ -277,6 +294,4 @@ If you want to donate to keep this firmware updated, please use the link below: [![paypal](https://www.paypalobjects.com/en_US/NL/i/btn/btn_donateCC_LG.gif)](https://www.paypal.com/cgi-bin/webscr?cmd=_donations&business=CU2SWN2XV9SCY¤cy_code=EUR&source=url) - --- - diff --git a/Src/bldc.c b/Src/bldc.c index cb22ade..d5fb4c9 100644 --- a/Src/bldc.c +++ b/Src/bldc.c @@ -39,13 +39,14 @@ extern RT_MODEL *const rtM_Right; extern DW rtDW_Left; /* Observable states */ extern ExtU rtU_Left; /* External inputs */ extern ExtY rtY_Left; /* External outputs */ +extern P rtP_Left; extern DW rtDW_Right; /* Observable states */ extern ExtU rtU_Right; /* External inputs */ extern ExtY rtY_Right; /* External outputs */ // ############################################################################### -static int16_t pwm_margin = 110; /* This margin allows to always have a window in the PWM signal for proper Phase currents measurement */ +static int16_t pwm_margin; /* This margin allows to have a window in the PWM signal for proper FOC Phase currents measurement */ extern uint8_t ctrlModReq; static int16_t curDC_max = (I_DC_MAX * A2BIT_CONV); @@ -60,7 +61,7 @@ extern volatile adc_buf_t adc_buffer; uint8_t buzzerFreq = 0; uint8_t buzzerPattern = 0; uint8_t buzzerCount = 0; -static uint32_t buzzerTimer = 0; +volatile uint32_t buzzerTimer = 0; static uint8_t buzzerPrev = 0; static uint8_t buzzerIdx = 0; @@ -146,6 +147,13 @@ void DMA1_Channel1_IRQHandler(void) { buzzerPrev = 0; } + // Adjust pwm_margin depending on the selected Control Type + if (rtP_Left.z_ctrlTypSel == FOC_CTRL) { + pwm_margin = 110; + } else { + pwm_margin = 0; + } + // ############################### MOTOR CONTROL ############################### int ul, vl, wl; diff --git a/Src/comms.c b/Src/comms.c new file mode 100644 index 0000000..835aded --- /dev/null +++ b/Src/comms.c @@ -0,0 +1,650 @@ +/** + * This file is part of the hoverboard-firmware-hack project. + * + * Copyright (C) 2020-2021 Emanuel FERU + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . +*/ + +// Includes +#include +#include +#include +#include "stm32f1xx_hal.h" +#include "config.h" +#include "defines.h" +#include "eeprom.h" +#include "BLDC_controller.h" +#include "util.h" +#include "comms.h" + +#if defined(DEBUG_SERIAL_PROTOCOL) +#if defined(DEBUG_SERIAL_PROTOCOL) && (defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)) + +#ifdef CONTROL_ADC + #define RAW_MIN 0 + #define RAW_MAX 4095 +#else + #define RAW_MIN -1000 + #define RAW_MAX 1000 +#endif + + +#define MAX_PARAM_WATCH 15 + +extern ExtY rtY_Left; /* External outputs */ +extern ExtU rtU_Left; /* External inputs */ +extern P rtP_Left; + +extern ExtY rtY_Right; /* External outputs */ +extern ExtU rtU_Right; /* External inputs */ +extern P rtP_Right; + + +extern InputStruct input1[]; // input structure +extern InputStruct input2[]; // input structure + +extern uint16_t VirtAddVarTab[NB_OF_VAR]; +extern int16_t speedAvg; // average measured speed +extern int16_t speedAvgAbs; // average measured speed in absolute +extern uint8_t ctrlModReqRaw; +extern int16_t batVoltageCalib; +extern int16_t board_temp_deg_c; +extern int16_t left_dc_curr; +extern int16_t right_dc_curr; +extern int16_t dc_curr; +extern int16_t cmdL; +extern int16_t cmdR; + + + +enum commandTypes {READ,WRITE}; +// Function0 - Function with 0 parameter +// Function1 - Function with 1 parameter (e.g. GET PARAM) +// Function2 - Function with 2 parameter (e.g. SET PARAM XXXX) +const command_entry commands[] = { + // Type ,Name ,Function0 ,Function1 ,Function2 ,Help + {READ ,"GET" ,printAllParamDef ,printParamDef ,NULL ,"Get Parameter/Variable"}, + {READ ,"HELP" ,printAllParamHelp ,printParamHelp ,NULL ,"Command/Parameter/Variable Help"}, + {READ ,"WATCH" ,NULL ,watchParamVal ,NULL ,"Toggle Parameter/Variable Watch"}, + {WRITE ,"SET" ,NULL ,NULL ,setParamValExt ,"Set Parameter"}, + {WRITE ,"INIT" ,NULL ,initParamVal ,NULL ,"Init Parameter from EEPROM or CONFIG.H"}, + {WRITE ,"SAVE" ,saveAllParamVal ,NULL ,NULL ,"Save Parameters to EEPROM"}, +}; + +enum paramTypes {PARAMETER,VARIABLE}; +const parameter_entry params[] = { + // CONTROL PARAMETERS + // Type ,Name ,Datatype ,ValueL ptr ,ValueR ,EEPRM Addr ,Init Int/Ext ,Min ,Max ,Div ,Mul ,Fix ,Callback Function ,Help text + {PARAMETER ,"CTRL_MOD" ,ADD_PARAM(ctrlModReqRaw) ,NULL ,0 ,CTRL_MOD_REQ ,0 ,1 ,3 ,0 ,0 ,0 ,NULL ,"Ctrl mode 1:VLT 2:SPD 3:TRQ"}, + {PARAMETER ,"CTRL_TYP" ,ADD_PARAM(rtP_Left.z_ctrlTypSel) ,&rtP_Right.z_ctrlTypSel ,0 ,CTRL_TYP_SEL ,0 ,0 ,2 ,0 ,0 ,0 ,NULL ,"Ctrl type 0:COM 1:SIN 2:FOC"}, + {PARAMETER ,"I_MOT_MAX" ,ADD_PARAM(rtP_Left.i_max) ,&rtP_Right.i_max ,1 ,I_MOT_MAX ,1 ,1 ,40 ,A2BIT_CONV ,0 ,4 ,NULL ,"Max phase current A"}, + {PARAMETER ,"N_MOT_MAX" ,ADD_PARAM(rtP_Left.n_max) ,&rtP_Right.n_max ,2 ,N_MOT_MAX ,1 ,10 ,2000 ,0 ,0 ,4 ,NULL ,"Max motor RPM"}, + {PARAMETER ,"FI_WEAK_ENA" ,ADD_PARAM(rtP_Left.b_fieldWeakEna) ,&rtP_Right.b_fieldWeakEna ,0 ,FIELD_WEAK_ENA ,0 ,0 ,1 ,0 ,0 ,0 ,NULL ,"Enable field weak"}, + {PARAMETER ,"FI_WEAK_HI" ,ADD_PARAM(rtP_Left.r_fieldWeakHi) ,&rtP_Right.r_fieldWeakHi ,0 ,FIELD_WEAK_HI ,1 ,0 ,1500 ,0 ,0 ,4 ,Input_Lim_Init ,"Field weak high RPM"}, + {PARAMETER ,"FI_WEAK_LO" ,ADD_PARAM(rtP_Left.r_fieldWeakLo) ,&rtP_Right.r_fieldWeakLo ,0 ,FIELD_WEAK_LO ,1 ,0 ,1000 ,0 ,0 ,4 ,Input_Lim_Init ,"Field weak low RPM"}, + {PARAMETER ,"FI_WEAK_MAX" ,ADD_PARAM(rtP_Left.id_fieldWeakMax) ,&rtP_Right.id_fieldWeakMax,0 ,FIELD_WEAK_MAX ,1 ,0 ,20 ,A2BIT_CONV ,0 ,4 ,NULL ,"Field weak max current A(FOC)"}, + {PARAMETER ,"PHA_ADV_MAX" ,ADD_PARAM(rtP_Left.a_phaAdvMax) ,&rtP_Right.a_phaAdvMax ,0 ,PHASE_ADV_MAX ,1 ,0 ,55 ,0 ,0 ,4 ,NULL ,"Max Phase Adv angle Deg(SIN)"}, + // INPUT PARAMETERS + // Type ,Name ,ValueL ptr ,ValueR ,EEPRM Addr ,Init Int/Ext ,Min ,Max ,Div ,Mul ,Fix ,Callback Function ,Help text + {VARIABLE ,"IN1_RAW" ,ADD_PARAM(input1[0].raw) ,NULL ,0 ,0 ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Input1 raw"}, + {PARAMETER ,"IN1_TYP" ,ADD_PARAM(input1[0].typ) ,NULL ,3 ,0 ,0 ,0 ,3 ,0 ,0 ,0 ,0 ,"Input1 type"}, + {PARAMETER ,"IN1_MIN" ,ADD_PARAM(input1[0].min) ,NULL ,4 ,RAW_MIN ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Input1 min"}, + {PARAMETER ,"IN1_MID" ,ADD_PARAM(input1[0].mid) ,NULL ,5 ,0 ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Input1 mid"}, + {PARAMETER ,"IN1_MAX" ,ADD_PARAM(input1[0].max) ,NULL ,6 ,RAW_MAX ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Input1 max"}, + {VARIABLE ,"IN1_CMD" ,ADD_PARAM(input1[0].cmd) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,"Input1 cmd"}, + + {VARIABLE ,"IN2_RAW" ,ADD_PARAM(input2[0].raw) ,NULL ,0 ,0 ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Input2 raw"}, + {PARAMETER ,"IN2_TYP" ,ADD_PARAM(input2[0].typ) ,NULL ,7 ,0 ,0 ,0 ,3 ,0 ,0 ,0 ,0 ,"Input2 type"}, + {PARAMETER ,"IN2_MIN" ,ADD_PARAM(input2[0].min) ,NULL ,8 ,RAW_MIN ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Input2 min"}, + {PARAMETER ,"IN2_MID" ,ADD_PARAM(input2[0].mid) ,NULL ,9 ,0 ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Input2 mid"}, + {PARAMETER ,"IN2_MAX" ,ADD_PARAM(input2[0].max) ,NULL ,10 ,RAW_MAX ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Input2 max"}, + {VARIABLE ,"IN2_CMD" ,ADD_PARAM(input2[0].cmd) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,"Input2 cmd"}, +#if defined(PRI_INPUT1) && defined(PRI_INPUT2) && defined(AUX_INPUT1) && defined(AUX_INPUT2) + // Type ,Name ,ValueL ptr ,ValueR ,EEPRM Addr ,Init Int/Ext ,Min ,Max ,Div ,Mul ,Fix ,Callback Function ,Help text + {VARIABLE ,"AUX_IN1_RAW" ,ADD_PARAM(input1[1].raw) ,NULL ,0 ,0 ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Aux. input1 raw"}, + {PARAMETER ,"AUX_IN1_TYP" ,ADD_PARAM(input1[1].typ) ,NULL ,11 ,0 ,0 ,0 ,3 ,0 ,0 ,0 ,0 ,"Aux. input1 type"}, + {PARAMETER ,"AUX_IN1_MIN" ,ADD_PARAM(input1[1].min) ,NULL ,12 ,RAW_MIN ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Aux. input1 min"}, + {PARAMETER ,"AUX_IN1_MID" ,ADD_PARAM(input1[1].mid) ,NULL ,13 ,0 ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Aux. input1 mid"}, + {PARAMETER ,"AUX_IN1_MAX" ,ADD_PARAM(input1[1].max) ,NULL ,14 ,RAW_MAX ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Aux. input1 max"}, + {VARIABLE ,"AUX_IN1_CMD" ,ADD_PARAM(input1[1].cmd) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,"Aux. input1 cmd"}, + + {VARIABLE ,"AUX_IN2_RAW" ,ADD_PARAM(input2[1].raw) ,NULL ,0 ,0 ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Aux. input2 raw"}, + {PARAMETER ,"AUX_IN2_TYP" ,ADD_PARAM(input2[1].typ) ,NULL ,15 ,0 ,0 ,0 ,3 ,0 ,0 ,0 ,0 ,"Aux. input2 type"}, + {PARAMETER ,"AUX_IN2_MIN" ,ADD_PARAM(input2[1].min) ,NULL ,16 ,RAW_MIN ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Aux. input2 min"}, + {PARAMETER ,"AUX_IN2_MID" ,ADD_PARAM(input2[1].mid) ,NULL ,17 ,0 ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Aux. input2 mid"}, + {PARAMETER ,"AUX_IN2_MAX" ,ADD_PARAM(input2[1].max) ,NULL ,18 ,RAW_MAX ,0 ,RAW_MIN,RAW_MAX,0 ,0 ,0 ,0 ,"Aux. input2 max"}, + {VARIABLE ,"AUX_IN2_CMD" ,ADD_PARAM(input2[1].cmd) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,"Aux. input2 cmd"}, +#endif + // FEEDBACK + // Type ,Name ,Datatype, ValueL ptr ,ValueR ,EEPRM Addr ,Init Int/Ext ,Min ,Max ,Div ,Mul ,Fix ,Callback Function ,Help text + {VARIABLE ,"DC_CURR" ,ADD_PARAM(dc_curr) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Total DC Link current A *100"}, + {VARIABLE ,"RDC_CURR" ,ADD_PARAM(right_dc_curr) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Right DC Link current A *100"}, + {VARIABLE ,"LDC_CURR" ,ADD_PARAM(left_dc_curr) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Left DC Link current A *100"}, + {VARIABLE ,"CMDL" ,ADD_PARAM(cmdL) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Left Motor Command"}, + {VARIABLE ,"CMDR" ,ADD_PARAM(cmdR) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Right Motor Command"}, + {VARIABLE ,"SPD_AVG" ,ADD_PARAM(speedAvg) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Motor Measured Avg RPM"}, + {VARIABLE ,"SPDL" ,ADD_PARAM(rtY_Left.n_mot) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Left Motor Measured RPM"}, + {VARIABLE ,"SPDR" ,ADD_PARAM(rtY_Right.n_mot) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Right Motor Measured RPM"}, + {VARIABLE ,"RATE" ,0 , NULL ,NULL ,0 ,RATE ,0 ,0 ,0 ,0 ,0 ,4 ,NULL ,"Rate *10"}, + {VARIABLE ,"SPD_COEF" ,0 , NULL ,NULL ,0 ,SPEED_COEFFICIENT ,0 ,0 ,0 ,0 ,10 ,14 ,NULL ,"Speed Coefficient *10"}, + {VARIABLE ,"STR_COEF" ,0 , NULL ,NULL ,0 ,STEER_COEFFICIENT ,0 ,0 ,0 ,0 ,10 ,14 ,NULL ,"Steer Coefficient *10"}, + {VARIABLE ,"BATV" ,ADD_PARAM(batVoltageCalib) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Calibrated Battery voltage *100"}, + {VARIABLE ,"TEMP" ,ADD_PARAM(board_temp_deg_c) ,NULL ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,NULL ,"Calibrated Temperature °C *10"}, + +}; + + +const char *errors[9] = { + "Command not found", // Err1 + "Parameter not found", // Err2 + "This command cannot be used with a Variable", // Err3 + "Value not in range", // Err4 + "Value expected", // Err5 + "Start of line expected", // Err6 + "End of line expected", // Err7 + "Parameter expected", // Err8 + "Uncaught error" // Err9 + "Watch list is full" // Err10 +}; + +debug_command command; +int8_t watchParamList[MAX_PARAM_WATCH] = {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1}; + +// Set Param with Value from external format +int8_t setParamValExt(uint8_t index, int32_t value) { + int8_t ret = 0; + // check min and max before conversion to internal values + if (IN_RANGE(value,params[index].min,params[index].max)){ + ret = setParamValInt(index,extToInt(index,value)); + printParamDef(index); + }else{ + printError(4); // Error - Value out of range + } + return ret; +} + +// Set Param with value from internal format +int8_t setParamValInt(uint8_t index, int32_t newValue) { + int32_t oldValue = getParamValInt(index); + if (oldValue != newValue){ + // if value is different, beep, cast and assign new value + switch (params[index].datatype){ + case UINT8_T: + if (params[index].valueL != NULL) *(uint8_t*)params[index].valueL = newValue; + if (params[index].valueR != NULL) *(uint8_t*)params[index].valueR = newValue; + break; + case UINT16_T: + if (params[index].valueL != NULL) *(uint16_t*)params[index].valueL = newValue; + if (params[index].valueR != NULL) *(uint16_t*)params[index].valueR = newValue; + break; + case UINT32_T: + if (params[index].valueL != NULL) *(uint32_t*)params[index].valueL = newValue; + if (params[index].valueR != NULL) *(uint32_t*)params[index].valueR = newValue; + break; + case INT8_T: + if (params[index].valueL != NULL) *(int8_t*)params[index].valueL = newValue; + if (params[index].valueR != NULL) *(int8_t*)params[index].valueR = newValue; + break; + case INT16_T: + if (params[index].valueL != NULL) *(int16_t*)params[index].valueL = newValue; + if (params[index].valueR != NULL) *(int16_t*)params[index].valueR = newValue; + break; + case INT32_T: + if (params[index].valueL != NULL) *(int32_t*)params[index].valueL = newValue; + if (params[index].valueR != NULL) *(int32_t*)params[index].valueR = newValue; + break; + } + + // Beep if value was modified + beepShort(5); + } + + // Run callback function if assigned + if (params[index].callback_function) (*params[index].callback_function)(); + return 1; +} + +// Get Parameter Internal value and translate to external +int32_t getParamValExt(uint8_t index) { + return intToExt(index,getParamValInt(index)); +} + +// Get Parameter Internal Value +int32_t getParamValInt(uint8_t index) { + int32_t value = 0; + + int8_t countVar = 0; + if (params[index].valueL != NULL) countVar++; + if (params[index].valueR != NULL) countVar++; + + if (countVar > 0){ + // Read Left and Right values and calculate average + // If left and right have to be summed up, DIV field could be adapted to multiply by 2 + // Cast to parameter datatype + switch (params[index].datatype){ + case UINT8_T: + if (params[index].valueL != NULL) value += *(uint8_t*)params[index].valueL; + if (params[index].valueR != NULL) value += *(uint8_t*)params[index].valueR; + break; + case UINT16_T: + if (params[index].valueL != NULL) value += *(uint16_t*)params[index].valueL; + if (params[index].valueR != NULL) value += *(uint16_t*)params[index].valueR; + break; + case UINT32_T: + if (params[index].valueL != NULL) value += *(uint32_t*)params[index].valueL; + if (params[index].valueR != NULL) value += *(uint32_t*)params[index].valueR; + break; + case INT8_T: + if (params[index].valueL != NULL) value += *(int8_t*)params[index].valueL; + if (params[index].valueR != NULL) value += *(int8_t*)params[index].valueR; + break; + case INT16_T: + if (params[index].valueL != NULL) value += *(int16_t*)params[index].valueL; + if (params[index].valueR != NULL) value += *(int16_t*)params[index].valueR; + break; + case INT32_T: + if (params[index].valueL != NULL) value += *(int32_t*)params[index].valueL; + if (params[index].valueR != NULL) value += *(int32_t*)params[index].valueR; + break; + default: + value = 0; + } + + // Divide by number of values provided for the parameter + value /= countVar; + }else{ + // No variable was provided, return init value that might contain a macro + value = params[index].init; + } + + return value; +} + +// Add or remove parameter from watch list +int8_t watchParamVal(uint8_t index){ + int8_t i,found = 0; + for(i=0;i < MAX_PARAM_WATCH && watchParamList[i]>-1;i++){ + if (watchParamList[i] == index) found = 1; + if (found) watchParamList[i] = (i < MAX_PARAM_WATCH-1)?watchParamList[i+1]:-1; + } + if (!found){ + if (watchParamList[i] == -1){ + watchParamList[i] = index; + return 1; + } + printError(10); + return 0; + } + return 1; +} + +// Print value for all parameters with watch flag +int8_t printParamVal(){ + int8_t i = 0; + for(i=0;i < MAX_PARAM_WATCH && watchParamList[i]>-1;i++){ + printf("%s:%li ",params[watchParamList[i]].name,getParamValExt(watchParamList[i])); + } + if (i>0) printf("\r\n"); + return 1; +} + +// Print help for Command +int8_t printCommandHelp(uint8_t index){ + printf("? %s:\"%s\"\r\n",commands[index].name,commands[index].help); + return 1; +} + +// Print help for parameter +int8_t printParamHelp(uint8_t index){ + printf("? %s:\"%s\" ",params[index].name,params[index].help); + if (params[index].type == PARAMETER) printf("[min:%li max:%li]",params[index].min,params[index].max); + printf("\r\n"); + return 1; +} + +// Print help for all parameters +int8_t printAllParamHelp(){ + printf("? Commands\r\n"); + for(int i=0;i>= params[index].fix; + return value; +} + +// Translate from External to Internal Format +int32_t extToInt(uint8_t index,int32_t value){ + // Multiply to translate to internal format + if(params[index].div) value *= params[index].div; + // Shift to translate to internal format + if (params[index].fix) value <<= params[index].fix; + // Divide for small number + if(params[index].mul) value /= params[index].mul; + return value; +} + +// Get Parameter Init value(EEPROM or init/config.h) and translate to external format +int32_t getParamInitExt(uint8_t index) { + return intToExt(index,getParamInitInt(index)); +} + +// Get Parameter value with EEprom data if address is avalaible, init/config.h value otherwise +int16_t getParamInitInt(uint8_t index){ + if (params[index].addr){ + // if EEPROM address is specified, init from EEPROM address + uint16_t writeCheck, readVal; + + HAL_FLASH_Unlock(); + EE_ReadVariable(VirtAddVarTab[0], &writeCheck); + EE_ReadVariable(VirtAddVarTab[params[index].addr] , &readVal); + HAL_FLASH_Lock(); + + // EEPROM was written, use stored value + if (writeCheck == FLASH_WRITE_KEY){ + return readVal; + }else{ + // Use init value from array + if (params[index].initFormat){ + // Init Value is in External format (e.g. PHA_ADV_MAX is 25 deg) + return extToInt(index,params[index].init); + }else{ + return params[index].init; + } + } + }else{ + if (params[index].initFormat){ + // Init Value is in External format (e.g. PHA_ADV_MAX is 25 deg) + return extToInt(index,params[index].init); + }else{ + return params[index].init; + } + } +} + + +// initialize Parameter value with EEprom data if address is avalaible, init/config.h value otherwise +int8_t initParamVal(uint8_t index) { + int8_t ret = 0; + ret = setParamValInt(index,(int32_t) getParamInitInt(index)); + printParamDef(index); + return ret; +} + +// Find command in commands array and return index +int8_t findCommand(uint8_t *userCommand, uint32_t len){ + for(int index=0;indexMAX_int16_T){command.error = 4;return;} + } + + if (count == 0){ + // Error - Value required + command.error = 5; + return; + } + + // Apply sign + value*= sign; + + // Command with parameter and value + if (commands[cindex].callback_function2 != NULL){ + if (*userCommand == '\n' || *userCommand == '\r'){ + command.semaphore = 1; + command.command_index = cindex; + command.param_index = pindex; + command.param_value = value; + }else{ + command.error = 7; // Error - End of line expected + } + return; + } + + // Uncaught error + command.error = 9; + +} + +void process_debug() +{ + + // Print parameters from watch list + printParamVal(); + + // Show Error if any + if(command.error> 0){ + printError(command.error); + command.error = 0; + return; + } + + // Nothing to do + if (command.semaphore == 0) return; + + int8_t ret = 0; + if (commands[command.command_index].callback_function0 != NULL && + command.param_index == -1){ + // This function needs no parameter + ret = (*commands[command.command_index].callback_function0)(); + if (ret==1){printf("OK\r\n");} + command.semaphore = 0; + return; + } + + if (commands[command.command_index].callback_function1 != NULL && + command.param_index != -1){ + // This function needs only a parameter + ret = (*commands[command.command_index].callback_function1)(command.param_index); + if (ret==1){printf("OK\r\n");} + command.semaphore = 0; + return; + } + + if (commands[command.command_index].callback_function2 != NULL && + command.param_index != -1){ + // This function needs an additional parameter + ret = (*commands[command.command_index].callback_function2)(command.param_index,command.param_value); + if (ret==1){printf("OK\r\n");} + command.semaphore = 0; + } +} + +#endif +#endif // DEBUG_SERIAL_PROTOCOL + diff --git a/Src/control.c b/Src/control.c index 3a82eb1..81dc179 100644 --- a/Src/control.c +++ b/Src/control.c @@ -8,10 +8,11 @@ TIM_HandleTypeDef TimHandle; TIM_HandleTypeDef TimHandle2; -uint8_t ppm_count = 0; -uint8_t pwm_count = 0; -uint32_t timeoutCnt = 0; -uint8_t nunchuk_data[6] = {0}; +uint8_t ppm_count = 0; +uint8_t pwm_count = 0; +uint32_t timeoutCntGen = TIMEOUT; +uint8_t timeoutFlgGen = 0; +uint8_t nunchuk_data[6] = {0}; uint8_t i2cBuffer[2]; @@ -34,7 +35,8 @@ void PPM_ISR_Callback(void) { if (rc_delay > 3000) { if (ppm_valid && ppm_count == PPM_NUM_CHANNELS) { ppm_timeout = 0; - timeoutCnt = 0; + timeoutCntGen = 0; + timeoutFlgGen = 0; memcpy(ppm_captured_value, ppm_captured_value_buffer, sizeof(ppm_captured_value)); } ppm_valid = true; @@ -122,7 +124,8 @@ void PWM_ISR_CH1_Callback(void) { } else { // Falling Edge interrupt -> measure pulse duration uint16_t rc_signal = TIM2->CNT - pwm_CNT_prev_ch1; if (IN_RANGE(rc_signal, 900, 2100)){ - timeoutCnt = 0; + timeoutCntGen = 0; + timeoutFlgGen = 0; pwm_timeout_ch1 = 0; pwm_captured_ch1_value = CLAMP(rc_signal, 1000, 2000) - 1000; } @@ -141,7 +144,8 @@ void PWM_ISR_CH2_Callback(void) { } else { // Falling Edge interrupt -> measure pulse duration uint16_t rc_signal = TIM2->CNT - pwm_CNT_prev_ch2; if (IN_RANGE(rc_signal, 900, 2100)){ - timeoutCnt = 0; + timeoutCntGen = 0; + timeoutFlgGen = 0; pwm_timeout_ch2 = 0; pwm_captured_ch2_value = CLAMP(rc_signal, 1000, 2000) - 1000; } @@ -237,11 +241,12 @@ void Nunchuk_Read(void) { HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 10); HAL_Delay(3); if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuk_data, 6, 10) == HAL_OK) { - timeoutCnt = 0; + timeoutCntGen = 0; + timeoutFlgGen = 0; } #ifndef TRANSPOTTER - if (timeoutCnt > 3) { + if (timeoutCntGen > 3) { HAL_Delay(50); Nunchuk_Init(); } diff --git a/Src/main.c b/Src/main.c index 2c6165d..eda4056 100644 --- a/Src/main.c +++ b/Src/main.c @@ -29,6 +29,7 @@ #include "util.h" #include "BLDC_controller.h" /* BLDC's header file */ #include "rtwtypes.h" +#include "comms.h" #if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) #include "hd44780.h" @@ -60,18 +61,21 @@ extern P rtP_Left; /* Block parameters (auto storage) */ extern P rtP_Right; /* Block parameters (auto storage) */ extern ExtY rtY_Left; /* External outputs */ extern ExtY rtY_Right; /* External outputs */ +extern ExtU rtU_Left; /* External inputs */ +extern ExtU rtU_Right; /* External inputs */ //--------------- -extern int16_t cmd1; // normalized input value. -1000 to 1000 -extern int16_t cmd2; // normalized input value. -1000 to 1000 -extern int16_t input1; // Non normalized input value -extern int16_t input2; // Non normalized input value +extern uint8_t inIdx; // input index used for dual-inputs +extern uint8_t inIdx_prev; +extern InputStruct input1[]; // input structure +extern InputStruct input2[]; // input structure extern int16_t speedAvg; // Average measured speed extern int16_t speedAvgAbs; // Average measured speed in absolute -extern volatile uint32_t timeoutCnt; // Timeout counter for the General timeout (PPM, PWM, Nunchuck) -extern uint8_t timeoutFlagADC; // Timeout Flag for for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data) -extern uint8_t timeoutFlagSerial; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) +extern volatile uint32_t timeoutCntGen; // Timeout counter for the General timeout (PPM, PWM, Nunchuk) +extern volatile uint8_t timeoutFlgGen; // Timeout Flag for the General timeout (PPM, PWM, Nunchuk) +extern uint8_t timeoutFlgADC; // Timeout Flag for for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data) +extern uint8_t timeoutFlgSerial; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) extern volatile int pwml; // global variable for pwm left. -1000 to 1000 extern volatile int pwmr; // global variable for pwm right. -1000 to 1000 @@ -99,7 +103,15 @@ extern volatile uint16_t pwm_captured_ch2_value; // Global variables set here in main.c //------------------------------------------------------------------------ uint8_t backwardDrive; +extern volatile uint32_t buzzerTimer; volatile uint32_t main_loop_counter; +int16_t batVoltageCalib; // global variable for calibrated battery voltage +int16_t board_temp_deg_c; // global variable for calibrated temperature in degrees Celsius +int16_t left_dc_curr; // global variable for Left DC Link current +int16_t right_dc_curr; // global variable for Right DC Link current +int16_t dc_curr; // global variable for Total DC Link current +int16_t cmdL; // global variable for Left Command +int16_t cmdR; // global variable for Right Command //------------------------------------------------------------------------ // Local variables @@ -146,10 +158,10 @@ static int16_t speed; // local variable for speed. -1000 to 10 static int32_t speedFixdt; // local fixed-point variable for speed low-pass filter #endif +static uint32_t buzzerTimer_prev = 0; static uint32_t inactivity_timeout_counter; static MultipleTap MultipleTapBrake; // define multiple tap functionality for the Brake pedal - int main(void) { HAL_Init(); @@ -189,24 +201,22 @@ int main(void) { poweronMelody(); HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET); - - int16_t cmdL = 0, cmdR = 0; - int16_t lastCmdL = 0, lastCmdR = 0; - + int32_t board_temp_adcFixdt = adc_buffer.temp << 16; // Fixed-point filter output initialized with current ADC converted to fixed-point int16_t board_temp_adcFilt = adc_buffer.temp; - int16_t board_temp_deg_c; + // Loop until button is released + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } while(1) { - HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms + if (buzzerTimer - buzzerTimer_prev > 16*DELAY_IN_MAIN_LOOP) { // 1 ms = 16 ticks buzzerTimer - readCommand(); // Read Command: cmd1, cmd2 + readCommand(); // Read Command: input1[inIdx].cmd, input2[inIdx].cmd calcAvgSpeed(); // Calculate average measured speed: speedAvg, speedAvgAbs #ifndef VARIANT_TRANSPOTTER // ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) ####### - if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){ + if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (input1[inIdx].cmd > -50 && input1[inIdx].cmd < 50) && (input2[inIdx].cmd > -50 && input2[inIdx].cmd < 50)){ beepShort(6); // make 2 beeps indicating the motor enable beepShort(4); HAL_Delay(100); steerFixdt = speedFixdt = 0; // reset filters @@ -216,7 +226,7 @@ int main(void) { #endif } - // ####### VARIANT_HOVERCAR ####### + // ####### VARIANT_HOVERCAR ####### #if defined(VARIANT_HOVERCAR) || defined(VARIANT_SKATEBOARD) || defined(ELECTRIC_BRAKE_ENABLE) uint16_t speedBlend; // Calculate speed Blend, a number between [0, 1] in fixdt(0,16,15) speedBlend = (uint16_t)(((CLAMP(speedAvgAbs,10,60) - 10) << 15) / 50); // speedBlend [0,1] is within [10 rpm, 60rpm] @@ -226,115 +236,119 @@ int main(void) { standstillHold(); // Apply Standstill Hold functionality. Only available and makes sense for VOLTAGE or TORQUE Mode #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 - multipleTapDet(cmd1, HAL_GetTick(), &MultipleTapBrake); // Brake pedal in this case is "cmd1" variable + multipleTapDet(input1[inIdx].cmd, HAL_GetTick(), &MultipleTapBrake); // Brake pedal in this case is "input1" variable } - - if (cmd1 > 30) { // If Brake pedal (cmd1) is pressed, bring to 0 also the Throttle pedal (cmd2) to avoid "Double pedal" driving - cmd2 = (int16_t)((cmd2 * speedBlend) >> 15); + + if (input1[inIdx].cmd > 30) { // If Brake pedal (input1) is pressed, bring to 0 also the Throttle pedal (input2) to avoid "Double pedal" driving + input2[inIdx].cmd = (int16_t)((input2[inIdx].cmd * speedBlend) >> 15); cruiseControl((uint8_t)rtP_Left.b_cruiseCtrlEna); // Cruise control deactivated by Brake pedal if it was active } + } #endif - #ifdef ELECTRIC_BRAKE_ENABLE + #ifdef ELECTRIC_BRAKE_ENABLE electricBrake(speedBlend, MultipleTapBrake.b_multipleTap); // Apply Electric Brake. Only available and makes sense for TORQUE Mode #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) - cmd1 = (int16_t)((-cmd1 * speedBlend) >> 15); + input1[inIdx].cmd = (int16_t)((-input1[inIdx].cmd * speedBlend) >> 15); } else { - cmd1 = (int16_t)(( cmd1 * speedBlend) >> 15); + input1[inIdx].cmd = (int16_t)(( input1[inIdx].cmd * speedBlend) >> 15); } + } #endif #ifdef VARIANT_SKATEBOARD - if (cmd2 < 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 - cmd2 = (int16_t)(( cmd2 * speedBlend) >> 15); + if (input2[inIdx].cmd < 0) { // When Throttle is negative, it acts as brake. This condition is to make sure it goes to 0 as we reach standstill (to avoid Reverse driving) + if (speedAvg > 0) { // Make sure the braking is opposite to the direction of motion + input2[inIdx].cmd = (int16_t)(( input2[inIdx].cmd * speedBlend) >> 15); } else { - cmd2 = (int16_t)((-cmd2 * speedBlend) >> 15); + input2[inIdx].cmd = (int16_t)((-input2[inIdx].cmd * speedBlend) >> 15); } } #endif // ####### LOW-PASS FILTER ####### - rateLimiter16(cmd1, RATE, &steerRateFixdt); - rateLimiter16(cmd2, RATE, &speedRateFixdt); + rateLimiter16(input1[inIdx].cmd , RATE, &steerRateFixdt); + rateLimiter16(input2[inIdx].cmd , RATE, &speedRateFixdt); filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt); filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt); steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer speed = (int16_t)(speedFixdt >> 16); // convert fixed-point to integer // ####### VARIANT_HOVERCAR ####### - #ifdef VARIANT_HOVERCAR + #ifdef VARIANT_HOVERCAR + if (inIdx == CONTROL_ADC) { // Only use use implementation below if pedals are in use (ADC input) if (!MultipleTapBrake.b_multipleTap) { // Check driving direction speed = steer + speed; // Forward driving: in this case steer = Brake, speed = Throttle } else { speed = steer - speed; // Reverse driving: in this case steer = Brake, speed = Throttle } + steer = 0; // Do not apply steering to avoid side effects if STEER_COEFFICIENT is NOT 0 + } #endif // ####### MIXER ####### - // cmdR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA); - // cmdL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA); + // cmdR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MAX); + // cmdL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MAX); mixerFcn(speed << 4, steer << 4, &cmdR, &cmdL); // This function implements the equations above // ####### SET OUTPUTS (if the target change is less than +/- 100) ####### - if ((cmdL > lastCmdL-100 && cmdL < lastCmdL+100) && (cmdR > lastCmdR-100 && cmdR < lastCmdR+100)) { - #ifdef INVERT_R_DIRECTION - pwmr = cmdR; - #else - pwmr = -cmdR; - #endif - #ifdef INVERT_L_DIRECTION - pwml = -cmdL; - #else - pwml = cmdL; - #endif - } + #ifdef INVERT_R_DIRECTION + pwmr = cmdR; + #else + pwmr = -cmdR; + #endif + #ifdef INVERT_L_DIRECTION + pwml = -cmdL; + #else + pwml = cmdL; + #endif #endif #ifdef VARIANT_TRANSPOTTER - distance = CLAMP(cmd1 - 180, 0, 4095); - steering = (cmd2 - 2048) / 2048.0; + distance = CLAMP(input1[inIdx].cmd - 180, 0, 4095); + steering = (input2[inIdx].cmd - 2048) / 2048.0; distanceErr = distance - (int)(setDistance * 1345); if (nunchuk_connected == 0) { cmdL = cmdL * 0.8f + (CLAMP(distanceErr + (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f); cmdR = cmdR * 0.8f + (CLAMP(distanceErr - (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f); - if ((cmdL < lastCmdL + 50 && cmdL > lastCmdL - 50) && (cmdR < lastCmdR + 50 && cmdR > lastCmdR - 50)) { - if (distanceErr > 0) { - enable = 1; - } - if (distanceErr > -300) { - #ifdef INVERT_R_DIRECTION - pwmr = cmdR; - #else - pwmr = -cmdR; - #endif - #ifdef INVERT_L_DIRECTION - pwml = -cmdL; - #else - pwml = cmdL; - #endif - - if (checkRemote) { - if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) { - //enable = 1; - } else { - enable = 0; - } + if (distanceErr > 0) { + enable = 1; + } + if (distanceErr > -300) { + #ifdef INVERT_R_DIRECTION + pwmr = cmdR; + #else + pwmr = -cmdR; + #endif + #ifdef INVERT_L_DIRECTION + pwml = -cmdL; + #else + pwml = cmdL; + #endif + + if (checkRemote) { + if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) { + //enable = 1; + } else { + enable = 0; } - } else { - enable = 0; } + } else { + enable = 0; } - timeoutCnt = 0; + timeoutCntGen = 0; + timeoutFlgGen = 0; } - if (timeoutCnt > TIMEOUT) { + if (timeoutFlgGen) { pwml = 0; pwmr = 0; enable = 0; @@ -368,7 +382,8 @@ int main(void) { #ifdef SUPPORT_LCD LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Nunchuk Control"); #endif - timeoutCnt = 0; + timeoutCntGen = 0; + timeoutFlgGen = 0; HAL_Delay(1000); nunchuk_connected = 1; } @@ -394,11 +409,11 @@ int main(void) { #endif // ####### SIDEBOARDS HANDLING ####### - #if defined(SIDEBOARD_SERIAL_USART2) + #if defined(SIDEBOARD_SERIAL_USART2) && defined(FEEDBACK_SERIAL_USART2) sideboardLeds(&sideboard_leds_L); sideboardSensors((uint8_t)Sideboard_L.sensors); #endif - #if defined(SIDEBOARD_SERIAL_USART3) + #if defined(SIDEBOARD_SERIAL_USART3) && defined(FEEDBACK_SERIAL_USART3) sideboardLeds(&sideboard_leds_R); sideboardSensors((uint8_t)Sideboard_R.sensors); #endif @@ -408,18 +423,30 @@ int main(void) { board_temp_adcFilt = (int16_t)(board_temp_adcFixdt >> 16); // convert fixed-point to integer board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C; + // ####### CALC CALIBRATED BATTERY VOLTAGE ####### + batVoltageCalib = batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC; + + // ####### CALC DC LINK CURRENT ####### + left_dc_curr = -(rtU_Left.i_DCLink * 100) / A2BIT_CONV; // Left DC Link Current * 100 + right_dc_curr = -(rtU_Right.i_DCLink * 100) / A2BIT_CONV; // Right DC Link Current * 100 + dc_curr = left_dc_curr + right_dc_curr; // Total DC Link Current * 100 + // ####### DEBUG SERIAL OUT ####### #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) - if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms - printf("in1:%i in2:%i cmdL:%i cmdR:%i BatADC:%i BatV:%i TempADC:%i Temp:%i\r\n", - input1, // 1: INPUT1 - input2, // 2: INPUT2 - cmdL, // 3: output command: [-1000, 1000] - cmdR, // 4: output command: [-1000, 1000] - adc_buffer.batt1, // 5: for battery voltage calibration - batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC, // 6: for verifying battery voltage calibration - board_temp_adcFilt, // 7: for board temperature calibration - board_temp_deg_c); // 8: for verifying board temperature calibration + if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms + #if defined(DEBUG_SERIAL_PROTOCOL) + process_debug(); + #else + printf("in1:%i in2:%i cmdL:%i cmdR:%i BatADC:%i BatV:%i TempADC:%i Temp:%i\r\n", + input1[inIdx].raw, // 1: INPUT1 + input2[inIdx].raw, // 2: INPUT2 + cmdL, // 3: output command: [-1000, 1000] + cmdR, // 4: output command: [-1000, 1000] + adc_buffer.batt1, // 5: for battery voltage calibration + batVoltageCalib, // 6: for verifying battery voltage calibration + board_temp_adcFilt, // 7: for board temperature calibration + board_temp_deg_c); // 8: for verifying board temperature calibration + #endif } #endif @@ -427,31 +454,31 @@ int main(void) { #if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3) if (main_loop_counter % 2 == 0) { // Send data periodically every 10 ms Feedback.start = (uint16_t)SERIAL_START_FRAME; - Feedback.cmd1 = (int16_t)cmd1; - Feedback.cmd2 = (int16_t)cmd2; + Feedback.cmd1 = (int16_t)input1[inIdx].cmd; + Feedback.cmd2 = (int16_t)input2[inIdx].cmd; Feedback.speedR_meas = (int16_t)rtY_Right.n_mot; Feedback.speedL_meas = (int16_t)rtY_Left.n_mot; - Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); + Feedback.batVoltage = (int16_t)batVoltageCalib; Feedback.boardTemp = (int16_t)board_temp_deg_c; #if defined(FEEDBACK_SERIAL_USART2) if(__HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) { - Feedback.cmdLed = (uint16_t)sideboard_leds_L; - Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas - ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed); + Feedback.cmdLed = (uint16_t)sideboard_leds_L; + Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas + ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed); HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&Feedback, sizeof(Feedback)); } #endif #if defined(FEEDBACK_SERIAL_USART3) if(__HAL_DMA_GET_COUNTER(huart3.hdmatx) == 0) { - Feedback.cmdLed = (uint16_t)sideboard_leds_R; - Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas - ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed); + Feedback.cmdLed = (uint16_t)sideboard_leds_R; + Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas + ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed); HAL_UART_Transmit_DMA(&huart3, (uint8_t *)&Feedback, sizeof(Feedback)); } - #endif + #endif } #endif @@ -464,11 +491,11 @@ int main(void) { } else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // 1 beep (low pitch): Motor error, disable motors enable = 0; beepCount(1, 24, 1); - } else if (timeoutFlagADC) { // 2 beeps (low pitch): ADC timeout + } else if (timeoutFlgADC) { // 2 beeps (low pitch): ADC timeout beepCount(2, 24, 1); - } else if (timeoutFlagSerial) { // 3 beeps (low pitch): Serial timeout + } else if (timeoutFlgSerial) { // 3 beeps (low pitch): Serial timeout beepCount(3, 24, 1); - } else if (timeoutCnt > TIMEOUT) { // 4 beeps (low pitch): General timeout (PPM, PWM, Nunchuck) + } else if (timeoutFlgGen) { // 4 beeps (low pitch): General timeout (PPM, PWM, Nunchuk) beepCount(4, 24, 1); } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // 5 beeps (low pitch): Mainboard temperature warning beepCount(5, 24, 1); @@ -495,12 +522,13 @@ int main(void) { poweroff(); } + // HAL_GPIO_TogglePin(LED_PORT, LED_PIN); // This is to measure the main() loop duration with an oscilloscope connected to LED_PIN - // Update main loop states - lastCmdL = cmdL; - lastCmdR = cmdR; + // Update states + inIdx_prev = inIdx; + buzzerTimer_prev = buzzerTimer; main_loop_counter++; - timeoutCnt++; + } } } diff --git a/Src/setup.c b/Src/setup.c index 435cd78..66ee441 100644 --- a/Src/setup.c +++ b/Src/setup.c @@ -638,7 +638,11 @@ void MX_ADC1_Init(void) { sConfig.Rank = 3; HAL_ADC_ConfigChannel(&hadc1, &sConfig); + #if BOARD_VARIANT == 0 sConfig.Channel = ADC_CHANNEL_12; // pc2 vbat + #elif BOARD_VARIANT == 1 + sConfig.Channel = ADC_CHANNEL_1; // pa1 vbat + #endif sConfig.Rank = 4; HAL_ADC_ConfigChannel(&hadc1, &sConfig); diff --git a/Src/util.c b/Src/util.c index 536348d..733ab02 100644 --- a/Src/util.c +++ b/Src/util.c @@ -29,6 +29,7 @@ #include "util.h" #include "BLDC_controller.h" #include "rtwtypes.h" +#include "comms.h" #if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) #include "hd44780.h" @@ -53,7 +54,8 @@ extern uint8_t buzzerPattern; // global variable for the buzzer patter extern uint8_t enable; // global variable for motor enable extern uint8_t nunchuk_data[6]; -extern volatile uint32_t timeoutCnt; // global variable for general timeout counter +extern volatile uint32_t timeoutCntGen; // global counter for general timeout counter +extern volatile uint8_t timeoutFlgGen; // global flag for general timeout counter extern volatile uint32_t main_loop_counter; #if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT) @@ -87,15 +89,20 @@ ExtU rtU_Right; /* External inputs */ ExtY rtY_Right; /* External outputs */ //--------------- -int16_t cmd1; // normalized input value. -1000 to 1000 -int16_t cmd2; // normalized input value. -1000 to 1000 -int16_t input1; // Non normalized input value -int16_t input2; // Non normalized input value +uint8_t inIdx = 0; +uint8_t inIdx_prev = 0; +#if defined(PRI_INPUT1) && defined(PRI_INPUT2) && defined(AUX_INPUT1) && defined(AUX_INPUT2) +InputStruct input1[INPUTS_NR] = { {0, 0, 0, PRI_INPUT1}, {0, 0, 0, AUX_INPUT1} }; +InputStruct input2[INPUTS_NR] = { {0, 0, 0, PRI_INPUT2}, {0, 0, 0, AUX_INPUT2} }; +#else +InputStruct input1[INPUTS_NR] = { {0, 0, 0, PRI_INPUT1} }; +InputStruct input2[INPUTS_NR] = { {0, 0, 0, PRI_INPUT2} }; +#endif int16_t speedAvg; // average measured speed int16_t speedAvgAbs; // average measured speed in absolute -uint8_t timeoutFlagADC = 0; // Timeout Flag for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data) -uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) +uint8_t timeoutFlgADC = 0; // Timeout Flag for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data) +uint8_t timeoutFlgSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) uint8_t ctrlModReqRaw = CTRL_MOD_REQ; uint8_t ctrlModReq = CTRL_MOD_REQ; // Final control mode request @@ -112,13 +119,14 @@ uint8_t nunchuk_connected = 0; #ifdef VARIANT_TRANSPOTTER float setDistance; -uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1337}; // Virtual address defined by the user: 0xFFFF value is prohibited +uint16_t VirtAddVarTab[NB_OF_VAR] = {1337}; // Virtual address defined by the user: 0xFFFF value is prohibited static uint16_t saveValue = 0; static uint8_t saveValue_valid = 0; #elif !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) -uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300, 1301, 1302, 1303, 1304, 1305, 1306, 1307, 1308, 1309, 1310}; +uint16_t VirtAddVarTab[NB_OF_VAR] = {1000, 1001, 1002, 1003, 1004, 1005, 1006, 1007, 1008, 1009, + 1010, 1011, 1012, 1013, 1014, 1015, 1016, 1017, 1018}; #else -uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300}; // Dummy virtual address to avoid warnings +uint16_t VirtAddVarTab[NB_OF_VAR] = {1000}; // Dummy virtual address to avoid warnings #endif @@ -132,27 +140,19 @@ static int16_t INPUT_MIN; // [-] Input target minimum limitation #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) static uint8_t cur_spd_valid = 0; static uint8_t inp_cal_valid = 0; - static uint16_t INPUT1_TYP_CAL = INPUT1_TYPE; - static uint16_t INPUT1_MIN_CAL = INPUT1_MIN; - static uint16_t INPUT1_MID_CAL = INPUT1_MID; - static uint16_t INPUT1_MAX_CAL = INPUT1_MAX; - static uint16_t INPUT2_TYP_CAL = INPUT2_TYPE; - static uint16_t INPUT2_MIN_CAL = INPUT2_MIN; - static uint16_t INPUT2_MID_CAL = INPUT2_MID; - static uint16_t INPUT2_MAX_CAL = INPUT2_MAX; #endif #if defined(CONTROL_ADC) -static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection +static uint16_t timeoutCntADC = ADC_PROTECT_TIMEOUT; // Timeout counter for ADC Protection #endif #if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) -static uint8_t rx_buffer_L[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer +static uint8_t rx_buffer_L[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer static uint32_t rx_buffer_L_len = ARRAY_LEN(rx_buffer_L); #endif #if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) -static uint16_t timeoutCntSerial_L = 0; // Timeout counter for Rx Serial command -static uint8_t timeoutFlagSerial_L = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) +static uint16_t timeoutCntSerial_L = SERIAL_TIMEOUT; // Timeout counter for Rx Serial command +static uint8_t timeoutFlgSerial_L = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) #endif #if defined(SIDEBOARD_SERIAL_USART2) SerialSideboard Sideboard_L; @@ -161,12 +161,12 @@ static uint32_t Sideboard_L_len = sizeof(Sideboard_L); #endif #if defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) -static uint8_t rx_buffer_R[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer +static uint8_t rx_buffer_R[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer static uint32_t rx_buffer_R_len = ARRAY_LEN(rx_buffer_R); #endif #if defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) -static uint16_t timeoutCntSerial_R = 0; // Timeout counter for Rx Serial command -static uint8_t timeoutFlagSerial_R = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) +static uint16_t timeoutCntSerial_R = SERIAL_TIMEOUT; // Timeout counter for Rx Serial command +static uint8_t timeoutFlgSerial_R = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) #endif #if defined(SIDEBOARD_SERIAL_USART3) SerialSideboard Sideboard_R; @@ -174,13 +174,21 @@ SerialSideboard Sideboard_R_raw; static uint32_t Sideboard_R_len = sizeof(Sideboard_R); #endif -#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) -static SerialCommand command; -static SerialCommand command_raw; -static uint32_t command_len = sizeof(command); +#if defined(CONTROL_SERIAL_USART2) +static SerialCommand commandL; +static SerialCommand commandL_raw; +static uint32_t commandL_len = sizeof(commandL); + #ifdef CONTROL_IBUS + static uint16_t ibusL_captured_value[IBUS_NUM_CHANNELS]; + #endif +#endif + +#if defined(CONTROL_SERIAL_USART3) +static SerialCommand commandR; +static SerialCommand commandR_raw; +static uint32_t commandR_len = sizeof(commandR); #ifdef CONTROL_IBUS - static uint16_t ibus_chksum; - static uint16_t ibus_captured_value[IBUS_NUM_CHANNELS]; + static uint16_t ibusR_captured_value[IBUS_NUM_CHANNELS]; #endif #endif @@ -224,8 +232,7 @@ static uint8_t standstillAcv = 0; #endif #endif - - + /* =========================== Initialization Functions =========================== */ void BLDC_Init(void) { @@ -262,7 +269,7 @@ void BLDC_Init(void) { BLDC_controller_initialize(rtM_Right); } -void Input_Lim_Init(void) { // Input Limitations - ! Do NOT touch ! +void Input_Lim_Init(void) { // Input Limitations - ! Do NOT touch ! if (rtP_Left.b_fieldWeakEna || rtP_Right.b_fieldWeakEna) { INPUT_MAX = MAX( 1000, FIELD_WEAK_HI); INPUT_MIN = MIN(-1000,-FIELD_WEAK_HI); @@ -302,36 +309,44 @@ void Input_Init(void) { #endif #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - uint16_t writeCheck, i_max, n_max; + uint16_t writeCheck, readVal; HAL_FLASH_Unlock(); EE_Init(); /* EEPROM Init */ EE_ReadVariable(VirtAddVarTab[0], &writeCheck); if (writeCheck == FLASH_WRITE_KEY) { - EE_ReadVariable(VirtAddVarTab[1] , &INPUT1_TYP_CAL); - EE_ReadVariable(VirtAddVarTab[2] , &INPUT1_MIN_CAL); - EE_ReadVariable(VirtAddVarTab[3] , &INPUT1_MID_CAL); - EE_ReadVariable(VirtAddVarTab[4] , &INPUT1_MAX_CAL); - EE_ReadVariable(VirtAddVarTab[5] , &INPUT2_TYP_CAL); - EE_ReadVariable(VirtAddVarTab[6] , &INPUT2_MIN_CAL); - EE_ReadVariable(VirtAddVarTab[7] , &INPUT2_MID_CAL); - EE_ReadVariable(VirtAddVarTab[8] , &INPUT2_MAX_CAL); - EE_ReadVariable(VirtAddVarTab[9] , &i_max); - EE_ReadVariable(VirtAddVarTab[10], &n_max); - rtP_Left.i_max = i_max; - rtP_Left.n_max = n_max; - rtP_Right.i_max = i_max; - rtP_Right.n_max = n_max; - } else { // Else If Input type is 3 (auto), identify the input type based on the values from config.h - if (INPUT1_TYPE == 3) { INPUT1_TYP_CAL = checkInputType(INPUT1_MIN, INPUT1_MID, INPUT1_MAX); } - if (INPUT2_TYPE == 3) { INPUT2_TYP_CAL = checkInputType(INPUT2_MIN, INPUT2_MID, INPUT2_MAX); } + EE_ReadVariable(VirtAddVarTab[1] , &readVal); rtP_Left.i_max = rtP_Right.i_max = (int16_t)readVal; + EE_ReadVariable(VirtAddVarTab[2] , &readVal); rtP_Left.n_max = rtP_Right.n_max = (int16_t)readVal; + for (uint8_t i=0; i in_mid - deadBand && u < in_mid + deadBand ) { - return 0; - } else if(u > in_mid) { - return CLAMP(MAP(u, in_mid + deadBand, in_max, 0, out_max), 0, out_max); - } else { - return CLAMP(MAP(u, in_mid - deadBand, in_min, 0, out_min), out_min, 0); - } - default: - return 0; - } -} - - /* - * Check Input Type - * This function identifies the input type: 0: Disabled, 1: Normal Pot, 2: Middle Resting Pot - */ -int checkInputType(int16_t min, int16_t mid, int16_t max){ - - int type = 0; - #ifdef CONTROL_ADC - int16_t threshold = 400; // Threshold to define if values are too close - #else - int16_t threshold = 200; - #endif - - if ((min / threshold) == (max / threshold) || (mid / threshold) == (max / threshold) || min > max || mid > max) { - type = 0; - #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) - printf("ignored"); // (MIN and MAX) OR (MID and MAX) are close, disable input - #endif - } else { - if ((min / threshold) == (mid / threshold)){ - type = 1; - #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) - printf("a normal pot"); // MIN and MID are close, it's a normal pot - #endif - } else { - type = 2; - #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) - printf("a mid-resting pot"); // it's a mid resting pot - #endif - } - - #ifdef CONTROL_ADC - if ((min + INPUT_MARGIN - ADC_PROTECT_THRESH) > 0 && (max - INPUT_MARGIN + ADC_PROTECT_THRESH) < 4095) { - #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) - printf(" AND protected"); - #endif - beepLong(2); // Indicate protection by a beep - } - #endif - } - - return type; -} - /* * Auto-calibration of the ADC Limits * This function finds the Minimum, Maximum, and Middle for the ADC input @@ -535,33 +483,42 @@ int checkInputType(int16_t min, int16_t mid, int16_t max){ * The Values will be saved to flash. Values are persistent if you flash with platformio. To erase them, make a full chip erase. */ void adcCalibLim(void) { +#ifdef AUTO_CALIBRATION_ENA + calcAvgSpeed(); if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning return; } - #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) +#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) printf("Input calibration started...\r\n"); #endif - readInput(); + readInputRaw(); // Inititalization: MIN = a high value, MAX = a low value - int32_t input1_fixdt = input1 << 16; - int32_t input2_fixdt = input2 << 16; + int32_t input1_fixdt = input1[inIdx].raw << 16; + int32_t input2_fixdt = input2[inIdx].raw << 16; int16_t INPUT1_MIN_temp = MAX_int16_T; int16_t INPUT1_MID_temp = 0; int16_t INPUT1_MAX_temp = MIN_int16_T; int16_t INPUT2_MIN_temp = MAX_int16_T; int16_t INPUT2_MID_temp = 0; int16_t INPUT2_MAX_temp = MIN_int16_T; + int16_t input_margin = 0; uint16_t input_cal_timeout = 0; + + #ifdef CONTROL_ADC + if (inIdx == CONTROL_ADC) { + input_margin = ADC_MARGIN; + } + #endif // Extract MIN, MAX and MID from ADC while the power button is not pressed while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && input_cal_timeout++ < 4000) { // 20 sec timeout - readInput(); - filtLowPass32(input1, FILTER, &input1_fixdt); - filtLowPass32(input2, FILTER, &input2_fixdt); + readInputRaw(); + filtLowPass32(input1[inIdx].raw, FILTER, &input1_fixdt); + filtLowPass32(input2[inIdx].raw, FILTER, &input2_fixdt); INPUT1_MID_temp = (int16_t)(input1_fixdt >> 16);// CLAMP(input1_fixdt >> 16, INPUT1_MIN, INPUT1_MAX); // convert fixed-point to integer INPUT2_MID_temp = (int16_t)(input2_fixdt >> 16);// CLAMP(input2_fixdt >> 16, INPUT2_MIN, INPUT2_MAX); @@ -575,16 +532,16 @@ void adcCalibLim(void) { #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) printf("Input1 is "); #endif - INPUT1_TYP_CAL = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp); - if (INPUT1_TYP_CAL == INPUT1_TYPE || INPUT1_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto) - INPUT1_MIN_CAL = INPUT1_MIN_temp + INPUT_MARGIN; - INPUT1_MID_CAL = INPUT1_MID_temp; - INPUT1_MAX_CAL = INPUT1_MAX_temp - INPUT_MARGIN; + input1[inIdx].typ = checkInputType(INPUT1_MIN_temp, INPUT1_MID_temp, INPUT1_MAX_temp); + if (input1[inIdx].typ == input1[inIdx].typDef || input1[inIdx].typDef == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto) + input1[inIdx].min = INPUT1_MIN_temp + input_margin; + input1[inIdx].mid = INPUT1_MID_temp; + input1[inIdx].max = INPUT1_MAX_temp - input_margin; #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) printf("..OK\r\n"); #endif } else { - INPUT1_TYP_CAL = 0; // Disable input + input1[inIdx].typ = 0; // Disable input #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) printf("..NOK\r\n"); #endif @@ -593,16 +550,16 @@ void adcCalibLim(void) { #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) printf("Input2 is "); #endif - INPUT2_TYP_CAL = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp); - if (INPUT2_TYP_CAL == INPUT2_TYPE || INPUT2_TYPE == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto) - INPUT2_MIN_CAL = INPUT2_MIN_temp + INPUT_MARGIN; - INPUT2_MID_CAL = INPUT2_MID_temp; - INPUT2_MAX_CAL = INPUT2_MAX_temp - INPUT_MARGIN; + input2[inIdx].typ = checkInputType(INPUT2_MIN_temp, INPUT2_MID_temp, INPUT2_MAX_temp); + if (input2[inIdx].typ == input2[inIdx].typDef || input2[inIdx].typDef == 3) { // Accept calibration only if the type is correct OR type was set to 3 (auto) + input2[inIdx].min = INPUT2_MIN_temp + input_margin; + input2[inIdx].mid = INPUT2_MID_temp; + input2[inIdx].max = INPUT2_MAX_temp - input_margin; #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) printf("..OK\r\n"); #endif } else { - INPUT2_TYP_CAL = 0; // Disable input + input2[inIdx].typ = 0; // Disable input #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) printf("..NOK\r\n"); #endif @@ -610,10 +567,12 @@ void adcCalibLim(void) { inp_cal_valid = 1; // Mark calibration to be saved in Flash at shutdown #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) printf("Limits Input1: TYP:%i MIN:%i MID:%i MAX:%i\r\nLimits Input2: TYP:%i MIN:%i MID:%i MAX:%i\r\n", - INPUT1_TYP_CAL, INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL, - INPUT2_TYP_CAL, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL); - #endif + input1[inIdx].typ, input1[inIdx].min, input1[inIdx].mid, input1[inIdx].max, + input2[inIdx].typ, input2[inIdx].min, input2[inIdx].mid, input2[inIdx].max); #endif + +#endif +#endif // AUTO_CALIBRATION_ENA } /* * Update Maximum Motor Current Limit (via ADC1) and Maximum Speed Limit (via ADC2) @@ -623,18 +582,19 @@ void adcCalibLim(void) { * - press the power button to confirm or wait for the 10 sec timeout */ void updateCurSpdLim(void) { + calcAvgSpeed(); if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning return; } - #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) +#if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) printf("Torque and Speed limits update started...\r\n"); #endif - int32_t input1_fixdt = input1 << 16; - int32_t input2_fixdt = input2 << 16; + int32_t input1_fixdt = input1[inIdx].raw << 16; + int32_t input2_fixdt = input2[inIdx].raw << 16; uint16_t cur_factor; // fixdt(0,16,16) uint16_t spd_factor; // fixdt(0,16,16) uint16_t cur_spd_timeout = 0; @@ -642,22 +602,22 @@ void updateCurSpdLim(void) { // Wait for the power button press while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout - readInput(); - filtLowPass32(input1, FILTER, &input1_fixdt); - filtLowPass32(input2, FILTER, &input2_fixdt); + readInputRaw(); + filtLowPass32(input1[inIdx].raw, FILTER, &input1_fixdt); + filtLowPass32(input2[inIdx].raw, FILTER, &input2_fixdt); HAL_Delay(5); } // Calculate scaling factors - cur_factor = CLAMP((input1_fixdt - ((int16_t)INPUT1_MIN_CAL << 16)) / ((int16_t)INPUT1_MAX_CAL - (int16_t)INPUT1_MIN_CAL), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A - spd_factor = CLAMP((input2_fixdt - ((int16_t)INPUT2_MIN_CAL << 16)) / ((int16_t)INPUT2_MAX_CAL - (int16_t)INPUT2_MIN_CAL), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm + cur_factor = CLAMP((input1_fixdt - (input1[inIdx].min << 16)) / (input1[inIdx].max - input1[inIdx].min), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A + spd_factor = CLAMP((input2_fixdt - (input2[inIdx].min << 16)) / (input2[inIdx].max - input2[inIdx].min), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm - if (INPUT1_TYP_CAL != 0){ + if (input1[inIdx].typ != 0){ // Update current limit rtP_Left.i_max = rtP_Right.i_max = (int16_t)((I_MOT_MAX * A2BIT_CONV * cur_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4) cur_spd_valid = 1; // Mark update to be saved in Flash at shutdown } - if (INPUT2_TYP_CAL != 0){ + if (input2[inIdx].typ != 0){ // Update speed limit rtP_Left.n_max = rtP_Right.n_max = (int16_t)((N_MOT_MAX * spd_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4) cur_spd_valid += 2; // Mark update to be saved in Flash at shutdown @@ -668,38 +628,8 @@ void updateCurSpdLim(void) { printf("Limits (%i)\r\nCurrent: fixdt:%li factor%i i_max:%i \r\nSpeed: fixdt:%li factor:%i n_max:%i\r\n", cur_spd_valid, input1_fixdt, cur_factor, rtP_Left.i_max, input2_fixdt, spd_factor, rtP_Left.n_max); #endif - #endif -} - /* - * Save Configuration to Flash - * This function makes sure data is not lost after power-off - */ -void saveConfig() { - #ifdef VARIANT_TRANSPOTTER - if (saveValue_valid) { - HAL_FLASH_Unlock(); - EE_WriteVariable(VirtAddVarTab[0], saveValue); - HAL_FLASH_Lock(); - } - #endif - #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - if (inp_cal_valid || cur_spd_valid) { - HAL_FLASH_Unlock(); - EE_WriteVariable(VirtAddVarTab[0] , FLASH_WRITE_KEY); - EE_WriteVariable(VirtAddVarTab[1] , INPUT1_TYP_CAL); - EE_WriteVariable(VirtAddVarTab[2] , INPUT1_MIN_CAL); - EE_WriteVariable(VirtAddVarTab[3] , INPUT1_MID_CAL); - EE_WriteVariable(VirtAddVarTab[4] , INPUT1_MAX_CAL); - EE_WriteVariable(VirtAddVarTab[5] , INPUT2_TYP_CAL); - EE_WriteVariable(VirtAddVarTab[6] , INPUT2_MIN_CAL); - EE_WriteVariable(VirtAddVarTab[7] , INPUT2_MID_CAL); - EE_WriteVariable(VirtAddVarTab[8] , INPUT2_MAX_CAL); - EE_WriteVariable(VirtAddVarTab[9] , rtP_Left.i_max); - EE_WriteVariable(VirtAddVarTab[10], rtP_Left.n_max); - HAL_FLASH_Lock(); - } - #endif +#endif } /* @@ -712,9 +642,9 @@ void saveConfig() { */ void standstillHold(void) { #if defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE) - if (!rtP_Left.b_cruiseCtrlEna) { // If Stanstill in NOT Active -> try Activation - if (((cmd1 > 50 || cmd2 < -50) && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small - || (cmd2 < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small + if (!rtP_Left.b_cruiseCtrlEna) { // If Stanstill in NOT Active -> try Activation + if (((input1[inIdx].cmd > 50 || input2[inIdx].cmd < -50) && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small + || (input2[inIdx].cmd < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small rtP_Left.n_cruiseMotTgt = 0; rtP_Right.n_cruiseMotTgt = 0; rtP_Left.b_cruiseCtrlEna = 1; @@ -722,8 +652,8 @@ void standstillHold(void) { standstillAcv = 1; } } - else { // If Stanstill is Active -> try Deactivation - if (cmd1 < 20 && cmd2 > 50 && !cruiseCtrlAcv) { // Check if Brake is released AND Throttle is pressed AND no Cruise Control + else { // If Stanstill is Active -> try Deactivation + if (input1[inIdx].cmd < 20 && input2[inIdx].cmd > 50 && !cruiseCtrlAcv) { // Check if Brake is released AND Throttle is pressed AND no Cruise Control rtP_Left.b_cruiseCtrlEna = 0; rtP_Right.b_cruiseCtrlEna = 0; standstillAcv = 0; @@ -738,7 +668,7 @@ void standstillHold(void) { * This is useful when a small amount of motor braking is desired instead of "freewheel". * * Input: speedBlend = fixdt(0,16,15), reverseDir = {0, 1} - * Output: cmd2 (Throtle) with brake component included + * Output: input2.cmd (Throtle) with brake component included */ void electricBrake(uint16_t speedBlend, uint8_t reverseDir) { #if defined(ELECTRIC_BRAKE_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ == TRQ_MODE) @@ -748,7 +678,7 @@ void electricBrake(uint16_t speedBlend, uint8_t reverseDir) { if (speedAvg > 0) { brakeVal = (int16_t)((-ELECTRIC_BRAKE_MAX * speedBlend) >> 15); } else { - brakeVal = (int16_t)(( ELECTRIC_BRAKE_MAX * speedBlend) >> 15); + brakeVal = (int16_t)(( ELECTRIC_BRAKE_MAX * speedBlend) >> 15); } // Check if direction is reversed @@ -756,15 +686,15 @@ void electricBrake(uint16_t speedBlend, uint8_t reverseDir) { brakeVal = -brakeVal; } - // Calculate the new cmd2 with brake component included - if (cmd2 >= 0 && cmd2 < ELECTRIC_BRAKE_THRES) { - cmd2 = MAX(brakeVal, ((ELECTRIC_BRAKE_THRES - cmd2) * brakeVal) / ELECTRIC_BRAKE_THRES); - } else if (cmd2 >= -ELECTRIC_BRAKE_THRES && cmd2 < 0) { - cmd2 = MIN(brakeVal, ((ELECTRIC_BRAKE_THRES + cmd2) * brakeVal) / ELECTRIC_BRAKE_THRES); - } else if (cmd2 >= ELECTRIC_BRAKE_THRES) { - cmd2 = MAX(brakeVal, ((cmd2 - ELECTRIC_BRAKE_THRES) * INPUT_MAX) / (INPUT_MAX - ELECTRIC_BRAKE_THRES)); - } else { // when (cmd2 < -ELECTRIC_BRAKE_THRES) - cmd2 = MIN(brakeVal, ((cmd2 + ELECTRIC_BRAKE_THRES) * INPUT_MIN) / (INPUT_MIN + ELECTRIC_BRAKE_THRES)); + // Calculate the new input2.cmd with brake component included + if (input2[inIdx].cmd >= 0 && input2[inIdx].cmd < ELECTRIC_BRAKE_THRES) { + input2[inIdx].cmd = MAX(brakeVal, ((ELECTRIC_BRAKE_THRES - input2[inIdx].cmd) * brakeVal) / ELECTRIC_BRAKE_THRES); + } else if (input2[inIdx].cmd >= -ELECTRIC_BRAKE_THRES && input2[inIdx].cmd < 0) { + input2[inIdx].cmd = MIN(brakeVal, ((ELECTRIC_BRAKE_THRES + input2[inIdx].cmd) * brakeVal) / ELECTRIC_BRAKE_THRES); + } else if (input2[inIdx].cmd >= ELECTRIC_BRAKE_THRES) { + input2[inIdx].cmd = MAX(brakeVal, ((input2[inIdx].cmd - ELECTRIC_BRAKE_THRES) * INPUT_MAX) / (INPUT_MAX - ELECTRIC_BRAKE_THRES)); + } else { // when (input2.cmd < -ELECTRIC_BRAKE_THRES) + input2[inIdx].cmd = MIN(brakeVal, ((input2[inIdx].cmd + ELECTRIC_BRAKE_THRES) * INPUT_MIN) / (INPUT_MIN + ELECTRIC_BRAKE_THRES)); } #endif } @@ -784,222 +714,337 @@ void cruiseControl(uint8_t button) { rtP_Left.b_cruiseCtrlEna = 1; rtP_Right.b_cruiseCtrlEna = 1; cruiseCtrlAcv = 1; - shortBeepMany(2, 1); // 200 ms beep delay. Acts as a debounce also. + beepShortMany(2, 1); // 200 ms beep delay. Acts as a debounce also. } else if (button && rtP_Left.b_cruiseCtrlEna && !standstillAcv) { // Cruise control deactivated if no Standstill Hold is active rtP_Left.b_cruiseCtrlEna = 0; rtP_Right.b_cruiseCtrlEna = 0; cruiseCtrlAcv = 0; - shortBeepMany(2, -1); + beepShortMany(2, -1); } #endif } + /* + * Check Input Type + * This function identifies the input type: 0: Disabled, 1: Normal Pot, 2: Middle Resting Pot + */ +int checkInputType(int16_t min, int16_t mid, int16_t max){ + int type = 0; + #ifdef CONTROL_ADC + int16_t threshold = 400; // Threshold to define if values are too close + #else + int16_t threshold = 200; + #endif -/* =========================== Poweroff Functions =========================== */ + if ((min / threshold) == (max / threshold) || (mid / threshold) == (max / threshold) || min > max || mid > max) { + type = 0; + #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) + printf("ignored"); // (MIN and MAX) OR (MID and MAX) are close, disable input + #endif + } else { + if ((min / threshold) == (mid / threshold)){ + type = 1; + #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) + printf("a normal pot"); // MIN and MID are close, it's a normal pot + #endif + } else { + type = 2; + #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) + printf("a mid-resting pot"); // it's a mid resting pot + #endif + } -void poweroff(void) { - enable = 0; - #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) - printf("-- Motors disabled --\r\n"); - #endif - buzzerCount = 0; // prevent interraction with beep counter - buzzerPattern = 0; - for (int i = 0; i < 8; i++) { - buzzerFreq = (uint8_t)i; - HAL_Delay(100); + #ifdef CONTROL_ADC + if ((min + ADC_MARGIN - ADC_PROTECT_THRESH) > 0 && (max - ADC_MARGIN + ADC_PROTECT_THRESH) < 4095) { + #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) + printf(" AND protected"); + #endif + beepLong(2); // Indicate protection by a beep + } + #endif } - saveConfig(); - HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET); - while(1) {} + + return type; } -void poweroffPressCheck(void) { - #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - enable = 0; - uint16_t cnt_press = 0; - while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - HAL_Delay(10); - if (cnt_press++ == 5 * 100) { beepShort(5); } - } - if (cnt_press >= 5 * 100) { // Check if press is more than 5 sec - HAL_Delay(1000); - if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { // Double press: Adjust Max Current, Max Speed - while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } - beepLong(8); - updateCurSpdLim(); - beepShort(5); - } else { // Long press: Calibrate ADC Limits - beepLong(16); - adcCalibLim(); - beepShort(5); - } - } else { // Short press: power off - poweroff(); - } - } - #elif defined(VARIANT_TRANSPOTTER) - if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - enable = 0; - while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } - beepShort(5); - HAL_Delay(300); - if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } - beepLong(5); - HAL_Delay(350); - poweroff(); + +/* =========================== 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 { - setDistance += 0.25; - if (setDistance > 2.6) { - setDistance = 0.5; - } - beepShort(setDistance / 0.25); - saveValue = setDistance * 1000; - saveValue_valid = 1; + in->cmd = CLAMP(MAP(in->raw, in->mid - in->dband, in->min, 0, out_min), out_min, 0); } - } - #else - if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - enable = 0; // disable motors - while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released - poweroff(); // release power-latch - } - #endif + break; + default: // Input is ignored + in->cmd = 0; + break; + } } - -/* =========================== Read Functions =========================== */ - /* - * Function to read the raw Input values from various input devices + * Function to read the Input Raw values from various input devices */ -void readInput(void) { +void readInputRaw(void) { + #ifdef CONTROL_ADC + if (inIdx == CONTROL_ADC) { + #ifdef ADC_ALTERNATE_CONNECT + input1[inIdx].raw = adc_buffer.l_rx2; + input2[inIdx].raw = adc_buffer.l_tx2; + #else + input1[inIdx].raw = adc_buffer.l_tx2; + input2[inIdx].raw = adc_buffer.l_rx2; + #endif + } + #endif + #if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) - if (nunchuk_connected != 0) { - Nunchuk_Read(); - input1 = (nunchuk_data[0] - 127) * 8; // X axis 0-255 - input2 = (nunchuk_data[1] - 128) * 8; // Y axis 0-255 - #ifdef SUPPORT_BUTTONS - button1 = (uint8_t)nunchuk_data[5] & 1; - button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1; - #endif + if (nunchuk_connected) { + Nunchuk_Read(); + if (inIdx == CONTROL_NUNCHUK) { + input1[inIdx].raw = (nunchuk_data[0] - 127) * 8; // X axis 0-255 + input2[inIdx].raw = (nunchuk_data[1] - 128) * 8; // Y axis 0-255 } + #ifdef SUPPORT_BUTTONS + button1 = (uint8_t)nunchuk_data[5] & 1; + button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1; + #endif + } #endif - #if defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT) - input1 = (ppm_captured_value[0] - 500) * 2; - input2 = (ppm_captured_value[1] - 500) * 2; - #ifdef SUPPORT_BUTTONS - button1 = ppm_captured_value[5] > 500; - button2 = 0; + #if defined(CONTROL_SERIAL_USART2) + if (inIdx == CONTROL_SERIAL_USART2) { + #ifdef CONTROL_IBUS + for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) { + ibusL_captured_value[(i/2)] = CLAMP(commandL.channels[i] + (commandL.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000 + } + input1[inIdx].raw = (ibusL_captured_value[0] - 500) * 2; + input2[inIdx].raw = (ibusL_captured_value[1] - 500) * 2; + #else + input1[inIdx].raw = commandL.steer; + input2[inIdx].raw = commandL.speed; #endif + } + #endif + #if defined(CONTROL_SERIAL_USART3) + if (inIdx == CONTROL_SERIAL_USART3) { + #ifdef CONTROL_IBUS + for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) { + ibusR_captured_value[(i/2)] = CLAMP(commandR.channels[i] + (commandR.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000 + } + input1[inIdx].raw = (ibusR_captured_value[0] - 500) * 2; + input2[inIdx].raw = (ibusR_captured_value[1] - 500) * 2; + #else + input1[inIdx].raw = commandR.steer; + input2[inIdx].raw = commandR.speed; + #endif + } #endif - #if defined(CONTROL_PWM_LEFT) || defined(CONTROL_PWM_RIGHT) - input1 = (pwm_captured_ch1_value - 500) * 2; - input2 = (pwm_captured_ch2_value - 500) * 2; + #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 (inIdx == SIDEBOARD_SERIAL_USART3) { + input1[inIdx].raw = Sideboard_R.cmd1; + input2[inIdx].raw = Sideboard_R.cmd2; + } #endif - #ifdef CONTROL_ADC - // ADC values range: 0-4095, see ADC-calibration in config.h - input1 = adc_buffer.l_tx2; - input2 = adc_buffer.l_rx2; - timeoutCnt = 0; + #if defined(CONTROL_PPM_LEFT) + if (inIdx == CONTROL_PPM_LEFT) { + input1[inIdx].raw = (ppm_captured_value[0] - 500) * 2; + input2[inIdx].raw = (ppm_captured_value[1] - 500) * 2; + } + #endif + #if defined(CONTROL_PPM_RIGHT) + if (inIdx == CONTROL_PPM_RIGHT) { + input1[inIdx].raw = (ppm_captured_value[0] - 500) * 2; + input2[inIdx].raw = (ppm_captured_value[1] - 500) * 2; + } + #endif + #if (defined(CONTROL_PPM_LEFT) || defined(CONTROL_PPM_RIGHT)) && defined(SUPPORT_BUTTONS) + button1 = ppm_captured_value[5] > 500; + button2 = 0; #endif - #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) - // Handle received data validity, timeout and fix out-of-sync if necessary - #ifdef CONTROL_IBUS - for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) { - ibus_captured_value[(i/2)] = CLAMP(command.channels[i] + (command.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000 - } - input1 = (ibus_captured_value[0] - 500) * 2; - input2 = (ibus_captured_value[1] - 500) * 2; - #else - input1 = command.steer; - input2 = command.speed; + #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 - timeoutCnt = 0; #endif } /* - * Function to calculate the command to the motors. This function also manages: - * - timeout detection - * - MIN/MAX limitations and deadband + * Function to handle the ADC, UART and General timeout (Nunchuk, PPM, PWM) */ -void readCommand(void) { - readInput(); +void handleTimeout(void) { #ifdef CONTROL_ADC + if (inIdx == CONTROL_ADC) { // If input1 or Input2 is either below MIN - Threshold or above MAX + Threshold, ADC protection timeout - if (IN_RANGE(input1, (int16_t)INPUT1_MIN_CAL - ADC_PROTECT_THRESH, (int16_t)INPUT1_MAX_CAL + ADC_PROTECT_THRESH) && - IN_RANGE(input2, (int16_t)INPUT2_MIN_CAL - ADC_PROTECT_THRESH, (int16_t)INPUT2_MAX_CAL + ADC_PROTECT_THRESH)){ - if (timeoutFlagADC) { // Check for previous timeout flag - if (timeoutCntADC-- <= 0) // Timeout de-qualification - timeoutFlagADC = 0; // Timeout flag cleared - } else { - timeoutCntADC = 0; // Reset the timeout counter - } + if (IN_RANGE(input1[inIdx].raw, input1[inIdx].min - ADC_PROTECT_THRESH, input1[inIdx].max + ADC_PROTECT_THRESH) && + IN_RANGE(input2[inIdx].raw, input2[inIdx].min - ADC_PROTECT_THRESH, input2[inIdx].max + ADC_PROTECT_THRESH)) { + timeoutFlgADC = 0; // Reset the timeout flag + timeoutCntADC = 0; // Reset the timeout counter } else { if (timeoutCntADC++ >= ADC_PROTECT_TIMEOUT) { // Timeout qualification - timeoutFlagADC = 1; // Timeout detected - timeoutCntADC = ADC_PROTECT_TIMEOUT; // Limit timout counter value + timeoutFlgADC = 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 + timeoutFlgSerial_L = 1; // Timeout detected + timeoutCntSerial_L = SERIAL_TIMEOUT; // Limit timout counter value + #if defined(DUAL_INPUTS) && ((defined(CONTROL_SERIAL_USART2) && CONTROL_SERIAL_USART2 == 1) || (defined(SIDEBOARD_SERIAL_USART2) && SIDEBOARD_SERIAL_USART2 == 1)) + inIdx = 0; // Switch to Primary input in case of Timeout on Auxiliary input + #endif + } else { // No Timeout + #if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART2) + if (Sideboard_L.sensors & SWA_SET) { // If SWA is set, switch to Sideboard control + inIdx = SIDEBOARD_SERIAL_USART2; + } else { + inIdx = !SIDEBOARD_SERIAL_USART2; + } + #elif defined(DUAL_INPUTS) && (defined(CONTROL_SERIAL_USART2) && CONTROL_SERIAL_USART2 == 1) + inIdx = 1; // Switch to Auxiliary input in case of NO Timeout on Auxiliary input + #endif } - timeoutFlagSerial = timeoutFlagSerial_L; + #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 - timeoutFlagSerial_R = 1; // Timeout detected - timeoutCntSerial_R = SERIAL_TIMEOUT; // Limit timout counter value + timeoutFlgSerial_R = 1; // Timeout detected + timeoutCntSerial_R = SERIAL_TIMEOUT; // Limit timout counter value + #if defined(DUAL_INPUTS) && ((defined(CONTROL_SERIAL_USART3) && CONTROL_SERIAL_USART3 == 1) || (defined(SIDEBOARD_SERIAL_USART3) && SIDEBOARD_SERIAL_USART3 == 1)) + inIdx = 0; // Switch to Primary input in case of Timeout on Auxiliary input + #endif + } else { // No Timeout + #if defined(DUAL_INPUTS) && defined(SIDEBOARD_SERIAL_USART3) + if (Sideboard_R.sensors & SWA_SET) { // If SWA is set, switch to Sideboard control + inIdx = SIDEBOARD_SERIAL_USART3; + } else { + inIdx = !SIDEBOARD_SERIAL_USART3; + } + #elif defined(DUAL_INPUTS) && (defined(CONTROL_SERIAL_USART3) && CONTROL_SERIAL_USART3 == 1) + inIdx = 1; // Switch to Auxiliary input in case of NO Timeout on Auxiliary input + #endif } - timeoutFlagSerial = timeoutFlagSerial_R; + #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) - timeoutFlagSerial = timeoutFlagSerial_L || timeoutFlagSerial_R; + 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) && CONTROL_PPM_LEFT == 1) || (defined(CONTROL_PPM_RIGHT) && CONTROL_PPM_RIGHT == 1) || \ + (defined(CONTROL_PWM_LEFT) && CONTROL_PWM_LEFT == 1) || (defined(CONTROL_PWM_RIGHT) && CONTROL_PWM_RIGHT == 1)) + inIdx = 0; // Switch to Primary input in case of Timeout on Auxiliary input + #endif + } else { + #if defined(DUAL_INPUTS) && ((defined(CONTROL_PPM_LEFT) && CONTROL_PPM_LEFT == 1) || (defined(CONTROL_PPM_RIGHT) && CONTROL_PPM_RIGHT == 1) || \ + (defined(CONTROL_PWM_LEFT) && CONTROL_PWM_LEFT == 1) || (defined(CONTROL_PWM_RIGHT) && CONTROL_PWM_RIGHT == 1)) + inIdx = 1; // Switch to Auxiliary input in case of NO Timeout on Auxiliary input + #endif + } + #endif + + // In case of timeout bring the system to a Safe State + if (timeoutFlgADC || timeoutFlgSerial || timeoutFlgGen) { + 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 + } + + // Beep in case of Input index change + if (inIdx && !inIdx_prev) { // rising edge + beepShort(8); + } else if (!inIdx && inIdx_prev) { // falling edge + beepShort(18); + } +} + + /* + * Function to calculate the command to the motors. This function also manages: + * - timeout detection + * - MIN/MAX limitations and deadband + */ +void readCommand(void) { + readInputRaw(); + #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) - cmd1 = addDeadBand(input1, INPUT1_TYP_CAL, INPUT1_DEADBAND, INPUT1_MIN_CAL, INPUT1_MID_CAL, INPUT1_MAX_CAL, INPUT_MIN, INPUT_MAX); + calcInputCmd(&input1[inIdx], INPUT_MIN, INPUT_MAX); #if !defined(VARIANT_SKATEBOARD) - cmd2 = addDeadBand(input2, INPUT2_TYP_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT_MIN, INPUT_MAX); - #else - cmd2 = addDeadBand(input2, INPUT2_TYP_CAL, INPUT2_DEADBAND, INPUT2_MIN_CAL, INPUT2_MID_CAL, INPUT2_MAX_CAL, INPUT2_BRAKE, INPUT_MAX); + calcInputCmd(&input2[inIdx], INPUT_MIN, INPUT_MAX); + #else + calcInputCmd(&input2[inIdx], INPUT_BRK, INPUT_MAX); #endif #endif - #ifdef VARIANT_TRANSPOTTER - #ifdef GAMETRAK_CONNECTION_NORMAL - cmd1 = adc_buffer.l_rx2; - cmd2 = adc_buffer.l_tx2; - #endif - #ifdef GAMETRAK_CONNECTION_ALTERNATE - cmd1 = adc_buffer.l_tx2; - cmd2 = adc_buffer.l_rx2; - #endif - #endif + handleTimeout(); #ifdef VARIANT_HOVERCAR - brakePressed = (uint8_t)(cmd1 > 50); - #endif - - if (timeoutFlagADC || timeoutFlagSerial || timeoutCnt > TIMEOUT) { // In case of timeout bring the system to a Safe State - ctrlModReq = OPEN_MODE; // Request OPEN_MODE. This will bring the motor power to 0 in a controlled way - cmd1 = 0; - cmd2 = 0; - } else { - ctrlModReq = ctrlModReqRaw; // Follow the Mode request + if (inIdx == CONTROL_ADC) { + brakePressed = (uint8_t)(input1[inIdx].cmd > 50); + } + else { + brakePressed = (uint8_t)(input2[inIdx].cmd < -50); } + #endif #if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT) button1 = !HAL_GPIO_ReadPin(BUTTON1_PORT, BUTTON1_PIN); @@ -1008,7 +1053,7 @@ void readCommand(void) { #if defined(CRUISE_CONTROL_SUPPORT) && (defined(SUPPORT_BUTTONS) || defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT)) cruiseControl(button1); // Cruise control activation/deactivation - #endif + #endif } @@ -1025,14 +1070,16 @@ void usart2_rx_check(void) #endif #if defined(DEBUG_SERIAL_USART2) + uint8_t ptr[SERIAL_BUFFER_SIZE]; if (pos != old_pos) { // Check change in received data if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one usart_process_debug(&rx_buffer_L[old_pos], pos - old_pos); // Process data } else { // "Overflow" buffer mode - usart_process_debug(&rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First Process data from the end of buffer + memcpy(&ptr[0], &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 - usart_process_debug(&rx_buffer_L[0], pos); // Process remaining data + memcpy(&ptr[rx_buffer_L_len - old_pos], &rx_buffer_L[0], pos); // Copy remaining data } + usart_process_debug(ptr, rx_buffer_L_len - old_pos + pos); // Process data } } #endif // DEBUG_SERIAL_USART2 @@ -1040,17 +1087,17 @@ void usart2_rx_check(void) #ifdef CONTROL_SERIAL_USART2 uint8_t *ptr; if (pos != old_pos) { // Check change in received data - ptr = (uint8_t *)&command_raw; // Initialize the pointer with command_raw address - if (pos > old_pos && (pos - old_pos) == command_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length - memcpy(ptr, &rx_buffer_L[old_pos], command_len); // Copy data. This is possible only if command_raw is contiguous! (meaning all the structure members have the same size) - usart_process_command(&command_raw, &command, 2); // Process data - } else if ((rx_buffer_L_len - old_pos + pos) == command_len) { // "Overflow" buffer mode: check if data length equals expected length + ptr = (uint8_t *)&commandL_raw; // Initialize the pointer with command_raw address + if (pos > old_pos && (pos - old_pos) == commandL_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length + memcpy(ptr, &rx_buffer_L[old_pos], commandL_len); // Copy data. This is possible only if command_raw is contiguous! (meaning all the structure members have the same size) + usart_process_command(&commandL_raw, &commandL, 2); // Process data + } else if ((rx_buffer_L_len - old_pos + pos) == commandL_len) { // "Overflow" buffer mode: check if data length equals expected length memcpy(ptr, &rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First copy data from the end of buffer if (pos > 0) { // Check and continue with beginning of buffer ptr += rx_buffer_L_len - old_pos; // Move to correct position in command_raw memcpy(ptr, &rx_buffer_L[0], pos); // Copy remaining data } - usart_process_command(&command_raw, &command, 2); // Process data + usart_process_command(&commandL_raw, &commandL, 2); // Process data } } #endif // CONTROL_SERIAL_USART2 @@ -1095,32 +1142,35 @@ void usart3_rx_check(void) #endif #if defined(DEBUG_SERIAL_USART3) + uint8_t ptr[SERIAL_BUFFER_SIZE]; + if (pos != old_pos) { // Check change in received data if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one usart_process_debug(&rx_buffer_R[old_pos], pos - old_pos); // Process data } else { // "Overflow" buffer mode - usart_process_debug(&rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First Process data from the end of buffer + memcpy(&ptr[0], &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 - usart_process_debug(&rx_buffer_R[0], pos); // Process remaining data + memcpy(&ptr[rx_buffer_R_len - old_pos], &rx_buffer_R[0], pos); // Copy remaining data } + usart_process_debug(ptr, rx_buffer_R_len - old_pos + pos); // Process data } } #endif // DEBUG_SERIAL_USART3 #ifdef CONTROL_SERIAL_USART3 - uint8_t *ptr; + uint8_t *ptr; if (pos != old_pos) { // Check change in received data - ptr = (uint8_t *)&command_raw; // Initialize the pointer with command_raw address - if (pos > old_pos && (pos - old_pos) == command_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length - memcpy(ptr, &rx_buffer_R[old_pos], command_len); // Copy data. This is possible only if command_raw is contiguous! (meaning all the structure members have the same size) - usart_process_command(&command_raw, &command, 3); // Process data - } else if ((rx_buffer_R_len - old_pos + pos) == command_len) { // "Overflow" buffer mode: check if data length equals expected length + ptr = (uint8_t *)&commandR_raw; // Initialize the pointer with command_raw address + if (pos > old_pos && (pos - old_pos) == commandR_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length + memcpy(ptr, &rx_buffer_R[old_pos], commandR_len); // Copy data. This is possible only if command_raw is contiguous! (meaning all the structure members have the same size) + usart_process_command(&commandR_raw, &commandR, 3); // Process data + } else if ((rx_buffer_R_len - old_pos + pos) == commandR_len) { // "Overflow" buffer mode: check if data length equals expected length memcpy(ptr, &rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First copy data from the end of buffer if (pos > 0) { // Check and continue with beginning of buffer ptr += rx_buffer_R_len - old_pos; // Move to correct position in command_raw memcpy(ptr, &rx_buffer_R[0], pos); // Copy remaining data } - usart_process_command(&command_raw, &command, 3); // Process data + usart_process_command(&commandR_raw, &commandR, 3); // Process data } } #endif // CONTROL_SERIAL_USART3 @@ -1157,13 +1207,11 @@ void usart3_rx_check(void) #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) void usart_process_debug(uint8_t *userCommand, uint32_t len) { - for (; len > 0; len--, userCommand++) { - if (*userCommand != '\n' && *userCommand != '\r') { // Do not accept 'new line' and 'carriage return' commands - printf("Command = %c\r\n", *userCommand); - // handle_input(*userCommand); // -> Create this function to handle the user commands - } - } + #ifdef DEBUG_SERIAL_PROTOCOL + handle_input(userCommand, len); + #endif } + #endif // SERIAL_DEBUG /* @@ -1174,6 +1222,7 @@ void usart_process_debug(uint8_t *userCommand, uint32_t len) void usart_process_command(SerialCommand *command_in, SerialCommand *command_out, uint8_t usart_idx) { #ifdef CONTROL_IBUS + uint16_t ibus_chksum; if (command_in->start == IBUS_LENGTH && command_in->type == IBUS_COMMAND) { ibus_chksum = 0xFFFF - IBUS_LENGTH - IBUS_COMMAND; for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i++) { @@ -1183,13 +1232,13 @@ void usart_process_command(SerialCommand *command_in, SerialCommand *command_out *command_out = *command_in; if (usart_idx == 2) { // Sideboard USART2 #ifdef CONTROL_SERIAL_USART2 - timeoutCntSerial_L = 0; // Reset timeout counter - timeoutFlagSerial_L = 0; // Clear timeout flag + timeoutFlgSerial_L = 0; // Clear timeout flag + timeoutCntSerial_L = 0; // Reset timeout counter #endif } else if (usart_idx == 3) { // Sideboard USART3 #ifdef CONTROL_SERIAL_USART3 - timeoutCntSerial_R = 0; // Reset timeout counter - timeoutFlagSerial_R = 0; // Clear timeout flag + timeoutFlgSerial_R = 0; // Clear timeout flag + timeoutCntSerial_R = 0; // Reset timeout counter #endif } } @@ -1202,13 +1251,13 @@ void usart_process_command(SerialCommand *command_in, SerialCommand *command_out *command_out = *command_in; if (usart_idx == 2) { // Sideboard USART2 #ifdef CONTROL_SERIAL_USART2 - timeoutCntSerial_L = 0; // Reset timeout counter - timeoutFlagSerial_L = 0; // Clear timeout flag + timeoutFlgSerial_L = 0; // Clear timeout flag + timeoutCntSerial_L = 0; // Reset timeout counter #endif } else if (usart_idx == 3) { // Sideboard USART3 #ifdef CONTROL_SERIAL_USART3 - timeoutCntSerial_R = 0; // Reset timeout counter - timeoutFlagSerial_R = 0; // Clear timeout flag + timeoutFlgSerial_R = 0; // Clear timeout flag + timeoutCntSerial_R = 0; // Reset timeout counter #endif } } @@ -1232,12 +1281,12 @@ void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sid if (usart_idx == 2) { // Sideboard USART2 #ifdef SIDEBOARD_SERIAL_USART2 timeoutCntSerial_L = 0; // Reset timeout counter - timeoutFlagSerial_L = 0; // Clear timeout flag + timeoutFlgSerial_L = 0; // Clear timeout flag #endif } else if (usart_idx == 3) { // Sideboard USART3 #ifdef SIDEBOARD_SERIAL_USART3 - timeoutCntSerial_R = 0; // Reset timeout counter - timeoutFlagSerial_R = 0; // Clear timeout flag + timeoutCntSerial_R = 0; // Reset timeout counter + timeoutFlgSerial_R = 0; // Clear timeout flag #endif } } @@ -1284,7 +1333,7 @@ void sideboardLeds(uint8_t *leds) { // Battery Level Indicator: use LED1, LED2, LED3 if (main_loop_counter % BAT_BLINK_INTERVAL == 0) { // | RED (LED1) | YELLOW (LED3) | GREEN (LED2) | if (batVoltage < BAT_DEAD) { // | 0 | 0 | 0 | - *leds &= ~LED1_SET & ~LED3_SET & ~LED2_SET; + *leds &= ~LED1_SET & ~LED3_SET & ~LED2_SET; } else if (batVoltage < BAT_LVL1) { // | B | 0 | 0 | *leds ^= LED1_SET; *leds &= ~LED3_SET & ~LED2_SET; @@ -1313,7 +1362,7 @@ void sideboardLeds(uint8_t *leds) { *leds |= LED1_SET; *leds &= ~LED3_SET & ~LED2_SET; } - if (timeoutFlagADC || timeoutFlagSerial) { + if (timeoutFlgADC || timeoutFlgSerial) { *leds |= LED3_SET; *leds &= ~LED1_SET & ~LED2_SET; } @@ -1327,52 +1376,78 @@ void sideboardLeds(uint8_t *leds) { */ void sideboardSensors(uint8_t sensors) { #if !defined(VARIANT_HOVERBOARD) && (defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3)) - static uint8_t sensor1_prev, sensor2_prev; - uint8_t sensor1_rising_edge, sensor2_rising_edge; - sensor1_rising_edge = (sensors & SENSOR1_SET) && !sensor1_prev; - sensor2_rising_edge = (sensors & SENSOR2_SET) && !sensor2_prev; - sensor1_prev = sensors & SENSOR1_SET; - sensor2_prev = sensors & SENSOR2_SET; - - // Control MODE and Control Type Handling: use Sensor1 as push button - static uint8_t sensor1_index; // holds the press index number for sensor1, when used as a button - if (sensor1_rising_edge) { - sensor1_index++; - if (sensor1_index > 4) { sensor1_index = 0; } + static uint8_t sensor1_index; // holds the press index number for sensor1, when used as a button + static uint8_t sensor1_prev, sensor2_prev; + uint8_t sensor1_trig = 0, sensor2_trig = 0; + #if defined(SIDEBOARD_SERIAL_USART2) + uint8_t sideboardIdx = SIDEBOARD_SERIAL_USART2; + uint16_t sideboardSns = Sideboard_L.sensors; + #else + uint8_t sideboardIdx = SIDEBOARD_SERIAL_USART3; + uint16_t sideboardSns = Sideboard_R.sensors; + #endif + + if (inIdx == sideboardIdx) { // Use Sideboard data + sensor1_index = 2 + ((sideboardSns & SWB_SET) >> 9); // SWB on RC transmitter is used to change Control Type + if (sensor1_index == 2) { // FOC control Type + sensor1_index = (sideboardSns & SWC_SET) >> 11; // SWC on RC transmitter is used to change Control Mode + } + sensor1_trig = sensor1_index != sensor1_prev; // rising or falling edge change detection + if (inIdx != inIdx_prev) { // Force one update at Input idx change + sensor1_trig = 1; + } + sensor1_prev = sensor1_index; + } else { // Use Optical switches + sensor1_trig = (sensors & SENSOR1_SET) && !sensor1_prev; // rising edge detection + sensor2_trig = (sensors & SENSOR2_SET) && !sensor2_prev; // rising edge detection + sensor1_prev = sensors & SENSOR1_SET; + sensor2_prev = sensors & SENSOR2_SET; + } + + // Control MODE and Control Type Handling + if (sensor1_trig) { switch (sensor1_index) { case 0: // FOC VOLTAGE - rtP_Left.z_ctrlTypSel = FOC_CTRL; - rtP_Right.z_ctrlTypSel = FOC_CTRL; - ctrlModReqRaw = VLT_MODE; + rtP_Left.z_ctrlTypSel = rtP_Right.z_ctrlTypSel = FOC_CTRL; + ctrlModReqRaw = VLT_MODE; break; case 1: // FOC SPEED - ctrlModReqRaw = SPD_MODE; + rtP_Left.z_ctrlTypSel = rtP_Right.z_ctrlTypSel = FOC_CTRL; + ctrlModReqRaw = SPD_MODE; break; case 2: // FOC TORQUE - ctrlModReqRaw = TRQ_MODE; + rtP_Left.z_ctrlTypSel = rtP_Right.z_ctrlTypSel = FOC_CTRL; + ctrlModReqRaw = TRQ_MODE; break; case 3: // SINUSOIDAL - rtP_Left.z_ctrlTypSel = SIN_CTRL; - rtP_Right.z_ctrlTypSel = SIN_CTRL; + rtP_Left.z_ctrlTypSel = rtP_Right.z_ctrlTypSel = SIN_CTRL; break; case 4: // COMMUTATION - rtP_Left.z_ctrlTypSel = COM_CTRL; - rtP_Right.z_ctrlTypSel = COM_CTRL; - break; + rtP_Left.z_ctrlTypSel = rtP_Right.z_ctrlTypSel = COM_CTRL; + break; } - beepShortMany(sensor1_index + 1, 1); + if (inIdx == inIdx_prev) { beepShortMany(sensor1_index + 1, 1); } + if (++sensor1_index > 4) { sensor1_index = 0; } } - // Field Weakening: use Sensor2 as push button - #ifdef CRUISE_CONTROL_SUPPORT - if (sensor2_rising_edge) { - cruiseControl(sensor2_rising_edge); + #ifdef CRUISE_CONTROL_SUPPORT // Cruise Control Activation/Deactivation + if (sensor2_trig) { + cruiseControl(sensor2_trig); } - #else - static uint8_t sensor2_index; // holds the press index number for sensor2, when used as a button - if (sensor2_rising_edge) { - sensor2_index++; - if (sensor2_index > 1) { sensor2_index = 0; } + #else // Field Weakening Activation/Deactivation + static uint8_t sensor2_index = 1; // holds the press index number for sensor2, when used as a button + + // Override in case the Sideboard control is Active + if (inIdx == sideboardIdx) { // Use Sideboard data + sensor2_index = (sideboardSns & SWD_SET) >> 13; // SWD on RC transmitter is used to Activate/Deactivate Field Weakening + sensor2_trig = sensor2_index != sensor2_prev; // rising or falling edge change detection + if (inIdx != inIdx_prev) { // Force one update at Input idx change + sensor2_trig = 1; + } + sensor2_prev = sensor2_index; + } + + if (sensor2_trig) { switch (sensor2_index) { case 0: // FW Disabled rtP_Left.b_fieldWeakEna = 0; @@ -1385,7 +1460,8 @@ void sideboardSensors(uint8_t sensors) { Input_Lim_Init(); break; } - beepShortMany(sensor2_index + 1, 1); + if (inIdx == inIdx_prev) { beepShortMany(sensor2_index + 1, 1); } + if (++sensor2_index > 1) { sensor2_index = 0; } } #endif // CRUISE_CONTROL_SUPPORT #endif @@ -1393,6 +1469,118 @@ void sideboardSensors(uint8_t sensors) { +/* =========================== Poweroff Functions =========================== */ + + /* + * Save Configuration to Flash + * This function makes sure data is not lost after power-off + */ +void saveConfig() { + #ifdef VARIANT_TRANSPOTTER + if (saveValue_valid) { + HAL_FLASH_Unlock(); + EE_WriteVariable(VirtAddVarTab[0], saveValue); + HAL_FLASH_Lock(); + } + #endif + #if !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) + if (inp_cal_valid || cur_spd_valid) { + HAL_FLASH_Unlock(); + EE_WriteVariable(VirtAddVarTab[0] , (uint16_t)FLASH_WRITE_KEY); + EE_WriteVariable(VirtAddVarTab[1] , (uint16_t)rtP_Left.i_max); + EE_WriteVariable(VirtAddVarTab[2] , (uint16_t)rtP_Left.n_max); + for (uint8_t i=0; i= 5 * 100) { // Check if press is more than 5 sec + HAL_Delay(1000); + if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { // Double press: Adjust Max Current, Max Speed + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } + beepLong(8); + updateCurSpdLim(); + beepShort(5); + } else { // Long press: Calibrate ADC Limits + #ifdef AUTO_CALIBRATION_ENA + beepLong(16); + adcCalibLim(); + beepShort(5); + #endif + } + } else if (cnt_press > 8) { // Short press: power off (80 ms debounce) + poweroff(); + } + } + #elif defined(VARIANT_TRANSPOTTER) + if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { + enable = 0; + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } + beepShort(5); + HAL_Delay(300); + if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } + beepLong(5); + HAL_Delay(350); + poweroff(); + } else { + setDistance += 0.25; + if (setDistance > 2.6) { + setDistance = 0.5; + } + beepShort(setDistance / 0.25); + saveValue = setDistance * 1000; + saveValue_valid = 1; + } + } + #else + if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { + enable = 0; // disable motors + while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released + poweroff(); // release power-latch + } + #endif +} + + + /* =========================== Filtering Functions =========================== */ /* Low pass filter fixed-point 32 bits: fixdt(1,32,16) diff --git a/docs/mainboard_pics.pptx b/docs/mainboard_pics.pptx deleted file mode 100644 index 4839f3f..0000000 Binary files a/docs/mainboard_pics.pptx and /dev/null differ diff --git a/docs/pictures/mainboard_pics.pptx b/docs/pictures/mainboard_pics.pptx new file mode 100644 index 0000000..c22f015 Binary files /dev/null and b/docs/pictures/mainboard_pics.pptx differ diff --git a/docs/motor_winding.pptx b/docs/pictures/motor_winding.pptx similarity index 100% rename from docs/motor_winding.pptx rename to docs/pictures/motor_winding.pptx diff --git a/docs/phase_currents_ADC.pptx b/docs/pictures/phase_currents_ADC.pptx similarity index 100% rename from docs/phase_currents_ADC.pptx rename to docs/pictures/phase_currents_ADC.pptx diff --git a/docs/pictures/wiki_hovercar/hovercar_pic.png b/docs/pictures/wiki_hovercar/hovercar_pic.png new file mode 100644 index 0000000..1468f37 Binary files /dev/null and b/docs/pictures/wiki_hovercar/hovercar_pic.png differ diff --git a/docs/pictures/wiki_hovercar/hovercar_rc.png b/docs/pictures/wiki_hovercar/hovercar_rc.png new file mode 100644 index 0000000..82d870e Binary files /dev/null and b/docs/pictures/wiki_hovercar/hovercar_rc.png differ diff --git a/docs/pictures/wiki_hovercar/hovercar_schematic.png b/docs/pictures/wiki_hovercar/hovercar_schematic.png new file mode 100644 index 0000000..7ced5b9 Binary files /dev/null and b/docs/pictures/wiki_hovercar/hovercar_schematic.png differ diff --git a/docs/pictures/unlock_mcu_1.png b/docs/pictures/wiki_unlock/unlock_mcu_1.png similarity index 100% rename from docs/pictures/unlock_mcu_1.png rename to docs/pictures/wiki_unlock/unlock_mcu_1.png diff --git a/docs/pictures/unlock_mcu_2.png b/docs/pictures/wiki_unlock/unlock_mcu_2.png similarity index 100% rename from docs/pictures/unlock_mcu_2.png rename to docs/pictures/wiki_unlock/unlock_mcu_2.png diff --git a/docs/pictures/unlock_mcu_3.png b/docs/pictures/wiki_unlock/unlock_mcu_3.png similarity index 100% rename from docs/pictures/unlock_mcu_3.png rename to docs/pictures/wiki_unlock/unlock_mcu_3.png diff --git a/docs/pictures/unlock_mcu_4.png b/docs/pictures/wiki_unlock/unlock_mcu_4.png similarity index 100% rename from docs/pictures/unlock_mcu_4.png rename to docs/pictures/wiki_unlock/unlock_mcu_4.png