mawei
/
body
mirror of https://github.com/commaai/body.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

angle sensor and safeties

This commit is contained in:
Igor Biletksyy 2022-05-27 19:15:52 -07:00
parent 4d8d7036eb
commit b4218f9263
16 changed files with 461 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
board/SConscript
View File

@ -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"],

59
board/bldc/bldc.c
View File

@ -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++;
}

48
board/boards.h Normal file
View File

@ -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) {}
}
}

9
board/bootstub.c
View File

@ -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();
}
}

34
board/comms.h
View File

@ -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; i<MSG_TRQ_LEN; i++) {
dat[i] = GET_MAILBOX_BYTE(&CAN2->sFIFOMailBox[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; i<MSG_TRQ_LEN; i++) {
dat[i] = GET_MAILBOX_BYTE(&CAN2->sFIFOMailBox[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);

3
board/config.h
View File

@ -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)

56
board/defines.h
View File

@ -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

69
board/drivers/angle_sensor.h Normal file
View File

@ -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);
}
}

2
board/drivers/clock.h
View File

@ -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;

2
board/drivers/llbxcan.h
View File

@ -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))

9
board/flasher.h
View File

@ -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;

2
board/inc/stm32f4xx_hal_conf.h
View File

@ -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 */

108
board/main.c
View File

@ -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;

118
board/setup.h
View File

@ -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);
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

37
board/util.c
View File

@ -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<PULL_EFFECTIVE_DELAY; i++);
bool ret = HAL_GPIO_ReadPin(GPIOx, GPIO_Pin);
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOx, &GPIO_InitStruct);
return ret;
}
uint8_t board_id(void) {
return (((!detect_with_pull(KEY1_PORT, KEY1_PIN, GPIO_PULLUP))) | (!detect_with_pull(KEY2_PORT, KEY2_PIN, GPIO_PULLUP)) << 1U);
}
uint8_t crc_checksum(uint8_t *dat, int len, const uint8_t poly) {
uint8_t crc = 0xFFU;
int i;

4
board/util.h
View File

@ -21,6 +21,10 @@ void calcAvgSpeed(void);
void poweroff(void);
void poweroffPressCheck(void);
// GPIO functions
uint8_t detect_with_pull(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin, uint32_t mode);
uint8_t board_id(void);
// Filtering Functions
void filtLowPass32(int32_t u, uint16_t coef, int32_t *y);
void rateLimiter16(int16_t u, int16_t rate, int16_t *y);