lukash · Relys · Dec 28, 2024
diff --git a/package.lisp b/package.lisp
@@ -8,6 +8,17 @@
 ; Set firmware version:
 (apply ext-set-fw-version (sysinfo 'fw-ver))
 
+(def version_major (first (sysinfo 'fw-ver)))
+(def version_minor (second (sysinfo 'fw-ver)))
+(if (or (eq (first (trap (get-bms-val 'bms-v-cell-min))) 'exit-ok) (or (>= version_major 7) (and (>= version_major 6) (>= version_minor 5)))) {
+    (loopwhile (and (< (get-bms-val 'bms-can-id) 0) (< (secs-since 0) 10.0)) (yield 1000000))
+    (if (>= (get-bms-val 'bms-can-id) 0) {
+        (import "src/bms.lisp" 'bms)
+        (read-eval-program bms)
+        (spawn bms-loop)
+    })
+})
+
 ; Set to 1 to monitor debug variables
 (define debug 1)
 

diff --git a/src/bms.c b/src/bms.c
@@ -0,0 +1,25 @@
+// Copyright 2024 Syler Clayton
+//
+// This file is part of the Refloat VESC package.
+//
+// Refloat VESC package is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by the
+// Free Software Foundation, either version 3 of the License, or (at your
+// option) any later version.
+//
+// Refloat VESC package is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+// or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+// more details.
+//
+// You should have received a copy of the GNU General Public License along with
+// this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "bms.h"
+
+bool bms_is_fault_set(uint32_t fault_mask, BMSFaultCode fault_code) {
+    if (fault_code < 1 || fault_code > 32) {
+        return false;
+    }
+    return (fault_mask & (1U << (fault_code - 1))) != 0;
+}
diff --git a/src/bms.h b/src/bms.h
@@ -0,0 +1,34 @@
+// Copyright 2024 Syler Clayton
+//
+// This file is part of the Refloat VESC package.
+//
+// Refloat VESC package is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by the
+// Free Software Foundation, either version 3 of the License, or (at your
+// option) any later version.
+//
+// Refloat VESC package is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+// or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+// more details.
+//
+// You should have received a copy of the GNU General Public License along with
+// this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef enum {
+    BMSF_NONE = 0,
+    BMSF_CONNECTION = 1,
+    BMSF_OVER_TEMP = 2,
+    BMSF_CELL_OVER_VOLTAGE = 3,
+    BMSF_CELL_UNDER_VOLTAGE = 4,
+    BMSF_CELL_OVER_TEMP = 5,
+    BMSF_CELL_UNDER_TEMP = 6,
+    BMSF_CELL_BALANCE = 7
+} BMSFaultCode;
+
+bool bms_is_fault_set(uint32_t fault_mask, BMSFaultCode fault_code);
diff --git a/src/bms.lisp b/src/bms.lisp
@@ -0,0 +1,93 @@
+(defun bms-loop () {
+    (var vmin-limit)
+    (var vmax-limit)
+    (var tmax-limit)
+    (var tmin-limit 0)
+    (var cell-balance 0.5)
+    (var mosfet-tmax-limit)
+    (var config-update-time)
+    (var fault 0u32)
+    (var v-cell-support (eq (first (trap (get-bms-val 'bms-v-cell-min))) 'exit-ok))
+    (var bms-data-version-support (and (eq (first (trap (get-bms-val 'bms-data-version))) 'exit-ok) (> (get-bms-val 'bms-data-version) 0)))
+    (defun update-config () {
+        (setq vmin-limit (conf-get 'bms-vmin-limit-start))
+        (setq vmax-limit (conf-get 'bms-vmax-limit-start))
+        (setq tmax-limit (- (conf-get 'bms-t-limit-start) 3)) ; Start 3 degrees before Motor CFG -> BMS limiting functionality would happen.
+        (setq mosfet-tmax-limit (+ tmax-limit 15)) ; Set bms temp limit to +15C past cell limit
+        (setq config-update-time (systime))
+    })
+    (var fault-codes '(
+        (BMSF_NONE . 0)
+        (BMSF_CONNECTION . 1)
+        (BMSF_OVER_TEMP . 2)
+        (BMSF_CELL_OVER_VOLTAGE . 3)
+        (BMSF_CELL_UNDER_VOLTAGE . 4)
+        (BMSF_CELL_OVER_TEMP . 5)
+        (BMSF_CELL_UNDER_TEMP . 6)
+        (BMSF_CELL_BALANCE . 7)
+    ))
+    (defun set-fault (fault-code) {
+        (if (eq fault-code 'BMSF_NONE) {
+            (setq fault 0u32)
+        }{
+            (setq fault (bitwise-or fault (shl 1 (- (assoc fault-codes fault-code) 1))))
+        })
+    })
+    (update-config)
+    (loopwhile t {
+        (if (> (secs-since config-update-time) 1.0) {
+            (update-config)
+        })
+        (if (or (= vmin-limit 0.0) (= vmax-limit 0.0) (= tmax-limit 0.0)) {
+            (set-fault 'BMSF_NONE)
+        } {
+            (if (>= (get-bms-val 'bms-msg-age) 2.0) {
+                (set-fault 'BMSF_CONNECTION)
+            } {
+                (set-fault 'BMSF_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))
+                        })
+                    })
+                })
+
+                (var t-cell-min)
+                (var t-cell-max)
+                (if bms-data-version-support {
+                    (setq t-cell-min (get-bms-val 'bms-temps-adc 1))
+                    (setq t-cell-max (get-bms-val 'bms-temps-adc 2))
+                    (var t-mosfet-temp (get-bms-val 'bms-temps-adc 3))
+                    (if (> t-mosfet-temp -280) {
+                        (if (>= t-mosfet-temp mosfet-tmax-limit) (set-fault 'BMSF_OVER_TEMP))
+                    })
+                } {
+                    (setq t-cell-min (get-bms-val 'bms-temp-cell-max))
+                    (setq t-cell-max t-cell-min)
+                })
+
+                (if (>= v-cell-max vmax-limit) (set-fault 'BMSF_CELL_OVER_VOLTAGE))
+                (if (<= v-cell-min vmin-limit) (set-fault 'BMSF_CELL_UNDER_VOLTAGE))
+                (if (>= t-cell-max tmax-limit) (set-fault 'BMSF_CELL_OVER_TEMP))
+                (if (<= t-cell-min tmin-limit) (set-fault 'BMSF_CELL_UNDER_TEMP))
+                (if (>= (abs (- v-cell-max v-cell-min)) cell-balance) (set-fault 'BMSF_CELL_BALANCE))
+            })
+        })
+        (ext-bms-set-fault fault)
+        (yield 200000)
+    })
+})
diff --git a/src/main.c b/src/main.c
@@ -22,6 +22,7 @@
 #include "vesc_c_if.h"
 
 #include "atr.h"
+#include "bms.h"
 #include "charging.h"
 #include "footpad_sensor.h"
 #include "lcm.h"
@@ -55,7 +56,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_OVER = 17
 } BeepReason;
 
 static const FootpadSensorState flywheel_konami_sequence[] = {
@@ -194,6 +202,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 +372,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 +683,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, BMSF_CELL_OVER_VOLTAGE)) {
         d->tb_highvoltage_timer = d->current_time;
     }
 
@@ -745,11 +758,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, BMSF_CELL_OVER_VOLTAGE))) {
+        if (bms_is_fault_set(d->bms_fault, BMSF_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, BMSF_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 +812,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, BMSF_CELL_OVER_TEMP) ||
+               bms_is_fault_set(d->bms_fault, BMSF_CELL_UNDER_TEMP) ||
+               bms_is_fault_set(d->bms_fault, BMSF_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, BMSF_CELL_OVER_TEMP)) {
+            d->beep_reason = BEEP_TEMP_CELL_OVER;
+        } else if (bms_is_fault_set(d->bms_fault, BMSF_CELL_UNDER_TEMP)) {
+            d->beep_reason = BEEP_TEMP_CELL_UNDER;
+        } else {
+            d->beep_reason = BEEP_BMS_TEMP_OVER;
+        }
+        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, BMSF_CELL_UNDER_VOLTAGE))) {
         beep_alert(d, 3, false);
-        d->beep_reason = BEEP_LV;
+        if (bms_is_fault_set(d->bms_fault, BMSF_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, BMSF_CELL_UNDER_VOLTAGE)) {
             if (d->motor.erpm > 0) {
                 d->setpoint_target = d->float_conf.tiltback_lv_angle;
             } else {
@@ -1443,6 +1488,17 @@ static void refloat_thd(void *arg) {
                 d->enable_upside_down = false;
                 d->state.darkride = false;
             }
+
+            if (bms_is_fault_set(d->bms_fault, BMSF_CONNECTION)) {
+                beep_alert(d, 3, true);
+                d->beep_reason = BEEP_BMS_CONNECTION;
+            }
+
+            if (bms_is_fault_set(d->bms_fault, BMSF_CELL_BALANCE)) {
+                beep_alert(d, 3, true);
+                d->beep_reason = BEEP_CELL_BALANCE;
+            }
+
             if (d->current_time - d->disengage_timer > 1800) {  // alert user after 30 minutes
                 if (d->current_time - d->nag_timer > 60) {  // beep every 60 seconds
                     d->nag_timer = d->current_time;
@@ -2582,6 +2638,16 @@ 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) {
+    if (argn != 1 || !VESC_IF->lbm_is_number(args[0])) {
+        return VESC_IF->lbm_enc_sym_eerror;
+    }
+    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 +2767,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/ui.qml.in b/ui.qml.in
@@ -867,6 +867,13 @@ Item {
             [8, "Low Battery"],
             [9, "Board Idle"],
             [10, "Other"],
+            [11, "BMS: Cell Under Temp"],
+            [12, "BMS: Cell Over Temp"],
+            [13, "BMS: Cell Low Voltage"],
+            [14, "BMS: Cell High Voltage"],
+            [15, "BMS: Cell Balance"],
+            [16, "BMS: Connection Lost"],
+            [17, "BMS: BMS Temp"],
         ])
 
         property int beepReason: 0