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

improvements

This commit is contained in:
Igor Biletskyy 2022-03-16 21:53:38 -07:00
parent d502d45450
commit 3f140ab659
9 changed files with 108 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
board/SConscript
View File

@ -75,6 +75,7 @@ c_bstub_sources = [
["hal", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c"],
["system", "inc/system_stm32f4xx.c"],
["it", "inc/stm32f4xx_it.c"],
["util", "util.c"],
]
def get_version(builder, build_type):

8
board/bldc/bldc.c
View File

@ -64,7 +64,7 @@ volatile uint32_t buzzerTimer = 0;
static uint8_t buzzerPrev = 0;
static uint8_t buzzerIdx = 0;
uint8_t enable = 0; // initially motors are disabled for SAFETY
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
@ -113,13 +113,13 @@ void DMA2_Stream0_IRQHandler(void) {
// Disable PWM when current limit is reached (current chopping)
// This is the Level 2 of current protection. The Level 1 should kick in first given by I_MOT_MAX
if(ABS(curL_DC) > curDC_max || enable == 0) {
if(ABS(curL_DC) > curDC_max || enable_motors == 0) {
LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
} else {
LEFT_TIM->BDTR |= TIM_BDTR_MOE;
}
if(ABS(curR_DC) > curDC_max || enable == 0) {
if(ABS(curR_DC) > curDC_max || enable_motors == 0) {
RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
} else {
RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
@ -161,7 +161,7 @@ void DMA2_Stream0_IRQHandler(void) {
OverrunFlag = true;
/* Make sure to stop BOTH motors in case of an error */
enableFin = enable && !rtY_Left.z_errCode && !rtY_Right.z_errCode;
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);

13
board/bootstub.c
View File

@ -19,6 +19,11 @@
#include "obj/cert.h"
#include "obj/gitversion.h"
#include "drivers/llbxcan.h"
#include "drivers/llflash.h"
#include "provision.h"
#include "util.h"
#include "flasher.h"
void __initialize_hardware_early(void) {
@ -46,10 +51,10 @@ int main(void) {
SystemClock_Config();
MX_GPIO_Init();
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET); // Activate Latch
HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, GPIO_PIN_SET);
out_enable(POWERSWITCH, true);
out_enable(LED_RED, false);
out_enable(LED_GREEN, false);
out_enable(LED_BLUE, false);
if (enter_bootloader_mode == ENTER_SOFTLOADER_MAGIC) {
enter_bootloader_mode = 0;

2
board/comms.h
View File

@ -141,7 +141,7 @@ void CAN2_RX0_IRQHandler(void) {
} else if ((address == BROADCAST_ADDR) || (address == FALLBACK_ADDR) || (address == ECU_ADDR)) { // Process UBS and OBD2 requests
process_ubs(address, GET_MAILBOX_BYTES_04(&CAN2->sFIFOMailBox[0]));
}
HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, GPIO_PIN_RESET);
out_enable(LED_BLUE, true);
// next
CAN2->RF0R |= CAN_RF0R_RFOM0;
}

7
board/defines.h
View File

@ -142,6 +142,13 @@
#define COMPILE_TIME_ASSERT(pred) ((void)sizeof(char[1 - (2 * ((int)(!(pred))))]))
#define LED_RED 0
#define LED_GREEN 1
#define LED_BLUE 2
#define IGNITION 3
#define POWERSWITCH 4
#define TRANSCEIVER 5
typedef struct {
uint32_t rrB;
uint32_t rrC;

18
board/flasher.h
View File

@ -1,7 +1,3 @@
#include "drivers/llbxcan.h"
#include "drivers/llflash.h"
#include "provision.h"
typedef union {
uint16_t w;
struct BW {
@ -57,7 +53,7 @@ int can_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp) {
flash_unlock();
resp[1] = 0xff;
}
HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_RESET);
out_enable(LED_GREEN, true);
unlocked = true;
prog_ptr = (uint32_t *)APP_START_ADDRESS;
break;
@ -103,12 +99,12 @@ int can_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp) {
}
void flash_data(void *data, int len) {
HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, GPIO_PIN_SET);
out_enable(LED_RED, false);
for (int i = 0; i < len/4; i++) {
flash_write_word(prog_ptr, *(uint32_t*)(data+(i*4)));
prog_ptr++;
}
HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, GPIO_PIN_RESET);
out_enable(LED_RED, true);
}
int prep_data(uint8_t *data, uint8_t *data_out) {
@ -241,7 +237,7 @@ void soft_flasher_start(void) {
enter_bootloader_mode = 0;
HAL_GPIO_WritePin(CAN_STBY_PORT, CAN_STBY_PIN, GPIO_PIN_RESET); // Enable transceiver by pulling STBY pin LOW (Normal mode)
out_enable(TRANSCEIVER, true);
__HAL_RCC_CAN1_CLK_ENABLE(); // Also needed for CAN2, dumb...
__HAL_RCC_CAN2_CLK_ENABLE();
@ -251,15 +247,15 @@ void soft_flasher_start(void) {
llcan_init(CAN);
// green LED on for flashing
HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_RESET);
out_enable(LED_GREEN, true);
uint64_t cnt = 0;
for (cnt=0;;cnt++) {
// blink the green LED fast
HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_SET);
out_enable(LED_GREEN, false);
delay(500000);
HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_RESET);
out_enable(LED_GREEN, true);
delay(500000);
}
}

86
board/main.c
View File

@ -42,7 +42,7 @@ extern int16_t speedAvgAbs; // Average measured speed in absolute
extern volatile int pwml; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
extern uint8_t enable; // global variable for motor enable
extern uint8_t enable_motors; // global variable for motor enable
extern int16_t batVoltage; // global variable for battery voltage
@ -65,6 +65,7 @@ static uint32_t buzzerTimer_prev = 0;
const uint8_t crc_poly = 0xD5U; // standard crc8
int main(void) {
HAL_Init();
HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);
@ -86,22 +87,21 @@ int main(void) {
HAL_ADC_Start(&hadc);
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET); // Activate Latch
HAL_GPIO_WritePin(IGNITION_PORT, IGNITION_PIN, GPIO_PIN_SET); // Set ignition pin HIGH (ON)
HAL_GPIO_WritePin(CAN_STBY_PORT, CAN_STBY_PIN, GPIO_PIN_RESET); // Enable transceiver by pulling STBY pin LOW (Normal mode)
out_enable(POWERSWITCH, true);
out_enable(IGNITION, true);
out_enable(TRANSCEIVER, true);
// Reset LEDs upon startup
HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, GPIO_PIN_SET);
out_enable(LED_RED, false);
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);
poweronMelody();
//poweronMelody();
ignition = 1;
@ -117,22 +117,22 @@ int main(void) {
if (ignition == 0) {
cmdL = cmdR = 0;
enable = 0;
enable_motors = 0;
}
if (!enable) {
if (!enable_motors) {
cmdL = 0;
cmdR = 0;
}
if (ignition == 1 && enable == 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);
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);
cmdL = cmdR = 0;
enable = 1; // enable motors
enable_motors = 1; // enable motors
}
HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, !ignition);
out_enable(LED_GREEN, ignition);
pwml = CLAMP((int)cmdL, -1000, 1000);
pwmr = -CLAMP((int)cmdR, -1000, 1000);
@ -146,29 +146,45 @@ int main(void) {
batVoltageCalib = batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC;
if (main_loop_counter % 2 == 0) { // runs at ~100Hz
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
dat[0] = (speedL >> 8U) & 0xFFU;
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[6] = (batVoltageCalib >> 8U) & 0xFFU;
dat[7] = batVoltageCalib & 0xFFU;
if (ignition) { // Send msg only with ignition on
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
dat[0] = (speedL >> 8U) & 0xFFU;
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[6] = rtY_Left.z_errCode;
dat[7] = rtY_Right.z_errCode;
// Calibrate chip temp
// dat[4] = (board_temp_adcFilt >> 8U) & 0xFFU;
// dat[5] = board_temp_adcFilt & 0xFFU;
// dat[6] = (board_temp_deg_c >> 8U) & 0xFFU;
// dat[7] = board_temp_deg_c & 0xFFU;
// speed_L(2), speed_R(2), hall_angle_L(1), hall_angle_R(1), left mot error(1), right motor error(1)
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, ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U);
if (main_loop_counter % 20 == 0) { // Runs at ~10Hz
uint8_t dat[2];
dat[0] = ignition;
dat[1] = enable_motors;
// ignition(1), enable_motors(1)
can_send_msg(0x202U, 0x0U, ((dat[1] << 8U) | dat[0]), 2U);
}
if (main_loop_counter % 200 == 0) { // Runs at ~1Hz
HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, GPIO_PIN_SET);
uint8_t dat[4];
dat[0] = (board_temp_deg_c >> 8U) & 0xFFU;
dat[1] = board_temp_deg_c & 0xFFU;
dat[2] = (batVoltageCalib >> 8U) & 0xFFU;
dat[3] = batVoltageCalib & 0xFFU;
// MCU temp(2), battery voltage(2)
can_send_msg(0x203U, 0x0U, ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 4U);
// Reset LED after CAN RX
out_enable(LED_BLUE, false);
}
poweroffPressCheck();
@ -176,7 +192,7 @@ int main(void) {
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();
} else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // 1 beep (low pitch): Motor error, disable motors
enable = 0;
enable_motors = 0;
beepCount(1, 24, 1);
} else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // 5 beeps (low pitch): Mainboard temperature warning
beepCount(5, 24, 1);

29
board/util.c
View File

@ -15,7 +15,7 @@ extern uint8_t buzzerCount; // global variable for the buzzer counts
extern uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t enable; // global variable for motor enable
extern uint8_t enable_motors; // global variable for motor enable
extern uint8_t ignition; // global variable for ignition on SBU2 line
//------------------------------------------------------------------------
@ -78,6 +78,29 @@ 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;
case LED_GREEN:
HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, !enabled);
break;
case LED_BLUE:
HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, !enabled);
break;
case IGNITION:
HAL_GPIO_WritePin(IGNITION_PORT, IGNITION_PIN, enabled);
break;
case POWERSWITCH:
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, enabled);
break;
case TRANSCEIVER:
HAL_GPIO_WritePin(CAN_STBY_PORT, CAN_STBY_PIN, !enabled);
break;
}
}
void poweronMelody(void) {
buzzerCount = 0;
for (int i = 8; i >= 0; i--) {
@ -131,14 +154,14 @@ void calcAvgSpeed(void) {
}
void poweroff(void) {
enable = 0;
enable_motors = 0;
buzzerCount = 0;
buzzerPattern = 0;
for (int i = 0; i < 8; i++) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
}
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET);
out_enable(POWERSWITCH, true);
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);

3
board/util.h
View File

@ -3,11 +3,13 @@
#define UTIL_H
#include <stdint.h>
#include <stdbool.h>
// Initialization Functions
void BLDC_Init(void);
// General Functions
void out_enable(uint8_t led, bool enabled);
void poweronMelody(void);
void beepCount(uint8_t cnt, uint8_t freq, uint8_t pattern);
void beepLong(uint8_t freq);
@ -24,6 +26,5 @@ void filtLowPass32(int32_t u, uint16_t coef, int32_t *y);
void rateLimiter16(int16_t u, int16_t rate, int16_t *y);
uint8_t crc_checksum(uint8_t *dat, int len, const uint8_t poly);
void delay(uint32_t a);
#endif