From 102e622701c7ec03de97599f6ea7dd33378564da Mon Sep 17 00:00:00 2001
From: Syler Clayton <2224238+Relys@users.noreply.github.com>
Date: Fri, 27 Dec 2024 16:09:07 -0800
Subject: [PATCH] Feature: Add Support for BMS Tiltback
---
package.lisp | 11 +++++++
src/bms.c | 25 ++++++++++++++
src/bms.h | 34 +++++++++++++++++++
src/bms.lisp | 93 ++++++++++++++++++++++++++++++++++++++++++++++++++++
src/main.c | 83 +++++++++++++++++++++++++++++++++++++++++-----
ui.qml.in | 7 ++++
6 files changed, 245 insertions(+), 8 deletions(-)
create mode 100644 src/bms.c
create mode 100644 src/bms.h
create mode 100644 src/bms.lisp
diff --git a/package.lisp b/package.lisp
index 40f6df3..296ea57 100644
--- 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
new file mode 100644
index 0000000..0a43be0
--- /dev/null
+++ 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 .
+
+#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
new file mode 100644
index 0000000..0e2ee58
--- /dev/null
+++ 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 .
+
+#pragma once
+
+#include
+#include
+
+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
new file mode 100644
index 0000000..d8746b3
--- /dev/null
+++ 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
index 68720d1..2b6d159 100644
--- 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,9 +812,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, 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;
@@ -802,7 +846,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, 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
index dcd4dd7..8f1c7cd 100644
--- 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