diff --git a/CMakeLists.txt b/CMakeLists.txt index da25ab4..62aec4b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,12 +36,18 @@ include_directories( #add_definitions(-DMOTOR_TEST) add_definitions(-DFEATURE_IGNORE_OTHER_MOTOR) +add_definitions(-DGSCHISSENES_HALL) +#add_definitions(-DGSCHISSENE_PWM_FREQ) #add_definitions(-DFEATURE_BUTTON) add_definitions(-DPETERS_PLATINE) -add_definitions(-DHUARN2) +#add_definitions(-DHUARN2) #add_definitions(-DHUARN3) -add_definitions(-DFEATURE_SERIAL_CONTROL) -add_definitions(-DFEATURE_SERIAL_FEEDBACK) +#add_definitions(-DFEATURE_SERIAL_CONTROL) +#add_definitions(-DFEATURE_SERIAL_FEEDBACK) +#add_definitions(-DLOG_TO_SERIAL) +add_definitions(-DFEATURE_CAN) +#add_definitions(-DCAN_LOG_UNKNOWN_ADDR) +add_definitions(-DIS_BACK) add_executable(firmware.elf STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal.c @@ -59,13 +65,16 @@ add_executable(firmware.elf STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_adc.c STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_uart.c STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_dma.c + STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_can.c bobbycar-foc-model/BLDC_controller.h bobbycar-foc-model/BLDC_controller.c bobbycar-foc-model/BLDC_controller_data.c bobbycar-foc-model/rtwtypes.h - bobbycar-protocol/protocol.h + bobbycar-protocol/bobbycar-can.h + bobbycar-protocol/bobbycar-common.h + bobbycar-protocol/bobbycar-serial.h startup_stm32f103xe.s system_stm32f1xx.c diff --git a/bobbycar-protocol b/bobbycar-protocol index 39f76cb..b6f0d6a 160000 --- a/bobbycar-protocol +++ b/bobbycar-protocol @@ -1 +1 @@ -Subproject commit 39f76cb62b32da2ea8d7a7154e1b2854dbf792ab +Subproject commit b6f0d6a185eb211382ce068b6b4d695d9a8b41af diff --git a/config.h b/config.h index 0e698b2..d495c51 100644 --- a/config.h +++ b/config.h @@ -1,17 +1,11 @@ #pragma once -#ifdef PETERS_PLATINE +#ifdef GSCHISSENE_PWM_FREQ #define PWM_FREQ 12000 // PWM frequency in Hz #else #define PWM_FREQ 16000 // PWM frequency in Hz #endif #define DEAD_TIME 48 // PWM deadtime -#ifdef MOTOR_TEST - #define DELAY_IN_MAIN_LOOP 20 -#else - #define DELAY_IN_MAIN_LOOP 5 -#endif -#define TIMEOUT 5 // number of wrong / missing input commands before emergency off #define A2BIT_CONV 50 // A to bit for current conversion on ADC. Example: 1 A = 50, 2 A = 100, etc // ADC conversion time definitions @@ -50,13 +44,7 @@ #define BAT_CALIB_REAL_VOLTAGE 3970 // input voltage measured by multimeter (multiplied by 100). For example 43.00 V * 100 = 4300 #define BAT_CALIB_ADC 1492 // adc-value measured by mainboard (value nr 5 on UART debug output) -#define BAT_CELLS 10 // battery number of cells. Normal Hoverboard battery: 10s -#define BAT_LOW_LVL1_ENABLE 0 // to beep or not to beep, 1 or 0 -#define BAT_LOW_LVL2_ENABLE 1 // to beep or not to beep, 1 or 0 -#define BAT_LOW_LVL1 (360 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // gently beeps at this voltage level. [V*100/cell]. In this case 3.60 V/cell -#define BAT_LOW_LVL2 (350 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // your battery is almost empty. Charge now! [V*100/cell]. In this case 3.50 V/cell -#define BAT_LOW_DEAD (337 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // undervoltage poweroff. (while not driving) [V*100/cell]. In this case 3.37 V/cell - +#define BAT_CELLS 12 // battery number of cells. Normal Hoverboard battery: 10s /* Board overheat detection: the sensor is inside the STM/GD chip. * It is very inaccurate without calibration (up to 45°C). So only enable this funcion after calibration! @@ -70,13 +58,6 @@ #define TEMP_CAL_LOW_DEG_C 358 // temperature 1: measured temperature [°C * 10]. Here 35.8 °C #define TEMP_CAL_HIGH_ADC 1588 // temperature 2: ADC value #define TEMP_CAL_HIGH_DEG_C 489 // 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 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 650 // overheat poweroff. (while not driving) [°C * 10]. Here 65.0 °C - -#define INACTIVITY_TIMEOUT 8 // minutes of not driving until poweroff. it is not very precise. - // ############################### INPUT ############################### diff --git a/defines.h b/defines.h index 67e30e4..604d407 100644 --- a/defines.h +++ b/defines.h @@ -23,33 +23,57 @@ #include "stm32f1xx_hal.h" #ifdef PETERS_PLATINE +#ifdef GSCHISSENES_HALL + #define LEFT_HALL_U_PIN GPIO_PIN_10 + #define LEFT_HALL_V_PIN GPIO_PIN_11 + #define LEFT_HALL_W_PIN GPIO_PIN_12 +#else #define LEFT_HALL_U_PIN GPIO_PIN_12 #define LEFT_HALL_V_PIN GPIO_PIN_11 #define LEFT_HALL_W_PIN GPIO_PIN_10 +#endif #define LEFT_HALL_U_PORT GPIOC #define LEFT_HALL_V_PORT GPIOC #define LEFT_HALL_W_PORT GPIOC +#ifdef GSCHISSENES_HALL + #define RIGHT_HALL_U_PIN GPIO_PIN_7 + #define RIGHT_HALL_V_PIN GPIO_PIN_6 + #define RIGHT_HALL_W_PIN GPIO_PIN_5 +#else #define RIGHT_HALL_U_PIN GPIO_PIN_5 #define RIGHT_HALL_V_PIN GPIO_PIN_6 #define RIGHT_HALL_W_PIN GPIO_PIN_7 +#endif #define RIGHT_HALL_U_PORT GPIOB #define RIGHT_HALL_V_PORT GPIOB #define RIGHT_HALL_W_PORT GPIOB +#else +#ifdef GSCHISSENES_HALL + #define LEFT_HALL_U_PIN GPIO_PIN_7 + #define LEFT_HALL_V_PIN GPIO_PIN_6 + #define LEFT_HALL_W_PIN GPIO_PIN_5 #else #define LEFT_HALL_U_PIN GPIO_PIN_5 #define LEFT_HALL_V_PIN GPIO_PIN_6 #define LEFT_HALL_W_PIN GPIO_PIN_7 +#endif #define LEFT_HALL_U_PORT GPIOB #define LEFT_HALL_V_PORT GPIOB #define LEFT_HALL_W_PORT GPIOB +#ifdef GSCHISSENES_HALL + #define RIGHT_HALL_U_PIN GPIO_PIN_12 + #define RIGHT_HALL_V_PIN GPIO_PIN_11 + #define RIGHT_HALL_W_PIN GPIO_PIN_10 +#else #define RIGHT_HALL_U_PIN GPIO_PIN_10 #define RIGHT_HALL_V_PIN GPIO_PIN_11 #define RIGHT_HALL_W_PIN GPIO_PIN_12 +#endif #define RIGHT_HALL_U_PORT GPIOC #define RIGHT_HALL_V_PORT GPIOC diff --git a/main.cpp b/main.cpp index bbc2c29..fed12a2 100644 --- a/main.cpp +++ b/main.cpp @@ -23,12 +23,21 @@ #include #include +#include +#include #include "stm32f1xx_hal.h" #include "defines.h" #include "config.h" -#include "protocol.h" + +#include "bobbycar-common.h" +#if FEATURE_SERIAL_CONTROL || FEATURE_SERIAL_FEEDBACK +#include "bobbycar-serial.h" +#endif +#ifdef FEATURE_CAN +#include "bobbycar-can.h" +#endif extern "C" { #include "BLDC_controller.h" @@ -36,6 +45,8 @@ extern const P rtP_Left; // default settings defined in BLDC_controller_data.c } namespace { +const P &defaultP{rtP_Left}; + TIM_HandleTypeDef htim_right; TIM_HandleTypeDef htim_left; ADC_HandleTypeDef hadc1; @@ -70,26 +81,100 @@ volatile struct { uint16_t l_rx2; } adc_buffer; +#ifdef FEATURE_CAN +CAN_HandleTypeDef CanHandle; + +/* Definition for CANx clock resources */ +#define CANx CAN1 +#define CANx_CLK_ENABLE() __HAL_RCC_CAN1_CLK_ENABLE() +#define CANx_GPIO_CLK_ENABLE() __HAL_RCC_GPIOB_CLK_ENABLE() + +#define CANx_FORCE_RESET() __HAL_RCC_CAN1_FORCE_RESET() +#define CANx_RELEASE_RESET() __HAL_RCC_CAN1_RELEASE_RESET() + +/* Definition for CANx Pins */ +#define CANx_TX_PIN GPIO_PIN_9 +#define CANx_TX_GPIO_PORT GPIOB +#define CANx_RX_PIN GPIO_PIN_8 +#define CANx_RX_GPIO_PORT GPIOB + +/* Definition for CANx AFIO Remap */ +#define CANx_AFIO_REMAP_CLK_ENABLE() __HAL_RCC_AFIO_CLK_ENABLE() +#define CANx_AFIO_REMAP_RX_TX_PIN() __HAL_AFIO_REMAP_CAN1_2() + +/* Definition for CAN's NVIC */ +#define CANx_RX_IRQn USB_LP_CAN1_RX0_IRQn +#define CANx_RX_IRQHandler USB_LP_CAN1_RX0_IRQHandler +#define CANx_TX_IRQn USB_HP_CAN1_TX_IRQn +#define CANx_TX_IRQHandler USB_HP_CAN1_TX_IRQHandler +#endif + +#ifdef LOG_TO_SERIAL +char logBuffer[512]; +#endif + +constexpr bool isBackBoard = +#ifdef IS_BACK + true +#else + false +#endif + ; + +template +void myPrintf(const char (&format)[formatLength], Targs ... args) +{ +#ifdef LOG_TO_SERIAL +#ifdef HUARN2 +#define UART_DMA_CHANNEL DMA1_Channel7 +#endif +#ifdef HUARN3 +#define UART_DMA_CHANNEL DMA1_Channel2 +#endif + + while (UART_DMA_CHANNEL->CNDTR != 0); + + char processedFormat[formatLength+2]; + std::copy(std::begin(format), std::end(format), std::begin(processedFormat)); + processedFormat[formatLength-1] = '\r'; + processedFormat[formatLength] = '\n'; + processedFormat[formatLength+1] = '\0'; + + const auto size = std::snprintf(logBuffer, sizeof(logBuffer), processedFormat, args ...); + if (size < 0) + return; + + UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN; + UART_DMA_CHANNEL->CNDTR = size; + UART_DMA_CHANNEL->CMAR = uint64_t(logBuffer); + UART_DMA_CHANNEL->CCR |= DMA_CCR_EN; +#endif +} + // ############################################################################### std::atomic timeout; + +#ifdef MOTOR_TEST +int pwm = 0; +int8_t dir = 1; +#endif + +#ifdef FEATURE_SERIAL_CONTROL int16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial command +Command command; +Feedback feedback; +#endif + +#ifdef FEATURE_CAN +std::atomic timeoutCntLeft = 0; +std::atomic timeoutCntRight = 0; +#endif + uint32_t main_loop_counter; -uint16_t offsetcount = 0; -int16_t offsetrl1 = 2000; -int16_t offsetrl2 = 2000; -int16_t offsetrr1 = 2000; -int16_t offsetrr2 = 2000; -int16_t offsetdcl = 2000; -int16_t offsetdcr = 2000; - int16_t batVoltage = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE; -int32_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 20; // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point - -int32_t board_temp_adcFixdt = adc_buffer.temp << 20; // 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; struct { @@ -99,24 +184,22 @@ struct { ExtU rtU; /* External inputs */ ExtY rtY; /* External outputs */ - MotorState state; + std::atomic enable{true}; + std::atomic iDcMax{7}; + std::atomic chops{}; - uint32_t chops = 0; + uint8_t hallBits() const + { + return (rtU.b_hallA ? 0 : 1) | (rtU.b_hallB ? 0 : 2) | (rtU.b_hallC ? 0 : 4); + } } left, right; struct { - BuzzerState state; - + uint8_t freq = 0; + uint8_t pattern = 0; uint32_t timer = 0; } buzzer; -Command command; -Feedback feedback; - - - -void filtLowPass32(int16_t u, uint16_t coef, int32_t *y); - void SystemClock_Config(); #ifdef HUARN2 @@ -127,6 +210,10 @@ void UART2_Init(); void UART3_Init(); #endif +#ifdef FEATURE_CAN +void CAN_Init(); +#endif + void MX_GPIO_Init(); void MX_TIM_Init(); @@ -135,7 +222,14 @@ void MX_ADC1_Init(); void MX_ADC2_Init(); + +#ifdef FEATURE_BUTTON void poweroff(); +#endif + +#ifdef MOTOR_TEST +void doMotorTest(); +#endif #ifdef FEATURE_SERIAL_CONTROL void parseCommand(); @@ -145,6 +239,20 @@ void parseCommand(); void sendFeedback(); #endif +#ifdef FEATURE_CAN +void parseCanCommand(); +void applyIncomingCanMessage(); +void sendCanFeedback(); +#endif + +#ifdef FEATURE_BUTTON +void handleButton(); +#endif + +void updateSensors(); + +void applyDefaultSettings(); + } // anonymous namespace int main() @@ -181,47 +289,36 @@ int main() HAL_ADC_Start(&hadc1); HAL_ADC_Start(&hadc2); - enum { CurrentMeasAB, CurrentMeasBC, CurrentMeasAC }; + { + constexpr auto doit = [](auto &motor){ + motor.rtP = defaultP; + motor.rtP.b_angleMeasEna = false; + motor.rtP.b_diagEna = DIAG_ENA; + motor.rtP.b_fieldWeakEna = FIELD_WEAK_ENA; + motor.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; + motor.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; + + motor.rtM.defaultParam = &motor.rtP; + motor.rtM.dwork = &motor.rtDW; + motor.rtM.inputs = &motor.rtU; + motor.rtM.outputs = &motor.rtY; + }; + + doit(left); + doit(right); + + enum { CurrentMeasAB, CurrentMeasBC, CurrentMeasAC }; - left.rtP = rtP_Left; - left.rtP.b_angleMeasEna = 0; #ifdef PETERS_PLATINE - left.rtP.z_selPhaCurMeasABC = CurrentMeasBC; // Left motor measured current phases = {iB, iC} -> do NOT change + left.rtP.z_selPhaCurMeasABC = CurrentMeasBC; #else - left.rtP.z_selPhaCurMeasABC = CurrentMeasAB; // Left motor measured current phases = {iA, iB} -> do NOT change + left.rtP.z_selPhaCurMeasABC = CurrentMeasAB; #endif - left.rtP.z_ctrlTypSel = uint8_t(left.state.ctrlTyp); - left.rtP.b_diagEna = DIAG_ENA; - left.rtP.i_max = (left.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - left.rtP.n_max = left.state.nMotMax << 4; // fixdt(1,16,4) - left.rtP.b_fieldWeakEna = FIELD_WEAK_ENA; - left.rtP.id_fieldWeakMax = (left.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - left.rtP.a_phaAdvMax = left.state.phaseAdvMax << 4; // fixdt(1,16,4) - left.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4) - left.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4) - right.rtP = rtP_Left; - right.rtP.b_angleMeasEna = 0; - right.rtP.z_selPhaCurMeasABC = CurrentMeasBC; // Right motor measured current phases = {iB, iC} -> do NOT change - right.rtP.z_ctrlTypSel = uint8_t(right.state.ctrlTyp); - right.rtP.b_diagEna = DIAG_ENA; - right.rtP.i_max = (right.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - right.rtP.n_max = right.state.nMotMax << 4; // fixdt(1,16,4) - right.rtP.b_fieldWeakEna = FIELD_WEAK_ENA; - right.rtP.id_fieldWeakMax = (right.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - right.rtP.a_phaAdvMax = right.state.phaseAdvMax << 4; // fixdt(1,16,4) - right.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4) - right.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4) + right.rtP.z_selPhaCurMeasABC = CurrentMeasBC; + } - left.rtM.defaultParam = &left.rtP; - left.rtM.dwork = &left.rtDW; - left.rtM.inputs = &left.rtU; - left.rtM.outputs = &left.rtY; - - right.rtM.defaultParam = &right.rtP; - right.rtM.dwork = &right.rtDW; - right.rtM.inputs = &right.rtU; - right.rtM.outputs = &right.rtY; + applyDefaultSettings(); /* Initialize BLDC controllers */ BLDC_controller_initialize(&left.rtM); @@ -229,10 +326,10 @@ int main() for (int i = 8; i >= 0; i--) { - buzzer.state.freq = (uint8_t)i; + buzzer.freq = (uint8_t)i; HAL_Delay(50); } - buzzer.state.freq = 0; + buzzer.freq = 0; #ifdef HUARN2 UART2_Init(); @@ -241,9 +338,8 @@ int main() UART3_Init(); #endif -#ifdef MOTOR_TEST - int pwm = 0; - int8_t dir = 1; +#ifdef FEATURE_CAN + CAN_Init(); #endif #ifdef FEATURE_SERIAL_CONTROL @@ -257,110 +353,62 @@ int main() while (true) { - HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms +#ifdef FEATURE_CAN + constexpr auto DELAY_WITH_CAN_POLL = [](uint32_t Delay){ + uint32_t tickstart = HAL_GetTick(); + uint32_t wait = Delay; + + /* Add a freq to guarantee minimum wait */ + if (wait < HAL_MAX_DELAY) + { + wait += (uint32_t)(uwTickFreq); + } + + while ((HAL_GetTick() - tickstart) < wait) + { + applyIncomingCanMessage(); + } + }; + + //DELAY_WITH_CAN_POLL(5); //delay in ms +#endif + HAL_Delay(5); //delay in ms + + updateSensors(); + +#ifdef MOTOR_TEST + doMotorTest(); +#endif #ifdef FEATURE_SERIAL_CONTROL parseCommand(); #endif - timeout = 0; - -#ifdef MOTOR_TEST - left.state.enable = true; - left.state.ctrlMod = ControlMode::Voltage; - left.state.ctrlTyp = ControlType::FieldOrientedControl; - left.state.pwm = pwm; - left.state.iMotMax = 2; - - right.state.enable = true; - right.state.ctrlMod = ControlMode::Voltage; - right.state.ctrlTyp = ControlType::FieldOrientedControl; - right.state.pwm = pwm; - right.state.iMotMax = 2; - - constexpr auto pwmMax = 250; - - pwm += dir; - if (pwm > pwmMax) { - pwm = pwmMax; - dir = -1; - } else if (pwm < -pwmMax) { - pwm = -pwmMax; - dir = 1; - } +#ifdef FEATURE_SERIAL_FEEDBACK + sendFeedback(); #endif - // ####### CALC BOARD TEMPERATURE ####### - filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt); - board_temp_adcFilt = (int16_t)(board_temp_adcFixdt >> 20); // 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; +#ifdef FEATURE_CAN + parseCanCommand(); -#ifdef FEATURE_SERIAL_FEEDBACK - if (main_loop_counter % 50 != 0) // Send data periodically - sendFeedback(); + sendCanFeedback(); #endif #ifdef FEATURE_BUTTON - if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) - { - left.state.enable = right.state.enable = 0; // disable motors - - while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released - - if(__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) { // do not power off after software reset (from a programmer/debugger) - __HAL_RCC_CLEAR_RESET_FLAGS(); // clear reset flags - } else { - poweroff(); // release power-latch - } - } + handleButton(); #endif - left.rtP.z_ctrlTypSel = uint8_t(left.state.ctrlTyp); - left.rtP.i_max = (left.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - left.rtP.n_max = left.state.nMotMax << 4; // fixdt(1,16,4) - left.rtP.id_fieldWeakMax = (left.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - left.rtP.a_phaAdvMax = left.state.phaseAdvMax << 4; // fixdt(1,16,4) - - left.rtU.z_ctrlModReq = uint8_t(left.state.ctrlMod); - left.rtU.r_inpTgt = left.state.pwm; - - right.rtP.z_ctrlTypSel = uint8_t(right.state.ctrlTyp); - right.rtP.i_max = (right.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - right.rtP.n_max = right.state.nMotMax << 4; // fixdt(1,16,4) - right.rtP.id_fieldWeakMax = (right.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - right.rtP.a_phaAdvMax = right.state.phaseAdvMax << 4; // fixdt(1,16,4) - - right.rtU.z_ctrlModReq = uint8_t(right.state.ctrlMod); - right.rtU.r_inpTgt = right.state.pwm; - main_loop_counter++; - timeout++; } } namespace { -void updateBatVoltage() -{ - if (buzzer.timer % 1000 == 0) // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point - { - filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt); - batVoltage = (int16_t)(batVoltageFixdt >> 20); // convert fixed-point to integer - } -} - void updateBuzzer() { - if (buzzer.timer % 1000 == 0) // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point - { - filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt); - batVoltage = (int16_t)(batVoltageFixdt >> 20); // convert fixed-point to integer - } - - //create square wave for buzzer buzzer.timer++; - if (buzzer.state.freq != 0 && (buzzer.timer / 1000) % (buzzer.state.pattern + 1) == 0) + if (buzzer.freq != 0 && (buzzer.timer / 1000) % (buzzer.pattern + 1) == 0) { - if (buzzer.timer % buzzer.state.freq == 0) + if (buzzer.timer % buzzer.freq == 0) { HAL_GPIO_TogglePin(BUZZER_PORT, BUZZER_PIN); } @@ -375,6 +423,14 @@ void updateMotors() { DMA1->IFCR = DMA_IFCR_CTCIF1; + static uint16_t offsetcount{}; + static int16_t offsetrl1{2000}; + static int16_t offsetrl2{2000}; + static int16_t offsetrr1{2000}; + static int16_t offsetrr2{2000}; + static int16_t offsetdcl{2000}; + static int16_t offsetdcr{2000}; + if (offsetcount < 2000) // calibrate ADC offsets { offsetcount++; @@ -407,24 +463,26 @@ void updateMotors() #endif int16_t curR_DC = (int16_t)(offsetdcr - adc_buffer.dcr); - const bool chopL = std::abs(curL_DC) > (left.state.iDcMax * A2BIT_CONV); + const bool chopL = std::abs(curL_DC) > (left.iDcMax.load() * A2BIT_CONV); if (chopL) left.chops++; - const bool chopR = std::abs(curR_DC) > (right.state.iDcMax * A2BIT_CONV); + const bool chopR = std::abs(curR_DC) > (right.iDcMax.load() * A2BIT_CONV); if (chopR) right.chops++; - const auto timeoutVal = timeout.load(); + const uint32_t timeoutVal = ++timeout; + const bool leftEnable = left.enable.load(); + const bool rightEnable = right.enable.load(); // Disable PWM when current limit is reached (current chopping) // This is the Level 2 of current protection. The Level 1 should kick in first given by I_MOT_MAX - if (chopL || timeoutVal > TIMEOUT || !left.state.enable) + if (chopL || timeoutVal > 500 || !leftEnable) LEFT_TIM->BDTR &= ~TIM_BDTR_MOE; else LEFT_TIM->BDTR |= TIM_BDTR_MOE; - if (chopR || timeoutVal > TIMEOUT || !right.state.enable) + if (chopR || timeoutVal > 500 || !rightEnable) RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE; else RIGHT_TIM->BDTR |= TIM_BDTR_MOE; @@ -451,8 +509,8 @@ void updateMotors() #endif ; - const bool enableLFin = left.state.enable && left.rtY.z_errCode == 0 && (right.rtY.z_errCode == 0 || ignoreOtherMotor); - const bool enableRFin = right.state.enable && (left.rtY.z_errCode == 0 || ignoreOtherMotor) && right.rtY.z_errCode == 0; + const bool enableLFin = leftEnable && left.rtY.z_errCode == 0 && (right.rtY.z_errCode == 0 || ignoreOtherMotor); + const bool enableRFin = rightEnable && (left.rtY.z_errCode == 0 || ignoreOtherMotor) && right.rtY.z_errCode == 0; // ========================= LEFT MOTOR ============================ // Get hall sensors values @@ -517,30 +575,6 @@ void updateMotors() OverrunFlag = false; } -// =========================================================== - /* Low pass filter fixed-point 32 bits: fixdt(1,32,20) - * Max: 2047.9375 - * Min: -2048 - * Res: 0.0625 - * - * Inputs: u = int16 - * Outputs: y = fixdt(1,32,20) - * Parameters: coef = fixdt(0,16,16) = [0,65535U] - * - * Example: - * If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point) - * filtLowPass16(u, 52429, &y); - * yint = (int16_t)(y >> 20); // the integer output is the fixed-point ouput shifted by 20 bits - */ -void filtLowPass32(int16_t u, uint16_t coef, int32_t *y) -{ - int tmp; - - tmp = (int16_t)(u << 4) - (*y >> 16); - tmp = std::clamp(tmp, -32768, 32767); // Overflow protection - *y = coef * tmp + (*y); -} - // =========================================================== /** System Clock Configuration @@ -736,6 +770,217 @@ void UART3_Init() } #endif +#ifdef FEATURE_CAN +void CAN_MspInit(CAN_HandleTypeDef *hcan); +void CAN_MspDeInit(CAN_HandleTypeDef *hcan); +void CAN_MspDeInit(CAN_HandleTypeDef *hcan); +void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle); +void CAN_TxMailboxCompleteCallback(CAN_HandleTypeDef *hcan); + +void CAN_Init() +{ + myPrintf("CAN_Init() called"); + + /* Configure the CAN peripheral */ + CanHandle.Instance = CANx; + + CanHandle.MspInitCallback = CAN_MspInit; + CanHandle.MspDeInitCallback = CAN_MspDeInit; + + CanHandle.Init.TimeTriggeredMode = DISABLE; + CanHandle.Init.AutoBusOff = ENABLE; + CanHandle.Init.AutoWakeUp = DISABLE; + CanHandle.Init.AutoRetransmission = ENABLE; + CanHandle.Init.ReceiveFifoLocked = DISABLE; + CanHandle.Init.TransmitFifoPriority = DISABLE; + CanHandle.Init.Mode = CAN_MODE_NORMAL; + + CanHandle.Init.SyncJumpWidth = CAN_SJW_1TQ; + CanHandle.Init.TimeSeg1 = CAN_BS1_3TQ; + CanHandle.Init.TimeSeg2 = CAN_BS2_4TQ; + CanHandle.Init.Prescaler = 16; + + if (const auto result = HAL_CAN_Init(&CanHandle); result == HAL_OK) + myPrintf("HAL_CAN_Init() succeeded"); + else + { + myPrintf("HAL_CAN_Init() failed with %i", result); + while (true); + } + + { + /* Configure the CAN Filter */ + CAN_FilterTypeDef sFilterConfig; + sFilterConfig.FilterBank = 0; + sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; + sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; + + // + // TTRR.....FL + sFilterConfig.FilterIdHigh = 0b00000000000; + sFilterConfig.FilterIdLow = 0b00000000000; + sFilterConfig.FilterMaskIdHigh = 0b00000000000; + sFilterConfig.FilterMaskIdLow = 0b11110000010; + // 0b0000.....0. + + + sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; + sFilterConfig.FilterActivation = CAN_FILTER_ENABLE; + sFilterConfig.SlaveStartFilterBank = 14; + + if (const auto result = HAL_CAN_ConfigFilter(&CanHandle, &sFilterConfig); result == HAL_OK) + myPrintf("HAL_CAN_ConfigFilter() succeeded"); + else + { + myPrintf("HAL_CAN_ConfigFilter() failed with %i", result); + while (true); + } + } + + if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_RX_FIFO0_MSG_PENDING_CB_ID, CAN_RxFifo0MsgPendingCallback); result == HAL_OK) + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_RX_FIFO0_MSG_PENDING_CB_ID succeeded"); + else + { + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_RX_FIFO0_MSG_PENDING_CB_ID failed with %i", result); + while (true); + } + + if (false) + { + if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID succeeded"); + else + { + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID failed with %i", result); + while (true); + } + + if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID succeeded"); + else + { + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID failed with %i", result); + while (true); + } + + if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID succeeded"); + else + { + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID failed with %i", result); + while (true); + } + } + + /* Start the CAN peripheral */ + if (const auto result = HAL_CAN_Start(&CanHandle); result == HAL_OK) + myPrintf("HAL_CAN_Start() succeeded"); + else + { + myPrintf("HAL_CAN_Start() failed with %i", result); + while (true); + } + + /* Activate CAN RX notification */ + if (const auto result = HAL_CAN_ActivateNotification(&CanHandle, CAN_IT_RX_FIFO0_MSG_PENDING); result == HAL_OK) + myPrintf("HAL_CAN_ActivateNotification() CAN_IT_RX_FIFO0_MSG_PENDING succeeded"); + else + { + myPrintf("HAL_CAN_ActivateNotification() CAN_IT_RX_FIFO0_MSG_PENDING failed with %i", result); + while (true); + } + + if (false) + { + /* Activate CAN TX notification */ + if (const auto result = HAL_CAN_ActivateNotification(&CanHandle, CAN_IT_TX_MAILBOX_EMPTY); result == HAL_OK) + myPrintf("HAL_CAN_ActivateNotification() CAN_IT_TX_MAILBOX_EMPTY succeeded"); + else + { + myPrintf("HAL_CAN_ActivateNotification() CAN_IT_TX_MAILBOX_EMPTY failed with %i", result); + while (true); + } + } +} + +void CAN_MspInit(CAN_HandleTypeDef *hcan) +{ + myPrintf("CAN_MspInit() called"); + + GPIO_InitTypeDef GPIO_InitStruct; + + /*##-1- Enable peripherals and GPIO Clocks #################################*/ + /* CAN1 Periph clock enable */ + CANx_CLK_ENABLE(); + /* Enable GPIO clock ****************************************/ + CANx_GPIO_CLK_ENABLE(); + /* Enable AFIO clock and Remap CAN PINs to PB8 and PB9*******/ + CANx_AFIO_REMAP_CLK_ENABLE(); + CANx_AFIO_REMAP_RX_TX_PIN(); + + /*##-2- Configure peripheral GPIO ##########################################*/ + /* CAN1 TX GPIO pin configuration */ + GPIO_InitStruct.Pin = CANx_TX_PIN; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Pull = GPIO_PULLUP; + + HAL_GPIO_Init(CANx_TX_GPIO_PORT, &GPIO_InitStruct); + + /* CAN1 RX GPIO pin configuration */ + GPIO_InitStruct.Pin = CANx_RX_PIN; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Pull = GPIO_PULLUP; + + HAL_GPIO_Init(CANx_RX_GPIO_PORT, &GPIO_InitStruct); + + /*##-3- Configure the NVIC #################################################*/ + /* NVIC configuration for CAN1 Reception complete interrupt */ + HAL_NVIC_SetPriority(CANx_RX_IRQn, 1, 0); + HAL_NVIC_EnableIRQ(CANx_RX_IRQn); + + HAL_NVIC_SetPriority(CANx_TX_IRQn, 1, 0); + HAL_NVIC_EnableIRQ(CANx_TX_IRQn); +} + +void CAN_MspDeInit(CAN_HandleTypeDef *hcan) +{ + myPrintf("CAN_MspDeInit() called"); + + /*##-1- Reset peripherals ##################################################*/ + CANx_FORCE_RESET(); + CANx_RELEASE_RESET(); + + /*##-2- Disable peripherals and GPIO Clocks ################################*/ + /* De-initialize the CAN1 TX GPIO pin */ + HAL_GPIO_DeInit(CANx_TX_GPIO_PORT, CANx_TX_PIN); + /* De-initialize the CAN1 RX GPIO pin */ + HAL_GPIO_DeInit(CANx_RX_GPIO_PORT, CANx_RX_PIN); + + /*##-4- Disable the NVIC for CAN reception #################################*/ + HAL_NVIC_DisableIRQ(CANx_RX_IRQn); +} + +void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle) +{ + //myPrintf("CAN_RxFifo0MsgPendingCallback() called"); + + applyIncomingCanMessage(); +} + +void CAN_TxMailboxCompleteCallback(CAN_HandleTypeDef *hcan) +{ + myPrintf("CAN_TxMailboxCompleteCallback() called"); + + // slightly yucky, but we don't want to block inside the IRQ handler + //if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) >= 2) + //{ + // can_feedc0de_poll(); + //} +} +#endif + void MX_GPIO_Init() { GPIO_InitTypeDef GPIO_InitStruct; @@ -887,7 +1132,11 @@ void MX_TIM_Init() #endif sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; +#ifdef PETERS_PLATINE + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#else sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_SET; +#endif HAL_TIM_PWM_ConfigChannel(&htim_right, &sConfigOC, TIM_CHANNEL_1); HAL_TIM_PWM_ConfigChannel(&htim_right, &sConfigOC, TIM_CHANNEL_2); HAL_TIM_PWM_ConfigChannel(&htim_right, &sConfigOC, TIM_CHANNEL_3); @@ -932,7 +1181,11 @@ void MX_TIM_Init() #endif sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; +#ifdef PETERS_PLATINE + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#else sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_SET; +#endif HAL_TIM_PWM_ConfigChannel(&htim_left, &sConfigOC, TIM_CHANNEL_1); HAL_TIM_PWM_ConfigChannel(&htim_left, &sConfigOC, TIM_CHANNEL_2); HAL_TIM_PWM_ConfigChannel(&htim_left, &sConfigOC, TIM_CHANNEL_3); @@ -1084,25 +1337,78 @@ void MX_ADC2_Init() __HAL_ADC_ENABLE(&hadc2); } +#ifdef FEATURE_BUTTON void poweroff() { - // if (abs(speed) < 20) { // wait for the speed to drop, then shut down -> this is commented out for SAFETY reasons - buzzer.state.pattern = 0; - left.state.enable = right.state.enable = 0; - for (int i = 0; i < 8; i++) { - buzzer.state.freq = (uint8_t)i; - HAL_Delay(50); - } - HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET); - for (int i = 0; i < 5; i++) - HAL_Delay(1000); - // } +// if (abs(speed) < 20) { // wait for the speed to drop, then shut down -> this is commented out for SAFETY reasons + buzzer.pattern = 0; + left.enable = false; + right.enable = 0; + + for (int i = 0; i < 8; i++) { + buzzer.freq = (uint8_t)i; + HAL_Delay(50); + } + HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET); + for (int i = 0; i < 5; i++) + HAL_Delay(1000); +// } } +#endif + +void communicationTimeout() +{ + applyDefaultSettings(); + + buzzer.freq = 24; + buzzer.pattern = 1; + + HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_RESET); +} + +#ifdef MOTOR_TEST +void doMotorTest() +{ + timeout = 0; // proove, that the controlling code is still running + + + left.enable = true; + left.rtU.r_inpTgt = pwm; + left.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); + left.rtU.z_ctrlModReq = uint8_t(ControlMode::Speed); + left.rtP.i_max = (2 * A2BIT_CONV) << 4; + left.iDcMax = 4; + left.rtP.n_max = 1000 << 4; + left.rtP.id_fieldWeakMax = (0 * A2BIT_CONV) << 4; + left.rtP.a_phaAdvMax = 40 << 4; + + right.enable = true; + right.rtU.r_inpTgt = pwm; + right.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); + right.rtU.z_ctrlModReq = uint8_t(ControlMode::Speed); + right.rtP.i_max = (2 * A2BIT_CONV) << 4; + right.iDcMax = 4; + right.rtP.n_max = 1000 << 4; + right.rtP.id_fieldWeakMax = (0 * A2BIT_CONV) << 4; + right.rtP.a_phaAdvMax = 40 << 4; + + constexpr auto pwmMax = 400; + + pwm += dir; + if (pwm > pwmMax) { + pwm = pwmMax; + dir = -1; + } else if (pwm < -pwmMax) { + pwm = -pwmMax; + dir = 1; + } +} +#endif #ifdef FEATURE_SERIAL_CONTROL void parseCommand() { - bool any_parsed{false}; + timeout = 0; // proove, that the controlling code is still running for (int i = 0; i < 1; i++) { @@ -1113,47 +1419,59 @@ void parseCommand() if (command.checksum != checksum) continue; - left.state = command.left; - right.state = command.right; + left.iDcMax = command.left.iDcMax; - buzzer.state = command.buzzer; + left.rtP.z_ctrlTypSel = uint8_t(command.left.ctrlTyp); + left.rtP.i_max = (command.left.iMotMax * A2BIT_CONV) << 4; + left.rtP.n_max = command.left.nMotMax << 4; + left.rtP.id_fieldWeakMax = (command.left.fieldWeakMax * A2BIT_CONV) << 4; + left.rtP.a_phaAdvMax = command.left.phaseAdvMax << 4; + left.rtU.z_ctrlModReq = uint8_t(command.left.ctrlMod); + left.rtU.r_inpTgt = command.left.pwm; + right.iDcMax = command.right.iDcMax; + + right.rtP.z_ctrlTypSel = uint8_t(command.right.ctrlTyp); + right.rtP.i_max = (command.right.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4) + right.rtP.n_max = command.right.nMotMax << 4; // fixdt(1,16,4) + right.rtP.id_fieldWeakMax = (command.right.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4) + right.rtP.a_phaAdvMax = command.right.phaseAdvMax << 4; // fixdt(1,16,4) + right.rtU.z_ctrlModReq = uint8_t(command.right.ctrlMod); + right.rtU.r_inpTgt = command.right.pwm; + + buzzer.freq = command.buzzer.freq; + buzzer.pattern = command.buzzer.pattern; + +#ifdef FEATURE_BUTTON if (command.poweroff) poweroff(); +#endif HAL_GPIO_WritePin(LED_PORT, LED_PIN, command.led ? GPIO_PIN_RESET : GPIO_PIN_SET); command.start = Command::INVALID_HEADER; // Change the Start Frame for timeout detection in the next cycle timeoutCntSerial = 0; // Reset the timeout counter - any_parsed = true; - break; + return; } - if (!any_parsed) + if (timeoutCntSerial++ >= 100) // Timeout qualification { - if (timeoutCntSerial++ >= 100) // Timeout qualification + timeoutCntSerial = 100; // Limit timout counter value + + communicationTimeout(); + + // Check periodically the received Start Frame. Try to re-sync by reseting the DMA + if (main_loop_counter % 25 == 0) { - timeoutCntSerial = 100; // Limit timout counter value - - left.state = right.state = {.enable=true}; - - buzzer.state = { 24, 1 }; - - HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_RESET); - - // Check periodically the received Start Frame. Try to re-sync by reseting the DMA - if (main_loop_counter % 25 == 0) - { #ifdef HUARN2 - HAL_UART_DMAStop(&huart2); - HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, sizeof(command)); + HAL_UART_DMAStop(&huart2); + HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, sizeof(command)); #endif #ifdef HUARN3 - HAL_UART_DMAStop(&huart3); - HAL_UART_Receive_DMA(&huart3, (uint8_t *)&command, sizeof(command)); + HAL_UART_DMAStop(&huart3); + HAL_UART_Receive_DMA(&huart3, (uint8_t *)&command, sizeof(command)); #endif - } } } } @@ -1186,10 +1504,8 @@ void sendFeedback() feedback.left.current = left.rtU.i_DCLink; feedback.right.current = right.rtU.i_DCLink; - feedback.left.chops = left.chops; - feedback.right.chops = right.chops; - left.chops = 0; - right.chops = 0; + feedback.left.chops = left.chops.exchange(0); + feedback.right.chops = right.chops.exchange(0); feedback.left.hallA = left.rtU.b_hallA; feedback.left.hallB = left.rtU.b_hallB; @@ -1211,6 +1527,243 @@ void sendFeedback() } #endif +#ifdef FEATURE_CAN +void parseCanCommand() +{ + timeout = 0; // proove, that the controlling code is still running + + const auto l = ++timeoutCntLeft; + const auto r = ++timeoutCntRight; + if (l >= 99 || r >= 99) + { + if (l > 100) + timeoutCntLeft = 100; + if (r > 100) + timeoutCntRight = 100; + + communicationTimeout(); + } +} + +void applyIncomingCanMessage() +{ + CAN_RxHeaderTypeDef header; + uint8_t buf[8]; + if (const auto result = HAL_CAN_GetRxMessage(&CanHandle, CAN_RX_FIFO0, &header, buf); result != HAL_OK) + { + myPrintf("HAL_CAN_GetRxMessage() failed with %i", result); + //while (true); + return; + } + + switch (header.StdId) + { + using namespace bobbycar::can; + case MotorController::Command::Enable: left .enable = *((bool *)buf); break; + case MotorController ::Command::Enable: right.enable = *((bool *)buf); break; + case MotorController::Command::InpTgt: left. rtU.r_inpTgt = *((int16_t*)buf); timeoutCntLeft = 0; break; + case MotorController ::Command::InpTgt: right.rtU.r_inpTgt = *((int16_t*)buf); timeoutCntRight = 0; break; + case MotorController::Command::CtrlTyp: left .rtP.z_ctrlTypSel = *((uint8_t*)buf); break; + case MotorController ::Command::CtrlTyp: right.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; + case MotorController::Command::CtrlMod: left .rtU.z_ctrlModReq = *((uint8_t*)buf); break; + case MotorController ::Command::CtrlMod: right.rtU.z_ctrlModReq = *((uint8_t*)buf); break; + case MotorController::Command::IMotMax: left .rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController ::Command::IMotMax: right.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController::Command::IDcMax: left .iDcMax = *((uint8_t*)buf); break; + case MotorController ::Command::IDcMax: right.iDcMax = *((uint8_t*)buf); break; + case MotorController::Command::NMotMax: left .rtP.n_max = *((uint16_t*)buf) << 4; break; + case MotorController ::Command::NMotMax: right.rtP.n_max = *((uint16_t*)buf) << 4; break; + case MotorController::Command::FieldWeakMax: left .rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController ::Command::FieldWeakMax: right.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController::Command::PhaseAdvMax: left .rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; + case MotorController ::Command::PhaseAdvMax: right.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; + case MotorController::Command::CruiseCtrlEna: left .rtP.b_cruiseCtrlEna = *((bool*)buf); break; + case MotorController ::Command::CruiseCtrlEna: right.rtP.b_cruiseCtrlEna = *((bool*)buf); break; + case MotorController::Command::CruiseMotTgt: left .rtP.n_cruiseMotTgt = *((int16_T*)buf); break; + case MotorController ::Command::CruiseMotTgt: right.rtP.n_cruiseMotTgt = *((int16_T*)buf); break; + case MotorController::Command::BuzzerFreq: + case MotorController ::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break; + case MotorController::Command::BuzzerPattern: + case MotorController ::Command::BuzzerPattern: buzzer.pattern = *((uint8_t*)buf); break; + case MotorController::Command::Led: + case MotorController ::Command::Led: + HAL_GPIO_WritePin(LED_PORT, LED_PIN, *((bool*)buf) ? GPIO_PIN_SET : GPIO_PIN_RESET); + break; + case MotorController::Command::Poweroff: + case MotorController::Command::Poweroff: +#ifdef FEATURE_BUTTON + if (*((bool*)buf)) + poweroff(); +#endif + break; + default: +#ifndef CAN_LOG_UNKNOWN_ADDR + if constexpr (false) +#endif + myPrintf("UNKNOWN %c%c %c%c %c%c%c%c%c %c%c %s", + header.StdId&1024?'1':'0', + header.StdId&512?'1':'0', + + header.StdId&256?'1':'0', + header.StdId&128?'1':'0', + + header.StdId&64?'1':'0', + header.StdId&32?'1':'0', + header.StdId&16?'1':'0', + header.StdId&8?'1':'0', + header.StdId&4?'1':'0', + + header.StdId&2?'1':'0', + header.StdId&1?'1':'0', + + bobbycarCanIdDesc(header.StdId) + ); + + if constexpr (false) + myPrintf("UNKNOWN StdId=%x %u ExtId=%x %u IDE=%x %u RTR=%x %u DLC=%x %u Timestamp=%x %u FilterMatchIndex=%x %u", + header.StdId, header.StdId, + header.ExtId, header.ExtId, + header.IDE, header.IDE, + header.RTR, header.RTR, + header.DLC, header.DLC, + header.Timestamp, header.Timestamp, + header.FilterMatchIndex, header.FilterMatchIndex + ); + } +} + +void sendCanFeedback() +{ + const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle); + if (!free) + return; + + constexpr auto send = [](uint32_t addr, auto value){ + CAN_TxHeaderTypeDef header; + header.StdId = addr; + header.ExtId = 0x01; + header.RTR = CAN_RTR_DATA; + header.IDE = CAN_ID_STD; + header.DLC = sizeof(value); + header.TransmitGlobalTime = DISABLE; + + uint8_t buf[8] {0}; + std::memcpy(buf, &value, sizeof(value)); + + static uint32_t TxMailbox; + if (const auto result = HAL_CAN_AddTxMessage(&CanHandle, &header, buf, &TxMailbox); result != HAL_OK) + { + myPrintf("HAL_CAN_AddTxMessage() failed with %i", result); + //while (true); + } + }; + + static uint8_t whichToSend{}; + + switch (whichToSend++) + { + using namespace bobbycar::can; + case 0: send(MotorController::Feedback::DcLink, left. rtU.i_DCLink); break; + case 1: send(MotorController:: Feedback::DcLink, right.rtU.i_DCLink); break; + case 2: send(MotorController::Feedback::Speed, left. rtY.n_mot); break; + case 3: send(MotorController:: Feedback::Speed, right.rtY.n_mot); break; + case 4: send(MotorController::Feedback::Error, left. rtY.z_errCode); break; + case 5: send(MotorController:: Feedback::Error, right.rtY.z_errCode); break; + case 6: send(MotorController::Feedback::Angle, left. rtY.a_elecAngle); break; + case 7: send(MotorController:: Feedback::Angle, right.rtY.a_elecAngle); break; + case 8: send(MotorController::Feedback::DcPhaA, left. rtY.DC_phaA); break; + case 9: send(MotorController:: Feedback::DcPhaA, right.rtY.DC_phaA); break; + case 10: send(MotorController::Feedback::DcPhaB, left. rtY.DC_phaB); break; + case 11: send(MotorController:: Feedback::DcPhaB, right.rtY.DC_phaB); break; + case 12: send(MotorController::Feedback::DcPhaC, left. rtY.DC_phaC); break; + case 13: send(MotorController:: Feedback::DcPhaC, right.rtY.DC_phaC); break; + case 14: send(MotorController::Feedback::Chops, left. chops.exchange(0)); break; + case 15: send(MotorController:: Feedback::Chops, right.chops.exchange(0)); break; + case 16: send(MotorController::Feedback::Hall, left.hallBits()); break; + case 17: send(MotorController:: Feedback::Hall, right.hallBits()); break; + case 18: send(MotorController::Feedback::Voltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; + case 19: send(MotorController:: Feedback::Voltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; + case 20: send(MotorController::Feedback::Temp, board_temp_deg_c); break; + case 21: send(MotorController:: Feedback::Temp, board_temp_deg_c); whichToSend = 0; break; + default: myPrintf("unreachable"); + } +} +#endif + +#ifdef FEATURE_BUTTON +void handleButton() +{ + if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) + { + left.enable = false; + right.enable = false; + + while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released + + if(__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) { // do not power off after software reset (from a programmer/debugger) + __HAL_RCC_CLEAR_RESET_FLAGS(); // clear reset flags + } else { + poweroff(); // release power-latch + } + } +} +#endif + +void updateSensors() +{ + /* Low pass filter fixed-point 32 bits: fixdt(1,32,20) + * Max: 2047.9375 + * Min: -2048 + * Res: 0.0625 + * + * Inputs: u = int16 + * Outputs: y = fixdt(1,32,20) + * Parameters: coef = fixdt(0,16,16) = [0,65535U] + * + * Example: + * If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point) + * filtLowPass16(u, 52429, &y); + * yint = (int16_t)(y >> 20); // the integer output is the fixed-point ouput shifted by 20 bits + */ + constexpr auto filtLowPass32 = [](int16_t u, uint16_t coef, int32_t &y) + { + int tmp = (int16_t)(u << 4) - (y >> 16); + tmp = std::clamp(tmp, -32768, 32767); // Overflow protection + y = coef * tmp + y; + }; + + // Fixed-point filter output initialized with current ADC converted to fixed-point + static int32_t board_temp_adcFixdt{} /*= adc_buffer.temp << 20*/; + filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, board_temp_adcFixdt); + int16_t board_temp_adcFilt = (int16_t)(board_temp_adcFixdt >> 20); // 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; + + // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point + static int32_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 20; + filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, batVoltageFixdt); + batVoltage = (int16_t)(batVoltageFixdt >> 20); // convert fixed-point to integer +} + +void applyDefaultSettings() +{ + constexpr auto doIt = [](auto &motor){ + motor.enable = true; + motor.rtU.r_inpTgt = 0; + motor.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); + motor.rtU.z_ctrlModReq = uint8_t(ControlMode::OpenMode); + motor.rtP.i_max = (5 * A2BIT_CONV) << 4; + motor.iDcMax = 7; + motor.rtP.n_max = 1000 << 4; + motor.rtP.id_fieldWeakMax = (1 * A2BIT_CONV) << 4; + motor.rtP.a_phaAdvMax = 40 << 4; + motor.rtP.b_cruiseCtrlEna = false; + motor.rtP.n_cruiseMotTgt = 0; + }; + + doIt(left); + doIt(right); +} + } // anonymous namespace /******************************************************************************/ @@ -1346,7 +1899,6 @@ extern "C" void DMA1_Channel1_IRQHandler() /* USER CODE END DMA1_Channel1_IRQn 0 */ updateMotors(); - updateBatVoltage(); updateBuzzer(); /* USER CODE BEGIN DMA1_Channel1_IRQn 1 */ @@ -1409,3 +1961,15 @@ extern "C" void DMA1_Channel3_IRQHandler() /* USER CODE END DMA1_Channel3_IRQn 1 */ } #endif + +#ifdef FEATURE_CAN +extern "C" void CANx_RX_IRQHandler(void) +{ + HAL_CAN_IRQHandler(&CanHandle); +} + +extern "C" void CANx_TX_IRQHandler(void) +{ + HAL_CAN_IRQHandler(&CanHandle); +} +#endif diff --git a/stm32f1xx_hal_conf.h b/stm32f1xx_hal_conf.h index 42311da..118bb67 100644 --- a/stm32f1xx_hal_conf.h +++ b/stm32f1xx_hal_conf.h @@ -36,7 +36,7 @@ extern "C" { */ #define HAL_MODULE_ENABLED #define HAL_ADC_MODULE_ENABLED -/* #define HAL_CAN_MODULE_ENABLED */ +#define HAL_CAN_MODULE_ENABLED /* #define HAL_CAN_LEGACY_MODULE_ENABLED */ /* #define HAL_CEC_MODULE_ENABLED */ #define HAL_CORTEX_MODULE_ENABLED @@ -129,7 +129,7 @@ extern "C" { #define PREFETCH_ENABLE 1U #define USE_HAL_ADC_REGISTER_CALLBACKS 0U /* ADC register callback disabled */ -#define USE_HAL_CAN_REGISTER_CALLBACKS 0U /* CAN register callback disabled */ +#define USE_HAL_CAN_REGISTER_CALLBACKS 1 /* CAN register callback disabled */ #define USE_HAL_CEC_REGISTER_CALLBACKS 0U /* CEC register callback disabled */ #define USE_HAL_DAC_REGISTER_CALLBACKS 0U /* DAC register callback disabled */ #define USE_HAL_ETH_REGISTER_CALLBACKS 0U /* ETH register callback disabled */