From b4218f9263eeed6759597ad0b9e9fdd4cec65898 Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Fri, 27 May 2022 19:15:52 -0700 Subject: [PATCH 01/18] angle sensor and safeties --- board/SConscript | 1 + board/bldc/bldc.c | 59 ++++++++++++++-- board/boards.h | 48 +++++++++++++ board/bootstub.c | 9 ++- board/comms.h | 34 ++++++++-- board/config.h | 3 + board/defines.h | 56 +++++++++------ board/drivers/angle_sensor.h | 69 +++++++++++++++++++ board/drivers/clock.h | 2 +- board/drivers/llbxcan.h | 2 +- board/flasher.h | 9 ++- board/inc/stm32f4xx_hal_conf.h | 2 +- board/main.c | 108 ++++++++++++++++++++++++----- board/setup.h | 120 +++++++++++++++++++++++---------- board/util.c | 37 ++++++++-- board/util.h | 4 ++ 16 files changed, 462 insertions(+), 101 deletions(-) create mode 100644 board/boards.h create mode 100644 board/drivers/angle_sensor.h diff --git a/board/SConscript b/board/SConscript index d6dc977..f9abe6a 100644 --- a/board/SConscript +++ b/board/SConscript @@ -48,6 +48,7 @@ c_sources = [ ["hal_flash", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c"], ["hal_pwr", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c"], ["hal_rcc", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c"], + ["hal_spi", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c"], ["hal_tim", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c"], ["hal_tim_ex", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c"], ["hal_adc_ex", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c"], diff --git a/board/bldc/bldc.c b/board/bldc/bldc.c index 3c1f107..8324c3a 100644 --- a/board/bldc/bldc.c +++ b/board/bldc/bldc.c @@ -32,6 +32,9 @@ #include "BLDC_controller.h" /* Model's header file */ #include "rtwtypes.h" +extern hall_sensor hall_left; +extern hall_sensor hall_right; + extern RT_MODEL *const rtM_Left; extern RT_MODEL *const rtM_Right; @@ -67,7 +70,7 @@ static uint8_t buzzerIdx = 0; uint8_t enable_motors = 0; // initially motors are disabled for SAFETY static uint8_t enableFin = 0; -static const uint16_t pwm_res = 64000000 / 2 / PWM_FREQ; // = 2000 ; TODO: should change to SystemCoreClock ? Needs testing +static const uint16_t pwm_res = CORE_FREQ / 2 / PWM_FREQ; static uint16_t offsetcount = 0; static int16_t offsetrlA = 2000; @@ -80,6 +83,8 @@ static int16_t offsetdcr = 2000; int16_t batVoltage = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE; static int32_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 16; // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point +int32_t motPosL = 0; +int32_t motPosR = 0; // DMA interrupt frequency =~ 16 kHz void DMA2_Stream0_IRQHandler(void) { @@ -164,9 +169,9 @@ void DMA2_Stream0_IRQHandler(void) { enableFin = enable_motors && !rtY_Left.z_errCode && !rtY_Right.z_errCode; // ========================= LEFT MOTOR ============================ - uint8_t hall_ul = !(LEFT_HALL_U_PORT->IDR & LEFT_HALL_U_PIN); - uint8_t hall_vl = !(LEFT_HALL_V_PORT->IDR & LEFT_HALL_V_PIN); - uint8_t hall_wl = !(LEFT_HALL_W_PORT->IDR & LEFT_HALL_W_PIN); + uint8_t hall_ul = !(hall_left.hall_portA->IDR & hall_left.hall_pinA); + uint8_t hall_vl = !(hall_left.hall_portB->IDR & hall_left.hall_pinB); + uint8_t hall_wl = !(hall_left.hall_portC->IDR & hall_left.hall_pinC); rtU_Left.b_motEna = enableFin; rtU_Left.z_ctrlModReq = ctrlModReq; @@ -194,9 +199,9 @@ void DMA2_Stream0_IRQHandler(void) { // ========================= RIGHT MOTOR =========================== - uint8_t hall_ur = !(RIGHT_HALL_U_PORT->IDR & RIGHT_HALL_U_PIN); - uint8_t hall_vr = !(RIGHT_HALL_V_PORT->IDR & RIGHT_HALL_V_PIN); - uint8_t hall_wr = !(RIGHT_HALL_W_PORT->IDR & RIGHT_HALL_W_PIN); + uint8_t hall_ur = !(hall_right.hall_portA->IDR & hall_right.hall_pinA); + uint8_t hall_vr = !(hall_right.hall_portB->IDR & hall_right.hall_pinB); + uint8_t hall_wr = !(hall_right.hall_portC->IDR & hall_right.hall_pinC); rtU_Right.b_motEna = enableFin; rtU_Right.z_ctrlModReq = ctrlModReq; @@ -223,4 +228,44 @@ void DMA2_Stream0_IRQHandler(void) { // ================================================================= OverrunFlag = false; + + static int16_t motAngleLeftLast = 0; + static int16_t motAngleRightLast = 0; + static int32_t cycleDegsL = 0; // wheel encoder roll over count in deg + static int32_t cycleDegsR = 0; // wheel encoder roll over count in deg + int16_t diffL = 0; + int16_t diffR = 0; + static int16_t cnt = 0; + + if (enable_motors == 0) { // Reset everything if motors are disabled + cycleDegsL = 0; + cycleDegsR = 0; + diffL = 0; + diffR = 0; + cnt = 0; + } + if (cnt == 0) { + motAngleLeftLast = rtY_Left.a_elecAngle; + motAngleRightLast = rtY_Right.a_elecAngle; + } + + diffL = rtY_Left.a_elecAngle - motAngleLeftLast; + if (diffL < -180) { + cycleDegsL = cycleDegsL - 360; + } else if (diffL > 180) { + cycleDegsL = cycleDegsL + 360; + } + motPosL = cycleDegsL + (360 - rtY_Left.a_elecAngle); + + diffR = rtY_Right.a_elecAngle - motAngleRightLast; + if (diffR < -180) { + cycleDegsR = cycleDegsR - 360; + } else if (diffR > 180) { + cycleDegsR = cycleDegsR + 360; + } + motPosR = cycleDegsR + (360 - rtY_Right.a_elecAngle); + + motAngleLeftLast = rtY_Left.a_elecAngle; + motAngleRightLast = rtY_Right.a_elecAngle; + cnt++; } diff --git a/board/boards.h b/board/boards.h new file mode 100644 index 0000000..9efd25f --- /dev/null +++ b/board/boards.h @@ -0,0 +1,48 @@ +extern hall_sensor hall_left; +extern hall_sensor hall_right; +uint32_t can_addr_offset; + +void board_detect(void) { + hw_type = board_id(); + // 0 = base, 1 = knee + if (hw_type == HW_TYPE_BASE) { + hall_left.hall_portA = GPIOC; + hall_left.hall_pinA = GPIO_PIN_13; + hall_left.hall_portB = GPIOC; + hall_left.hall_pinB = GPIO_PIN_14; + hall_left.hall_portC = GPIOC; + hall_left.hall_pinC = GPIO_PIN_15; + + hall_right.hall_portA = GPIOC; + hall_right.hall_pinA = GPIO_PIN_10; + hall_right.hall_portB = GPIOC; + hall_right.hall_pinB = GPIO_PIN_11; + hall_right.hall_portC = GPIOC; + hall_right.hall_pinC = GPIO_PIN_12; + + can_addr_offset = 0x0U; + + MX_GPIO_LED_Base_Init(); + } else if (hw_type == HW_TYPE_KNEE) { + hall_left.hall_portA = GPIOC; + hall_left.hall_pinA = GPIO_PIN_14; + hall_left.hall_portB = GPIOC; + hall_left.hall_pinB = GPIO_PIN_15; + hall_left.hall_portC = GPIOC; + hall_left.hall_pinC = GPIO_PIN_13; + + hall_right.hall_portA = GPIOD; + hall_right.hall_pinA = GPIO_PIN_2; + hall_right.hall_portB = GPIOC; + hall_right.hall_pinB = GPIO_PIN_0; + hall_right.hall_portC = GPIOC; + hall_right.hall_pinC = GPIO_PIN_1; + + can_addr_offset = 0x100U; + + MX_SPI3_Init(); + } else { + // Fail to detect, halt + while(1) {} + } +} diff --git a/board/bootstub.c b/board/bootstub.c index e984ef7..6a5a766 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -51,7 +51,13 @@ int main(void) { HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0); SystemClock_Config(); - MX_GPIO_Init(); + MX_GPIO_Clocks_Init(); + + hw_type = board_id(); + if (hw_type == HW_TYPE_BASE) { + MX_GPIO_LED_Base_Init(); + } + MX_GPIO_Common_Init(); out_enable(POWERSWITCH, true); out_enable(LED_RED, false); @@ -64,6 +70,7 @@ int main(void) { HAL_Delay(10); cnt_press++; if (cnt_press == (RECOVERY_MODE_DELAY * 100)) { + out_enable(LED_GREEN, true); soft_flasher_start(); } } diff --git a/board/comms.h b/board/comms.h index f66d78d..12aeaf7 100644 --- a/board/comms.h +++ b/board/comms.h @@ -14,8 +14,12 @@ #include "drivers/llbxcan.h" #include "uds.h" -extern int16_t cmdL; // global variable for Left Command -extern int16_t cmdR; // global variable for Right Command +extern P rtP_Left; +extern P rtP_Right; +extern volatile int16_t cmdL; // global variable for Left Command +extern volatile int16_t cmdR; // global variable for Right Command +extern uint8_t hw_type; +extern uint32_t can_addr_offset; extern uint32_t enter_bootloader_mode; extern volatile uint32_t torque_cmd_timeout; @@ -104,13 +108,14 @@ void CAN2_SCE_IRQHandler(void) { void CAN2_RX0_IRQHandler(void) { while ((CAN2->RF0R & CAN_RF0R_FMP0) != 0) { int address = CAN2->sFIFOMailBox[0].RIR >> 21; - if (address == 0x250U) { + if (address == (int32_t)(0x250U + can_addr_offset)) { if ((GET_MAILBOX_BYTES_04(&CAN2->sFIFOMailBox[0]) == 0xdeadface) && (GET_MAILBOX_BYTES_48(&CAN2->sFIFOMailBox[0]) == 0x0ab00b1e)) { enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; NVIC_SystemReset(); } - uint8_t dat[8]; - for (int i=0; i<8; i++) { + #define MSG_TRQ_LEN 6 + uint8_t dat[MSG_TRQ_LEN]; + for (int i=0; isFIFOMailBox[0], i); } uint16_t valueL = ((dat[0] << 8U) | dat[1]); @@ -125,7 +130,24 @@ void CAN2_RX0_IRQHandler(void) { } current_idx = idx; } - } else if ((address == BROADCAST_ADDR) || (address == FALLBACK_ADDR) || (address == ECU_ADDR) || (address == DEBUG_ADDR)) { // Process UBS and OBD2 requests + } else if (address == (int32_t)(0x251U + can_addr_offset)) { + #define MSG_SPD_LEN 5 + uint8_t dat[MSG_TRQ_LEN]; + for (int i=0; isFIFOMailBox[0], i); + } + uint16_t valueL = ((dat[0] << 8U) | dat[1]); + uint16_t valueR = ((dat[2] << 8U) | dat[3]); + + if (crc_checksum(dat, 4, crc_poly) == dat[4]) { + if ((valueL == 0) || (valueR == 0)) { + rtP_Left.n_max = rtP_Right.n_max = N_MOT_MAX << 4; + } else { + rtP_Left.n_max = valueL << 4; + rtP_Right.n_max = valueR << 4; + } + } + } else if ((hw_type == HW_TYPE_BASE) && ((address == BROADCAST_ADDR) || (address == FALLBACK_ADDR) || (address == ECU_ADDR) || (address == DEBUG_ADDR))) { // Process UBS and OBD2 requests, ignore for knee process_uds(address, GET_MAILBOX_BYTES_04(&CAN2->sFIFOMailBox[0])); } out_enable(LED_BLUE, true); diff --git a/board/config.h b/board/config.h index b543eac..f162e9f 100644 --- a/board/config.h +++ b/board/config.h @@ -4,6 +4,7 @@ #include "stm32f4xx_hal.h" +#define CORE_FREQ 72000000U // MCU frequency in hertz #define PWM_FREQ 16000 // PWM frequency in Hz / is also used for buzzer #define DEAD_TIME 48 // PWM deadtime #define DELAY_IN_MAIN_LOOP 5 // in ms. default 5. it is independent of all the timing critical stuff. do not touch if you do not know what you are doing. @@ -13,6 +14,8 @@ #define ADC_CLOCK_DIV (4) #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 +#define ANGLE_TO_DEGREES 0.021972656 // Convert 14 bit angle sensor output to degrees + #define BAT_FILT_COEF 655 // battery voltage filter coefficient in fixed-point. coef_fixedPoint = coef_floatingPoint * 2^16. In this case 655 = 0.01 * 2^16 #define BAT_CALIB_REAL_VOLTAGE 3192 // input voltage measured by multimeter (multiplied by 100). In this case 43.00 V * 100 = 4300 #define BAT_CALIB_ADC 1275 // adc-value measured by mainboard (value nr 5 on UART debug output) diff --git a/board/defines.h b/board/defines.h index 01bcf11..ae008bb 100644 --- a/board/defines.h +++ b/board/defines.h @@ -5,22 +5,6 @@ #include "stm32f4xx_hal.h" #include "config.h" -#define LEFT_HALL_U_PIN GPIO_PIN_13 -#define LEFT_HALL_V_PIN GPIO_PIN_14 -#define LEFT_HALL_W_PIN GPIO_PIN_15 - -#define LEFT_HALL_U_PORT GPIOC -#define LEFT_HALL_V_PORT GPIOC -#define LEFT_HALL_W_PORT GPIOC - -#define RIGHT_HALL_U_PIN GPIO_PIN_10 -#define RIGHT_HALL_V_PIN GPIO_PIN_11 -#define RIGHT_HALL_W_PIN GPIO_PIN_12 - -#define RIGHT_HALL_U_PORT GPIOC -#define RIGHT_HALL_V_PORT GPIOC -#define RIGHT_HALL_W_PORT GPIOC - #define LEFT_TIM TIM8 #define LEFT_TIM_U CCR1 #define LEFT_TIM_UH_PIN GPIO_PIN_6 @@ -71,18 +55,26 @@ #define RIGHT_U_CUR_PORT GPIOA #define RIGHT_V_CUR_PORT GPIOA -#define DCLINK_PIN GPIO_PIN_4 -#define DCLINK_PORT GPIOA +#define BATT_PIN GPIO_PIN_4 +#define BATT_PORT GPIOA -// RED -#define LED_RED_PIN GPIO_PIN_2 -#define LED_RED_PORT GPIOD +#define SPI3_SCK_PIN GPIO_PIN_10 +#define SPI3_MISO_PIN GPIO_PIN_11 +#define SPI3_MOSI_PIN GPIO_PIN_12 +#define SPI3_PORT GPIOC + +#define AS5048_CS_PORT GPIOB +#define AS5048A_CS_PIN GPIO_PIN_2 // GREEN #define LED_GREEN_PIN GPIO_PIN_15 #define LED_GREEN_PORT GPIOA -// BLUE +// RED (only for base) +#define LED_RED_PIN GPIO_PIN_2 +#define LED_RED_PORT GPIOD + +// BLUE (only for base) #define LED_BLUE_PIN GPIO_PIN_1 #define LED_BLUE_PORT GPIOC @@ -104,6 +96,12 @@ #define CHARGER_PIN GPIO_PIN_12 #define CHARGER_PORT GPIOA +// UID pins +#define KEY1_PIN GPIO_PIN_10 +#define KEY1_PORT GPIOB +#define KEY2_PIN GPIO_PIN_9 +#define KEY2_PORT GPIOC + #define DELAY_TIM_FREQUENCY_US 1000000 #define MILLI_R (R * 1000) @@ -149,6 +147,9 @@ #define POWERSWITCH 4 #define TRANSCEIVER 5 +#define HW_TYPE_BASE 0 +#define HW_TYPE_KNEE 1 + typedef struct { uint32_t rrB; uint32_t rrC; @@ -160,4 +161,15 @@ typedef struct { uint32_t temp; } adc_buf_t; +typedef struct { + GPIO_TypeDef* hall_portA; + uint16_t hall_pinA; + GPIO_TypeDef* hall_portB; + uint16_t hall_pinB; + GPIO_TypeDef* hall_portC; + uint16_t hall_pinC; +} hall_sensor; + +uint8_t hw_type; // type of the board detected(0 - base, 1 - knee) + #endif // DEFINES_H diff --git a/board/drivers/angle_sensor.h b/board/drivers/angle_sensor.h new file mode 100644 index 0000000..78dd96a --- /dev/null +++ b/board/drivers/angle_sensor.h @@ -0,0 +1,69 @@ +#define SENSOR_COUNT 2 // Limit is 4! If more is needed - CAN message should be changed + +#define SPI_TIMEOUT 2710 + +#define SPI_CMD_READ 0x4000U // Read command +#define SPI_REG_AGC 0x3FFDU // AGC register +#define SPI_REG_MAG 0x3FFEU // Magnitude register +#define SPI_REG_DATA 0x3FFFU // Angle register +#define SPI_REG_CLRERR 0x0001U // Clear error flag, should be used with read command +#define SPI_NOP 0x0000U // NOP, to read data on the next transfer +#define PARITY_BIT_SET 0x8000U + +extern SPI_HandleTypeDef hspi3; + +uint16_t angle_data[SENSOR_COUNT] = { 0 }; + +const uint16_t clear_error_cmd = (SPI_CMD_READ | SPI_REG_CLRERR); +const uint16_t read_angle_cmd = (PARITY_BIT_SET | SPI_CMD_READ | SPI_REG_DATA); +const uint16_t nop_cmd = SPI_NOP; + + +uint8_t spiCalcEvenParity(uint16_t value) { + uint8_t cnt = 0; + for (uint8_t i = 0; i < 16; i++) { + if (value & 0x1) { + cnt++; + } + value >>= 1; + } + return cnt & 0x1; +} + +void angle_sensor_read(uint16_t *sensor_angle) { + HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_RESET); + for (uint8_t i = 0; i < SENSOR_COUNT; i++) { + HAL_SPI_Transmit(&hspi3, (uint8_t*)&read_angle_cmd, 1, SPI_TIMEOUT); + } + HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_SET); + HAL_Delay(1); + + HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_RESET); + for (int8_t i = (SENSOR_COUNT-1); i >= 0; i--) { + HAL_SPI_TransmitReceive(&hspi3, (uint8_t*)&nop_cmd, (uint8_t*)(&angle_data[i]), 1, 2710); + } + HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_SET); + HAL_Delay(1); + + bool error_flag_set = false; + for (uint8_t i = 0; i < SENSOR_COUNT; i++) { + if ((angle_data[i] >> 15) == spiCalcEvenParity((angle_data[i] & 0x7fff))) { + if (angle_data[i] & SPI_CMD_READ) { + error_flag_set = true; + } else { + if ((sensor_angle[i] == 0) || (ABS(sensor_angle[i] - (angle_data[i] & 0x3fff)) < 200)) { + sensor_angle[i] = (angle_data[i] & 0x3fff); + } + } + } + } + + if (error_flag_set) { + HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_RESET); + for (uint8_t i = 0; i < SENSOR_COUNT; i++) { + HAL_SPI_Transmit(&hspi3, (uint8_t*)&clear_error_cmd, 1, SPI_TIMEOUT); + } + HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_SET); + HAL_Delay(1); + } +} diff --git a/board/drivers/clock.h b/board/drivers/clock.h index d71bf03..35afc90 100644 --- a/board/drivers/clock.h +++ b/board/drivers/clock.h @@ -29,7 +29,7 @@ void SystemClock_Config(void) { RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; RCC_OscInitStruct.PLL.PLLM = 8; - RCC_OscInitStruct.PLL.PLLN = 72; + RCC_OscInitStruct.PLL.PLLN = 72; // Gives 72 Mhz core clock RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; RCC_OscInitStruct.PLL.PLLQ = 2; RCC_OscInitStruct.PLL.PLLR = 2; diff --git a/board/drivers/llbxcan.h b/board/drivers/llbxcan.h index ce5dc58..6d1103b 100644 --- a/board/drivers/llbxcan.h +++ b/board/drivers/llbxcan.h @@ -3,7 +3,7 @@ #define CAN_SEQ1 6U // roundf(quanta * 0.875f) - 1; #define CAN_SEQ2 1U // roundf(quanta * 0.125f); -#define CAN_PCLK 36000U +#define CAN_PCLK (CORE_FREQ / 2U / 1000U) // 333 = 33.3 kbps // 5000 = 500 kbps #define can_speed_to_prescaler(x) (CAN_PCLK / CAN_QUANTA * 10U / (x)) diff --git a/board/flasher.h b/board/flasher.h index 3faab0d..8d8fec4 100644 --- a/board/flasher.h +++ b/board/flasher.h @@ -26,6 +26,7 @@ bool unlocked = false; #define CAN CAN2 +// Also being offset by hw_type value*2, for different board types #define CAN_BL_INPUT 0x1 #define CAN_BL_OUTPUT 0x2 @@ -146,12 +147,12 @@ void bl_can_send(uint8_t *odat) { CAN->sTxMailBox[0].TDLR = ((uint32_t*)odat)[0]; CAN->sTxMailBox[0].TDHR = ((uint32_t*)odat)[1]; CAN->sTxMailBox[0].TDTR = 8; - CAN->sTxMailBox[0].TIR = (CAN_BL_OUTPUT << 21) | 1; + CAN->sTxMailBox[0].TIR = ((CAN_BL_OUTPUT+(hw_type*2U)) << 21) | 1; } void CAN2_RX0_IRQHandler(void) { while (CAN->RF0R & CAN_RF0R_FMP0) { - if ((CAN->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) { + if ((CAN->sFIFOMailBox[0].RIR>>21) == (CAN_BL_INPUT+(hw_type*2U))) { uint8_t dat[8]; for (int i = 0; i < 8; i++) { dat[i] = GET_MAILBOX_BYTE(&CAN->sFIFOMailBox[0], i); @@ -264,8 +265,10 @@ void soft_flasher_start(void) { llcan_set_speed(CAN, 5000, false, false); llcan_init(CAN); - // green LED on for flashing out_enable(LED_BLUE, true); + // Wait for power button release + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} + out_enable(LED_GREEN, false); uint64_t cnt = 0; diff --git a/board/inc/stm32f4xx_hal_conf.h b/board/inc/stm32f4xx_hal_conf.h index de92e6d..755d3ab 100644 --- a/board/inc/stm32f4xx_hal_conf.h +++ b/board/inc/stm32f4xx_hal_conf.h @@ -62,7 +62,7 @@ /* #define HAL_RTC_MODULE_ENABLED */ /* #define HAL_SAI_MODULE_ENABLED */ /* #define HAL_SD_MODULE_ENABLED */ -/* #define HAL_SPI_MODULE_ENABLED */ +#define HAL_SPI_MODULE_ENABLED #define HAL_TIM_MODULE_ENABLED #define HAL_UART_MODULE_ENABLED /* #define HAL_USART_MODULE_ENABLED */ diff --git a/board/main.c b/board/main.c index a205ca5..0c79a24 100644 --- a/board/main.c +++ b/board/main.c @@ -13,19 +13,17 @@ #include "comms.h" #include "drivers/clock.h" #include "early_init.h" +#include "drivers/angle_sensor.h" +#include "boards.h" -uint32_t enter_bootloader_mode; - -void __initialize_hardware_early(void) { - early_initialization(); -} //------------------------------------------------------------------------ // Global variables set externally //------------------------------------------------------------------------ extern TIM_HandleTypeDef htim_left; extern TIM_HandleTypeDef htim_right; extern ADC_HandleTypeDef hadc; +extern SPI_HandleTypeDef hspi3; extern volatile adc_buf_t adc_buffer; // Matlab defines - from auto-code generation @@ -48,6 +46,9 @@ extern uint8_t enable_motors; // global variable for motor enab extern int16_t batVoltage; // global variable for battery voltage +extern int32_t motPosL; +extern int32_t motPosR; + //------------------------------------------------------------------------ // Global variables set here in main.c //------------------------------------------------------------------------ @@ -57,8 +58,10 @@ volatile uint32_t torque_cmd_timeout; volatile uint32_t ignition_off_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 cmdL; // global variable for Left Command -int16_t cmdR; // global variable for Right Command +volatile int16_t cmdL; // global variable for Left Command +volatile int16_t cmdR; // global variable for Right Command + +uint32_t can_addr_offset; // CAN messages addresses offset between different board types uint8_t ignition = 0; // global variable for ignition on SBU2 line uint8_t charger_connected = 0; // status of the charger port @@ -70,6 +73,9 @@ uint8_t pkt_idx = 0; // For CAN msg counter //------------------------------------------------------------------------ static uint32_t buzzerTimer_prev = 0U; +void __initialize_hardware_early(void) { + early_initialization(); +} int main(void) { HAL_Init(); @@ -83,9 +89,12 @@ int main(void) { HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0); SystemClock_Config(); + MX_GPIO_Clocks_Init(); __HAL_RCC_DMA2_CLK_DISABLE(); - MX_GPIO_Init(); + + board_detect(); + MX_GPIO_Common_Init(); MX_TIM_Init(); MX_ADC_Init(); BLDC_Init(); @@ -111,6 +120,11 @@ int main(void) { 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; + uint8_t angle_sensor_error = 0; + uint16_t sensor_angle[SENSOR_COUNT] = { 0 }; + uint16_t hall_angle_offset[SENSOR_COUNT] = { 0 }; + angle_sensor_read(sensor_angle); + // Loop until button is released while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } @@ -131,12 +145,52 @@ int main(void) { beepShort(6); // make 2 beeps indicating the motor enable beepShort(4); HAL_Delay(100); + angle_sensor_error = 0; + sensor_angle[0] = 0; + sensor_angle[1] = 0; + angle_sensor_read(sensor_angle); + hall_angle_offset[0] = (sensor_angle[0] * ANGLE_TO_DEGREES); + hall_angle_offset[1] = (sensor_angle[1] * ANGLE_TO_DEGREES); cmdL = cmdR = 0; enable_motors = 1; // enable motors } - pwml = CLAMP((int)cmdL, -1000, 1000); - pwmr = -CLAMP((int)cmdR, -1000, 1000); + if (hw_type == HW_TYPE_KNEE) { + angle_sensor_read(sensor_angle); + // Safety to stop operation if angle sensor reading failed TODO: adjust sensivity and add lowpass to angle sensor? + if ((ABS((hall_angle_offset[0] + ((motPosL / 15 / 11) % 360)) - (sensor_angle[0] * ANGLE_TO_DEGREES)) > 5) || + (ABS((hall_angle_offset[1] + ((motPosR / 15 / 11) % 360)) - (sensor_angle[1] * ANGLE_TO_DEGREES)) > 5)) { + angle_sensor_error = 1; + cmdL = cmdR = 0; + } + // Safety to stop movement when reaching dead angles, around 20 and 340 degrees + if (((sensor_angle[0] < 900) && (cmdL < 0)) || ((sensor_angle[0] > 15500) && (cmdL > 0))) { + cmdL = 0; + } + if (((sensor_angle[1] < 900) && (cmdR < 0)) || ((sensor_angle[1] > 15500) && (cmdR > 0))) { + cmdR = 0; + } + } + + if (hw_type == HW_TYPE_KNEE) { + if ((ABS(cmdL) < 20) || angle_sensor_error) { + rtP_Left.n_cruiseMotTgt = 0; + rtP_Left.b_cruiseCtrlEna = 1; + } else { + rtP_Left.b_cruiseCtrlEna = 0; + pwml = -CLAMP((int)cmdL, -1000, 1000); + } + if ((ABS(cmdR) < 20) || angle_sensor_error) { + rtP_Right.n_cruiseMotTgt = 0; + rtP_Right.b_cruiseCtrlEna = 1; + } else { + rtP_Right.b_cruiseCtrlEna = 0; + pwmr = -CLAMP((int)cmdR, -1000, 1000); + } + } else { + pwml = CLAMP((int)cmdL, -1000, 1000); + pwmr = -CLAMP((int)cmdR, -1000, 1000); + } // ####### CALC BOARD TEMPERATURE ####### filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt); @@ -157,11 +211,11 @@ int main(void) { dat[1] = speedL & 0xFFU; dat[2] = (speedR >> 8U) & 0xFFU; dat[3] = speedR & 0xFFU; - dat[4] = rtY_Left.a_elecAngle; - dat[5] = rtY_Right.a_elecAngle; + dat[4] = 0; // TODO: remove from OP and dbc, no value + dat[5] = 0; // TODO: remove from OP and dbc, no value dat[6] = pkt_idx; dat[7] = crc_checksum(dat, 7, crc_poly); - can_send_msg(0x201U, ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); + can_send_msg((0x201U + can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); ++pkt_idx; pkt_idx &= 0xFU; @@ -174,7 +228,27 @@ int main(void) { dat[5] = rtU_Right.i_phaAB & 0xFFU; dat[6] = (rtU_Right.i_phaBC >> 8U) & 0xFFU; dat[7] = rtU_Right.i_phaBC & 0xFFU; - can_send_msg(0x204U, ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); + can_send_msg((0x204U + can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); + + uint16_t one; + uint16_t two; + if (hw_type == HW_TYPE_KNEE) { + one = hall_angle_offset[0] + ((motPosL / 15 / 11) % 360); + two = hall_angle_offset[1] + ((motPosR / 15 / 11) % 360); + } else { + one = motPosL / 15; + two = -motPosR / 15; + } + // first angle sensor(2), second angle sensor(2) + dat[0] = (sensor_angle[0]>>8U) & 0xFFU; + dat[1] = sensor_angle[0] & 0xFFU; + dat[2] = (sensor_angle[1]>>8U) & 0xFFU; + dat[3] = sensor_angle[1] & 0xFFU; + dat[4] = (one>>8U) & 0xFFU; + dat[5] = one & 0xFFU; + dat[6] = (two>>8U) & 0xFFU; + dat[7] = two & 0xFFU; + can_send_msg((0x205U + can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); } } @@ -186,7 +260,7 @@ int main(void) { dat[0] = (((fault_status & 0x3F) << 2U) | (enable_motors << 1U) | ignition); dat[1] = rtY_Left.z_errCode; dat[2] = rtY_Right.z_errCode; - can_send_msg(0x202U, 0x0U, ((dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 3U); + can_send_msg((0x202U + can_addr_offset), 0x0U, ((dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 3U); } out_enable(LED_GREEN, ignition); } @@ -202,7 +276,7 @@ int main(void) { dat[1] = (batVoltageCalib >> 8U) & 0xFFU; dat[2] = batVoltageCalib & 0xFFU; dat[3] = (((battery_percent & 0x7FU) << 1U) | charger_connected); - can_send_msg(0x203U, 0x0U, ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 4U); + can_send_msg((0x203U + can_addr_offset), 0x0U, ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 4U); out_enable(LED_BLUE, false); // Reset LED after CAN RX out_enable(LED_GREEN, true); // Always use LED to show that body is on @@ -231,7 +305,7 @@ int main(void) { beepCount(0, 10, 30); } else { // do not beep beepCount(0, 0, 0); - out_enable(LED_RED, false); + //out_enable(LED_RED, false); } buzzerTimer_prev = buzzerTimer; diff --git a/board/setup.h b/board/setup.h index 01d85c3..63ecb1e 100644 --- a/board/setup.h +++ b/board/setup.h @@ -7,40 +7,45 @@ TIM_HandleTypeDef htim_right; TIM_HandleTypeDef htim_left; ADC_HandleTypeDef hadc; +SPI_HandleTypeDef hspi3; volatile adc_buf_t adc_buffer; +hall_sensor hall_left; +hall_sensor hall_right; -void MX_GPIO_Init(void) { - GPIO_InitTypeDef GPIO_InitStruct; - +void MX_GPIO_Clocks_Init(void) { /* GPIO Ports Clock Enable */ __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); __HAL_RCC_GPIOC_CLK_ENABLE(); __HAL_RCC_GPIOD_CLK_ENABLE(); +} + +void MX_GPIO_Common_Init(void) { + GPIO_InitTypeDef GPIO_InitStruct; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.Pin = LEFT_HALL_U_PIN; - HAL_GPIO_Init(LEFT_HALL_U_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = hall_left.hall_pinA; + HAL_GPIO_Init(hall_left.hall_portA, &GPIO_InitStruct); - GPIO_InitStruct.Pin = LEFT_HALL_V_PIN; - HAL_GPIO_Init(LEFT_HALL_V_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = hall_left.hall_pinB; + HAL_GPIO_Init(hall_left.hall_portB, &GPIO_InitStruct); - GPIO_InitStruct.Pin = LEFT_HALL_W_PIN; - HAL_GPIO_Init(LEFT_HALL_W_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = hall_left.hall_pinC; + HAL_GPIO_Init(hall_left.hall_portC, &GPIO_InitStruct); - GPIO_InitStruct.Pin = RIGHT_HALL_U_PIN; - HAL_GPIO_Init(RIGHT_HALL_U_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = hall_right.hall_pinA; + HAL_GPIO_Init(hall_right.hall_portA, &GPIO_InitStruct); - GPIO_InitStruct.Pin = RIGHT_HALL_V_PIN; - HAL_GPIO_Init(RIGHT_HALL_V_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = hall_right.hall_pinB; + HAL_GPIO_Init(hall_right.hall_portB, &GPIO_InitStruct); - GPIO_InitStruct.Pin = RIGHT_HALL_W_PIN; - HAL_GPIO_Init(RIGHT_HALL_W_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = hall_right.hall_pinC; + HAL_GPIO_Init(hall_right.hall_portC, &GPIO_InitStruct); GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Pin = CHARGER_PIN; @@ -54,15 +59,9 @@ void MX_GPIO_Init(void) { GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pin = LED_RED_PIN; - HAL_GPIO_Init(LED_RED_PORT, &GPIO_InitStruct); - GPIO_InitStruct.Pin = LED_GREEN_PIN; HAL_GPIO_Init(LED_GREEN_PORT, &GPIO_InitStruct); - GPIO_InitStruct.Pin = LED_BLUE_PIN; - HAL_GPIO_Init(LED_BLUE_PORT, &GPIO_InitStruct); - GPIO_InitStruct.Pin = CAN_STBY_PIN; HAL_GPIO_Init(CAN_STBY_PORT, &GPIO_InitStruct); @@ -96,8 +95,8 @@ void MX_GPIO_Init(void) { GPIO_InitStruct.Pin = RIGHT_V_CUR_PIN; HAL_GPIO_Init(RIGHT_V_CUR_PORT, &GPIO_InitStruct); - GPIO_InitStruct.Pin = DCLINK_PIN; - HAL_GPIO_Init(DCLINK_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = BATT_PIN; + HAL_GPIO_Init(BATT_PORT, &GPIO_InitStruct); GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Alternate = GPIO_AF3_TIM8; @@ -151,6 +150,65 @@ void MX_GPIO_Init(void) { HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); } +void MX_GPIO_LED_Base_Init(void) { + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + + GPIO_InitStruct.Pin = LED_RED_PIN; + HAL_GPIO_Init(LED_RED_PORT, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = LED_BLUE_PIN; + HAL_GPIO_Init(LED_BLUE_PORT, &GPIO_InitStruct); + +} + +void MX_SPI3_Init(void) { + GPIO_InitTypeDef GPIO_InitStruct; + + GPIO_InitStruct.Pin = AS5048A_CS_PIN; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_PULLUP; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(AS5048_CS_PORT, &GPIO_InitStruct); + HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_SET); + + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Alternate = GPIO_AF6_SPI3; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Pin = SPI3_SCK_PIN; + HAL_GPIO_Init(SPI3_PORT, &GPIO_InitStruct); + + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Pin = SPI3_MISO_PIN; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(SPI3_PORT, &GPIO_InitStruct); + + GPIO_InitStruct.Pull = GPIO_PULLDOWN; + GPIO_InitStruct.Pin = SPI3_MOSI_PIN; + HAL_GPIO_Init(SPI3_PORT, &GPIO_InitStruct); + + __HAL_RCC_SPI3_CLK_ENABLE(); + + hspi3.Instance = SPI3; + hspi3.Init.Mode = SPI_MODE_MASTER; + hspi3.Init.Direction = SPI_DIRECTION_2LINES; + hspi3.Init.DataSize = SPI_DATASIZE_16BIT; + hspi3.Init.CLKPolarity = SPI_POLARITY_LOW; + hspi3.Init.CLKPhase = SPI_PHASE_2EDGE; + hspi3.Init.NSS = SPI_NSS_SOFT; + hspi3.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32; // 1.125 MHz at 72MHz clock + hspi3.Init.FirstBit = SPI_FIRSTBIT_MSB; + hspi3.Init.TIMode = SPI_TIMODE_DISABLE; + hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + hspi3.Init.CRCPolynomial = 10; + + HAL_SPI_Init(&hspi3); +} + void MX_TIM_Init(void) { __HAL_RCC_TIM1_CLK_ENABLE(); __HAL_RCC_TIM8_CLK_ENABLE(); @@ -163,7 +221,7 @@ void MX_TIM_Init(void) { htim_right.Instance = RIGHT_TIM; htim_right.Init.Prescaler = 0; htim_right.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; - htim_right.Init.Period = SystemCoreClock / 2 / PWM_FREQ; // Was 64000000 before SystemCoreClock + htim_right.Init.Period = CORE_FREQ / 2 / PWM_FREQ; htim_right.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim_right.Init.RepetitionCounter = 0; htim_right.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; @@ -196,7 +254,7 @@ void MX_TIM_Init(void) { htim_left.Instance = LEFT_TIM; htim_left.Init.Prescaler = 0; htim_left.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; - htim_left.Init.Period = SystemCoreClock / 2 / PWM_FREQ; // Was 64000000 before SystemCoreClock + htim_left.Init.Period = CORE_FREQ / 2 / PWM_FREQ; htim_left.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim_left.Init.RepetitionCounter = 0; htim_left.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; @@ -273,16 +331,8 @@ void MX_ADC_Init(void) { hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; hadc.Init.NbrOfConversion = 8; HAL_ADC_Init(&hadc); - /**Enable or disable the remapping of ADC1_ETRGREG: - * ADC1 External Event regular conversion is connected to TIM8 TRG0 - */ - //__HAL_AFIO_REMAP_ADC1_ETRGREG_ENABLE(); - - /**Configure the ADC multi-mode - */ - //multimode.Mode = ADC_DUALMODE_REGSIMULT; - HAL_ADCEx_MultiModeConfigChannel(&hadc, &multimode); + HAL_ADCEx_MultiModeConfigChannel(&hadc, &multimode); sConfig.SamplingTime = ADC_SAMPLETIME_15CYCLES; sConfig.Channel = ADC_CHANNEL_5; // pa5 left b -> right @@ -313,7 +363,6 @@ void MX_ADC_Init(void) { sConfig.Rank = 7; HAL_ADC_ConfigChannel(&hadc, &sConfig); - //temperature requires at least 17.1uS sampling time sConfig.SamplingTime = ADC_SAMPLETIME_144CYCLES; sConfig.Channel = ADC_CHANNEL_TEMPSENSOR; // internal temp sConfig.Rank = 8; @@ -340,4 +389,3 @@ void MX_ADC_Init(void) { #endif - diff --git a/board/util.c b/board/util.c index a6e0a3e..7ffcf9e 100644 --- a/board/util.c +++ b/board/util.c @@ -17,6 +17,8 @@ extern uint8_t buzzerPattern; // global variable for the buzzer patter extern uint8_t enable_motors; // global variable for motor enable extern uint8_t ignition; // global variable for ignition on SBU2 line +extern uint8_t hw_type; + //------------------------------------------------------------------------ // Matlab defines - from auto-code generation //--------------- @@ -58,6 +60,7 @@ void BLDC_Init(void) { rtP_Left.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4) rtP_Right = rtP_Left; // Copy the Left motor parameters to the Right motor parameters + rtP_Right.n_max = N_MOT_MAX << 4; // But add separate max RPM limit rtP_Right.z_selPhaCurMeasABC = 1; // Right motor measured current phases {Blue, Yellow} = {iB, iC} -> do NOT change /* Pack LEFT motor data into RTM */ @@ -77,16 +80,20 @@ void BLDC_Init(void) { BLDC_controller_initialize(rtM_Right); } -void out_enable(uint8_t led, bool enabled) { - switch(led) { - case LED_RED: - HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, !enabled); - break; +void out_enable(uint8_t out, bool enabled) { + switch(out) { case LED_GREEN: HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, !enabled); break; + case LED_RED: + if (hw_type == HW_TYPE_BASE) { + HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, !enabled); + } + break; case LED_BLUE: - HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, !enabled); + if (hw_type == HW_TYPE_BASE) { + HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, !enabled); + } break; case IGNITION: HAL_GPIO_WritePin(IGNITION_PORT, IGNITION_PIN, enabled); @@ -184,6 +191,24 @@ void poweroffPressCheck(void) { } } +#define PULL_EFFECTIVE_DELAY 4096 +uint8_t detect_with_pull(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin, uint32_t mode) { + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Pin = GPIO_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = mode; + HAL_GPIO_Init(GPIOx, &GPIO_InitStruct); + for (volatile int i=0; i Date: Mon, 20 Jun 2022 15:32:57 -0700 Subject: [PATCH 02/18] 19 gb --- board/drivers/angle_sensor.h | 2 +- board/main.c | 25 +++++++++++++++++-------- 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/board/drivers/angle_sensor.h b/board/drivers/angle_sensor.h index 78dd96a..28463f2 100644 --- a/board/drivers/angle_sensor.h +++ b/board/drivers/angle_sensor.h @@ -51,7 +51,7 @@ void angle_sensor_read(uint16_t *sensor_angle) { if (angle_data[i] & SPI_CMD_READ) { error_flag_set = true; } else { - if ((sensor_angle[i] == 0) || (ABS(sensor_angle[i] - (angle_data[i] & 0x3fff)) < 200)) { + if ((sensor_angle[i] == 0) || (ABS(sensor_angle[i] - (angle_data[i] & 0x3fff)) < 400)) { sensor_angle[i] = (angle_data[i] & 0x3fff); } } diff --git a/board/main.c b/board/main.c index 0c79a24..673f138 100644 --- a/board/main.c +++ b/board/main.c @@ -120,6 +120,9 @@ int main(void) { 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; + #define GEARBOX_RATIO_LEFT 19 + #define GEARBOX_RATIO_RIGHT 19 + uint8_t angle_sensor_error = 0; uint16_t sensor_angle[SENSOR_COUNT] = { 0 }; uint16_t hall_angle_offset[SENSOR_COUNT] = { 0 }; @@ -158,10 +161,10 @@ int main(void) { if (hw_type == HW_TYPE_KNEE) { angle_sensor_read(sensor_angle); // Safety to stop operation if angle sensor reading failed TODO: adjust sensivity and add lowpass to angle sensor? - if ((ABS((hall_angle_offset[0] + ((motPosL / 15 / 11) % 360)) - (sensor_angle[0] * ANGLE_TO_DEGREES)) > 5) || - (ABS((hall_angle_offset[1] + ((motPosR / 15 / 11) % 360)) - (sensor_angle[1] * ANGLE_TO_DEGREES)) > 5)) { - angle_sensor_error = 1; - cmdL = cmdR = 0; + if ((ABS((hall_angle_offset[0] + ((motPosL / 15 / GEARBOX_RATIO_LEFT) % 360)) - (sensor_angle[0] * ANGLE_TO_DEGREES)) > 5) || + (ABS((hall_angle_offset[1] + ((motPosR / 15 / GEARBOX_RATIO_RIGHT) % 360)) - (sensor_angle[1] * ANGLE_TO_DEGREES)) > 5)) { + angle_sensor_error += 1; + //cmdL = cmdR = 0; } // Safety to stop movement when reaching dead angles, around 20 and 340 degrees if (((sensor_angle[0] < 900) && (cmdL < 0)) || ((sensor_angle[0] > 15500) && (cmdL > 0))) { @@ -173,14 +176,14 @@ int main(void) { } if (hw_type == HW_TYPE_KNEE) { - if ((ABS(cmdL) < 20) || angle_sensor_error) { + if ((ABS(cmdL) < 20) /*|| angle_sensor_error*/) { rtP_Left.n_cruiseMotTgt = 0; rtP_Left.b_cruiseCtrlEna = 1; } else { rtP_Left.b_cruiseCtrlEna = 0; pwml = -CLAMP((int)cmdL, -1000, 1000); } - if ((ABS(cmdR) < 20) || angle_sensor_error) { + if ((ABS(cmdR) < 20) /*|| angle_sensor_error*/) { rtP_Right.n_cruiseMotTgt = 0; rtP_Right.b_cruiseCtrlEna = 1; } else { @@ -233,8 +236,8 @@ int main(void) { uint16_t one; uint16_t two; if (hw_type == HW_TYPE_KNEE) { - one = hall_angle_offset[0] + ((motPosL / 15 / 11) % 360); - two = hall_angle_offset[1] + ((motPosR / 15 / 11) % 360); + one = hall_angle_offset[0] + ((motPosL / 15 / GEARBOX_RATIO_LEFT) % 360); + two = hall_angle_offset[1] + ((motPosR / 15 / GEARBOX_RATIO_RIGHT) % 360); } else { one = motPosL / 15; two = -motPosR / 15; @@ -249,6 +252,12 @@ int main(void) { dat[6] = (two>>8U) & 0xFFU; dat[7] = two & 0xFFU; can_send_msg((0x205U + can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); + + /// TEST MSG FOR SENSOR ERROR COUNTER + if (angle_sensor_error > 0) { + can_send_msg((0x20U), (0x0U), (0x0U), 1U); + angle_sensor_error -= 1; + } } } From d3afaf06ea1a7433e9bb5cdfd08725707c2c41ef Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Thu, 23 Jun 2022 14:34:01 -0700 Subject: [PATCH 03/18] switch to i2c --- board/SConscript | 3 +- board/bldc/bldc.c | 15 ++-- board/boards.h | 109 +++++++++++++++++---------- board/bootstub.c | 6 +- board/comms.h | 70 +++++++++++------- board/defines.h | 50 +++++++------ board/drivers/angle_sensor.h | 81 +++++--------------- board/drivers/llbxcan.h | 20 +++-- board/flasher.h | 2 +- board/inc/stm32f4xx_hal_conf.h | 20 ++--- board/main.c | 27 +++---- board/setup.h | 131 ++++++++++++++------------------- board/util.c | 16 ++-- 13 files changed, 273 insertions(+), 277 deletions(-) diff --git a/board/SConscript b/board/SConscript index f9abe6a..356d188 100644 --- a/board/SConscript +++ b/board/SConscript @@ -48,7 +48,8 @@ c_sources = [ ["hal_flash", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c"], ["hal_pwr", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c"], ["hal_rcc", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c"], - ["hal_spi", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c"], + ["hal_i2c", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c"], + ["hal_i2c_ex", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c"], ["hal_tim", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c"], ["hal_tim_ex", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c"], ["hal_adc_ex", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c"], diff --git a/board/bldc/bldc.c b/board/bldc/bldc.c index 8324c3a..1c5ea11 100644 --- a/board/bldc/bldc.c +++ b/board/bldc/bldc.c @@ -32,8 +32,7 @@ #include "BLDC_controller.h" /* Model's header file */ #include "rtwtypes.h" -extern hall_sensor hall_left; -extern hall_sensor hall_right; +extern board_t board; extern RT_MODEL *const rtM_Left; extern RT_MODEL *const rtM_Right; @@ -169,9 +168,9 @@ void DMA2_Stream0_IRQHandler(void) { enableFin = enable_motors && !rtY_Left.z_errCode && !rtY_Right.z_errCode; // ========================= LEFT MOTOR ============================ - uint8_t hall_ul = !(hall_left.hall_portA->IDR & hall_left.hall_pinA); - uint8_t hall_vl = !(hall_left.hall_portB->IDR & hall_left.hall_pinB); - uint8_t hall_wl = !(hall_left.hall_portC->IDR & hall_left.hall_pinC); + uint8_t hall_ul = !(board.hall_left.hall_portA->IDR & board.hall_left.hall_pinA); + uint8_t hall_vl = !(board.hall_left.hall_portB->IDR & board.hall_left.hall_pinB); + uint8_t hall_wl = !(board.hall_left.hall_portC->IDR & board.hall_left.hall_pinC); rtU_Left.b_motEna = enableFin; rtU_Left.z_ctrlModReq = ctrlModReq; @@ -199,9 +198,9 @@ void DMA2_Stream0_IRQHandler(void) { // ========================= RIGHT MOTOR =========================== - uint8_t hall_ur = !(hall_right.hall_portA->IDR & hall_right.hall_pinA); - uint8_t hall_vr = !(hall_right.hall_portB->IDR & hall_right.hall_pinB); - uint8_t hall_wr = !(hall_right.hall_portC->IDR & hall_right.hall_pinC); + uint8_t hall_ur = !(board.hall_right.hall_portA->IDR & board.hall_right.hall_pinA); + uint8_t hall_vr = !(board.hall_right.hall_portB->IDR & board.hall_right.hall_pinB); + uint8_t hall_wr = !(board.hall_right.hall_portC->IDR & board.hall_right.hall_pinC); rtU_Right.b_motEna = enableFin; rtU_Right.z_ctrlModReq = ctrlModReq; diff --git a/board/boards.h b/board/boards.h index 9efd25f..29efb9a 100644 --- a/board/boards.h +++ b/board/boards.h @@ -1,46 +1,79 @@ -extern hall_sensor hall_left; -extern hall_sensor hall_right; -uint32_t can_addr_offset; +board_t board; void board_detect(void) { hw_type = board_id(); - // 0 = base, 1 = knee + // 0 = base, 3 = knee if (hw_type == HW_TYPE_BASE) { - hall_left.hall_portA = GPIOC; - hall_left.hall_pinA = GPIO_PIN_13; - hall_left.hall_portB = GPIOC; - hall_left.hall_pinB = GPIO_PIN_14; - hall_left.hall_portC = GPIOC; - hall_left.hall_pinC = GPIO_PIN_15; - - hall_right.hall_portA = GPIOC; - hall_right.hall_pinA = GPIO_PIN_10; - hall_right.hall_portB = GPIOC; - hall_right.hall_pinB = GPIO_PIN_11; - hall_right.hall_portC = GPIOC; - hall_right.hall_pinC = GPIO_PIN_12; - - can_addr_offset = 0x0U; - - MX_GPIO_LED_Base_Init(); + board.hall_left.hall_portA = GPIOC; + board.hall_left.hall_pinA = GPIO_PIN_13; + board.hall_left.hall_portB = GPIOC; + board.hall_left.hall_pinB = GPIO_PIN_14; + board.hall_left.hall_portC = GPIOC; + board.hall_left.hall_pinC = GPIO_PIN_15; + + board.hall_right.hall_portA = GPIOC; + board.hall_right.hall_pinA = GPIO_PIN_10; + board.hall_right.hall_portB = GPIOC; + board.hall_right.hall_pinB = GPIO_PIN_11; + board.hall_right.hall_portC = GPIOC; + board.hall_right.hall_pinC = GPIO_PIN_12; + + board.CAN = CAN2; + board.can_alt_tx = GPIO_AF9_CAN2; + board.can_alt_rx = GPIO_AF9_CAN2; + board.can_pinRX = GPIO_PIN_5; + board.can_portRX = GPIOB; + board.can_pinTX = GPIO_PIN_6; + board.can_portTX = GPIOB; + board.can_pinEN = GPIO_PIN_7; + board.can_portEN = GPIOB; + + board.led_pinR = GPIO_PIN_2; + board.led_portR = GPIOD; + board.led_pinG = GPIO_PIN_15; + board.led_portG = GPIOA; + board.led_pinB = GPIO_PIN_1; + board.led_portB = GPIOC; + + board.can_addr_offset = 0x0U; + } else if (hw_type == HW_TYPE_KNEE) { - hall_left.hall_portA = GPIOC; - hall_left.hall_pinA = GPIO_PIN_14; - hall_left.hall_portB = GPIOC; - hall_left.hall_pinB = GPIO_PIN_15; - hall_left.hall_portC = GPIOC; - hall_left.hall_pinC = GPIO_PIN_13; - - hall_right.hall_portA = GPIOD; - hall_right.hall_pinA = GPIO_PIN_2; - hall_right.hall_portB = GPIOC; - hall_right.hall_pinB = GPIO_PIN_0; - hall_right.hall_portC = GPIOC; - hall_right.hall_pinC = GPIO_PIN_1; - - can_addr_offset = 0x100U; - - MX_SPI3_Init(); + board.hall_left.hall_portA = GPIOC; + board.hall_left.hall_pinA = GPIO_PIN_14; + board.hall_left.hall_portB = GPIOC; + board.hall_left.hall_pinB = GPIO_PIN_15; + board.hall_left.hall_portC = GPIOC; + board.hall_left.hall_pinC = GPIO_PIN_13; + + board.hall_right.hall_portA = GPIOD; + board.hall_right.hall_pinA = GPIO_PIN_2; + board.hall_right.hall_portB = GPIOC; + board.hall_right.hall_pinB = GPIO_PIN_0; + board.hall_right.hall_portC = GPIOC; + board.hall_right.hall_pinC = GPIO_PIN_1; + + board.CAN = CAN1; + board.can_alt_tx = GPIO_AF8_CAN1; + board.can_alt_rx = GPIO_AF9_CAN1; + board.can_pinRX = GPIO_PIN_11; + board.can_portRX = GPIOA; + board.can_pinTX = GPIO_PIN_9; + board.can_portTX = GPIOB; + board.can_pinEN = 0; // No pin, pulled down with 10k resistor + board.can_portEN = GPIOB; + + board.led_pinR = GPIO_PIN_2; + board.led_portR = GPIOB; + board.led_pinG = GPIO_PIN_15; + board.led_portG = GPIOA; + board.led_pinB = GPIO_PIN_5; + board.led_portB = GPIOB; + + board.can_addr_offset = 0x100U; + + #ifndef BOOTSTUB + MX_I2C_Init(); + #endif } else { // Fail to detect, halt while(1) {} diff --git a/board/bootstub.c b/board/bootstub.c index 6a5a766..a444eb6 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -25,6 +25,7 @@ #include "drivers/llflash.h" #include "provision.h" #include "util.h" +#include "boards.h" #include "flasher.h" @@ -53,10 +54,7 @@ int main(void) { SystemClock_Config(); MX_GPIO_Clocks_Init(); - hw_type = board_id(); - if (hw_type == HW_TYPE_BASE) { - MX_GPIO_LED_Base_Init(); - } + board_detect(); MX_GPIO_Common_Init(); out_enable(POWERSWITCH, true); diff --git a/board/comms.h b/board/comms.h index 12aeaf7..0295f56 100644 --- a/board/comms.h +++ b/board/comms.h @@ -19,7 +19,7 @@ extern P rtP_Right; extern volatile int16_t cmdL; // global variable for Left Command extern volatile int16_t cmdR; // global variable for Right Command extern uint8_t hw_type; -extern uint32_t can_addr_offset; +extern board_t board; extern uint32_t enter_bootloader_mode; extern volatile uint32_t torque_cmd_timeout; @@ -85,38 +85,28 @@ void can_send_msg(uint32_t addr, uint32_t dhr, uint32_t dlr, uint8_t len) { void process_can(void) { CAN_FIFOMailBox_TypeDef to_send; - if (CAN2->TSR & CAN_TSR_TME0) { + if (board.CAN->TSR & CAN_TSR_TME0) { if (can_pop(&can_tx_q, &to_send)) { - CAN2->sTxMailBox[0].TDLR = to_send.RDLR; - CAN2->sTxMailBox[0].TDHR = to_send.RDHR; - CAN2->sTxMailBox[0].TDTR = to_send.RDTR; - CAN2->sTxMailBox[0].TIR = to_send.RIR; + board.CAN->sTxMailBox[0].TDLR = to_send.RDLR; + board.CAN->sTxMailBox[0].TDHR = to_send.RDHR; + board.CAN->sTxMailBox[0].TDTR = to_send.RDTR; + board.CAN->sTxMailBox[0].TIR = to_send.RIR; } } } -void CAN2_TX_IRQHandler(void) { - // clear interrupt - CAN2->TSR |= CAN_TSR_RQCP0; - process_can(); -} - -void CAN2_SCE_IRQHandler(void) { - llcan_clear_send(CAN2); -} - -void CAN2_RX0_IRQHandler(void) { - while ((CAN2->RF0R & CAN_RF0R_FMP0) != 0) { - int address = CAN2->sFIFOMailBox[0].RIR >> 21; - if (address == (int32_t)(0x250U + can_addr_offset)) { - if ((GET_MAILBOX_BYTES_04(&CAN2->sFIFOMailBox[0]) == 0xdeadface) && (GET_MAILBOX_BYTES_48(&CAN2->sFIFOMailBox[0]) == 0x0ab00b1e)) { +void can_rx(void) { + while ((board.CAN->RF0R & CAN_RF0R_FMP0) != 0) { + int address = board.CAN->sFIFOMailBox[0].RIR >> 21; + if (address == (int32_t)(0x250U + board.can_addr_offset)) { + if ((GET_MAILBOX_BYTES_04(&board.CAN->sFIFOMailBox[0]) == 0xdeadface) && (GET_MAILBOX_BYTES_48(&board.CAN->sFIFOMailBox[0]) == 0x0ab00b1e)) { enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; NVIC_SystemReset(); } #define MSG_TRQ_LEN 6 uint8_t dat[MSG_TRQ_LEN]; for (int i=0; isFIFOMailBox[0], i); + dat[i] = GET_MAILBOX_BYTE(&board.CAN->sFIFOMailBox[0], i); } uint16_t valueL = ((dat[0] << 8U) | dat[1]); uint16_t valueR = ((dat[2] << 8U) | dat[3]); @@ -130,11 +120,11 @@ void CAN2_RX0_IRQHandler(void) { } current_idx = idx; } - } else if (address == (int32_t)(0x251U + can_addr_offset)) { + } else if (address == (int32_t)(0x251U + board.can_addr_offset)) { #define MSG_SPD_LEN 5 uint8_t dat[MSG_TRQ_LEN]; for (int i=0; isFIFOMailBox[0], i); + dat[i] = GET_MAILBOX_BYTE(&board.CAN->sFIFOMailBox[0], i); } uint16_t valueL = ((dat[0] << 8U) | dat[1]); uint16_t valueR = ((dat[2] << 8U) | dat[3]); @@ -148,12 +138,40 @@ void CAN2_RX0_IRQHandler(void) { } } } else if ((hw_type == HW_TYPE_BASE) && ((address == BROADCAST_ADDR) || (address == FALLBACK_ADDR) || (address == ECU_ADDR) || (address == DEBUG_ADDR))) { // Process UBS and OBD2 requests, ignore for knee - process_uds(address, GET_MAILBOX_BYTES_04(&CAN2->sFIFOMailBox[0])); + process_uds(address, GET_MAILBOX_BYTES_04(&board.CAN->sFIFOMailBox[0])); } out_enable(LED_BLUE, true); // next - CAN2->RF0R |= CAN_RF0R_RFOM0; + board.CAN->RF0R |= CAN_RF0R_RFOM0; } } +void CAN1_TX_IRQHandler(void) { + // clear interrupt + board.CAN->TSR |= CAN_TSR_RQCP0; + process_can(); +} + +void CAN1_SCE_IRQHandler(void) { + llcan_clear_send(board.CAN); +} + +void CAN1_RX0_IRQHandler(void) { + can_rx(); +} + +void CAN2_TX_IRQHandler(void) { + // clear interrupt + board.CAN->TSR |= CAN_TSR_RQCP0; + process_can(); +} + +void CAN2_SCE_IRQHandler(void) { + llcan_clear_send(board.CAN); +} + +void CAN2_RX0_IRQHandler(void) { + can_rx(); +} + #endif diff --git a/board/defines.h b/board/defines.h index ae008bb..92c57ff 100644 --- a/board/defines.h +++ b/board/defines.h @@ -58,29 +58,6 @@ #define BATT_PIN GPIO_PIN_4 #define BATT_PORT GPIOA -#define SPI3_SCK_PIN GPIO_PIN_10 -#define SPI3_MISO_PIN GPIO_PIN_11 -#define SPI3_MOSI_PIN GPIO_PIN_12 -#define SPI3_PORT GPIOC - -#define AS5048_CS_PORT GPIOB -#define AS5048A_CS_PIN GPIO_PIN_2 - -// GREEN -#define LED_GREEN_PIN GPIO_PIN_15 -#define LED_GREEN_PORT GPIOA - -// RED (only for base) -#define LED_RED_PIN GPIO_PIN_2 -#define LED_RED_PORT GPIOD - -// BLUE (only for base) -#define LED_BLUE_PIN GPIO_PIN_1 -#define LED_BLUE_PORT GPIOC - -#define CAN_STBY_PIN GPIO_PIN_7 -#define CAN_STBY_PORT GPIOB - #define IGNITION_PIN GPIO_PIN_9 #define IGNITION_PORT GPIOB @@ -148,7 +125,7 @@ #define TRANSCEIVER 5 #define HW_TYPE_BASE 0 -#define HW_TYPE_KNEE 1 +#define HW_TYPE_KNEE 3 typedef struct { uint32_t rrB; @@ -170,6 +147,31 @@ typedef struct { uint16_t hall_pinC; } hall_sensor; +typedef struct { + hall_sensor hall_left; + hall_sensor hall_right; + + CAN_TypeDef* CAN; + uint8_t can_alt_tx; + uint8_t can_alt_rx; + GPIO_TypeDef* can_portTX; + uint16_t can_pinTX; + GPIO_TypeDef* can_portRX; + uint16_t can_pinRX; + GPIO_TypeDef* can_portEN; + uint16_t can_pinEN; + + uint32_t can_addr_offset; + + GPIO_TypeDef* led_portR; + uint16_t led_pinR; + GPIO_TypeDef* led_portG; + uint16_t led_pinG; + GPIO_TypeDef* led_portB; + uint16_t led_pinB; + +} board_t; + uint8_t hw_type; // type of the board detected(0 - base, 1 - knee) #endif // DEFINES_H diff --git a/board/drivers/angle_sensor.h b/board/drivers/angle_sensor.h index 28463f2..a8d625b 100644 --- a/board/drivers/angle_sensor.h +++ b/board/drivers/angle_sensor.h @@ -1,69 +1,28 @@ -#define SENSOR_COUNT 2 // Limit is 4! If more is needed - CAN message should be changed +#define SENSOR_COUNT 2 -#define SPI_TIMEOUT 2710 +// Default addresses for AS5048B +#define AS5048_ADDRESS_LEFT 0x40 +#define AS5048_ADDRESS_RIGHT 0x41 +#define AS5048B_PROG_REG 0x03 +#define AS5048B_ADDR_REG 0x15 +#define AS5048B_ZEROMSB_REG 0x16 //bits 0..7 +#define AS5048B_ZEROLSB_REG 0x17 //bits 0..5 +#define AS5048B_GAIN_REG 0xFA +#define AS5048B_DIAG_REG 0xFB +#define AS5048B_MAGNMSB_REG 0xFC //bits 0..7 +#define AS5048B_MAGNLSB_REG 0xFD //bits 0..5 +#define AS5048B_ANGLMSB_REG 0xFE //bits 0..7 +#define AS5048B_ANGLLSB_REG 0xFF //bits 0..5 -#define SPI_CMD_READ 0x4000U // Read command -#define SPI_REG_AGC 0x3FFDU // AGC register -#define SPI_REG_MAG 0x3FFEU // Magnitude register -#define SPI_REG_DATA 0x3FFFU // Angle register -#define SPI_REG_CLRERR 0x0001U // Clear error flag, should be used with read command -#define SPI_NOP 0x0000U // NOP, to read data on the next transfer -#define PARITY_BIT_SET 0x8000U +extern I2C_HandleTypeDef hi2c1; -extern SPI_HandleTypeDef hspi3; - -uint16_t angle_data[SENSOR_COUNT] = { 0 }; - -const uint16_t clear_error_cmd = (SPI_CMD_READ | SPI_REG_CLRERR); -const uint16_t read_angle_cmd = (PARITY_BIT_SET | SPI_CMD_READ | SPI_REG_DATA); -const uint16_t nop_cmd = SPI_NOP; - - -uint8_t spiCalcEvenParity(uint16_t value) { - uint8_t cnt = 0; - for (uint8_t i = 0; i < 16; i++) { - if (value & 0x1) { - cnt++; - } - value >>= 1; - } - return cnt & 0x1; -} void angle_sensor_read(uint16_t *sensor_angle) { - HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_RESET); - for (uint8_t i = 0; i < SENSOR_COUNT; i++) { - HAL_SPI_Transmit(&hspi3, (uint8_t*)&read_angle_cmd, 1, SPI_TIMEOUT); + uint8_t buf[2]; + if (HAL_I2C_Mem_Read(&hi2c1, (AS5048_ADDRESS_LEFT<<1), AS5048B_ANGLMSB_REG, I2C_MEMADD_SIZE_8BIT, buf, 2, 100) == HAL_OK) { + sensor_angle[0] = (buf[0] << 6) | (buf[1] & 0x3F); } - HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_SET); - HAL_Delay(1); - - HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_RESET); - for (int8_t i = (SENSOR_COUNT-1); i >= 0; i--) { - HAL_SPI_TransmitReceive(&hspi3, (uint8_t*)&nop_cmd, (uint8_t*)(&angle_data[i]), 1, 2710); - } - HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_SET); - HAL_Delay(1); - - bool error_flag_set = false; - for (uint8_t i = 0; i < SENSOR_COUNT; i++) { - if ((angle_data[i] >> 15) == spiCalcEvenParity((angle_data[i] & 0x7fff))) { - if (angle_data[i] & SPI_CMD_READ) { - error_flag_set = true; - } else { - if ((sensor_angle[i] == 0) || (ABS(sensor_angle[i] - (angle_data[i] & 0x3fff)) < 400)) { - sensor_angle[i] = (angle_data[i] & 0x3fff); - } - } - } - } - - if (error_flag_set) { - HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_RESET); - for (uint8_t i = 0; i < SENSOR_COUNT; i++) { - HAL_SPI_Transmit(&hspi3, (uint8_t*)&clear_error_cmd, 1, SPI_TIMEOUT); - } - HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_SET); - HAL_Delay(1); + if (HAL_I2C_Mem_Read(&hi2c1, (AS5048_ADDRESS_RIGHT<<1), AS5048B_ANGLMSB_REG, I2C_MEMADD_SIZE_8BIT, buf, 2, 100) == HAL_OK) { + sensor_angle[1] = (buf[0] << 6) | (buf[1] & 0x3F); } } diff --git a/board/drivers/llbxcan.h b/board/drivers/llbxcan.h index 6d1103b..2d535f4 100644 --- a/board/drivers/llbxcan.h +++ b/board/drivers/llbxcan.h @@ -98,12 +98,20 @@ bool llcan_init(CAN_TypeDef *CAN_obj) { CAN1->IER = 0U; // When we want to use only CAN2 - need to do that CAN_obj->IER = CAN_IER_FMPIE0 | CAN_IER_TMEIE | CAN_IER_WKUIE; - HAL_NVIC_SetPriority(CAN2_TX_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(CAN2_TX_IRQn); - HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn); - HAL_NVIC_SetPriority(CAN2_SCE_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(CAN2_SCE_IRQn); + if (CAN_obj == CAN1) { + HAL_NVIC_EnableIRQ(CAN1_TX_IRQn); + HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); + HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(CAN1_SCE_IRQn); + } else { + HAL_NVIC_SetPriority(CAN2_TX_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(CAN2_TX_IRQn); + HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn); + HAL_NVIC_SetPriority(CAN2_SCE_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(CAN2_SCE_IRQn); + } } return ret; } diff --git a/board/flasher.h b/board/flasher.h index 8d8fec4..05888d1 100644 --- a/board/flasher.h +++ b/board/flasher.h @@ -237,7 +237,7 @@ void check_powerdown(void) { out_enable(POWERSWITCH, false); while(1) { // Temporarily, to see that we went to power off but can't switch the latch - HAL_GPIO_TogglePin(LED_RED_PORT, LED_RED_PIN); + HAL_GPIO_TogglePin(board.led_portR, board.led_pinR); HAL_Delay(100); } } diff --git a/board/inc/stm32f4xx_hal_conf.h b/board/inc/stm32f4xx_hal_conf.h index 755d3ab..37e7626 100644 --- a/board/inc/stm32f4xx_hal_conf.h +++ b/board/inc/stm32f4xx_hal_conf.h @@ -51,7 +51,7 @@ /* #define HAL_SDRAM_MODULE_ENABLED */ /* #define HAL_HASH_MODULE_ENABLED */ #define HAL_GPIO_MODULE_ENABLED -/* #define HAL_I2C_MODULE_ENABLED */ +#define HAL_I2C_MODULE_ENABLED /* #define HAL_I2S_MODULE_ENABLED */ /* #define HAL_IWDG_MODULE_ENABLED */ /* #define HAL_LTDC_MODULE_ENABLED */ @@ -62,7 +62,7 @@ /* #define HAL_RTC_MODULE_ENABLED */ /* #define HAL_SAI_MODULE_ENABLED */ /* #define HAL_SD_MODULE_ENABLED */ -#define HAL_SPI_MODULE_ENABLED +/* #define HAL_SPI_MODULE_ENABLED */ #define HAL_TIM_MODULE_ENABLED #define HAL_UART_MODULE_ENABLED /* #define HAL_USART_MODULE_ENABLED */ @@ -259,12 +259,12 @@ /* Includes ------------------------------------------------------------------*/ /** - * @brief Include module's header file + * @brief Include module's header file */ #ifdef HAL_RCC_MODULE_ENABLED #include "stm32f4xx_hal_rcc.h" - #include "stm32f4xx_hal_rcc_ex.h" + #include "stm32f4xx_hal_rcc_ex.h" #endif /* HAL_RCC_MODULE_ENABLED */ #ifdef HAL_GPIO_MODULE_ENABLED @@ -298,8 +298,8 @@ #endif /* HAL_CRC_MODULE_ENABLED */ #ifdef HAL_CRYP_MODULE_ENABLED - #include "stm32f4xx_hal_cryp.h" - #include "stm32f4xx_hal_cryp_ex.h" + #include "stm32f4xx_hal_cryp.h" + #include "stm32f4xx_hal_cryp_ex.h" #endif /* HAL_CRYP_MODULE_ENABLED */ #ifdef HAL_DAC_MODULE_ENABLED @@ -333,11 +333,11 @@ #ifdef HAL_PCCARD_MODULE_ENABLED #include "stm32f4xx_hal_pccard.h" -#endif /* HAL_PCCARD_MODULE_ENABLED */ - +#endif /* HAL_PCCARD_MODULE_ENABLED */ + #ifdef HAL_SDRAM_MODULE_ENABLED #include "stm32f4xx_hal_sdram.h" -#endif /* HAL_SDRAM_MODULE_ENABLED */ +#endif /* HAL_SDRAM_MODULE_ENABLED */ #ifdef HAL_HASH_MODULE_ENABLED #include "stm32f4xx_hal_hash.h" @@ -417,7 +417,7 @@ #ifdef HAL_HCD_MODULE_ENABLED #include "stm32f4xx_hal_hcd.h" #endif /* HAL_HCD_MODULE_ENABLED */ - + #ifdef HAL_QSPI_MODULE_ENABLED #include "stm32f4xx_hal_qspi.h" #endif /* HAL_QSPI_MODULE_ENABLED */ diff --git a/board/main.c b/board/main.c index 673f138..b37443c 100644 --- a/board/main.c +++ b/board/main.c @@ -23,7 +23,6 @@ extern TIM_HandleTypeDef htim_left; extern TIM_HandleTypeDef htim_right; extern ADC_HandleTypeDef hadc; -extern SPI_HandleTypeDef hspi3; extern volatile adc_buf_t adc_buffer; // Matlab defines - from auto-code generation @@ -49,6 +48,8 @@ extern int16_t batVoltage; // global variable for battery voltage extern int32_t motPosL; extern int32_t motPosR; +extern board_t board; + //------------------------------------------------------------------------ // Global variables set here in main.c //------------------------------------------------------------------------ @@ -61,8 +62,6 @@ int16_t board_temp_deg_c; // global variable for calibrated temperature i volatile int16_t cmdL; // global variable for Left Command volatile int16_t cmdR; // global variable for Right Command -uint32_t can_addr_offset; // CAN messages addresses offset between different board types - uint8_t ignition = 0; // global variable for ignition on SBU2 line uint8_t charger_connected = 0; // status of the charger port uint8_t fault_status = 0; // fault status of the whole system @@ -110,10 +109,8 @@ int main(void) { out_enable(LED_GREEN, false); out_enable(LED_BLUE, false); - __HAL_RCC_CAN1_CLK_ENABLE(); // Also needed for CAN2, dumb... - __HAL_RCC_CAN2_CLK_ENABLE(); - llcan_set_speed(CAN2, 5000, false, false); - llcan_init(CAN2); + llcan_set_speed(board.CAN, 5000, false, false); + llcan_init(board.CAN); poweronMelody(); @@ -138,7 +135,11 @@ int main(void) { if (ignition == 0) { cmdL = cmdR = 0; enable_motors = 0; - } + } //else { + // enable_motors = 1; + // torque_cmd_timeout = 0; + // cmdL = cmdR = 100; + // } if (!enable_motors || (torque_cmd_timeout > 20)) { cmdL = 0; cmdR = 0; @@ -218,7 +219,7 @@ int main(void) { dat[5] = 0; // TODO: remove from OP and dbc, no value dat[6] = pkt_idx; dat[7] = crc_checksum(dat, 7, crc_poly); - can_send_msg((0x201U + can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); + can_send_msg((0x201U + board.can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); ++pkt_idx; pkt_idx &= 0xFU; @@ -231,7 +232,7 @@ int main(void) { dat[5] = rtU_Right.i_phaAB & 0xFFU; dat[6] = (rtU_Right.i_phaBC >> 8U) & 0xFFU; dat[7] = rtU_Right.i_phaBC & 0xFFU; - can_send_msg((0x204U + can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); + can_send_msg((0x204U + board.can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); uint16_t one; uint16_t two; @@ -251,7 +252,7 @@ int main(void) { dat[5] = one & 0xFFU; dat[6] = (two>>8U) & 0xFFU; dat[7] = two & 0xFFU; - can_send_msg((0x205U + can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); + can_send_msg((0x205U + board.can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); /// TEST MSG FOR SENSOR ERROR COUNTER if (angle_sensor_error > 0) { @@ -269,7 +270,7 @@ int main(void) { dat[0] = (((fault_status & 0x3F) << 2U) | (enable_motors << 1U) | ignition); dat[1] = rtY_Left.z_errCode; dat[2] = rtY_Right.z_errCode; - can_send_msg((0x202U + can_addr_offset), 0x0U, ((dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 3U); + can_send_msg((0x202U + board.can_addr_offset), 0x0U, ((dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 3U); } out_enable(LED_GREEN, ignition); } @@ -285,7 +286,7 @@ int main(void) { dat[1] = (batVoltageCalib >> 8U) & 0xFFU; dat[2] = batVoltageCalib & 0xFFU; dat[3] = (((battery_percent & 0x7FU) << 1U) | charger_connected); - can_send_msg((0x203U + can_addr_offset), 0x0U, ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 4U); + can_send_msg((0x203U + board.can_addr_offset), 0x0U, ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 4U); out_enable(LED_BLUE, false); // Reset LED after CAN RX out_enable(LED_GREEN, true); // Always use LED to show that body is on diff --git a/board/setup.h b/board/setup.h index 63ecb1e..16fc101 100644 --- a/board/setup.h +++ b/board/setup.h @@ -7,11 +7,11 @@ TIM_HandleTypeDef htim_right; TIM_HandleTypeDef htim_left; ADC_HandleTypeDef hadc; -SPI_HandleTypeDef hspi3; +I2C_HandleTypeDef hi2c1; +// SPI_HandleTypeDef hspi3; volatile adc_buf_t adc_buffer; -hall_sensor hall_left; -hall_sensor hall_right; +extern board_t board; void MX_GPIO_Clocks_Init(void) { @@ -20,6 +20,9 @@ void MX_GPIO_Clocks_Init(void) { __HAL_RCC_GPIOB_CLK_ENABLE(); __HAL_RCC_GPIOC_CLK_ENABLE(); __HAL_RCC_GPIOD_CLK_ENABLE(); + + __HAL_RCC_CAN1_CLK_ENABLE(); + __HAL_RCC_CAN2_CLK_ENABLE(); } void MX_GPIO_Common_Init(void) { @@ -29,23 +32,23 @@ void MX_GPIO_Common_Init(void) { GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.Pin = hall_left.hall_pinA; - HAL_GPIO_Init(hall_left.hall_portA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = board.hall_left.hall_pinA; + HAL_GPIO_Init(board.hall_left.hall_portA, &GPIO_InitStruct); - GPIO_InitStruct.Pin = hall_left.hall_pinB; - HAL_GPIO_Init(hall_left.hall_portB, &GPIO_InitStruct); + GPIO_InitStruct.Pin = board.hall_left.hall_pinB; + HAL_GPIO_Init(board.hall_left.hall_portB, &GPIO_InitStruct); - GPIO_InitStruct.Pin = hall_left.hall_pinC; - HAL_GPIO_Init(hall_left.hall_portC, &GPIO_InitStruct); + GPIO_InitStruct.Pin = board.hall_left.hall_pinC; + HAL_GPIO_Init(board.hall_left.hall_portC, &GPIO_InitStruct); - GPIO_InitStruct.Pin = hall_right.hall_pinA; - HAL_GPIO_Init(hall_right.hall_portA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = board.hall_right.hall_pinA; + HAL_GPIO_Init(board.hall_right.hall_portA, &GPIO_InitStruct); - GPIO_InitStruct.Pin = hall_right.hall_pinB; - HAL_GPIO_Init(hall_right.hall_portB, &GPIO_InitStruct); + GPIO_InitStruct.Pin = board.hall_right.hall_pinB; + HAL_GPIO_Init(board.hall_right.hall_portB, &GPIO_InitStruct); - GPIO_InitStruct.Pin = hall_right.hall_pinC; - HAL_GPIO_Init(hall_right.hall_portC, &GPIO_InitStruct); + GPIO_InitStruct.Pin = board.hall_right.hall_pinC; + HAL_GPIO_Init(board.hall_right.hall_portC, &GPIO_InitStruct); GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Pin = CHARGER_PIN; @@ -59,11 +62,19 @@ void MX_GPIO_Common_Init(void) { GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pin = LED_GREEN_PIN; - HAL_GPIO_Init(LED_GREEN_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = board.led_pinR; + HAL_GPIO_Init(board.led_portR, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = board.led_pinG; + HAL_GPIO_Init(board.led_portG, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = board.led_pinB; + HAL_GPIO_Init(board.led_portB, &GPIO_InitStruct); - GPIO_InitStruct.Pin = CAN_STBY_PIN; - HAL_GPIO_Init(CAN_STBY_PORT, &GPIO_InitStruct); + if (board.can_pinEN != 0) { + GPIO_InitStruct.Pin = board.can_pinEN; + HAL_GPIO_Init(board.can_portEN, &GPIO_InitStruct); + } GPIO_InitStruct.Pin = IGNITION_PIN; HAL_GPIO_Init(IGNITION_PORT, &GPIO_InitStruct); @@ -139,76 +150,44 @@ void MX_GPIO_Common_Init(void) { GPIO_InitStruct.Pin = RIGHT_TIM_WL_PIN; HAL_GPIO_Init(RIGHT_TIM_WL_PORT, &GPIO_InitStruct); - // Pins for CAN2 + // CAN bus GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF9_CAN2; - GPIO_InitStruct.Pin = GPIO_PIN_5; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + GPIO_InitStruct.Alternate = board.can_alt_rx; + GPIO_InitStruct.Pin = board.can_pinRX; + HAL_GPIO_Init(board.can_portRX, &GPIO_InitStruct); - GPIO_InitStruct.Pin = GPIO_PIN_6; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + GPIO_InitStruct.Alternate = board.can_alt_tx; + GPIO_InitStruct.Pin = board.can_pinTX; + HAL_GPIO_Init(board.can_portTX, &GPIO_InitStruct); } -void MX_GPIO_LED_Base_Init(void) { - GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - - GPIO_InitStruct.Pin = LED_RED_PIN; - HAL_GPIO_Init(LED_RED_PORT, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = LED_BLUE_PIN; - HAL_GPIO_Init(LED_BLUE_PORT, &GPIO_InitStruct); -} - -void MX_SPI3_Init(void) { +void MX_I2C_Init(void) { GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Pin = AS5048A_CS_PIN; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7; + GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; GPIO_InitStruct.Pull = GPIO_PULLUP; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; - HAL_GPIO_Init(AS5048_CS_PORT, &GPIO_InitStruct); - HAL_GPIO_WritePin(AS5048_CS_PORT, AS5048A_CS_PIN, GPIO_PIN_SET); - - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Alternate = GPIO_AF6_SPI3; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF4_I2C1; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Pin = SPI3_SCK_PIN; - HAL_GPIO_Init(SPI3_PORT, &GPIO_InitStruct); - - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Pin = SPI3_MISO_PIN; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(SPI3_PORT, &GPIO_InitStruct); - - GPIO_InitStruct.Pull = GPIO_PULLDOWN; - GPIO_InitStruct.Pin = SPI3_MOSI_PIN; - HAL_GPIO_Init(SPI3_PORT, &GPIO_InitStruct); - - __HAL_RCC_SPI3_CLK_ENABLE(); - - hspi3.Instance = SPI3; - hspi3.Init.Mode = SPI_MODE_MASTER; - hspi3.Init.Direction = SPI_DIRECTION_2LINES; - hspi3.Init.DataSize = SPI_DATASIZE_16BIT; - hspi3.Init.CLKPolarity = SPI_POLARITY_LOW; - hspi3.Init.CLKPhase = SPI_PHASE_2EDGE; - hspi3.Init.NSS = SPI_NSS_SOFT; - hspi3.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32; // 1.125 MHz at 72MHz clock - hspi3.Init.FirstBit = SPI_FIRSTBIT_MSB; - hspi3.Init.TIMode = SPI_TIMODE_DISABLE; - hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; - hspi3.Init.CRCPolynomial = 10; - - HAL_SPI_Init(&hspi3); + __HAL_RCC_I2C1_CLK_ENABLE(); + + hi2c1.Instance = I2C1; + hi2c1.Init.ClockSpeed = 400000; + hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2; + hi2c1.Init.OwnAddress1 = 0; + hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + hi2c1.Init.OwnAddress2 = 0; + hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; + HAL_I2C_Init(&hi2c1); } + void MX_TIM_Init(void) { __HAL_RCC_TIM1_CLK_ENABLE(); __HAL_RCC_TIM8_CLK_ENABLE(); diff --git a/board/util.c b/board/util.c index 7ffcf9e..e7f0bc8 100644 --- a/board/util.c +++ b/board/util.c @@ -19,6 +19,8 @@ extern uint8_t ignition; // global variable for ignition on SBU2 extern uint8_t hw_type; +extern board_t board; + //------------------------------------------------------------------------ // Matlab defines - from auto-code generation //--------------- @@ -83,17 +85,13 @@ void BLDC_Init(void) { void out_enable(uint8_t out, bool enabled) { switch(out) { case LED_GREEN: - HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, !enabled); + HAL_GPIO_WritePin(board.led_portG, board.led_pinG, !enabled); break; case LED_RED: - if (hw_type == HW_TYPE_BASE) { - HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, !enabled); - } + HAL_GPIO_WritePin(board.led_portR, board.led_pinR, !enabled); break; case LED_BLUE: - if (hw_type == HW_TYPE_BASE) { - HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, !enabled); - } + HAL_GPIO_WritePin(board.led_portB, board.led_pinB, !enabled); break; case IGNITION: HAL_GPIO_WritePin(IGNITION_PORT, IGNITION_PIN, enabled); @@ -102,7 +100,7 @@ void out_enable(uint8_t out, bool enabled) { HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, enabled); break; case TRANSCEIVER: - HAL_GPIO_WritePin(CAN_STBY_PORT, CAN_STBY_PIN, !enabled); + HAL_GPIO_WritePin(board.can_portEN, board.can_pinEN, !enabled); break; } } @@ -170,7 +168,7 @@ void poweroff(void) { out_enable(POWERSWITCH, false); while(1) { // Temporarily, to see that we went to power off but can't switch the latch - HAL_GPIO_TogglePin(LED_RED_PORT, LED_RED_PIN); + HAL_GPIO_TogglePin(board.led_portR, board.led_pinR); HAL_Delay(100); } } From bfbc1e8d25974232456c0401c25a2b551089da6a Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Thu, 23 Jun 2022 19:42:59 -0700 Subject: [PATCH 04/18] ignition --- board/bootstub.c | 2 +- board/drivers/angle_sensor.h | 4 ++-- board/main.c | 21 +++++++++++++-------- 3 files changed, 16 insertions(+), 11 deletions(-) diff --git a/board/bootstub.c b/board/bootstub.c index a444eb6..20e7a79 100644 --- a/board/bootstub.c +++ b/board/bootstub.c @@ -62,7 +62,7 @@ int main(void) { out_enable(LED_GREEN, false); out_enable(LED_BLUE, false); - if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { + if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && (hw_type == HW_TYPE_BASE)) { uint16_t cnt_press = 0; while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); diff --git a/board/drivers/angle_sensor.h b/board/drivers/angle_sensor.h index a8d625b..6230493 100644 --- a/board/drivers/angle_sensor.h +++ b/board/drivers/angle_sensor.h @@ -19,10 +19,10 @@ extern I2C_HandleTypeDef hi2c1; void angle_sensor_read(uint16_t *sensor_angle) { uint8_t buf[2]; - if (HAL_I2C_Mem_Read(&hi2c1, (AS5048_ADDRESS_LEFT<<1), AS5048B_ANGLMSB_REG, I2C_MEMADD_SIZE_8BIT, buf, 2, 100) == HAL_OK) { + if (HAL_I2C_Mem_Read(&hi2c1, (AS5048_ADDRESS_LEFT<<1), AS5048B_ANGLMSB_REG, I2C_MEMADD_SIZE_8BIT, buf, 2, 5) == HAL_OK) { sensor_angle[0] = (buf[0] << 6) | (buf[1] & 0x3F); } - if (HAL_I2C_Mem_Read(&hi2c1, (AS5048_ADDRESS_RIGHT<<1), AS5048B_ANGLMSB_REG, I2C_MEMADD_SIZE_8BIT, buf, 2, 100) == HAL_OK) { + if (HAL_I2C_Mem_Read(&hi2c1, (AS5048_ADDRESS_RIGHT<<1), AS5048B_ANGLMSB_REG, I2C_MEMADD_SIZE_8BIT, buf, 2, 5) == HAL_OK) { sensor_angle[1] = (buf[0] << 6) | (buf[1] & 0x3F); } } diff --git a/board/main.c b/board/main.c index b37443c..5064226 100644 --- a/board/main.c +++ b/board/main.c @@ -100,10 +100,16 @@ int main(void) { HAL_ADC_Start(&hadc); - out_enable(POWERSWITCH, true); - out_enable(IGNITION, ignition); - out_enable(TRANSCEIVER, true); - + if (hw_type == HW_TYPE_BASE) { + out_enable(POWERSWITCH, true); + out_enable(IGNITION, ignition); + out_enable(TRANSCEIVER, true); + // Loop until button is released, only for base board + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } + } else { + out_enable(POWERSWITCH, false); + ignition = 1; + } // Reset LEDs upon startup out_enable(LED_RED, false); out_enable(LED_GREEN, false); @@ -125,9 +131,6 @@ int main(void) { uint16_t hall_angle_offset[SENSOR_COUNT] = { 0 }; angle_sensor_read(sensor_angle); - // Loop until button is released - while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } - while(1) { if (buzzerTimer - buzzerTimer_prev > 16*DELAY_IN_MAIN_LOOP) { // 1 ms = 16 ticks buzzerTimer calcAvgSpeed(); @@ -299,7 +302,9 @@ int main(void) { } process_can(); - poweroffPressCheck(); + if (hw_type == HW_TYPE_BASE) { + poweroffPressCheck(); + } if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20) || (batVoltage < BAT_DEAD && speedAvgAbs < 20)) { // poweroff before mainboard burns OR low bat 3 poweroff(); From 70ca68dc1fe52247bd4bfa1ba726f1b700fa06f4 Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Tue, 28 Jun 2022 13:02:03 -0700 Subject: [PATCH 05/18] sensor count remove --- board/drivers/angle_sensor.h | 2 -- board/main.c | 4 ++-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/board/drivers/angle_sensor.h b/board/drivers/angle_sensor.h index 6230493..162d91d 100644 --- a/board/drivers/angle_sensor.h +++ b/board/drivers/angle_sensor.h @@ -1,5 +1,3 @@ -#define SENSOR_COUNT 2 - // Default addresses for AS5048B #define AS5048_ADDRESS_LEFT 0x40 #define AS5048_ADDRESS_RIGHT 0x41 diff --git a/board/main.c b/board/main.c index 5064226..76cc111 100644 --- a/board/main.c +++ b/board/main.c @@ -127,8 +127,8 @@ int main(void) { #define GEARBOX_RATIO_RIGHT 19 uint8_t angle_sensor_error = 0; - uint16_t sensor_angle[SENSOR_COUNT] = { 0 }; - uint16_t hall_angle_offset[SENSOR_COUNT] = { 0 }; + uint16_t sensor_angle[2] = { 0 }; + uint16_t hall_angle_offset[2] = { 0 }; angle_sensor_read(sensor_angle); while(1) { From 0f7db7b49f57c2346f5d8bda53e735b1845a328b Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Wed, 20 Jul 2022 13:03:17 -0700 Subject: [PATCH 06/18] few things --- board/bldc/BLDC_controller_data.c | 2 +- board/boards.h | 8 ++++- board/comms.h | 2 ++ board/config.h | 4 +++ board/defines.h | 6 ++-- board/main.c | 57 +++++++++++++------------------ board/setup.h | 7 ++-- board/uds.h | 5 +-- board/util.c | 2 +- board/version.h | 2 +- 10 files changed, 50 insertions(+), 45 deletions(-) diff --git a/board/bldc/BLDC_controller_data.c b/board/bldc/BLDC_controller_data.c index 626c537..0a256b4 100644 --- a/board/bldc/BLDC_controller_data.c +++ b/board/bldc/BLDC_controller_data.c @@ -337,7 +337,7 @@ P rtP_Left = { * '/cf_nKiLimProt' * '/cf_nKiLimProt' */ - 246U, + 650U, /* Variable: n_polePairs * Referenced by: '/n_polePairs' diff --git a/board/boards.h b/board/boards.h index 29efb9a..7cf473c 100644 --- a/board/boards.h +++ b/board/boards.h @@ -28,6 +28,9 @@ void board_detect(void) { board.can_pinEN = GPIO_PIN_7; board.can_portEN = GPIOB; + board.ignition_pin = GPIO_PIN_9; + board.ignition_port = GPIOB; + board.led_pinR = GPIO_PIN_2; board.led_portR = GPIOD; board.led_pinG = GPIO_PIN_15; @@ -62,6 +65,9 @@ void board_detect(void) { board.can_pinEN = 0; // No pin, pulled down with 10k resistor board.can_portEN = GPIOB; + board.ignition_pin = 0; // No pin, always enabled + board.ignition_port = GPIOB; + board.led_pinR = GPIO_PIN_2; board.led_portR = GPIOB; board.led_pinG = GPIO_PIN_15; @@ -69,7 +75,7 @@ void board_detect(void) { board.led_pinB = GPIO_PIN_5; board.led_portB = GPIOB; - board.can_addr_offset = 0x100U; + board.can_addr_offset = KNEE_ADDR_OFFSET; #ifndef BOOTSTUB MX_I2C_Init(); diff --git a/board/comms.h b/board/comms.h index 0295f56..3586b60 100644 --- a/board/comms.h +++ b/board/comms.h @@ -139,6 +139,8 @@ void can_rx(void) { } } else if ((hw_type == HW_TYPE_BASE) && ((address == BROADCAST_ADDR) || (address == FALLBACK_ADDR) || (address == ECU_ADDR) || (address == DEBUG_ADDR))) { // Process UBS and OBD2 requests, ignore for knee process_uds(address, GET_MAILBOX_BYTES_04(&board.CAN->sFIFOMailBox[0])); + } else if ((hw_type == HW_TYPE_BASE) && (address == 0x203U + KNEE_ADDR_OFFSET)) { // detect knee by body and set flag for use with UDS message + knee_detected = 1; } out_enable(LED_BLUE, true); // next diff --git a/board/config.h b/board/config.h index f162e9f..2a3437f 100644 --- a/board/config.h +++ b/board/config.h @@ -15,6 +15,10 @@ #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 #define ANGLE_TO_DEGREES 0.021972656 // Convert 14 bit angle sensor output to degrees +#define TRQ_LIMIT_LEFT 1000 // Torque limit for knee gearbox(left) +#define TRQ_LIMIT_RIGHT 1000 // Torque limit for hip gearbox(right) + +#define KNEE_ADDR_OFFSET 0x100U #define BAT_FILT_COEF 655 // battery voltage filter coefficient in fixed-point. coef_fixedPoint = coef_floatingPoint * 2^16. In this case 655 = 0.01 * 2^16 #define BAT_CALIB_REAL_VOLTAGE 3192 // input voltage measured by multimeter (multiplied by 100). In this case 43.00 V * 100 = 4300 diff --git a/board/defines.h b/board/defines.h index 92c57ff..0c50d7d 100644 --- a/board/defines.h +++ b/board/defines.h @@ -58,9 +58,6 @@ #define BATT_PIN GPIO_PIN_4 #define BATT_PORT GPIOA -#define IGNITION_PIN GPIO_PIN_9 -#define IGNITION_PORT GPIOB - #define BUZZER_PIN GPIO_PIN_2 #define BUZZER_PORT GPIOC @@ -161,6 +158,9 @@ typedef struct { GPIO_TypeDef* can_portEN; uint16_t can_pinEN; + GPIO_TypeDef* ignition_port; + uint16_t ignition_pin; + uint32_t can_addr_offset; GPIO_TypeDef* led_portR; diff --git a/board/main.c b/board/main.c index 76cc111..06dcd32 100644 --- a/board/main.c +++ b/board/main.c @@ -138,11 +138,8 @@ int main(void) { if (ignition == 0) { cmdL = cmdR = 0; enable_motors = 0; - } //else { - // enable_motors = 1; - // torque_cmd_timeout = 0; - // cmdL = cmdR = 100; - // } + } + if (!enable_motors || (torque_cmd_timeout > 20)) { cmdL = 0; cmdR = 0; @@ -185,14 +182,14 @@ int main(void) { rtP_Left.b_cruiseCtrlEna = 1; } else { rtP_Left.b_cruiseCtrlEna = 0; - pwml = -CLAMP((int)cmdL, -1000, 1000); + pwml = -CLAMP((int)cmdL, -TRQ_LIMIT_LEFT, TRQ_LIMIT_LEFT); } if ((ABS(cmdR) < 20) /*|| angle_sensor_error*/) { rtP_Right.n_cruiseMotTgt = 0; rtP_Right.b_cruiseCtrlEna = 1; } else { rtP_Right.b_cruiseCtrlEna = 0; - pwmr = -CLAMP((int)cmdR, -1000, 1000); + pwmr = -CLAMP((int)cmdR, -TRQ_LIMIT_RIGHT, TRQ_LIMIT_RIGHT); } } else { pwml = CLAMP((int)cmdL, -1000, 1000); @@ -210,7 +207,7 @@ int main(void) { // runs at ~100Hz if (main_loop_counter % 2 == 0) { if (ignition_off_counter <= 10) { - // speed_L(2), speed_R(2), hall_angle_L(1), hall_angle_R(1), counter(1), checksum(1) + // MOTORS_DATA: speed_L(2), speed_R(2), counter(1), checksum(1) uint8_t dat[8]; uint16_t speedL = rtY_Left.n_mot; uint16_t speedR = -(rtY_Right.n_mot); // Invert speed sign for the right wheel @@ -218,15 +215,13 @@ int main(void) { dat[1] = speedL & 0xFFU; dat[2] = (speedR >> 8U) & 0xFFU; dat[3] = speedR & 0xFFU; - dat[4] = 0; // TODO: remove from OP and dbc, no value - dat[5] = 0; // TODO: remove from OP and dbc, no value - dat[6] = pkt_idx; - dat[7] = crc_checksum(dat, 7, crc_poly); - can_send_msg((0x201U + board.can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); + dat[4] = pkt_idx; + dat[5] = crc_checksum(dat, 5, crc_poly); + can_send_msg((0x201U + board.can_addr_offset), ((dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 6U); ++pkt_idx; pkt_idx &= 0xFU; - // left_pha_ab(2), left_pha_bc(2), right_pha_ab(2), right_pha_bc(2) + //MOTORS_CURRENT: left_pha_ab(2), left_pha_bc(2), right_pha_ab(2), right_pha_bc(2) dat[0] = (rtU_Left.i_phaAB >> 8U) & 0xFFU; dat[1] = rtU_Left.i_phaAB & 0xFFU; dat[2] = (rtU_Left.i_phaBC >> 8U) & 0xFFU; @@ -237,38 +232,32 @@ int main(void) { dat[7] = rtU_Right.i_phaBC & 0xFFU; can_send_msg((0x204U + board.can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); - uint16_t one; - uint16_t two; + uint16_t left_hall_angle; + uint16_t right_hall_angle; if (hw_type == HW_TYPE_KNEE) { - one = hall_angle_offset[0] + ((motPosL / 15 / GEARBOX_RATIO_LEFT) % 360); - two = hall_angle_offset[1] + ((motPosR / 15 / GEARBOX_RATIO_RIGHT) % 360); + left_hall_angle = hall_angle_offset[0] + ((motPosL / 15 / GEARBOX_RATIO_LEFT) % 360); + right_hall_angle = hall_angle_offset[1] + ((motPosR / 15 / GEARBOX_RATIO_RIGHT) % 360); } else { - one = motPosL / 15; - two = -motPosR / 15; + left_hall_angle = motPosL / 15; + right_hall_angle = -motPosR / 15; } - // first angle sensor(2), second angle sensor(2) + //MOTORS_ANGLE: left angle sensor(2), right angle sensor(2), left hall angle(2), right hall angle(2) dat[0] = (sensor_angle[0]>>8U) & 0xFFU; dat[1] = sensor_angle[0] & 0xFFU; dat[2] = (sensor_angle[1]>>8U) & 0xFFU; dat[3] = sensor_angle[1] & 0xFFU; - dat[4] = (one>>8U) & 0xFFU; - dat[5] = one & 0xFFU; - dat[6] = (two>>8U) & 0xFFU; - dat[7] = two & 0xFFU; + dat[4] = (left_hall_angle>>8U) & 0xFFU; + dat[5] = left_hall_angle & 0xFFU; + dat[6] = (right_hall_angle>>8U) & 0xFFU; + dat[7] = right_hall_angle & 0xFFU; can_send_msg((0x205U + board.can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5] << 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); - - /// TEST MSG FOR SENSOR ERROR COUNTER - if (angle_sensor_error > 0) { - can_send_msg((0x20U), (0x0U), (0x0U), 1U); - angle_sensor_error -= 1; - } } } // runs at ~10Hz if (main_loop_counter % 20 == 0) { if (ignition_off_counter <= 10) { - // fault_status(0:6), enable_motors(0:1), ignition(0:1), left motor error(1), right motor error(1), global fault status(1) + // VAR_VALUES: fault_status(0:6), enable_motors(0:1), ignition(0:1), left motor error(1), right motor error(1), global fault status(1) uint8_t dat[2]; dat[0] = (((fault_status & 0x3F) << 2U) | (enable_motors << 1U) | ignition); dat[1] = rtY_Left.z_errCode; @@ -283,7 +272,7 @@ int main(void) { charger_connected = !HAL_GPIO_ReadPin(CHARGER_PORT, CHARGER_PIN); uint8_t battery_percent = 100 - (((420 * BAT_CELLS) - batVoltageCalib) / BAT_CELLS / VOLTS_PER_PERCENT / 100); // Battery % left - // MCU temp(2), battery voltage(2), battery_percent(0:7), charger_connected(0:1) + // BODY_DATA: MCU temp(2), battery voltage(2), battery_percent(0:7), charger_connected(0:1) uint8_t dat[4]; dat[0] = board_temp_deg_c & 0xFFU; dat[1] = (batVoltageCalib >> 8U) & 0xFFU; @@ -320,7 +309,7 @@ int main(void) { beepCount(0, 10, 30); } else { // do not beep beepCount(0, 0, 0); - //out_enable(LED_RED, false); + out_enable(LED_RED, false); } buzzerTimer_prev = buzzerTimer; diff --git a/board/setup.h b/board/setup.h index 16fc101..83e548f 100644 --- a/board/setup.h +++ b/board/setup.h @@ -76,8 +76,11 @@ void MX_GPIO_Common_Init(void) { HAL_GPIO_Init(board.can_portEN, &GPIO_InitStruct); } - GPIO_InitStruct.Pin = IGNITION_PIN; - HAL_GPIO_Init(IGNITION_PORT, &GPIO_InitStruct); + if (board.ignition_pin != 0) { + GPIO_InitStruct.Pin = board.ignition_pin; + HAL_GPIO_Init(board.ignition_port, &GPIO_InitStruct); + + } GPIO_InitStruct.Pin = BUZZER_PIN; HAL_GPIO_Init(BUZZER_PORT, &GPIO_InitStruct); diff --git a/board/uds.h b/board/uds.h index e53d908..5c3fa85 100644 --- a/board/uds.h +++ b/board/uds.h @@ -2,6 +2,7 @@ void can_send_msg(uint32_t addr, uint32_t dhr, uint32_t dlr, uint8_t len); uint8_t uid[10]; uint32_t uds_request = 0; +uint8_t knee_detected = 0; void process_uds(uint32_t addr, uint32_t dlr) { memcpy(uid, (void *)0x1FFF7A10U, 0xAU); @@ -55,7 +56,7 @@ void process_uds(uint32_t addr, uint32_t dlr) { break; // SYSTEM NAME OR ENGINE TYPE : F197 case 0x97F12203U: - can_send_msg(ECU_R_ADDR, 0x454C4597U, 0xF1620B10U, 8U); + can_send_msg(ECU_R_ADDR, 0x454C4597U, 0xF1620C10U, 8U); uds_request = 0xF197U; break; // FLOW CONTROL MESSAGE @@ -79,7 +80,7 @@ void process_uds(uint32_t addr, uint32_t dlr) { break; // SYSTEM NAME OR ENGINE TYPE : F197 case 0xF197U: - can_send_msg(ECU_R_ADDR, 0x4349U, 0x52544321U, 8U); + can_send_msg(ECU_R_ADDR, (((knee_detected + 0x30) << 16U) | 0x4349U), 0x52544321U, 8U); uds_request = 0; break; } diff --git a/board/util.c b/board/util.c index e7f0bc8..b2ef961 100644 --- a/board/util.c +++ b/board/util.c @@ -94,7 +94,7 @@ void out_enable(uint8_t out, bool enabled) { HAL_GPIO_WritePin(board.led_portB, board.led_pinB, !enabled); break; case IGNITION: - HAL_GPIO_WritePin(IGNITION_PORT, IGNITION_PIN, enabled); + HAL_GPIO_WritePin(board.ignition_port, board.ignition_pin, enabled); break; case POWERSWITCH: HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, enabled); diff --git a/board/version.h b/board/version.h index 8bd7de6..65b6f88 100644 --- a/board/version.h +++ b/board/version.h @@ -1 +1 @@ -const uint8_t version[6] = "0.0.01"; +const uint8_t version[6] = "0.0.02"; From 3b7e54bbcee7c4bdeaba7d9d1f6b724153e2dec5 Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Mon, 25 Jul 2022 07:00:16 -0700 Subject: [PATCH 07/18] blink only on known CAN msgs --- board/comms.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/board/comms.h b/board/comms.h index 3586b60..b7b3b29 100644 --- a/board/comms.h +++ b/board/comms.h @@ -120,6 +120,7 @@ void can_rx(void) { } current_idx = idx; } + out_enable(LED_BLUE, true); } else if (address == (int32_t)(0x251U + board.can_addr_offset)) { #define MSG_SPD_LEN 5 uint8_t dat[MSG_TRQ_LEN]; @@ -137,12 +138,13 @@ void can_rx(void) { rtP_Right.n_max = valueR << 4; } } + out_enable(LED_BLUE, true); } else if ((hw_type == HW_TYPE_BASE) && ((address == BROADCAST_ADDR) || (address == FALLBACK_ADDR) || (address == ECU_ADDR) || (address == DEBUG_ADDR))) { // Process UBS and OBD2 requests, ignore for knee process_uds(address, GET_MAILBOX_BYTES_04(&board.CAN->sFIFOMailBox[0])); + out_enable(LED_BLUE, true); } else if ((hw_type == HW_TYPE_BASE) && (address == 0x203U + KNEE_ADDR_OFFSET)) { // detect knee by body and set flag for use with UDS message knee_detected = 1; } - out_enable(LED_BLUE, true); // next board.CAN->RF0R |= CAN_RF0R_RFOM0; } From e9634ffd24d20d2a6d5c7a39cfbb865affe24bfd Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Mon, 25 Jul 2022 07:00:39 -0700 Subject: [PATCH 08/18] few cleanups --- board/defines.h | 2 +- board/main.c | 34 +++++++++++++++++----------------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/board/defines.h b/board/defines.h index 0c50d7d..4b24b24 100644 --- a/board/defines.h +++ b/board/defines.h @@ -172,6 +172,6 @@ typedef struct { } board_t; -uint8_t hw_type; // type of the board detected(0 - base, 1 - knee) +uint8_t hw_type; // type of the board detected(0 - base, 3 - knee) #endif // DEFINES_H diff --git a/board/main.c b/board/main.c index 06dcd32..c1909eb 100644 --- a/board/main.c +++ b/board/main.c @@ -126,7 +126,6 @@ int main(void) { #define GEARBOX_RATIO_LEFT 19 #define GEARBOX_RATIO_RIGHT 19 - uint8_t angle_sensor_error = 0; uint16_t sensor_angle[2] = { 0 }; uint16_t hall_angle_offset[2] = { 0 }; angle_sensor_read(sensor_angle); @@ -149,7 +148,6 @@ int main(void) { beepShort(6); // make 2 beeps indicating the motor enable beepShort(4); HAL_Delay(100); - angle_sensor_error = 0; sensor_angle[0] = 0; sensor_angle[1] = 0; angle_sensor_read(sensor_angle); @@ -164,7 +162,6 @@ int main(void) { // Safety to stop operation if angle sensor reading failed TODO: adjust sensivity and add lowpass to angle sensor? if ((ABS((hall_angle_offset[0] + ((motPosL / 15 / GEARBOX_RATIO_LEFT) % 360)) - (sensor_angle[0] * ANGLE_TO_DEGREES)) > 5) || (ABS((hall_angle_offset[1] + ((motPosR / 15 / GEARBOX_RATIO_RIGHT) % 360)) - (sensor_angle[1] * ANGLE_TO_DEGREES)) > 5)) { - angle_sensor_error += 1; //cmdL = cmdR = 0; } // Safety to stop movement when reaching dead angles, around 20 and 340 degrees @@ -176,24 +173,27 @@ int main(void) { } } - if (hw_type == HW_TYPE_KNEE) { - if ((ABS(cmdL) < 20) /*|| angle_sensor_error*/) { - rtP_Left.n_cruiseMotTgt = 0; - rtP_Left.b_cruiseCtrlEna = 1; - } else { - rtP_Left.b_cruiseCtrlEna = 0; + if (ABS(cmdL) < 10) { + rtP_Left.n_cruiseMotTgt = 0; + rtP_Left.b_cruiseCtrlEna = 1; + } else { + rtP_Left.b_cruiseCtrlEna = 0; + if (hw_type == HW_TYPE_KNEE) { pwml = -CLAMP((int)cmdL, -TRQ_LIMIT_LEFT, TRQ_LIMIT_LEFT); - } - if ((ABS(cmdR) < 20) /*|| angle_sensor_error*/) { - rtP_Right.n_cruiseMotTgt = 0; - rtP_Right.b_cruiseCtrlEna = 1; } else { - rtP_Right.b_cruiseCtrlEna = 0; - pwmr = -CLAMP((int)cmdR, -TRQ_LIMIT_RIGHT, TRQ_LIMIT_RIGHT); + pwml = CLAMP((int)cmdL, -1000, 1000); } + } + if (ABS(cmdR) < 10) { + rtP_Right.n_cruiseMotTgt = 0; + rtP_Right.b_cruiseCtrlEna = 1; } else { - pwml = CLAMP((int)cmdL, -1000, 1000); - pwmr = -CLAMP((int)cmdR, -1000, 1000); + rtP_Right.b_cruiseCtrlEna = 0; + if (hw_type == HW_TYPE_KNEE) { + pwmr = -CLAMP((int)cmdR, -TRQ_LIMIT_RIGHT, TRQ_LIMIT_RIGHT); + } else { + pwmr = -CLAMP((int)cmdR, -1000, 1000); + } } // ####### CALC BOARD TEMPERATURE ####### From 0dac121823939cc43833eaa1130545e36c75e5f8 Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Mon, 25 Jul 2022 15:52:35 -0700 Subject: [PATCH 09/18] fix can flashed --- board/flasher.h | 58 ++++++++++++++++++++++++++----------------------- 1 file changed, 31 insertions(+), 27 deletions(-) diff --git a/board/flasher.h b/board/flasher.h index 05888d1..fea6332 100644 --- a/board/flasher.h +++ b/board/flasher.h @@ -24,9 +24,6 @@ USB_Setup_TypeDef; uint32_t *prog_ptr = NULL; bool unlocked = false; -#define CAN CAN2 - -// Also being offset by hw_type value*2, for different board types #define CAN_BL_INPUT 0x1 #define CAN_BL_OUTPUT 0x2 @@ -125,7 +122,7 @@ int prep_data(uint8_t *data, uint8_t *data_out) { void CAN2_TX_IRQHandler(void) { // clear interrupt - CAN->TSR |= CAN_TSR_RQCP0; + board.CAN->TSR |= CAN_TSR_RQCP0; } #define ISOTP_BUF_SIZE 0x110 @@ -141,21 +138,21 @@ int isotp_buf_out_idx = 0; void bl_can_send(uint8_t *odat) { // wait for send - while (!(CAN->TSR & CAN_TSR_TME0)); + while (!(board.CAN->TSR & CAN_TSR_TME0)); // send continue - CAN->sTxMailBox[0].TDLR = ((uint32_t*)odat)[0]; - CAN->sTxMailBox[0].TDHR = ((uint32_t*)odat)[1]; - CAN->sTxMailBox[0].TDTR = 8; - CAN->sTxMailBox[0].TIR = ((CAN_BL_OUTPUT+(hw_type*2U)) << 21) | 1; + board.CAN->sTxMailBox[0].TDLR = ((uint32_t*)odat)[0]; + board.CAN->sTxMailBox[0].TDHR = ((uint32_t*)odat)[1]; + board.CAN->sTxMailBox[0].TDTR = 8; + board.CAN->sTxMailBox[0].TIR = (CAN_BL_OUTPUT << 21) | 1; } void CAN2_RX0_IRQHandler(void) { - while (CAN->RF0R & CAN_RF0R_FMP0) { - if ((CAN->sFIFOMailBox[0].RIR>>21) == (CAN_BL_INPUT+(hw_type*2U))) { + while (board.CAN->RF0R & CAN_RF0R_FMP0) { + if ((board.CAN->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) { uint8_t dat[8]; for (int i = 0; i < 8; i++) { - dat[i] = GET_MAILBOX_BYTE(&CAN->sFIFOMailBox[0], i); + dat[i] = GET_MAILBOX_BYTE(&board.CAN->sFIFOMailBox[0], i); } uint8_t odat[8]; uint8_t type = dat[0] & 0xF0; @@ -163,7 +160,7 @@ void CAN2_RX0_IRQHandler(void) { // continue while (isotp_buf_out_remain > 0) { // wait for send - while (!(CAN->TSR & CAN_TSR_TME0)); + while (!(board.CAN->TSR & CAN_TSR_TME0)); odat[0] = 0x20 | isotp_buf_out_idx; memcpy(odat+1, isotp_buf_out_ptr, 7); @@ -219,16 +216,28 @@ void CAN2_RX0_IRQHandler(void) { } } // next - CAN->RF0R |= CAN_RF0R_RFOM0; + board.CAN->RF0R |= CAN_RF0R_RFOM0; } } void CAN2_SCE_IRQHandler(void) { - llcan_clear_send(CAN); + llcan_clear_send(board.CAN); +} + +void CAN1_TX_IRQHandler(void) { + CAN2_TX_IRQHandler(); +} + +void CAN1_RX0_IRQHandler(void) { + CAN2_RX0_IRQHandler(); +} + +void CAN1_SCE_IRQHandler(void) { + CAN2_SCE_IRQHandler(); } void check_powerdown(void) { - if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { + if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && (hw_type == HW_TYPE_BASE)) { uint16_t cnt_press = 0; while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); @@ -247,13 +256,6 @@ void check_powerdown(void) { void soft_flasher_start(void) { - HAL_NVIC_SetPriority(CAN2_TX_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(CAN2_TX_IRQn); - HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn); - HAL_NVIC_SetPriority(CAN2_SCE_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(CAN2_SCE_IRQn); - enter_bootloader_mode = 0; out_enable(TRANSCEIVER, true); @@ -262,12 +264,14 @@ void soft_flasher_start(void) { __HAL_RCC_CAN2_CLK_ENABLE(); // init can - llcan_set_speed(CAN, 5000, false, false); - llcan_init(CAN); + llcan_set_speed(board.CAN, 5000, false, false); + llcan_init(board.CAN); out_enable(LED_BLUE, true); - // Wait for power button release - while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} + // Wait for power button release, only for the base + if (hw_type == HW_TYPE_BASE) { + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} + } out_enable(LED_GREEN, false); uint64_t cnt = 0; From 86784225d6573eae62e1aa2178851621ecb7fcdf Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Mon, 25 Jul 2022 20:27:53 -0700 Subject: [PATCH 10/18] add knee to canloader --- board/canloader.py | 58 ++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 48 insertions(+), 10 deletions(-) diff --git a/board/canloader.py b/board/canloader.py index 51e775a..2c540e0 100755 --- a/board/canloader.py +++ b/board/canloader.py @@ -5,6 +5,7 @@ from panda import Panda from panda.tests.pedal.canhandle import CanHandle + def heartbeat_thread(p): while True: try: @@ -13,6 +14,27 @@ def heartbeat_thread(p): except Exception: continue +def flasher(p, addr, file): + p.can_send(addr, b"\xce\xfa\xad\xde\x1e\x0b\xb0\x0a", 0) + time.sleep(0.1) + print("flashing", file) + code = open(file, "rb").read() + retries = 3 # How many times to retry on timeout error + while(retries+1>0): + try: + Panda.flash_static(CanHandle(p, 0), code) + except TimeoutError: + print("Timeout, trying again...") + retries -= 1 + else: + print("Successfully flashed") + break + +def flush_panda(): + while(1): + if len(p.can_recv()) == 0: + break + if __name__ == "__main__": parser = argparse.ArgumentParser(description='Flash body over can') @@ -23,16 +45,32 @@ def heartbeat_thread(p): _thread.start_new_thread(heartbeat_thread, (p,)) p.set_safety_mode(Panda.SAFETY_BODY) - while 1: - if len(p.can_recv()) == 0: - break + is_base_found = False + is_knee_found = False + + print("Looking for boards to flash...") + start_time = time.time() + received_msgs = [] + while (time.time() - start_time) < 1: + buff = p.can_recv() + if len(buff): + received_msgs.extend(buff) + + for msg in received_msgs: + if msg[0] == 0x203: + is_base_found = True + elif msg[0] == 0x303: + is_knee_found = True + + if is_knee_found and args.fn: + print("Flashing knee motherboard") + flasher(p, 0x350, args.fn) - p.can_send(0x250, b"\xce\xfa\xad\xde\x1e\x0b\xb0\x0a", 0) + time.sleep(0.2) + flush_panda() - if args.fn: - time.sleep(0.1) - print("flashing", args.fn) - code = open(args.fn, "rb").read() - Panda.flash_static(CanHandle(p, 0), code) + if is_base_found and args.fn: + print("Flashing base motherboard") + flasher(p, 0x250, args.fn) - print("can flash done") + print("CAN flashing done") From ce80a0e12b8dfc65c6bd3534699a5cce7ed47288 Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Mon, 25 Jul 2022 20:32:54 -0700 Subject: [PATCH 11/18] print git commit version on build --- board/SConscript | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/board/SConscript b/board/SConscript index 356d188..d1d6ea0 100644 --- a/board/SConscript +++ b/board/SConscript @@ -121,8 +121,10 @@ def objcopy(source, target, env, for_signature): return '$OBJCOPY -O binary %s %s' % (source[0], target[0]) # Common autogenerated includes +git_ver = get_version(BUILDER, BUILD_TYPE) with open("obj/gitversion.h", "w") as f: - f.write(f'const uint8_t gitversion[8] = "{get_version(BUILDER, BUILD_TYPE)}";\n') + f.write(f'const uint8_t gitversion[8] = "{git_ver}";\n') +print(f"Git commit hash for gitversion.h: {git_ver}") certs = [get_key_header(n) for n in ["debug", "release"]] with open("obj/cert.h", "w") as f: From 56959cbee87e072da98571366770310371bcb97d Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Tue, 26 Jul 2022 13:25:38 -0700 Subject: [PATCH 12/18] revert 0x201 msg size --- board/main.c | 27 ++++++++++++--------------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/board/main.c b/board/main.c index c1909eb..8bc174b 100644 --- a/board/main.c +++ b/board/main.c @@ -110,7 +110,7 @@ int main(void) { out_enable(POWERSWITCH, false); ignition = 1; } - // Reset LEDs upon startup + // Reset LEDs on startup out_enable(LED_RED, false); out_enable(LED_GREEN, false); out_enable(LED_BLUE, false); @@ -123,12 +123,13 @@ int main(void) { 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; - #define GEARBOX_RATIO_LEFT 19 - #define GEARBOX_RATIO_RIGHT 19 - uint16_t sensor_angle[2] = { 0 }; uint16_t hall_angle_offset[2] = { 0 }; - angle_sensor_read(sensor_angle); + if (hw_type == HW_TYPE_KNEE) { + angle_sensor_read(sensor_angle); + hall_angle_offset[0] = (sensor_angle[0] * ANGLE_TO_DEGREES); + hall_angle_offset[1] = (sensor_angle[1] * ANGLE_TO_DEGREES); + } while(1) { if (buzzerTimer - buzzerTimer_prev > 16*DELAY_IN_MAIN_LOOP) { // 1 ms = 16 ticks buzzerTimer @@ -140,19 +141,13 @@ int main(void) { } if (!enable_motors || (torque_cmd_timeout > 20)) { - cmdL = 0; - cmdR = 0; + cmdL = cmdR = 0; } if (ignition == 1 && enable_motors == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (ABS(cmdL) < 50 && ABS(cmdR) < 50)) { beepShort(6); // make 2 beeps indicating the motor enable beepShort(4); HAL_Delay(100); - sensor_angle[0] = 0; - sensor_angle[1] = 0; - angle_sensor_read(sensor_angle); - hall_angle_offset[0] = (sensor_angle[0] * ANGLE_TO_DEGREES); - hall_angle_offset[1] = (sensor_angle[1] * ANGLE_TO_DEGREES); cmdL = cmdR = 0; enable_motors = 1; // enable motors } @@ -215,9 +210,11 @@ int main(void) { dat[1] = speedL & 0xFFU; dat[2] = (speedR >> 8U) & 0xFFU; dat[3] = speedR & 0xFFU; - dat[4] = pkt_idx; - dat[5] = crc_checksum(dat, 5, crc_poly); - can_send_msg((0x201U + board.can_addr_offset), ((dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 6U); + dat[4] = 0; + dat[5] = 0; + dat[6] = pkt_idx; + dat[7] = crc_checksum(dat, 7, crc_poly); + can_send_msg((0x201U + board.can_addr_offset), ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U); ++pkt_idx; pkt_idx &= 0xFU; From 517113db3b713c906a69e0c00523bdb9acff2d1f Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Tue, 26 Jul 2022 13:26:06 -0700 Subject: [PATCH 13/18] move defines --- board/config.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/board/config.h b/board/config.h index 2a3437f..71c26e7 100644 --- a/board/config.h +++ b/board/config.h @@ -15,10 +15,12 @@ #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 #define ANGLE_TO_DEGREES 0.021972656 // Convert 14 bit angle sensor output to degrees +#define GEARBOX_RATIO_LEFT 19 +#define GEARBOX_RATIO_RIGHT 19 #define TRQ_LIMIT_LEFT 1000 // Torque limit for knee gearbox(left) #define TRQ_LIMIT_RIGHT 1000 // Torque limit for hip gearbox(right) -#define KNEE_ADDR_OFFSET 0x100U +#define KNEE_ADDR_OFFSET 0x100 #define BAT_FILT_COEF 655 // battery voltage filter coefficient in fixed-point. coef_fixedPoint = coef_floatingPoint * 2^16. In this case 655 = 0.01 * 2^16 #define BAT_CALIB_REAL_VOLTAGE 3192 // input voltage measured by multimeter (multiplied by 100). In this case 43.00 V * 100 = 4300 From 7d87bde3605dced9d940a28f1464888f63eecd0c Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Tue, 26 Jul 2022 15:20:44 -0700 Subject: [PATCH 14/18] i2c move to 100kHz --- board/setup.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/board/setup.h b/board/setup.h index 83e548f..6384449 100644 --- a/board/setup.h +++ b/board/setup.h @@ -179,7 +179,7 @@ void MX_I2C_Init(void) { __HAL_RCC_I2C1_CLK_ENABLE(); hi2c1.Instance = I2C1; - hi2c1.Init.ClockSpeed = 400000; + hi2c1.Init.ClockSpeed = 100000; hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2; hi2c1.Init.OwnAddress1 = 0; hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; From 49e1d27b12ab8ddc0ab6346d5a64cf32d89ecf16 Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Wed, 27 Jul 2022 14:54:56 -0700 Subject: [PATCH 15/18] revert I2C to 400kHz --- board/setup.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/board/setup.h b/board/setup.h index 6384449..83e548f 100644 --- a/board/setup.h +++ b/board/setup.h @@ -179,7 +179,7 @@ void MX_I2C_Init(void) { __HAL_RCC_I2C1_CLK_ENABLE(); hi2c1.Instance = I2C1; - hi2c1.Init.ClockSpeed = 100000; + hi2c1.Init.ClockSpeed = 400000; hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2; hi2c1.Init.OwnAddress1 = 0; hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; From 5c2daa13f6f570796bcf23614f9b84da8d5a1712 Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Mon, 1 Aug 2022 17:36:34 -0700 Subject: [PATCH 16/18] revert BLDC Ki change for speed limit --- board/bldc/BLDC_controller_data.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/board/bldc/BLDC_controller_data.c b/board/bldc/BLDC_controller_data.c index 0a256b4..626c537 100644 --- a/board/bldc/BLDC_controller_data.c +++ b/board/bldc/BLDC_controller_data.c @@ -337,7 +337,7 @@ P rtP_Left = { * '/cf_nKiLimProt' * '/cf_nKiLimProt' */ - 650U, + 246U, /* Variable: n_polePairs * Referenced by: '/n_polePairs' From 160b1d98886f139f3750e37e2ce2c15a670dbc74 Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Mon, 1 Aug 2022 17:41:08 -0700 Subject: [PATCH 17/18] change torque limits --- board/config.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/board/config.h b/board/config.h index 71c26e7..accb892 100644 --- a/board/config.h +++ b/board/config.h @@ -17,8 +17,8 @@ #define ANGLE_TO_DEGREES 0.021972656 // Convert 14 bit angle sensor output to degrees #define GEARBOX_RATIO_LEFT 19 #define GEARBOX_RATIO_RIGHT 19 -#define TRQ_LIMIT_LEFT 1000 // Torque limit for knee gearbox(left) -#define TRQ_LIMIT_RIGHT 1000 // Torque limit for hip gearbox(right) +#define TRQ_LIMIT_LEFT 400 // Torque limit for knee gearbox(left) +#define TRQ_LIMIT_RIGHT 200 // Torque limit for hip gearbox(right) #define KNEE_ADDR_OFFSET 0x100 @@ -63,6 +63,7 @@ #define I_MOT_MAX 15 // [A] Maximum single motor current limit #define I_DC_MAX 17 // [A] Maximum stage2 DC Link current limit for Commutation and Sinusoidal types (This is the final current protection. Above this value, current chopping is applied. To avoid this make sure that I_DC_MAX = I_MOT_MAX + 2A) #define N_MOT_MAX 100 // [rpm] Maximum motor speed limit // 100 ~= 52m/m +#define TORQUE_BASE_MAX 1000 // Field Weakening / Phase Advance #define FIELD_WEAK_ENA 0 // [-] Field Weakening / Phase Advance enable flag: 0 = Disabled (default), 1 = Enabled From 354d2da53e91730e953a30c2ed1ee52790b52dbd Mon Sep 17 00:00:00 2001 From: Igor Biletksyy Date: Mon, 1 Aug 2022 17:41:42 -0700 Subject: [PATCH 18/18] enable angle sensor safety and base trq limit --- board/main.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/board/main.c b/board/main.c index 8bc174b..4cc4490 100644 --- a/board/main.c +++ b/board/main.c @@ -157,7 +157,7 @@ int main(void) { // Safety to stop operation if angle sensor reading failed TODO: adjust sensivity and add lowpass to angle sensor? if ((ABS((hall_angle_offset[0] + ((motPosL / 15 / GEARBOX_RATIO_LEFT) % 360)) - (sensor_angle[0] * ANGLE_TO_DEGREES)) > 5) || (ABS((hall_angle_offset[1] + ((motPosR / 15 / GEARBOX_RATIO_RIGHT) % 360)) - (sensor_angle[1] * ANGLE_TO_DEGREES)) > 5)) { - //cmdL = cmdR = 0; + cmdL = cmdR = 0; } // Safety to stop movement when reaching dead angles, around 20 and 340 degrees if (((sensor_angle[0] < 900) && (cmdL < 0)) || ((sensor_angle[0] > 15500) && (cmdL > 0))) { @@ -176,7 +176,7 @@ int main(void) { if (hw_type == HW_TYPE_KNEE) { pwml = -CLAMP((int)cmdL, -TRQ_LIMIT_LEFT, TRQ_LIMIT_LEFT); } else { - pwml = CLAMP((int)cmdL, -1000, 1000); + pwml = CLAMP((int)cmdL, -TORQUE_BASE_MAX, TORQUE_BASE_MAX); } } if (ABS(cmdR) < 10) { @@ -187,7 +187,7 @@ int main(void) { if (hw_type == HW_TYPE_KNEE) { pwmr = -CLAMP((int)cmdR, -TRQ_LIMIT_RIGHT, TRQ_LIMIT_RIGHT); } else { - pwmr = -CLAMP((int)cmdR, -1000, 1000); + pwmr = -CLAMP((int)cmdR, -TORQUE_BASE_MAX, TORQUE_BASE_MAX); } }