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

changes for independant left and right speed serial control

This commit is contained in:
interfisch 2019-11-30 21:42:32 +01:00
parent 1841257078
commit 7e670e06d2
5 changed files with 95 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
Inc/config.h
View File

@ -61,11 +61,11 @@
* Enable warning and/or poweroff and make and flash firmware. * Enable warning and/or poweroff and make and flash firmware.
*/ */
#define TEMP_FILT_COEF 655 // temperature filter coefficient in fixed-point. coef_fixedPoint = coef_floatingPoint * 2^16. In this case 655 = 0.01 * 2^16 #define TEMP_FILT_COEF 655 // temperature filter coefficient in fixed-point. coef_fixedPoint = coef_floatingPoint * 2^16. In this case 655 = 0.01 * 2^16
#define TEMP_CAL_LOW_ADC 1655 // temperature 1: ADC value #define TEMP_CAL_LOW_ADC 1716 // temperature 1: ADC value
#define TEMP_CAL_LOW_DEG_C 358 // temperature 1: measured temperature [°C * 10]. Here 35.8 °C #define TEMP_CAL_LOW_DEG_C 210 // temperature 1: measured temperature [°C * 10]. Here 35.8 °C
#define TEMP_CAL_HIGH_ADC 1588 // temperature 2: ADC value #define TEMP_CAL_HIGH_ADC 1600 // temperature 2: ADC value
#define TEMP_CAL_HIGH_DEG_C 489 // temperature 2: measured temperature [°C * 10]. Here 48.9 °C #define TEMP_CAL_HIGH_DEG_C 420 // temperature 2: measured temperature [°C * 10]. Here 48.9 °C
#define TEMP_WARNING_ENABLE 0 // to beep or not to beep, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION! #define TEMP_WARNING_ENABLE 1 // to beep or not to beep, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION!
#define TEMP_WARNING 600 // annoying fast beeps [°C * 10]. Here 60.0 °C #define TEMP_WARNING 600 // annoying fast beeps [°C * 10]. Here 60.0 °C
#define TEMP_POWEROFF_ENABLE 0 // to poweroff or not to poweroff, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION! #define TEMP_POWEROFF_ENABLE 0 // to poweroff or not to poweroff, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION!
#define TEMP_POWEROFF 650 // overheat poweroff. (while not driving) [°C * 10]. Here 65.0 °C #define TEMP_POWEROFF 650 // overheat poweroff. (while not driving) [°C * 10]. Here 65.0 °C
@ -91,15 +91,15 @@
#define USART2_BAUD 38400 // UART2 baud rate (long wired cable) #define USART2_BAUD 38400 // UART2 baud rate (long wired cable)
#define USART2_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B #define USART2_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
// #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino
// #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
// #define DEBUG_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! // #define DEBUG_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
#define USART3_BAUD 38400 // UART3 baud rate (short wired cable) #define USART3_BAUD 38400 // UART3 baud rate (short wired cable)
#define USART3_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B #define USART3_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
// #define CONTROL_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used! For Arduino control check the hoverSerial.ino // #define CONTROL_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used! For Arduino control check the hoverSerial.ino
// #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used! // #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used! //#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
#if defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) #if defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)
#define UART_DMA_CHANNEL DMA1_Channel7 #define UART_DMA_CHANNEL DMA1_Channel7
@ -122,7 +122,7 @@
* For middle resting potis: Let the potis in the middle resting position, write value 1 to ADC1_MID and value 2 to ADC2_MID * For middle resting potis: Let the potis in the middle resting position, write value 1 to ADC1_MID and value 2 to ADC2_MID
* Make, flash and test it. * Make, flash and test it.
*/ */
#define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! //#define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
#define ADC1_MID_POT // ADC1 middle resting poti: comment-out if NOT a middle resting poti #define ADC1_MID_POT // ADC1 middle resting poti: comment-out if NOT a middle resting poti
#define ADC2_MID_POT // ADC2 middle resting poti: comment-out if NOT a middle resting poti #define ADC2_MID_POT // ADC2 middle resting poti: comment-out if NOT a middle resting poti
#define ADC1_MIN 0 // min ADC1-value while poti at minimum-position (0 - 4095) #define ADC1_MIN 0 // min ADC1-value while poti at minimum-position (0 - 4095)
@ -173,7 +173,7 @@
* - button1 and button2: digital input values. 0 or 1 * - button1 and button2: digital input values. 0 or 1
* - adc_buffer.l_tx2 and adc_buffer.l_rx2: unfiltered ADC values (you do not need them). 0 to 4095 * - adc_buffer.l_tx2 and adc_buffer.l_rx2: unfiltered ADC values (you do not need them). 0 to 4095
* Outputs: * Outputs:
* - speedR and speedL: normal driving -1000 to 1000 * - speedR and speedL: normal driving -1000 to 1000
*/ */
// Value of RATE is in fixdt(1,16,4): VAL_fixedPoint = VAL_floatingPoint * 2^4. In this case 480 = 30 * 2^4 // Value of RATE is in fixdt(1,16,4): VAL_fixedPoint = VAL_floatingPoint * 2^4. In this case 480 = 30 * 2^4
@ -185,8 +185,8 @@
// Value of COEFFICIENT is in fixdt(1,16,14) // Value of COEFFICIENT is in fixdt(1,16,14)
// If VAL_floatingPoint >= 0, VAL_fixedPoint = VAL_floatingPoint * 2^14 // If VAL_floatingPoint >= 0, VAL_fixedPoint = VAL_floatingPoint * 2^14
// If VAL_floatingPoint < 0, VAL_fixedPoint = 2^16 + floor(VAL_floatingPoint * 2^14). // If VAL_floatingPoint < 0, VAL_fixedPoint = 2^16 + floor(VAL_floatingPoint * 2^14).
#define SPEED_COEFFICIENT 16384 // 1.0f [-] higher value == stronger. [0, 65535] = [-2.0, 2.0]. In this case 16384 = 1.0 * 2^14 #define SPEED_COEFFICIENT 16384 // 1.0f [-] higher value == stronger. [0, 65535] = [-2.0, 2.0]. In this case 16384 = 1.0 * 2^14
#define STEER_COEFFICIENT 8192 // 0.5f [-] higher value == stronger. [0, 65535] = [-2.0, 2.0]. In this case 8192 = 0.5 * 2^14. If you do not want any steering, set it to 0. #define STEER_COEFFICIENT 8192 // 0.5f [-] higher value == stronger. [0, 65535] = [-2.0, 2.0]. In this case 8192 = 0.5 * 2^14. If you do not want any steering, set it to 0.
#define INVERT_R_DIRECTION #define INVERT_R_DIRECTION
#define INVERT_L_DIRECTION #define INVERT_L_DIRECTION
@ -194,7 +194,7 @@
// ###### SIMPLE BOBBYCAR ###### // ###### SIMPLE BOBBYCAR ######
// for better bobbycar code see: https://github.com/larsmm/hoverboard-firmware-hack-bbcar // for better bobbycar code see: https://github.com/larsmm/hoverboard-firmware-hack-bbcar
// #define FILTER 6553 // 0.1f // #define FILTER 6553 // 0.1f
// #define SPEED_COEFFICIENT 49152 // -1.0f // #define SPEED_COEFFICIENT 49152 // -1.0f
// #define STEER_COEFFICIENT 0 // 0.0f // #define STEER_COEFFICIENT 0 // 0.0f

2
Makefile
View File

@ -52,7 +52,7 @@ startup_stm32f103xe.s
####################################### #######################################
# binaries # binaries
####################################### #######################################
PREFIX = arm-none-eabi- PREFIX = ~/gcc-arm-none-eabi-7-2018-q2-update/bin/arm-none-eabi-
CC = $(PREFIX)gcc CC = $(PREFIX)gcc
AS = $(PREFIX)gcc -x assembler-with-cpp AS = $(PREFIX)gcc -x assembler-with-cpp
CP = $(PREFIX)objcopy CP = $(PREFIX)objcopy

138
Src/main.c
View File

@ -69,8 +69,10 @@ static UART_HandleTypeDef huart;
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
typedef struct{ typedef struct{
uint16_t start; uint16_t start;
int16_t steer; //int16_t steer;
int16_t speed; int16_t speedLeft;
//int16_t speed;
int16_t speedRight;
uint16_t checksum; uint16_t checksum;
} Serialcommand; } Serialcommand;
static volatile Serialcommand command; static volatile Serialcommand command;
@ -103,12 +105,18 @@ uint8_t ctrlModReqRaw = CTRL_MOD_REQ;
uint8_t ctrlModReq; // Final control mode request uint8_t ctrlModReq; // Final control mode request
static int cmd1; // normalized input value. -1000 to 1000 static int cmd1; // normalized input value. -1000 to 1000
static int cmd2; // normalized input value. -1000 to 1000 static int cmd2; // normalized input value. -1000 to 1000
static int16_t steer; // local variable for steering. -1000 to 1000 //static int16_t steer; // local variable for steering. -1000 to 1000
static int16_t speed; // local variable for speed. -1000 to 1000 //static int16_t speed; // local variable for speed. -1000 to 1000
static int16_t steerFixdt; // local fixed-point variable for steering low-pass filter //static int16_t steerFixdt; // local fixed-point variable for steering low-pass filter
static int16_t speedFixdt; // local fixed-point variable for speed low-pass filter //static int16_t speedFixdt; // local fixed-point variable for speed low-pass filter
static int16_t steerRateFixdt; // local fixed-point variable for steering rate limiter static int16_t speedLeftFixdt; // local fixed-point variable for speedLeft low-pass filter
static int16_t speedRateFixdt; // local fixed-point variable for speed rate limiter static int16_t speedRightFixdt; // local fixed-point variable for speedRight low-pass filter
//static int16_t steerRateFixdt; // local fixed-point variable for steering rate limiter
//static int16_t speedRateFixdt; // local fixed-point variable for speed rate limiter
static int16_t speedLeftRateFixdt; // local fixed-point variable for steering rate limiter
static int16_t speedRightRateFixdt; // local fixed-point variable for speed rate limiter
static int16_t speed; // local variable for speed. -1000 to 1000. only used for security checks. will be calculated by speedL and speedR
extern volatile int pwml; // global variable for pwm left. -1000 to 1000 extern volatile int pwml; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000 extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
@ -180,23 +188,23 @@ int main(void) {
// Matlab Init // Matlab Init
// ############################################################################### // ###############################################################################
/* Set BLDC controller parameters */ /* Set BLDC controller parameters */
rtP_Right = rtP_Left; // Copy the Left motor parameters to the Right motor parameters rtP_Right = rtP_Left; // Copy the Left motor parameters to the Right motor parameters
rtP_Left.b_selPhaABCurrMeas = 1; // Left motor measured current phases = {iA, iB} -> do NOT change rtP_Left.b_selPhaABCurrMeas = 1; // Left motor measured current phases = {iA, iB} -> do NOT change
rtP_Left.z_ctrlTypSel = CTRL_TYP_SEL; rtP_Left.z_ctrlTypSel = CTRL_TYP_SEL;
rtP_Left.b_diagEna = DIAG_ENA; rtP_Left.b_diagEna = DIAG_ENA;
rtP_Left.b_fieldWeakEna = FIELD_WEAK_ENA; rtP_Left.b_fieldWeakEna = FIELD_WEAK_ENA;
rtP_Left.i_max = I_MOT_MAX; rtP_Left.i_max = I_MOT_MAX;
rtP_Left.n_max = N_MOT_MAX; rtP_Left.n_max = N_MOT_MAX;
rtP_Right.b_selPhaABCurrMeas = 0; // Left motor measured current phases = {iB, iC} -> do NOT change rtP_Right.b_selPhaABCurrMeas = 0; // Left motor measured current phases = {iB, iC} -> do NOT change
rtP_Right.z_ctrlTypSel = CTRL_TYP_SEL; rtP_Right.z_ctrlTypSel = CTRL_TYP_SEL;
rtP_Right.b_diagEna = DIAG_ENA; rtP_Right.b_diagEna = DIAG_ENA;
rtP_Right.b_fieldWeakEna = FIELD_WEAK_ENA; rtP_Right.b_fieldWeakEna = FIELD_WEAK_ENA;
rtP_Right.i_max = I_MOT_MAX; rtP_Right.i_max = I_MOT_MAX;
rtP_Right.n_max = N_MOT_MAX; rtP_Right.n_max = N_MOT_MAX;
/* Pack LEFT motor data into RTM */ /* Pack LEFT motor data into RTM */
rtM_Left->defaultParam = &rtP_Left; rtM_Left->defaultParam = &rtP_Left;
@ -299,18 +307,18 @@ int main(void) {
#ifdef CONTROL_ADC #ifdef CONTROL_ADC
// ADC values range: 0-4095, see ADC-calibration in config.h // ADC values range: 0-4095, see ADC-calibration in config.h
#ifdef ADC1_MID_POT #ifdef ADC1_MID_POT
cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MID) * 1000 / (ADC1_MAX - ADC1_MID), 0, 1000) cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MID) * 1000 / (ADC1_MAX - ADC1_MID), 0, 1000)
-CLAMP((ADC1_MID - adc_buffer.l_tx2) * 1000 / (ADC1_MID - ADC1_MIN), 0, 1000); // ADC1 -CLAMP((ADC1_MID - adc_buffer.l_tx2) * 1000 / (ADC1_MID - ADC1_MIN), 0, 1000); // ADC1
#else #else
cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN) * 1000 / (ADC1_MAX - ADC1_MIN), 0, 1000); // ADC1 cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN) * 1000 / (ADC1_MAX - ADC1_MIN), 0, 1000); // ADC1
#endif #endif
#ifdef ADC2_MID_POT #ifdef ADC2_MID_POT
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MID) * 1000 / (ADC2_MAX - ADC2_MID), 0, 1000) cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MID) * 1000 / (ADC2_MAX - ADC2_MID), 0, 1000)
-CLAMP((ADC2_MID - adc_buffer.l_rx2) * 1000 / (ADC2_MID - ADC2_MIN), 0, 1000); // ADC2 -CLAMP((ADC2_MID - adc_buffer.l_rx2) * 1000 / (ADC2_MID - ADC2_MIN), 0, 1000); // ADC2
#else #else
cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN) * 1000 / (ADC2_MAX - ADC2_MIN), 0, 1000); // ADC2 cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN) * 1000 / (ADC2_MAX - ADC2_MIN), 0, 1000); // ADC2
#endif #endif
// use ADCs as button inputs: // use ADCs as button inputs:
button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1 button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1
@ -322,16 +330,30 @@ int main(void) {
#if defined CONTROL_SERIAL_USART2 || defined CONTROL_SERIAL_USART3 #if defined CONTROL_SERIAL_USART2 || defined CONTROL_SERIAL_USART3
// Handle received data validity, timeout and fix out-of-sync if necessary // Handle received data validity, timeout and fix out-of-sync if necessary
if (command.start == START_FRAME && command.checksum == (command.start ^ command.steer ^ command.speed)) { //if (command.start == START_FRAME && command.checksum == (command.start ^ command.steer ^ command.speed)) {
if (timeoutFlag) { // Check for previous timeout flag if (command.start == START_FRAME && command.checksum == (command.start ^ command.speedLeft ^ command.speedRight)) {
if (timeoutFlag) { // Check for previous timeout flag
if (timeoutCnt-- <= 0) // Timeout de-qualification if (timeoutCnt-- <= 0) // Timeout de-qualification
timeoutFlag = 0; // Timeout flag cleared timeoutFlag = 0; // Timeout flag cleared
} else { } else {
cmd1 = CLAMP((int16_t)command.steer, -1000, 1000); //cmd1 = CLAMP((int16_t)command.steer, -1000, 1000);
cmd2 = CLAMP((int16_t)command.speed, -1000, 1000); cmd1 = CLAMP((int16_t)command.speedLeft, -1000, 1000);
//cmd2 = CLAMP((int16_t)command.speed, -1000, 1000);
cmd2 = CLAMP((int16_t)command.speedRight, -1000, 1000);
command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle
timeoutCnt = 0; // Reset the timeout counter timeoutCnt = 0; // Reset the timeout counter
} }
// ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
if (enable == 0 && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){
buzzerPattern = 0;
buzzerFreq = 6; HAL_Delay(100); // make 2 beeps indicating the motor enable
buzzerFreq = 4; HAL_Delay(200);
buzzerFreq = 0;
enable = 1; // enable motors
consoleLog("-- Motors enabled --\r\n");
}
} else { } else {
if (timeoutCnt++ >= SERIAL_TIMEOUT) { // Timeout qualification if (timeoutCnt++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlag = 1; // Timeout detected timeoutFlag = 1; // Timeout detected
@ -340,10 +362,10 @@ int main(void) {
// Check the received Start Frame. If it is NOT OK, most probably we are out-of-sync. // Check the received Start Frame. If it is NOT OK, most probably we are out-of-sync.
// Try to re-sync by reseting the DMA // Try to re-sync by reseting the DMA
if (command.start != START_FRAME && command.start != 0xFFFF) { if (command.start != START_FRAME && command.start != 0xFFFF) {
HAL_UART_DMAStop(&huart); HAL_UART_DMAStop(&huart);
HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command)); HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command));
} }
} }
if (timeoutFlag) { // In case of timeout bring the system to a Safe State if (timeoutFlag) { // In case of timeout bring the system to a Safe State
ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way
@ -357,28 +379,30 @@ int main(void) {
#endif #endif
// ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
if (enable == 0 && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){
buzzerPattern = 0;
buzzerFreq = 6; HAL_Delay(100); // make 2 beeps indicating the motor enable
buzzerFreq = 4; HAL_Delay(200);
buzzerFreq = 0;
enable = 1; // enable motors
consoleLog("-- Motors enabled --\r\n");
}
// ####### LOW-PASS FILTER ####### // ####### LOW-PASS FILTER #######
/*
rateLimiter16(cmd1, RATE, &steerRateFixdt); rateLimiter16(cmd1, RATE, &steerRateFixdt);
rateLimiter16(cmd2, RATE, &speedRateFixdt); rateLimiter16(cmd2, RATE, &speedRateFixdt);
filtLowPass16(steerRateFixdt >> 4, FILTER, &steerFixdt); filtLowPass16(steerRateFixdt >> 4, FILTER, &steerFixdt);
filtLowPass16(speedRateFixdt >> 4, FILTER, &speedFixdt); filtLowPass16(speedRateFixdt >> 4, FILTER, &speedFixdt);
steer = steerFixdt >> 4; // convert fixed-point to integer steer = steerFixdt >> 4; // convert fixed-point to integer
speed = speedFixdt >> 4; // convert fixed-point to integer speed = speedFixdt >> 4; // convert fixed-point to integer
*/
rateLimiter16(cmd1, RATE, &speedLeftRateFixdt);
rateLimiter16(cmd2, RATE, &speedRightRateFixdt);
filtLowPass16(speedLeftRateFixdt >> 4, FILTER, &speedLeftFixdt);
filtLowPass16(speedRightRateFixdt >> 4, FILTER, &speedRightFixdt);
speedL = speedLeftFixdt >> 4; // convert fixed-point to integer
speedR = speedRightFixdt >> 4; // convert fixed-point to integer
speed = (speedL+speedR)/2;
// ####### MIXER ####### // ####### MIXER #######
// speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000); // speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000);
// speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000); // speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000);
mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above //mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above
// ####### SET OUTPUTS (if the target change is less than +/- 50) ####### // ####### SET OUTPUTS (if the target change is less than +/- 50) #######
if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) { if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) {
@ -434,15 +458,15 @@ int main(void) {
Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC);
Feedback.boardTemp = (int16_t)board_temp_deg_c; Feedback.boardTemp = (int16_t)board_temp_deg_c;
Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR ^ Feedback.speedL Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR ^ Feedback.speedL
^ Feedback.speedR_meas ^ Feedback.speedL_meas ^ Feedback.batVoltage ^ Feedback.boardTemp); ^ Feedback.speedR_meas ^ Feedback.speedL_meas ^ Feedback.batVoltage ^ Feedback.boardTemp);
UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN; UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN;
UART_DMA_CHANNEL->CNDTR = sizeof(Feedback); UART_DMA_CHANNEL->CNDTR = sizeof(Feedback);
UART_DMA_CHANNEL->CMAR = (uint32_t)&Feedback; UART_DMA_CHANNEL->CMAR = (uint32_t)&Feedback;
UART_DMA_CHANNEL->CCR |= DMA_CCR_EN; UART_DMA_CHANNEL->CCR |= DMA_CCR_EN;
} }
#endif #endif
} }
HAL_GPIO_TogglePin(LED_PORT, LED_PIN); HAL_GPIO_TogglePin(LED_PORT, LED_PIN);
// ####### POWEROFF BY POWER-BUTTON ####### // ####### POWEROFF BY POWER-BUTTON #######
@ -543,12 +567,12 @@ void SystemClock_Config(void) {
* Max: 2047.9375 * Max: 2047.9375
* Min: -2048 * Min: -2048
* Res: 0.0625 * Res: 0.0625
* *
* Inputs: u = int16 * Inputs: u = int16
* Outputs: y = fixdt(1,16,4) * Outputs: y = fixdt(1,16,4)
* Parameters: coef = fixdt(0,16,16) = [0,65535U] * Parameters: coef = fixdt(0,16,16) = [0,65535U]
* *
* Example: * Example:
* If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point) * If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point)
* filtLowPass16(u, 52429, &y); * filtLowPass16(u, 52429, &y);
* yint = y >> 4; // the integer output is the fixed-point ouput shifted by 4 bits * yint = y >> 4; // the integer output is the fixed-point ouput shifted by 4 bits
@ -557,7 +581,7 @@ void filtLowPass16(int16_t u, uint16_t coef, int16_t *y)
{ {
int32_t tmp; int32_t tmp;
tmp = (((int16_t)(u << 4) * coef) >> 16) + tmp = (((int16_t)(u << 4) * coef) >> 16) +
(((int32_t)(65535U - coef) * (*y)) >> 16); (((int32_t)(65535U - coef) * (*y)) >> 16);
// Overflow protection // Overflow protection
@ -571,12 +595,12 @@ void filtLowPass16(int16_t u, uint16_t coef, int16_t *y)
* Max: 32767.99998474121 * Max: 32767.99998474121
* Min: -32768 * Min: -32768
* Res: 1.52587890625e-5 * Res: 1.52587890625e-5
* *
* Inputs: u = int32 * Inputs: u = int32
* Outputs: y = fixdt(1,32,16) * Outputs: y = fixdt(1,32,16)
* Parameters: coef = fixdt(0,16,16) = [0,65535U] * Parameters: coef = fixdt(0,16,16) = [0,65535U]
* *
* Example: * Example:
* If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point) * If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point)
* filtLowPass16(u, 52429, &y); * filtLowPass16(u, 52429, &y);
* yint = y >> 16; // the integer output is the fixed-point ouput shifted by 16 bits * yint = y >> 16; // the integer output is the fixed-point ouput shifted by 16 bits
@ -603,7 +627,7 @@ void filtLowPass32(int32_t u, uint16_t coef, int32_t *y)
} }
// =========================================================== // ===========================================================
/* mixerFcn(rtu_speed, rtu_steer, &rty_speedR, &rty_speedL); /* mixerFcn(rtu_speed, rtu_steer, &rty_speedR, &rty_speedL);
* Inputs: rtu_speed, rtu_steer = fixdt(1,16,4) * Inputs: rtu_speed, rtu_steer = fixdt(1,16,4)
* Outputs: rty_speedR, rty_speedL = int16_t * Outputs: rty_speedR, rty_speedL = int16_t
* Parameters: SPEED_COEFFICIENT, STEER_COEFFICIENT = fixdt(0,16,14) * Parameters: SPEED_COEFFICIENT, STEER_COEFFICIENT = fixdt(0,16,14)
@ -617,9 +641,9 @@ void mixerFcn(int16_t rtu_speed, int16_t rtu_steer, int16_t *rty_speedR, int16_t
prodSpeed = (int16_t)((rtu_speed * (int16_t)SPEED_COEFFICIENT) >> 14); prodSpeed = (int16_t)((rtu_speed * (int16_t)SPEED_COEFFICIENT) >> 14);
prodSteer = (int16_t)((rtu_steer * (int16_t)STEER_COEFFICIENT) >> 14); prodSteer = (int16_t)((rtu_steer * (int16_t)STEER_COEFFICIENT) >> 14);
tmp = prodSpeed - prodSteer; tmp = prodSpeed - prodSteer;
tmp = CLAMP(tmp, -32768, 32767); // Overflow protection tmp = CLAMP(tmp, -32768, 32767); // Overflow protection
*rty_speedR = (int16_t)(tmp >> 4); // Convert from fixed-point to int *rty_speedR = (int16_t)(tmp >> 4); // Convert from fixed-point to int
*rty_speedR = CLAMP(*rty_speedR, -1000, 1000); *rty_speedR = CLAMP(*rty_speedR, -1000, 1000);
tmp = prodSpeed + prodSteer; tmp = prodSpeed + prodSteer;
@ -653,4 +677,4 @@ void rateLimiter16(int16_t u, int16_t rate, int16_t *y)
*y = q0 + *y; *y = q0 + *y;
} }
// =========================================================== // ===========================================================

BIN
build/firmware.bin
View File

Binary file not shown.

BIN
build/firmware.elf
View File

Binary file not shown.