Skip to content

Commit 6509065

Browse files
vedderbvedderb
committed
Added BMS module
1 parent b5eb228 commit 6509065

10 files changed

+577
-17
lines changed

‎Makefile

+1
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,7 @@ CSRC = $(STARTUPSRC) \
160160
shutdown.c \
161161
mempools.c \
162162
worker.c \
163+
bms.c \
163164
$(HWSRC) \
164165
$(APPSRC) \
165166
$(NRFSRC) \

‎bms.c

+375
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,375 @@
1+
/*
2+
Copyright 2020 Benjamin Vedder [email protected]
3+
4+
This file is part of the VESC firmware.
5+
6+
The VESC firmware is free software: you can redistribute it and/or modify
7+
it under the terms of the GNU General Public License as published by
8+
the Free Software Foundation, either version 3 of the License, or
9+
(at your option) any later version.
10+
11+
The VESC firmware is distributed in the hope that it will be useful,
12+
but WITHOUT ANY WARRANTY; without even the implied warranty of
13+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14+
GNU General Public License for more details.
15+
16+
You should have received a copy of the GNU General Public License
17+
along with this program. If not, see <http://www.gnu.org/licenses/>.
18+
*/
19+
20+
/**
21+
* This is the BMS module of the VESC firmware. It mainly supports the VESC BMS, but
22+
* the intention is to have it extendible to other BMSs too. The first step is
23+
* to add the BMS you want to support to the BMS_TYPE enum, and then you need to update
24+
* this module to interpret CAN-messages from it properly.
25+
*/
26+
27+
#include "bms.h"
28+
#include "buffer.h"
29+
#include "utils.h"
30+
#include "datatypes.h"
31+
#include "comm_can.h"
32+
#include <string.h>
33+
#include <math.h>
34+
35+
// Settings
36+
#define MAX_CAN_AGE_SEC 2.0
37+
38+
// Private variables
39+
static volatile bms_config m_conf;
40+
static volatile bms_values m_values;
41+
static volatile bms_soc_soh_temp_stat m_stat_temp_max;
42+
static volatile bms_soc_soh_temp_stat m_stat_soc_min;
43+
static volatile bms_soc_soh_temp_stat m_stat_soc_max;
44+
45+
void bms_init(bms_config *conf) {
46+
m_conf = *conf;
47+
memset((void*)&m_values, 0, sizeof(m_values));
48+
memset((void*)&m_stat_temp_max, 0, sizeof(m_stat_temp_max));
49+
memset((void*)&m_stat_soc_min, 0, sizeof(m_stat_soc_min));
50+
memset((void*)&m_stat_soc_max, 0, sizeof(m_stat_soc_max));
51+
memset((void*)&m_values, 0, sizeof(m_values));
52+
m_values.can_id = -1;
53+
m_stat_temp_max.id = -1;
54+
m_stat_soc_min.id = -1;
55+
m_stat_soc_max.id = -1;
56+
}
57+
58+
bool bms_process_can_frame(uint32_t can_id, uint8_t *data8, int len, bool is_ext) {
59+
bool used_data = false;
60+
61+
if (m_conf.type == BMS_TYPE_VESC) {
62+
if (is_ext) {
63+
uint8_t id = can_id & 0xFF;
64+
CAN_PACKET_ID cmd = can_id >> 8;
65+
66+
switch (cmd) {
67+
case CAN_PACKET_BMS_SOC_SOH_TEMP_STAT: {
68+
used_data = true;
69+
70+
int32_t ind = 0;
71+
bms_soc_soh_temp_stat msg;
72+
msg.id = id;
73+
msg.rx_time = chVTGetSystemTime();
74+
msg.v_cell_min = buffer_get_float16(data8, 1e3, &ind);
75+
msg.v_cell_max = buffer_get_float16(data8, 1e3, &ind);
76+
msg.soc = ((float)((uint8_t)data8[ind++])) / 255.0;
77+
msg.soh = ((float)((uint8_t)data8[ind++])) / 255.0;
78+
msg.t_cell_max = (float)((int8_t)data8[ind++]);
79+
uint8_t stat = data8[ind++];
80+
msg.is_charging = (stat >> 0) & 1;
81+
msg.is_balancing = (stat >> 1) & 1;
82+
msg.is_charge_allowed = (stat >> 2) & 1;
83+
84+
if (id == m_values.can_id || UTILS_AGE_S(m_values.update_time) > MAX_CAN_AGE_SEC) {
85+
m_values.can_id = id;
86+
m_values.update_time = chVTGetSystemTimeX();
87+
m_values.soc = msg.soc;
88+
m_values.soh = msg.soh;
89+
m_values.temp_max_cell = msg.t_cell_max;
90+
}
91+
92+
// In case there is more than one BMS, keep track of the limiting
93+
// values for all of them.
94+
95+
if (m_stat_temp_max.id < 0 ||
96+
UTILS_AGE_S(m_stat_temp_max.rx_time) > MAX_CAN_AGE_SEC ||
97+
m_stat_temp_max.t_cell_max < msg.t_cell_max) {
98+
m_stat_temp_max = msg;
99+
} else if (m_stat_temp_max.id == msg.id) {
100+
m_stat_temp_max = msg;
101+
}
102+
103+
if (m_stat_soc_min.id < 0 ||
104+
UTILS_AGE_S(m_stat_soc_min.rx_time) > MAX_CAN_AGE_SEC ||
105+
m_stat_soc_min.soc > msg.soc) {
106+
m_stat_soc_min = msg;
107+
} else if (m_stat_soc_min.id == msg.id) {
108+
m_stat_soc_min = msg;
109+
}
110+
111+
if (m_stat_soc_max.id < 0 ||
112+
UTILS_AGE_S(m_stat_soc_max.rx_time) > MAX_CAN_AGE_SEC ||
113+
m_stat_soc_max.soc < msg.soc) {
114+
m_stat_soc_max = msg;
115+
} else if (m_stat_soc_max.id == msg.id) {
116+
m_stat_soc_max = msg;
117+
}
118+
} break;
119+
120+
case CAN_PACKET_BMS_V_TOT: {
121+
used_data = true;
122+
123+
if (id == m_values.can_id || UTILS_AGE_S(m_values.update_time) > MAX_CAN_AGE_SEC) {
124+
int32_t ind = 0;
125+
m_values.can_id = id;
126+
m_values.update_time = chVTGetSystemTimeX();
127+
m_values.v_tot = buffer_get_float32_auto(data8, &ind);
128+
m_values.v_charge = buffer_get_float32_auto(data8, &ind);
129+
}
130+
} break;
131+
132+
case CAN_PACKET_BMS_I: {
133+
used_data = true;
134+
135+
if (id == m_values.can_id || UTILS_AGE_S(m_values.update_time) > MAX_CAN_AGE_SEC) {
136+
int32_t ind = 0;
137+
m_values.can_id = id;
138+
m_values.update_time = chVTGetSystemTimeX();
139+
m_values.i_in = buffer_get_float32_auto(data8, &ind);
140+
m_values.i_in_ic = buffer_get_float32_auto(data8, &ind);
141+
}
142+
} break;
143+
144+
case CAN_PACKET_BMS_AH_WH: {
145+
used_data = true;
146+
147+
if (id == m_values.can_id || UTILS_AGE_S(m_values.update_time) > MAX_CAN_AGE_SEC) {
148+
int32_t ind = 0;
149+
m_values.can_id = id;
150+
m_values.update_time = chVTGetSystemTimeX();
151+
m_values.ah_cnt = buffer_get_float32_auto(data8, &ind);
152+
m_values.wh_cnt = buffer_get_float32_auto(data8, &ind);
153+
}
154+
} break;
155+
156+
case CAN_PACKET_BMS_V_CELL: {
157+
used_data = true;
158+
159+
if (id == m_values.can_id || UTILS_AGE_S(m_values.update_time) > MAX_CAN_AGE_SEC) {
160+
int32_t ind = 0;
161+
m_values.can_id = id;
162+
m_values.update_time = chVTGetSystemTimeX();
163+
unsigned int ofs = data8[ind++];
164+
m_values.cell_num = data8[ind++];
165+
166+
while(ind < len) {
167+
if (ofs >= (sizeof(m_values.v_cell) / sizeof(float))) {
168+
// Out of buffer space
169+
break;
170+
}
171+
172+
m_values.v_cell[ofs++] = buffer_get_float16(data8, 1e3, &ind);
173+
}
174+
}
175+
} break;
176+
177+
case CAN_PACKET_BMS_BAL: {
178+
used_data = true;
179+
180+
if (id == m_values.can_id || UTILS_AGE_S(m_values.update_time) > MAX_CAN_AGE_SEC) {
181+
int32_t ind = 0;
182+
m_values.can_id = id;
183+
m_values.update_time = chVTGetSystemTimeX();
184+
185+
uint64_t bal_state_0 = buffer_get_uint32(data8, &ind);
186+
uint64_t bal_state_1 = buffer_get_uint32(data8, &ind);
187+
uint64_t bal_state = bal_state_0 << 32 | bal_state_1;
188+
ind = 0;
189+
while (ind < (int)(sizeof(m_values.bal_state) / sizeof(bool)) && ind < 64) {
190+
m_values.bal_state[ind] = (bal_state >> ind) & 1;
191+
ind++;
192+
}
193+
}
194+
} break;
195+
196+
case CAN_PACKET_BMS_TEMPS: {
197+
used_data = true;
198+
199+
if (id == m_values.can_id || UTILS_AGE_S(m_values.update_time) > MAX_CAN_AGE_SEC) {
200+
int32_t ind = 0;
201+
m_values.can_id = id;
202+
m_values.update_time = chVTGetSystemTimeX();
203+
unsigned int ofs = data8[ind++];
204+
m_values.temp_adc_num = data8[ind++];
205+
206+
while(ind < len) {
207+
if (ofs >= (sizeof(m_values.temps_adc) / sizeof(float))) {
208+
// Out of buffer space
209+
break;
210+
}
211+
212+
m_values.temps_adc[ofs++] = buffer_get_float16(data8, 1e2, &ind);
213+
}
214+
}
215+
} break;
216+
217+
case CAN_PACKET_BMS_HUM: {
218+
used_data = true;
219+
220+
if (id == m_values.can_id || UTILS_AGE_S(m_values.update_time) > MAX_CAN_AGE_SEC) {
221+
int32_t ind = 0;
222+
m_values.can_id = id;
223+
m_values.update_time = chVTGetSystemTimeX();
224+
m_values.temp_hum = buffer_get_float16(data8, 1e2, &ind);
225+
m_values.hum = buffer_get_float16(data8, 1e2, &ind);
226+
m_values.temp_ic = buffer_get_float16(data8, 1e2, &ind);
227+
}
228+
} break;
229+
230+
default:
231+
break;
232+
}
233+
}
234+
}
235+
236+
return used_data;
237+
}
238+
239+
void bms_update_limits(float *i_in_min, float *i_in_max,
240+
float i_in_min_conf, float i_in_max_conf) {
241+
float i_in_min_bms = i_in_min_conf;
242+
float i_in_max_bms = i_in_max_conf;
243+
244+
// Temperature
245+
if (UTILS_AGE_S(m_stat_temp_max.rx_time) < MAX_CAN_AGE_SEC) {
246+
float temp = m_stat_temp_max.t_cell_max;
247+
248+
if (temp < m_conf.t_limit_start) {
249+
// OK
250+
} else if (temp > m_conf.t_limit_end) {
251+
i_in_min_bms = 0.0;
252+
i_in_max_bms = 0.0;
253+
// Maybe add fault code?
254+
// mc_interface_fault_stop(FAULT_CODE_OVER_TEMP_FET, false, false);
255+
} else {
256+
float maxc = fabsf(i_in_max_conf);
257+
if (fabsf(i_in_min_conf) > maxc) {
258+
maxc = fabsf(i_in_min_conf);
259+
}
260+
261+
maxc = utils_map(temp, m_conf.t_limit_start, m_conf.t_limit_end, maxc, 0.0);
262+
263+
if (fabsf(i_in_min_bms) > maxc) {
264+
i_in_min_bms = SIGN(i_in_min_bms) * maxc;
265+
}
266+
267+
if (fabsf(i_in_max_bms) > maxc) {
268+
i_in_max_bms = SIGN(i_in_max_bms) * maxc;
269+
}
270+
}
271+
}
272+
273+
// TODO: add support for conf->l_temp_accel_dec to still have braking.
274+
275+
// SOC
276+
if (UTILS_AGE_S(m_stat_soc_min.rx_time) < MAX_CAN_AGE_SEC) {
277+
float soc = m_stat_soc_min.soc;
278+
279+
if (soc > m_conf.soc_limit_start) {
280+
// OK
281+
} else if (soc < m_conf.soc_limit_end) {
282+
i_in_max_bms = 0.0;
283+
} else {
284+
i_in_max_bms = utils_map(soc, m_conf.soc_limit_start,
285+
m_conf.soc_limit_end, i_in_max_conf, 0.0);
286+
}
287+
}
288+
289+
if (fabsf(i_in_min_bms) < fabsf(*i_in_min)) {
290+
*i_in_min = i_in_min_bms;
291+
}
292+
293+
if (fabsf(i_in_max_bms) < fabsf(*i_in_max)) {
294+
*i_in_max = i_in_max_bms;
295+
}
296+
}
297+
298+
void bms_process_cmd(unsigned char *data, unsigned int len,
299+
void(*reply_func)(unsigned char *data, unsigned int len)) {
300+
COMM_PACKET_ID packet_id;
301+
302+
packet_id = data[0];
303+
data++;
304+
len--;
305+
306+
switch (packet_id) {
307+
case COMM_BMS_GET_VALUES: {
308+
int32_t ind = 0;
309+
uint8_t send_buffer[128];
310+
311+
send_buffer[ind++] = packet_id;
312+
313+
buffer_append_float32(send_buffer, m_values.v_tot, 1e6, &ind);
314+
buffer_append_float32(send_buffer, m_values.v_charge, 1e6, &ind);
315+
buffer_append_float32(send_buffer, m_values.i_in, 1e6, &ind);
316+
buffer_append_float32(send_buffer, m_values.i_in_ic, 1e6, &ind);
317+
buffer_append_float32(send_buffer, m_values.ah_cnt, 1e3, &ind);
318+
buffer_append_float32(send_buffer, m_values.wh_cnt, 1e3, &ind);
319+
320+
// Cell voltages
321+
send_buffer[ind++] = m_values.cell_num;
322+
for (int i = 0;i < m_values.cell_num;i++) {
323+
buffer_append_float16(send_buffer, m_values.v_cell[i], 1e3, &ind);
324+
}
325+
326+
// Balancing state
327+
for (int i = 0;i < m_values.cell_num;i++) {
328+
send_buffer[ind++] = m_values.bal_state[i];
329+
}
330+
331+
// Temperatures
332+
send_buffer[ind++] = m_values.temp_adc_num;
333+
for (int i = 0;i < m_values.temp_adc_num;i++) {
334+
buffer_append_float16(send_buffer, m_values.temps_adc[i], 1e2, &ind);
335+
}
336+
buffer_append_float16(send_buffer, m_values.temp_ic, 1e2, &ind);
337+
338+
// Humidity
339+
buffer_append_float16(send_buffer, m_values.temp_hum, 1e2, &ind);
340+
buffer_append_float16(send_buffer, m_values.hum, 1e2, &ind);
341+
342+
// Highest cell temperature
343+
buffer_append_float16(send_buffer, m_values.temp_max_cell, 1e2, &ind);
344+
345+
// State of charge and state of health
346+
buffer_append_float16(send_buffer, m_values.soc, 1e3, &ind);
347+
buffer_append_float16(send_buffer, m_values.soh, 1e3, &ind);
348+
349+
reply_func(send_buffer, ind);
350+
} break;
351+
352+
default:
353+
break;
354+
}
355+
356+
if (m_conf.type == BMS_TYPE_VESC && UTILS_AGE_S(m_values.update_time) < MAX_CAN_AGE_SEC) {
357+
switch (packet_id) {
358+
case COMM_BMS_SET_CHARGE_ALLOWED:
359+
case COMM_BMS_SET_BALANCE_OVERRIDE:
360+
case COMM_BMS_RESET_COUNTERS:
361+
case COMM_BMS_FORCE_BALANCE:
362+
case COMM_BMS_ZERO_CURRENT_OFFSET: {
363+
comm_can_send_buffer(m_values.can_id, data - 1, len + 1, 0);
364+
} break;
365+
366+
default:
367+
break;
368+
}
369+
}
370+
371+
}
372+
373+
bms_values *bms_get_values(void) {
374+
return (bms_values*)&m_values;
375+
}

0 commit comments

Comments
 (0)