Add Support for BMS Tiltback

Relys · Relys · commit 7138dee019bf · 2024-12-18T18:39:47.000-08:00
diff --git a/package.lisp b/package.lisp
@@ -8,6 +8,15 @@
 ; Set firmware version:
 (apply ext-set-fw-version (sysinfo 'fw-ver))
 
+(if (eq (first (trap (get-bms-val 'bms-v-cell-min))) 'exit-ok) {
+    (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 {
+    NONE = 0,
+    BMS_CONNECTION = 1,
+    BMS_OVER_TEMP = 2,
+    BMS_CELL_OVER_VOLTAGE = 3,
+    BMS_CELL_UNDER_VOLTAGE = 4,
+    BMS_CELL_OVER_TEMP = 5,
+    BMS_CELL_UNDER_TEMP = 6,
+    BMS_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,49 @@
+(defun bms-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 Motor CFG -> BMS limiting functionality would happen.
+    (var bms-tmin-limit-start 0)
+    (var bms-cell-balance-start 0.5)
+    (var bms-ic-tmax-limit-start (+ bms-tmax-limit-start 15)) ; Set bms temp limit to +15C past cell limit
+    (var bms-config-update-time (systime))
+    (var bms-fault 0u32)
+    (var bms-fault-codes '(
+        (NONE . 0)
+        (BMS_CONNECTION . 1)
+        (BMS_OVER_TEMP . 2)
+        (BMS_CELL_OVER_VOLTAGE . 3)
+        (BMS_CELL_UNDER_VOLTAGE . 4)
+        (BMS_CELL_OVER_TEMP . 5)
+        (BMS_CELL_UNDER_TEMP . 6)
+        (BMS_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))
+            (setq bms-ic-tmax-limit-start (+ bms-tmax-limit-start 15))
+            (setq bms-config-update-time (systime))
+        })
+        (if (>= (get-bms-val 'bms-msg-age) 2.0) {
+            (bms-set-fault 'BMS_CONNECTION)
+        } {
+            (bms-set-fault 'NONE)
+            (if (>= (get-bms-val 'bms-v-cell-max) bms-vmax-limit-start) (bms-set-fault 'BMS_CELL_OVER_VOLTAGE))
+            (if (<= (get-bms-val 'bms-v-cell-min) bms-vmin-limit-start) (bms-set-fault 'BMS_CELL_UNDER_VOLTAGE))
+            (if (>= (get-bms-val 'bms-temp-cell-max) bms-tmax-limit-start) (bms-set-fault 'BMS_CELL_OVER_TEMP))
+            (if (<= (get-bms-val 'bms-temp-cell-max) bms-tmin-limit-start) (bms-set-fault 'BMS_CELL_UNDER_TEMP))
+            (if (>= (abs (- v-cell-max v-cell-min)) bms-cell-balance-start)  (bms-set-fault 'BMS_CELL_BALANCE))
+            (if (>= (get-bms-val 'bms-temp-ic) bms-ic-tmax-limit-start) (bms-set-fault 'BMS_OVER_TEMP))
+        })
+        (apply ext-bms-set-fault bms-fault)
+        (yield 10000)
+    })
+})
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, BMS_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, BMS_CELL_OVER_VOLTAGE))) {
+        if (bms_is_fault_set(d->bms_fault, BMS_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, BMS_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, BMS_CELL_OVER_TEMP) ||
+               bms_is_fault_set(d->bms_fault, BMS_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, BMS_CELL_OVER_TEMP)) {
+            d->beep_reason = BEEP_TEMP_CELL_OVER;
+        } else if (bms_is_fault_set(d->bms_fault, BMS_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, BMS_CELL_UNDER_VOLTAGE))) {
         beep_alert(d, 3, false);
-        d->beep_reason = BEEP_LV;
+        if (bms_is_fault_set(d->bms_fault, BMS_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, BMS_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, BMS_CONNECTION)) {
+                beep_alert(d, 3, true);
+                d->beep_reason = BEEP_BMS_CONNECTION;
+            }
+
+            if (bms_is_fault_set(d->bms_fault, BMS_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,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 +2765,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, "Cell Under Temp"],
+            [12, "Cell Over Temp"],
+            [13, "Cell Low Voltage"],
+            [14, "Cell High Voltage"],
+            [15, "Cell Balance"],
+            [16, "BMS Connection"],
+            [17, "BMS Over Temp"],
         ])
 
         property int beepReason: 0
