SoC for stacks

charger-display.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 
+from enum import Enum
 import math
 import os
 import struct
@@ -33,8 +34,8 @@ LOG_FRAME_LENGTH = 8  # bytes
 PARALLEL_CELLS = 9
 CELLS_PER_SLAVE = 10
 TEMP_SENSORS_PER_SLAVE = 32
-VOLTAGE_CONV = 5.0 / 255  # volts/quantum
-TEMP_CONV = 0.0625 * 16  # °C/quantum
+VOLTAGE_CONV = 5.0 / 0xFFFF  # volts/quantum
+TEMP_CONV = 0.0625  # °C/quantum
 
 ERRORCODE_TIMEOUT_SLAVE = 1
 ERRORCODE_SLAVE_PANIC = 2
@@ -53,6 +54,22 @@ MASTER_THRESH_OT = 1
 MASTER_THRESH_UV = 2
 MASTER_THRESH_OV = 3
 
+INTERNAL_RESISTANCE_CURVE_X = [2.0, 4.12]
+INTERNAL_RESISTANCE_CURVE_Y = [0.0528, 0.0294]
+SOC_OCV_X = [2.1, 2.9, 3.2, 3.3, 3.4, 3.5, 3.68, 4.0, 4.15, 4.2]
+SOC_OCV_Y = [0, 0.023, 0.06, 0.08, 0.119, 0.227, 0.541, 0.856, 0.985, 1.0]
+
+
+def estimate_soc(voltage: float) -> float:
+    r_i = np.interp(
+        [voltage],
+        INTERNAL_RESISTANCE_CURVE_X,
+        INTERNAL_RESISTANCE_CURVE_Y,
+    )[0]
+    i = data.current / PARALLEL_CELLS
+    ocv = voltage - i * r_i
+    return np.interp([ocv], SOC_OCV_X, SOC_OCV_Y)[0] * 100
+
 
 class SlaveData:
     cell_voltages: list[float]
@@ -60,7 +77,20 @@ class SlaveData:
 
     def __init__(self) -> None:
         self.cell_voltages = [-1] * CELLS_PER_SLAVE
-        self.cell_temps = [-1] * TEMP_SENSORS_PER_SLAVE
+        self.cell_temps = ([-1] * (TEMP_SENSORS_PER_SLAVE // 2)) + (
+            [0] * (TEMP_SENSORS_PER_SLAVE // 2)
+        )
+
+
+class TSState(Enum):
+    TS_INACTIVE = 0
+    TS_ACTIVE = 1
+    TS_PRECHARGE = 2
+    TS_DISCHARGE = 3
+    TS_ERROR = 4
+    TS_CHARGING_CHECK = 5
+    TS_CHARGING = 6
+    TS_BALANCING = 7
 
 
 class AccumulatorData:
@@ -77,6 +107,7 @@ class AccumulatorData:
     panic: bool
     panic_errorcode: int
     panic_errorarg: int
+    ts_state: TSState
 
     def __init__(self) -> None:
         self.slaves = [SlaveData() for _ in range(N_SLAVES)]
@@ -90,6 +121,7 @@ class AccumulatorData:
         self.panic = False
         self.panic_errorcode = self.panic_errorarg = 0
         self.time_since_last_frame = 0
+        self.ts_state = TSState.TS_INACTIVE
 
     def fill_dummy_data(self):
         self.min_voltage = random.uniform(1, 3)
@@ -119,63 +151,58 @@ class AccumulatorData:
 class StackGuiElement:
     title: str
     stack_id: int
-    min_voltage_label: QLabel
-    max_voltage_label: QLabel
-    min_temp_label: QLabel
-    max_temp_label: QLabel
+    voltage_label: QLabel
+    soc_label: QLabel
+    temp_label: QLabel
     groupBox: QGroupBox
     detail_popup: QWidget
 
     def __init__(self, title: str, stack_id: int):
         self.title = title
         self.stack_id = stack_id
-        self.min_voltage_label = QLabel()
-        self.max_voltage_label = QLabel()
-        self.min_temp_label = QLabel()
-        self.max_temp_label = QLabel()
+        self.voltage_label = QLabel()
+        self.soc_label = QLabel()
+        self.temp_label = QLabel()
         self.groupBox = QGroupBox()
         self.detail_popup = None
         self.__post__init__()
 
     def __post__init__(self):
-        l1 = QLabel("Min Voltage [V]")
-        l2 = QLabel("Max Voltage [V]")
-        l3 = QLabel("Min Temperature [°C]")
-        l4 = QLabel("Max Temperature [°C]")
-        popup_btn = QPushButton(self.title + " details")
+        l1 = QLabel("Voltage [V]")
+        l2 = QLabel("SoC [%]")
+        l3 = QLabel("Temperature [°C]")
+        popup_btn = QPushButton("Details")
         popup_btn.clicked.connect(self.show_popup)
 
         l1.setAlignment(Qt.AlignLeft)
         l2.setAlignment(Qt.AlignLeft)
         l3.setAlignment(Qt.AlignLeft)
-        l4.setAlignment(Qt.AlignLeft)
 
-        self.min_voltage_label.setAlignment(Qt.AlignLeft)
-        self.max_voltage_label.setAlignment(Qt.AlignLeft)
-        self.min_temp_label.setAlignment(Qt.AlignLeft)
-        self.max_temp_label.setAlignment(Qt.AlignLeft)
+        self.voltage_label.setAlignment(Qt.AlignLeft)
+        self.soc_label.setAlignment(Qt.AlignLeft)
+        self.temp_label.setAlignment(Qt.AlignLeft)
 
         grid = QGridLayout()
         grid.addWidget(l1, 0, 0)
         grid.addWidget(l2, 1, 0)
         grid.addWidget(l3, 2, 0)
-        grid.addWidget(l4, 3, 0)
 
-        grid.addWidget(self.min_voltage_label, 0, 1)
-        grid.addWidget(self.max_voltage_label, 1, 1)
-        grid.addWidget(self.min_temp_label, 2, 1)
-        grid.addWidget(self.max_temp_label, 3, 1)
+        grid.addWidget(self.voltage_label, 0, 1)
+        grid.addWidget(self.soc_label, 1, 1)
+        grid.addWidget(self.temp_label, 2, 1)
 
-        grid.addWidget(popup_btn, 0, 2)
+        grid.addWidget(popup_btn, 3, 0)
 
         self.groupBox.setTitle(self.title)
         self.groupBox.setLayout(grid)
 
     def update_data_from_slave(self, slave: SlaveData):
-        self.min_voltage_label.setNum(min(slave.cell_voltages))
-        self.max_voltage_label.setNum(max(slave.cell_voltages))
-        self.min_temp_label.setNum(min(slave.cell_temps))
-        self.max_temp_label.setNum(max(slave.cell_temps))
+        min_v, max_v = min(slave.cell_voltages), max(slave.cell_voltages)
+        self.voltage_label.setText(f"[{min_v:.02f}, {max_v:.02f}]")
+        min_soc, max_soc = estimate_soc(min_v), estimate_soc(max_v)
+        self.soc_label.setText(f"[{min_soc:02.0f}, {max_soc:02.0f}]")
+        min_t, max_t = min(slave.cell_temps), max(slave.cell_temps)
+        self.temp_label.setText(f"[{min_t:.02f}, {max_t:.02f}]")
 
     def show_popup(self):
         if self.detail_popup is None:
@@ -249,6 +276,7 @@ class StackPopup(QWidget):
         groupbox.setLayout(grid)
         layout.addWidget(groupbox)
         self.setLayout(layout)
+        self.setWindowTitle(f"FT22 Charger Display - Stack {stack_id} Details")
         self.update_data()
         timer.timeout.connect(self.update_data)
 
@@ -313,15 +341,15 @@ class Window(QWidget):
         self.l_current.setNum(data.current)
         self.l_current.setAlignment(Qt.AlignLeft)
 
-        self.l8 = QLabel("Error")
-        self.l_error = QLabel()
-        self.l_error.setText(str(data.panic))
-        self.l_error.setAlignment(Qt.AlignLeft)
+        self.l8 = QLabel("State")
+        self.l_state = QLabel()
+        self.l_state.setText(data.ts_state.name)
+        self.l_state.setAlignment(Qt.AlignLeft)
         self.l_errorcode = QLabel()
-        self.l_errorcode.setText(str(data.panic_errorcode))
+        self.l_errorcode.setText("")
         self.l_errorcode.setAlignment(Qt.AlignLeft)
         self.l_errorarg = QLabel()
-        self.l_errorarg.setText(str(data.panic_errorarg))
+        self.l_errorarg.setText("")
         self.l_errorcode.setAlignment(Qt.AlignLeft)
 
         self.l9 = QLabel("Time Since Last Dataframe")
@@ -348,7 +376,7 @@ class Window(QWidget):
         grid_accumulator.addWidget(self.l_min_temp, 0, 3)
         grid_accumulator.addWidget(self.l_max_temp, 1, 3)
         grid_accumulator.addWidget(self.l_current, 2, 3)
-        grid_accumulator.addWidget(self.l_error, 5, 1)
+        grid_accumulator.addWidget(self.l_state, 5, 1)
         grid_accumulator.addWidget(self.l_errorcode, 5, 2)
         grid_accumulator.addWidget(self.l_errorarg, 5, 3)
         grid_accumulator.addWidget(self.l_time_since_last_frame, 3, 3)
@@ -386,14 +414,18 @@ class Window(QWidget):
         self.btn_stop.resize(self.btn_stop.sizeHint())
         self.btn_stop.move(150, 50)
         self.btn_charge = QPushButton("Start Charging")
-        self.btn_charge.resize(self.btn_stop.sizeHint())
-        self.btn_charge.move(150, 50)
+        self.btn_charge.resize(self.btn_charge.sizeHint())
+        self.btn_charge.move(250, 50)
+        self.btn_balance = QPushButton("Start Balancing")
+        self.btn_balance.resize(self.btn_balance.sizeHint())
+        self.btn_balance.move(350, 50)
 
         ### LAYOUT BUTTONS ###
         vbox_controls = QVBoxLayout()
         vbox_controls.addWidget(self.btn_start)
         vbox_controls.addWidget(self.btn_stop)
         vbox_controls.addWidget(self.btn_charge)
+        vbox_controls.addWidget(self.btn_balance)
         groupBox_controls = QGroupBox("Controls")
         groupBox_controls.setLayout(vbox_controls)
         win.addWidget(groupBox_controls)
@@ -407,6 +439,9 @@ class Window(QWidget):
         # Charge Button action
         self.btn_charge.clicked.connect(self.start_charge)
 
+        # Balance Button action
+        self.btn_balance.clicked.connect(self.start_balance)
+
         self.setLayout(win)
         self.setWindowTitle("FT22 Charger Display")
 
@@ -440,19 +475,34 @@ class Window(QWidget):
     def start_charge(self):
         ser.write(b"C")
 
+    def start_balance(self):
+        ser.write(b"B")
+
     def update(self):
         # Accumulator
         self.l_min_voltage.setText(f"{data.min_voltage:.02f}")
         self.l_max_voltage.setText(f"{data.max_voltage:.02f}")
-        self.l_min_soc.setText(f"{data.min_soc:.01f}")
-        self.l_max_soc.setText(f"{data.max_soc:.01f}")
-        self.l_min_temp.setText(f"{data.min_temp:.00f}")
-        self.l_max_temp.setText(f"{data.max_temp:.00f}")
-        self.l_current.setText(f"{data.current:.03f}")
-        self.l_error.setText(str(data.panic))
-        self.l_errorcode.setText(str(data.panic_errorcode))
-        self.l_errorarg.setText(str(data.panic_errorarg))
-        self.l_time_since_last_frame.setText(str(time.time() - data.last_frame))
+        self.l_min_soc.setText(f"{data.min_soc:02.0f}")
+        self.l_max_soc.setText(f"{data.max_soc:02.0f}")
+        self.l_min_temp.setText(f"{data.min_temp:.02f}")
+        self.l_max_temp.setText(f"{data.max_temp:.02f}")
+        self.l_current.setText(f"{data.current:.02f}")
+        self.l_state.setText(data.ts_state.name)
+        if data.panic:
+            self.l_errorcode.setText(str(data.panic_errorcode))
+            self.l_errorarg.setText(str(data.panic_errorarg))
+            for l in (self.l_state, self.l_errorcode, self.l_errorarg):
+                l.setStyleSheet("color: red; font-weight: bold;")
+        elif data.ts_state == TSState.TS_CHARGING:
+            for l in (self.l_state, self.l_errorcode, self.l_errorarg):
+                l.setStyleSheet("color: green; font-weight: bold;")
+        else:
+            self.l_errorcode.setText("")
+            self.l_errorarg.setText("")
+            for l in (self.l_state, self.l_errorcode, self.l_errorarg):
+                l.setStyleSheet("color: black; font-weight: normal;")
+        last_time = time.time() - data.last_frame
+        self.l_time_since_last_frame.setText(f"{last_time:.03f}")
 
         # Stacks
         for i, sge in enumerate(self.stack_gui_elements):
@@ -492,6 +542,10 @@ class Worker(QObject):
                 self.decode_panic_frame(rx_data[frame_start + 3 : frame_start + 11])
                 rx_data = rx_data[frame_start + 11 :]
                 continue
+            elif (frame_start := self.check_status_start(rx_data)) != -1:
+                self.decode_status_frame(rx_data[frame_start + 3 : frame_start + 11])
+                rx_data = rx_data[frame_start + 11 :]
+                continue
             break
 
     def check_log_start(self, buf: bytes):
@@ -503,6 +557,9 @@ class Worker(QObject):
     def check_panic_start(self, buf: bytes):
         return buf[:-12].find(b"PAN")
 
+    def check_status_start(self, buf: bytes):
+        return buf[:-12].find(b"STA")
+
     def decode_log_frame(self, buf: bytes):
         msg_id = buf[0]
         slave = msg_id >> 4
@@ -512,25 +569,44 @@ class Worker(QObject):
             return
 
         if frame_id == 0:
-            for i in range(7):
-                data.slaves[slave].cell_voltages[i] = buf[i + 1] * VOLTAGE_CONV
+            for i in range(3):
+                raw = (buf[i * 2 + 1] << 8) | buf[i * 2 + 2]
+                data.slaves[slave].cell_voltages[i] = raw * VOLTAGE_CONV
         elif frame_id == 1:
             for i in range(3):
-                data.slaves[slave].cell_voltages[i + 7] = buf[i + 1] * VOLTAGE_CONV
-            for i in range(4):
-                data.slaves[slave].cell_temps[i] = buf[i + 4] * TEMP_CONV
+                raw = (buf[i * 2 + 1] << 8) | buf[i * 2 + 2]
+                data.slaves[slave].cell_voltages[i + 3] = raw * VOLTAGE_CONV
         elif frame_id == 2:
-            for i in range(7):
-                data.slaves[slave].cell_temps[i + 4] = buf[i + 1] * TEMP_CONV
+            for i in range(3):
+                raw = (buf[i * 2 + 1] << 8) | buf[i * 2 + 2]
+                data.slaves[slave].cell_voltages[i + 6] = raw * VOLTAGE_CONV
         elif frame_id == 3:
-            for i in range(7):
-                data.slaves[slave].cell_temps[i + 11] = buf[i + 1] * TEMP_CONV
+            for i in range(1):
+                raw = (buf[i * 2 + 1] << 8) | buf[i * 2 + 2]
+                data.slaves[slave].cell_voltages[i + 9] = raw * VOLTAGE_CONV
+            for i in range(2):
+                raw = (buf[i * 2 + 3] << 8) | buf[i * 2 + 4]
+                data.slaves[slave].cell_temps[i] = raw * TEMP_CONV
         elif frame_id == 4:
-            for i in range(7):
-                data.slaves[slave].cell_temps[i + 18] = buf[i + 1] * TEMP_CONV
+            for i in range(3):
+                raw = (buf[i * 2 + 1] << 8) | buf[i * 2 + 2]
+                data.slaves[slave].cell_temps[i + 2] = raw * TEMP_CONV
         elif frame_id == 5:
-            for i in range(7):
-                data.slaves[slave].cell_temps[i + 25] = buf[i + 1] * TEMP_CONV
+            for i in range(3):
+                raw = (buf[i * 2 + 1] << 8) | buf[i * 2 + 2]
+                data.slaves[slave].cell_temps[i + 5] = raw * TEMP_CONV
+        elif frame_id == 6:
+            for i in range(3):
+                raw = (buf[i * 2 + 1] << 8) | buf[i * 2 + 2]
+                data.slaves[slave].cell_temps[i + 8] = raw * TEMP_CONV
+        elif frame_id == 7:
+            for i in range(3):
+                raw = (buf[i * 2 + 1] << 8) | buf[i * 2 + 2]
+                data.slaves[slave].cell_temps[i + 11] = raw * TEMP_CONV
+        elif frame_id == 8:
+            for i in range(3):
+                raw = (buf[i * 2 + 1] << 8) | buf[i * 2 + 2]
+                data.slaves[slave].cell_temps[i + 14] = raw * TEMP_CONV
         else:
             print(f"Unknown frame ID: {frame_id} (buf: {repr(buf)})", file=sys.stderr)
             # time.sleep(1)
@@ -548,8 +624,8 @@ class Worker(QObject):
         ]
         data.min_voltage = min(voltages)
         data.max_voltage = max(voltages)
-        data.min_soc = self.calculate_soc(data.min_voltage)
-        data.max_soc = self.calculate_soc(data.max_voltage)
+        data.min_soc = estimate_soc(data.min_voltage)
+        data.max_soc = estimate_soc(data.max_voltage)
         data.min_temp = min(temps)
         data.max_temp = max(temps)
         data.time_since_last_frame = time.time() - data.last_frame
@@ -609,21 +685,10 @@ class Worker(QObject):
             data.panic_errorcode = buf[0]
             data.panic_errorarg = buf[1]
 
-    INTERNAL_RESISTANCE_CURVE_X = [2.0, 4.12]
-    INTERNAL_RESISTANCE_CURVE_Y = [0.0528, 0.0294]
-    SOC_OCV_X = [2.1, 2.9, 3.2, 3.3, 3.4, 3.5, 3.68, 4.0, 4.15, 4.2]
-    SOC_OCV_Y = [0, 0.023, 0.06, 0.08, 0.119, 0.227, 0.541, 0.856, 0.985, 1.0]
-
-    def calculate_soc(self, voltage: float):
-        r_i = np.interp(
-            [voltage],
-            self.INTERNAL_RESISTANCE_CURVE_X,
-            self.INTERNAL_RESISTANCE_CURVE_Y,
-        )[0]
-        # i = data.current / PARALLEL_CELLS
-        i = 3 / PARALLEL_CELLS
-        ocv = voltage - i * r_i
-        return np.interp([ocv], self.SOC_OCV_X, self.SOC_OCV_Y)[0] * 100
+    def decode_status_frame(self, buf: bytes):
+        state = buf[0] & 0x7F
+        data.ts_state = TSState(state)
+        data.panic = data.ts_state == TSState.TS_ERROR
 
     def parse_cell_temps(self, slave: int):
         temps = list(
@@ -633,8 +698,8 @@ class Worker(QObject):
             temps = [-1]
         min_t = min(temps)
         max_t = max(temps)
-        for i in range(16, 32):
-            data.slaves[slave].cell_temps[i] = random.randint(min_t, max_t)
+        # for i in range(16, 32):
+        #     data.slaves[slave].cell_temps[i] = random.randint(min_t, max_t)
 
     def stop(self):
         self.continue_run = False  # set the run condition to false on stop