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

Support both 7s and 10s battery packs

This commit is contained in:
Igor Biletksyy 2022-04-15 14:13:40 -07:00
parent 02b1ac0b79
commit 998d1bc4f6
3 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
board/bldc/bldc.c
View File

@ -77,8 +77,8 @@ static int16_t offsetrrC = 2000;
static int16_t offsetdcl = 2000;
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
int16_t batVoltage = (420 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE;
static int32_t batVoltageFixdt = (420 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 16; // Fixed-point filter output initialized at 420 V*100/cell = 4.2 V/cell converted to fixed-point
// DMA interrupt frequency =~ 16 kHz
@ -96,7 +96,7 @@ void DMA2_Stream0_IRQHandler(void) {
return;
}
if (buzzerTimer % 1000 == 0) { // Filter battery voltage at a slower sampling rate
if (buzzerTimer % 100 == 0) { // Filter battery voltage at a slower sampling rate
filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt);
batVoltage = (int16_t)(batVoltageFixdt >> 16); // convert fixed-point to integer
}

11
board/config.h
View File

@ -17,13 +17,10 @@
#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)
#define BAT_CELLS 7 // battery number of cells. Normal Hoverboard battery: 10s
#define VOLTS_PER_PERCENT 0.00814 // Volts per percent, for conversion of volts to percentage
#define BAT_LVL5 (390 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE
#define BAT_LVL4 (380 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE
#define BAT_LVL3 (370 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE
#define BAT_LVL2 (360 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE
#define BAT_LVL1 (350 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE
#define BAT_DEAD (337 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE
#define VOLTS_PER_PERCENT 0.00814 // Volts per percent, for conversion of volts to percentage
#define BAT_LVL2(cells) (360 * (cells) * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE
#define BAT_LVL1(cells) (350 * (cells) * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE
#define BAT_DEAD(cells) (337 * (cells) * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE
#define TEMP_FILT_COEF 655 // temperature filter coefficient in fixed-point. coef_fixedPoint = coef_floatingPoint * 2^16. In this case 655 = 0.01 * 2^16
#define TEMP_CAL_LOW_ADC 945 // temperature 1: ADC value

26
board/main.c
View File

@ -62,6 +62,8 @@ int16_t cmdR; // global variable for Right Command
uint8_t ignition = 0; // global variable for ignition on SBU2 line
uint8_t charger_connected = 0; // status of the charger port
uint8_t cells_count = BAT_CELLS; // init with 7s battery
bool is_cells_adjusted = false; // if cells count needs adjustment
uint8_t fault_status = 0; // fault status of the whole system
uint8_t pkt_idx = 0; // For CAN msg counter
@ -194,15 +196,16 @@ int main(void) {
// runs at ~1Hz
if (main_loop_counter % 200 == 0) {
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
uint8_t battery_percent = 100 - (((420 * cells_count) - batVoltageCalib) / cells_count / VOLTS_PER_PERCENT / 100); // Battery % left
// MCU temp(2), battery voltage(2), battery_percent(0:7), charger_connected(0:1)
uint8_t dat[4];
// MCU temp(1), battery voltage(2), battery_percent(0:7), charger_connected(0:1)
uint8_t dat[5];
dat[0] = board_temp_deg_c & 0xFFU;
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);
dat[4] = cells_count & 0xFU;
can_send_msg(0x203U, (dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 5U);
out_enable(LED_BLUE, false); // Reset LED after CAN RX
out_enable(LED_GREEN, true); // Always use LED to show that body is on
@ -217,17 +220,26 @@ int main(void) {
process_can();
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
// Set number of cells once after 5s of runtime
if ((main_loop_counter > 1000) && !is_cells_adjusted) {
if (batVoltageCalib > 3000) {
cells_count = 10U;
} else {
cells_count = 7U;
}
}
if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20) || (batVoltage < BAT_DEAD(cells_count) && 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_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);
} else if (batVoltage < BAT_LVL1) { // 1 beep fast (medium pitch): Low bat 1
} else if (batVoltage < BAT_LVL1(cells_count)) { // 1 beep fast (medium pitch): Low bat 1
beepCount(0, 10, 6);
out_enable(LED_RED, true);
} else if (batVoltage < BAT_LVL2) { // 1 beep slow (medium pitch): Low bat 2
} else if (batVoltage < BAT_LVL2(cells_count)) { // 1 beep slow (medium pitch): Low bat 2
beepCount(0, 10, 30);
} else { // do not beep
beepCount(0, 0, 0);