Add Support for BMS Tiltback

BMS Pushback/Haptic Signed-off-by: Dado Mista <[email protected]> Refactored lisp code. Remove some faults for first implementation.
lukash · Dec 12, 2024 · 7311911 · 7311911
1 parent 1ad13ed
commit 7311911
Show file tree

Hide file tree

Showing 7 changed files with 167 additions and 8 deletions.
diff --git a/package.lisp b/package.lisp
@@ -7,6 +7,14 @@
 
 ; Set firmware version:
 (apply ext-set-fw-version (sysinfo 'fw-ver))
+(if (>= (+ (first (sysinfo 'fw-ver)) (* (second (sysinfo 'fw-ver)) 0.01)) 6.05) {
+    (loopwhile (or (< (get-bms-val 'bms-can-id) 0) (< (secs-since 0) 10)) (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
@@ -0,0 +1,70 @@
+(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 v-cell-support (>= (+ (first (sysinfo 'fw-ver)) (* (second (sysinfo 'fw-ver)) 0.01)) 6.06))
+
+    (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)
+            (var v-cell-max)
+            (if v-cell-support {
+                (setq v-cell-min (get-bms-val 'bms-v-cell-min))
+                (setq v-cell-max (get-bms-val 'bms-v-cell-max))
+            } {
+                (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)
+    })
+})
diff --git a/src/conf/datatypes.h b/src/conf/datatypes.h
@@ -21,6 +21,17 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+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
@@ -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,11 +757,18 @@ 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;
@@ -792,17 +811,42 @@ 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;
         // 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_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
@@ -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