diff --git a/package.lisp b/package.lisp index 40f6df3..660ae09 100644 --- a/package.lisp +++ b/package.lisp @@ -8,6 +8,16 @@ ; Set firmware version: (apply ext-set-fw-version (sysinfo 'fw-ver)) +(if (and (>= (+ (first (sysinfo 'fw-ver)) (* (second (sysinfo 'fw-ver)) 0.01)) 6.05)) { + (def bms-discovery-timer (systime)) + (loopwhile (or (< (get-bms-val 'bms-can-id) 0) (< (secs-since bms-discovery-timer) 5)) (sleep 1)) + (if (>= (get-bms-val 'bms-can-id) 0){ + (import "src/bms-state.lisp" 'bms-state) + (read-eval-program bms-state) + (spawn bms-state-loop) + }) +}) + ; Set to 1 to monitor debug variables (define debug 1) diff --git a/src/bms-state.lisp b/src/bms-state.lisp new file mode 100644 index 0000000..1fe9e2b --- /dev/null +++ b/src/bms-state.lisp @@ -0,0 +1,63 @@ +(defun bms-state-loop () { + (var bms-vmin-limit-start (conf-get 'bms-vmin-limit-start)) + (var bms-vmax-limit-start (conf-get 'bms-vmax-limit-start)) + (var bms-tmax-limit-start (- (conf-get 'bms-t-limit-start) 3)) ; start 3 degrees before we start limiting + (var bms-cell-balance-start 0.5) + (var bms-discharge-tmax-limit-start (+ bms-tmax-limit-start 10)) ; set bms mosfet discharge limit to +10 deg c past cells + (var bms-config-update-time (systime)) + (var bms-fault 0u32) + + (var bms-fault-codes '( + (NONE . 0) + (BMS_COMM_TIMEOUT . 1) + (BMS_OVER_TEMP . 2) + (CELL_OVER_VOLTAGE . 3) + (CELL_UNDER_VOLTAGE . 4) + (CELL_OVER_TEMP . 5) + (CELL_UNDER_TEMP . 6) + (CELL_BALANCE . 7) + )) + (defun bms-set-fault (fault-code) { + (if (eq fault-code 'NONE) { + (setq bms-fault 0u32) + }{ + (setq bms-fault (bitwise-or bms-fault (shl 1 (- (assoc bms-fault-codes fault-code) 1)))) + }) + }) + + (loopwhile t { + (if (and (<= (get-duty) 0.05) (> (secs-since bms-config-update-time) 1.0)) { ; Only bother updating config values while board is idle + (setq bms-vmin-limit-start (conf-get 'bms-vmin-limit-start)) + (setq bms-vmax-limit-start (conf-get 'bms-vmax-limit-start)) + (setq bms-tmax-limit-start (- (conf-get 'bms-t-limit-start) 3)) ; start 3 degrees before we start limiting + (setq bms-discharge-tmax-limit-start (+ bms-tmax-limit-start 10)) ; set bms mosfet discharge limit to +10 deg c past cells + (setq bms-config-update-time (systime)) + }) + (if (>= (get-bms-val 'bms-msg-age) 2.0) { + (bms-set-fault 'BMS_COMM_TIMEOUT) + } { + (bms-set-fault 'NONE) + (var v-cell-min) ;(setq v-cell-min (get-bms-val 'bms-v-cell-min)) ;future + (var v-cell-max) ;(setq v-cell-max (get-bms-val 'bms-v-cell-max)) ;future + (var num-cells (get-bms-val 'bms-cell-num)) + (if (> num-cells 1) { + (setq v-cell-max (get-bms-val 'bms-v-cell 0)) + (setq v-cell-min (get-bms-val 'bms-v-cell 0)) + (looprange i 1 num-cells { + (var cell-volt (get-bms-val 'bms-v-cell i)) + (if (> cell-volt v-cell-max) + (setq v-cell-max cell-volt)) + (if (< cell-volt v-cell-min) + (setq v-cell-min cell-volt)) + }) + }) + (if (>= v-cell-max bms-vmax-limit-start) (bms-set-fault 'CELL_OVER_VOLTAGE)) + (if (<= v-cell-min bms-vmin-limit-start) (bms-set-fault 'CELL_UNDER_VOLTAGE)) + (if (>= (get-bms-val 'bms-temp-cell-max) bms-tmax-limit-start) (bms-set-fault 'CELL_OVER_TEMP)) + (if (>= (abs (- v-cell-max v-cell-min)) bms-cell-balance-start) (bms-set-fault 'CELL_BALANCE)) + (if (>= (get-bms-val 'bms-temp-ic) bms-discharge-tmax-limit-start) (bms-set-fault 'BMS_OVER_TEMP)) + }) + (apply ext-bms-set-fault bms-fault) + (yield 50000) + }) +}) \ No newline at end of file diff --git a/src/conf/datatypes.h b/src/conf/datatypes.h index 9f2aca0..e3ea713 100644 --- a/src/conf/datatypes.h +++ b/src/conf/datatypes.h @@ -21,6 +21,17 @@ #include #include +typedef enum { + NONE = 0, + BMS_COMM_TIMEOUT = 1, + BMS_OVER_TEMP = 2, + CELL_OVER_VOLTAGE = 3, + CELL_UNDER_VOLTAGE = 4, + CELL_OVER_TEMP = 5, + CELL_UNDER_TEMP = 6, + CELL_BALANCE = 7 +} BMS_FAULT_CODES; + typedef enum { INPUTTILT_NONE = 0, INPUTTILT_UART, diff --git a/src/main.c b/src/main.c index 68720d1..7a0a780 100644 --- a/src/main.c +++ b/src/main.c @@ -55,7 +55,14 @@ typedef enum { BEEP_SENSORS = 7, BEEP_LOWBATT = 8, BEEP_IDLE = 9, - BEEP_ERROR = 10 + BEEP_ERROR = 10, + BEEP_TEMP_CELL_UNDER = 11, + BEEP_TEMP_CELL_OVER = 12, + BEEP_CELL_LV = 13, + BEEP_CELL_HV = 14, + BEEP_CELL_BALANCE = 15, + BEEP_BMS_CONNECTION = 16, + BEEP_BMS_TEMP = 17 } BeepReason; static const FootpadSensorState flywheel_konami_sequence[] = { @@ -194,6 +201,8 @@ typedef struct { Konami flywheel_konami; Konami headlights_on_konami; Konami headlights_off_konami; + + uint32_t bms_fault; } data; static void brake(data *d); @@ -362,6 +371,8 @@ static void configure(data *d) { sizeof(headlights_off_konami_sequence) ); + d->bms_fault = 0; + reconfigure(d); if (d->state.state == STATE_DISABLED) { @@ -671,7 +682,8 @@ static bool check_faults(data *d) { static void calculate_setpoint_target(data *d) { float input_voltage = VESC_IF->mc_get_input_voltage_filtered(); - if (input_voltage < d->float_conf.tiltback_hv) { + if ((input_voltage < d->float_conf.tiltback_hv) && + !bms_is_fault_set(d->bms_fault, CELL_OVER_VOLTAGE)) { d->tb_highvoltage_timer = d->current_time; } @@ -745,23 +757,30 @@ static void calculate_setpoint_target(data *d) { if (d->state.mode != MODE_FLYWHEEL) { d->state.sat = SAT_PB_DUTY; } - } else if (d->motor.duty_cycle > 0.05 && input_voltage > d->float_conf.tiltback_hv) { - d->beep_reason = BEEP_HV; + } else if (d->motor.duty_cycle > 0.05 && + (input_voltage > d->float_conf.tiltback_hv || + bms_is_fault_set(d->bms_fault, CELL_OVER_VOLTAGE))) { + if (bms_is_fault_set(d->bms_fault, CELL_OVER_VOLTAGE)) { + d->beep_reason = BEEP_CELL_HV; + } else { + d->beep_reason = BEEP_HV; + } beep_alert(d, 3, false); if (((d->current_time - d->tb_highvoltage_timer) > .5) || - (input_voltage > d->float_conf.tiltback_hv + 1)) { + (input_voltage > d->float_conf.tiltback_hv + 1) || + bms_is_fault_set(d->bms_fault, CELL_OVER_VOLTAGE)) { // 500ms have passed or voltage is another volt higher, time for some tiltback if (d->motor.erpm > 0) { d->setpoint_target = d->float_conf.tiltback_hv_angle; } else { d->setpoint_target = -d->float_conf.tiltback_hv_angle; } - - d->state.sat = SAT_PB_HIGH_VOLTAGE; } else { // The rider has 500ms to react to the triple-beep, or maybe it was just a short spike - d->state.sat = SAT_NONE; + d->setpoint_target = 0; } + // Setting type to HV to ensure that we trigger haptic buzz! + d->state.sat = SAT_PB_HIGH_VOLTAGE; } else if (VESC_IF->mc_temp_fet_filtered() > d->mc_max_temp_fet) { // Use the angle from Low-Voltage tiltback, but slower speed from High-Voltage tiltback beep_alert(d, 3, true); @@ -792,9 +811,33 @@ static void calculate_setpoint_target(data *d) { // The rider has 1 degree Celsius left before we start tilting back d->state.sat = SAT_NONE; } - } else if (d->motor.duty_cycle > 0.05 && input_voltage < d->float_conf.tiltback_lv) { + } else if (bms_is_fault_set(d->bms_fault, CELL_OVER_TEMP) || + bms_is_fault_set(d->bms_fault, CELL_UNDER_TEMP) || + bms_is_fault_set(d->bms_fault, BMS_OVER_TEMP)) { + // Use the angle from Low-Voltage tiltback, but slower speed from High-Voltage tiltback + beep_alert(d, 3, true); + if (bms_is_fault_set(d->bms_fault, CELL_OVER_TEMP)) { + d->beep_reason = BEEP_TEMP_CELL_OVER; + } else if (bms_is_fault_set(d->bms_fault, CELL_UNDER_TEMP)) { + d->beep_reason = BEEP_TEMP_CELL_UNDER; + } else { + d->beep_reason = BEEP_BMS_TEMP; + } + if (d->motor.erpm > 0) { + d->setpoint_target = d->float_conf.tiltback_lv_angle; + } else { + d->setpoint_target = -d->float_conf.tiltback_lv_angle; + } + d->state.sat = SAT_PB_TEMPERATURE; + } else if (d->motor.duty_cycle > 0.05 && + (input_voltage < d->float_conf.tiltback_lv || + bms_is_fault_set(d->bms_fault, CELL_UNDER_VOLTAGE))) { beep_alert(d, 3, false); - d->beep_reason = BEEP_LV; + if (bms_is_fault_set(d->bms_fault, CELL_UNDER_VOLTAGE)) { + d->beep_reason = BEEP_CELL_LV; + } else { + d->beep_reason = BEEP_LV; + } float abs_motor_current = fabsf(d->motor.current); float vdelta = d->float_conf.tiltback_lv - input_voltage; float ratio = vdelta * 20 / abs_motor_current; @@ -802,7 +845,8 @@ static void calculate_setpoint_target(data *d) { // a) we're 2V below lv threshold // b) motor current is small (we cannot assume vsag) // c) we have more than 20A per Volt of difference (we tolerate some amount of vsag) - if ((vdelta > 2) || (abs_motor_current < 5) || (ratio > 1)) { + if ((vdelta > 2) || (abs_motor_current < 5) || (ratio > 1) || + bms_is_fault_set(d->bms_fault, CELL_UNDER_VOLTAGE)) { if (d->motor.erpm > 0) { d->setpoint_target = d->float_conf.tiltback_lv_angle; } else { @@ -2582,6 +2626,14 @@ static lbm_value ext_set_fw_version(lbm_value *args, lbm_uint argn) { return VESC_IF->lbm_enc_sym_true; } +// Called from Lisp to pass in the fault code of the bms. +static lbm_value ext_bms_set_fault(lbm_value *args, lbm_uint argn) { + unused(argn); + data *d = (data *) ARG; + d->bms_fault = VESC_IF->lbm_dec_as_u32(args[0]); + return VESC_IF->lbm_enc_sym_true; +} + // Used to send the current or default configuration to VESC Tool. static int get_cfg(uint8_t *buffer, bool is_default) { data *d = (data *) ARG; @@ -2701,6 +2753,7 @@ INIT_FUN(lib_info *info) { VESC_IF->set_app_data_handler(on_command_received); VESC_IF->lbm_add_extension("ext-dbg", ext_dbg); VESC_IF->lbm_add_extension("ext-set-fw-version", ext_set_fw_version); + VESC_IF->lbm_add_extension("ext-bms-set-fault", ext_bms_set_fault); return true; } diff --git a/src/utils.c b/src/utils.c index 1537384..9e44331 100644 --- a/src/utils.c +++ b/src/utils.c @@ -37,3 +37,10 @@ float clampf(float value, float min, float max) { const float m = value < min ? min : value; return m > max ? max : m; } + +bool bms_is_fault_set(uint32_t fault_mask, BMS_FAULT_CODES fault_code) { + if (fault_code < 1 || fault_code > 32) { + return false; + } + return (fault_mask & (1U << (fault_code - 1))) != 0; +} diff --git a/src/utils.h b/src/utils.h index 9bca854..46ffec6 100644 --- a/src/utils.h +++ b/src/utils.h @@ -17,6 +17,7 @@ #pragma once +#include "conf/datatypes.h" #include "vesc_c_if.h" #include diff --git a/ui.qml.in b/ui.qml.in index dcd4dd7..e840847 100644 --- a/ui.qml.in +++ b/ui.qml.in @@ -867,6 +867,13 @@ Item { [8, "Low Battery"], [9, "Board Idle"], [10, "Other"], + [11, "Cell Under Temp"], + [12, "Cell Over Temp"], + [13, "Cell Low Voltage"], + [14, "Cell High Voltage"], + [15, "Cell Balance"], + [16, "BMS Connection"], + [17, "BMS Temp"], ]) property int beepReason: 0