Add Support for BMS Tiltback

Relys · Relys · commit db4730c0f494 · 2024-11-25T12:04:34.000-08:00
diff --git a/package.lisp b/package.lisp
@@ -8,6 +8,12 @@
 ; Set firmware version:
 (apply ext-set-fw-version (sysinfo 'fw-ver))
 
+(if (>= (+ (first (sysinfo 'fw-ver)) (* (second (sysinfo 'fw-ver)) 0.01)) 6.05) {
+    (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
@@ -0,0 +1,151 @@
+(def bms-fault-codes '(
+    (NONE . 0)
+    (BMS_COMM_TIMEOUT . 1)
+    (BMS_SOFT_OVER_TEMP . 2)
+    (BMS_HARD_OVER_TEMP . 3)
+    (CELL_SOFT_OVER_VOLTAGE . 4)
+    (CELL_HARD_OVER_VOLTAGE . 5)
+    (CELL_SOFT_UNDER_VOLTAGE . 6)
+    (CELL_HARD_UNDER_VOLTAGE . 7)
+    (CELL_SOFT_OVER_TEMP . 8)
+    (CELL_HARD_OVER_TEMP . 9)
+    (CELL_SOFT_UNDER_TEMP . 10)
+    (CELL_HARD_UNDER_TEMP . 11)
+    (CELL_SOFT_BALANCE . 12)
+    (CELL_HARD_BALANCE . 13)
+))
+(def bms-fault)
+(def bms-vmin-limit-start)
+(def bms-vmin-limit-end)
+(def bms-vmax-limit-start)
+(def bms-vmax-limit-end)
+(def bms-tmax-limit-start)
+(def bms-tmax-limit-end)
+(def bms-discharge-tmax-limit-start)
+(def bms-discharge-tmax-limit-end)
+(def bms-tmin-limit-start 0)
+(def bms-tmin-limit-end -10)
+(def bms-cell-balance-start 0.05)
+(def bms-cell-balance-end 0.1)
+(def bms-config-update-time (systime))
+
+(defun bms-set-fault (fault-code set-bit) {
+    (if (eq fault-code 'NONE) {
+        (setq bms-fault 0u32)
+    }{
+        (var bit-pos (- (assoc bms-fault-codes fault-code) 1))
+        (if (= set-bit 1) (setq bms-fault (bitwise-or bms-fault (shl 1 bit-pos))) (setq bms-fault (bitwise-and bms-fault (bitwise-not (shl 1 bit-pos)))))
+    })
+})
+
+(defun bms-update-config () {
+    (if (!= (conf-get 'bms-limit-mode) 0) (conf-set 'bms-limit-mode 0)) ;Disable bms limit mode (soc temp volt) so it doesn't dump us.
+    (setq bms-vmin-limit-start (conf-get 'bms-vmin-limit-start))
+    (setq bms-vmin-limit-end (conf-get 'bms-vmin-limit-end))
+
+    (setq bms-vmax-limit-start (conf-get 'bms-vmax-limit-start))
+    (setq bms-vmax-limit-end (conf-get 'bms-vmax-limit-end))
+
+    (setq bms-tmax-limit-start (conf-get 'bms-t-limit-start))
+    (setq bms-tmax-limit-end (conf-get 'bms-t-limit-end))
+    (setq bms-discharge-tmax-limit-start (+ bms-tmax-limit-start 10))
+    (setq bms-discharge-tmax-limit-end (+ bms-tmax-limit-end 10))
+
+    (setq bms-config-update-time (systime))
+})
+
+(defun bms-check-cell-volts () {
+    (var num-cells (get-bms-val 'bms-cell-num))
+    (if (> num-cells 0) {
+        (var max-volt (get-bms-val 'bms-v-cell 0))
+        (var min-volt (get-bms-val 'bms-v-cell 0))
+        (looprange i 0 num-cells {
+            (var cell-volt (get-bms-val 'bms-v-cell i))
+            (if (> cell-volt max-volt)
+                (setq max-volt cell-volt))
+            (if (< cell-volt min-volt)
+                (setq min-volt cell-volt))
+            (cond
+                ((> cell-volt bms-vmax-limit-end) {
+                    (bms-set-fault 'CELL_HARD_OVER_VOLTAGE 1)
+                    (bms-set-fault 'CELL_SOFT_OVER_VOLTAGE 0)
+                    (break)
+                })
+                ((> cell-volt bms-vmax-limit-start)
+                    (bms-set-fault 'CELL_SOFT_OVER_VOLTAGE 1))
+                (t nil)
+            )
+            (cond
+                ((< cell-volt bms-vmin-limit-end) {
+                    (bms-set-fault 'CELL_HARD_UNDER_VOLTAGE 1)
+                    (bms-set-fault 'CELL_SOFT_UNDER_VOLTAGE 0)
+                    (break)
+                })
+                ((< cell-volt bms-vmin-limit-start)
+                    (bms-set-fault 'CELL_SOFT_UNDER_VOLTAGE 1))
+                (t nil)
+            )
+        })
+        (var volt-diff (abs (- max-volt min-volt)))
+        (cond
+            ((> volt-diff bms-cell-balance-end) {
+                (bms-set-fault 'CELL_HARD_BALANCE 1)
+            })
+            ((> volt-diff bms-cell-balance-start)
+                (bms-set-fault 'CELL_SOFT_BALANCE 1))
+            (t nil)
+        )
+    })
+})
+
+(defun bms-check-temps () {
+    (looprange i 0 (get-bms-val 'bms-temp-adc-num) {
+        (var adc-temp (get-bms-val 'bms-temps-adc i))
+        (cond
+            ((> adc-temp bms-tmax-limit-end) {
+                (bms-set-fault 'CELL_HARD_OVER_TEMP 1)
+                (bms-set-fault 'CELL_SOFT_OVER_TEMP 0)
+                (break)
+            })
+            ((> adc-temp bms-tmax-limit-start)
+                (bms-set-fault 'CELL_SOFT_OVER_TEMP 1))
+            (t nil)
+        )
+        (cond
+            ((< adc-temp bms-tmin-limit-end) {
+                (bms-set-fault 'CELL_HARD_UNDER_TEMP 1)
+                (bms-set-fault 'CELL_SOFT_UNDER_TEMP 0)
+                (break)
+            })
+            ((< adc-temp bms-tmin-limit-start)
+                (bms-set-fault 'CELL_SOFT_UNDER_TEMP 1))
+            (t nil)
+        )
+    })
+    (cond
+        ((> (get-bms-val 'bms-temp-ic) bms-discharge-tmax-limit-end)
+            (bms-set-fault 'BMS_HARD_OVER_TEMP 1))
+        ((> (get-bms-val 'bms-temp-ic) bms-discharge-tmax-limit-start)
+            (bms-set-fault 'BMS_SOFT_OVER_TEMP 1))
+        (t nil)
+    )
+})
+
+(defun bms-state-loop () {
+    (bms-update-config)
+    (bms-set-fault 'NONE 0)
+    (loopwhile t {
+        (if (>= (get-bms-val 'bms-can-id) 0) {
+            (if (> (secs-since bms-config-update-time) 1.0) (bms-update-config))
+            (if (> (get-bms-val 'bms-msg-age) 5.0) {
+                (bms-set-fault 'BMS_COMM_TIMEOUT 1)
+            } {
+                (bms-set-fault 'NONE 0)
+                (bms-check-cell-volts)
+                (bms-check-temps)
+            })
+            (apply ext-bms-set-fault bms-fault)
+        })
+        (yield 100000)
+    })
+})
diff --git a/src/conf/datatypes.h b/src/conf/datatypes.h
@@ -21,6 +21,23 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+typedef enum {
+    NONE = 0,
+    BMS_COMM_TIMEOUT = 1,
+    BMS_SOFT_OVER_TEMP = 2,
+    BMS_HARD_OVER_TEMP = 3,
+    CELL_SOFT_OVER_VOLTAGE = 4,
+    CELL_HARD_OVER_VOLTAGE = 5,
+    CELL_SOFT_UNDER_VOLTAGE = 6,
+    CELL_HARD_UNDER_VOLTAGE = 7,
+    CELL_SOFT_OVER_TEMP = 8,
+    CELL_HARD_OVER_TEMP = 9,
+    CELL_SOFT_UNDER_TEMP = 10,
+    CELL_HARD_UNDER_TEMP = 11,
+    CELL_SOFT_BALANCE = 12,
+    CELL_HARD_BALANCE = 13
+} BMS_FAULT_CODES;
+
 typedef enum {
     INPUTTILT_NONE = 0,
     INPUTTILT_UART,
diff --git 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) {
@@ -745,11 +756,20 @@ 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_SOFT_OVER_VOLTAGE) ||
+                bms_is_fault_set(d->bms_fault, CELL_HARD_OVER_VOLTAGE))) {
+        if (bms_is_fault_set(d->bms_fault, CELL_SOFT_OVER_VOLTAGE) ||
+            bms_is_fault_set(d->bms_fault, CELL_HARD_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_HARD_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;
@@ -792,17 +812,50 @@ 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_SOFT_OVER_TEMP) ||
+               bms_is_fault_set(d->bms_fault, CELL_HARD_OVER_TEMP) ||
+               bms_is_fault_set(d->bms_fault, CELL_SOFT_UNDER_TEMP) ||
+               bms_is_fault_set(d->bms_fault, CELL_HARD_UNDER_TEMP) ||
+               bms_is_fault_set(d->bms_fault, BMS_SOFT_OVER_TEMP) ||
+               bms_is_fault_set(d->bms_fault, BMS_HARD_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_SOFT_OVER_TEMP) ||
+            bms_is_fault_set(d->bms_fault, CELL_HARD_OVER_TEMP)) {
+            d->beep_reason = BEEP_TEMP_CELL_OVER;
+        } else if (bms_is_fault_set(d->bms_fault, CELL_SOFT_UNDER_TEMP) ||
+                   bms_is_fault_set(d->bms_fault, CELL_HARD_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_SOFT_UNDER_VOLTAGE) ||
+                bms_is_fault_set(d->bms_fault, CELL_HARD_UNDER_VOLTAGE))) {
         beep_alert(d, 3, false);
-        d->beep_reason = BEEP_LV;
+        if (bms_is_fault_set(d->bms_fault, CELL_SOFT_UNDER_VOLTAGE) ||
+            bms_is_fault_set(d->bms_fault, CELL_HARD_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;
         // When to do LV tiltback:
         // 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_SOFT_UNDER_VOLTAGE) ||
+            bms_is_fault_set(d->bms_fault, CELL_HARD_UNDER_VOLTAGE)) {
             if (d->motor.erpm > 0) {
                 d->setpoint_target = d->float_conf.tiltback_lv_angle;
             } else {
@@ -2582,6 +2635,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 +2762,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
@@ -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
@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include "conf/datatypes.h"
 #include "vesc_c_if.h"
 
 #include <stdint.h>
@@ -118,3 +119,5 @@ float clampf(float value, float min, float max);
  * @param step A maximum unit of change of @p value.
  */
 void rate_limitf(float *value, float target, float step);
+
+bool bms_is_fault_set(uint32_t fault_mask, BMS_FAULT_CODES fault_code);
diff --git 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
