Handle bad BMS cell temperature probes

inc/sp140/bms.h

@@ -3,13 +3,54 @@
 #include <Arduino.h>
 #include <SPI.h>
 #include <BMS_CAN.h>
+#include <math.h>
 
 #include "sp140/structs.h"
 
 // BMS-related constants
 #define MCP_CS 5        // MCP2515 CS pin
 #define MCP_BAUDRATE 250000
 
+// BMS cell probe disconnect handling.
+// Requirement: a raw value of exactly -40C indicates a disconnected probe
+// for single-value sanitization.
+constexpr float BMS_CELL_TEMP_DISCONNECTED_C = -40.0f;
+constexpr uint8_t BMS_CELL_PROBE_COUNT = 4;
+constexpr uint8_t BMS_MAX_IGNORED_DISCONNECTED_PROBES = 2;
+
+inline float sanitizeBmsCellTempC(float tempC) {
+  // Return NaN for disconnected/invalid readings so monitor/UI logic can skip
+  // this probe the same way we handle disconnected ESC motor temp.
+  return (!isnan(tempC) && tempC > BMS_CELL_TEMP_DISCONNECTED_C) ? tempC : NAN;
+}
+
+inline void sanitizeCellProbeTemps(
+    const float rawTemps[BMS_CELL_PROBE_COUNT],
+    float sanitizedTemps[BMS_CELL_PROBE_COUNT]) {
+  uint8_t ignoredDisconnectedProbeCount = 0;
+
+  for (uint8_t i = 0; i < BMS_CELL_PROBE_COUNT; i++) {
+    const float tempC = rawTemps[i];
+
+    if (isnan(tempC) || tempC < BMS_CELL_TEMP_DISCONNECTED_C) {
+      sanitizedTemps[i] = NAN;
+      continue;
+    }
+
+    if (tempC == BMS_CELL_TEMP_DISCONNECTED_C) {
+      if (ignoredDisconnectedProbeCount < BMS_MAX_IGNORED_DISCONNECTED_PROBES) {
+        sanitizedTemps[i] = NAN;
+        ignoredDisconnectedProbeCount++;
+      } else {
+        sanitizedTemps[i] = tempC;
+      }
+      continue;
+    }
+
+    sanitizedTemps[i] = tempC;
+  }
+}
+
 // External declarations
 extern STR_BMS_TELEMETRY_140 bmsTelemetryData;
 extern BMS_CAN* bms_can;

inc/sp140/structs.h

@@ -76,8 +76,8 @@ typedef struct {
   float power;                  // Power (kW)
   float highest_cell_voltage;   // Highest individual cell voltage (V)
   float lowest_cell_voltage;    // Lowest individual cell voltage (V)
-  float highest_temperature;    // Highest temperature reading (°C)
-  float lowest_temperature;     // Lowest temperature reading (°C)
+  float highest_temperature;    // Highest valid temperature reading (°C), NaN if unavailable
+  float lowest_temperature;     // Lowest valid temperature reading (°C), NaN if unavailable
   float energy_cycle;          // Energy per cycle (kWh)
   uint32_t battery_cycle;      // Battery cycle count
   uint8_t battery_fail_level;  // Battery failure status
@@ -92,10 +92,10 @@ typedef struct {
   // Individual temperature sensors
   float mos_temperature;        // BMS MOSFET temperature (°C) - index 0
   float balance_temperature;    // BMS balance resistor temperature (°C) - index 1
-  float t1_temperature;         // T1 cell temperature sensor (°C) - index 2
-  float t2_temperature;         // T2 cell temperature sensor (°C) - index 3
-  float t3_temperature;         // T3 cell temperature sensor (°C) - index 4
-  float t4_temperature;         // T4 cell temperature sensor (°C) - index 5
+  float t1_temperature;         // T1 cell temperature sensor (°C), NaN if disconnected - index 2
+  float t2_temperature;         // T2 cell temperature sensor (°C), NaN if disconnected - index 3
+  float t3_temperature;         // T3 cell temperature sensor (°C), NaN if disconnected - index 4
+  float t4_temperature;         // T4 cell temperature sensor (°C), NaN if disconnected - index 5
 } STR_BMS_TELEMETRY_140;
 #pragma pack(pop)
 

src/sp140/ble/bms_service.cpp

@@ -199,19 +199,29 @@ void updateBMSTelemetry(const STR_BMS_TELEMETRY_140& telemetry) {
   // Sensor mapping: [0]=MOS, [1]=Balance, [2]=T1, [3]=T2, [4]=T3, [5]=T4, [6-7]=Reserved
   if (pBMSTemperatures) {
     uint8_t temp_buffer[17];
-
-    // Byte 0: Valid sensor bitmap (bit N = sensor N is valid)
-    // 0b00111111 = sensors 0-5 valid (6 temperature sensors)
-    temp_buffer[0] = 0b00111111;
+    uint8_t validBitmap = 0;
 
     // Bytes 1-16: 8×int16_t temperatures in deci-degrees C (0.1°C resolution)
     int16_t* temps = reinterpret_cast<int16_t*>(&temp_buffer[1]);
-    temps[0] = static_cast<int16_t>(telemetry.mos_temperature * 10.0f);      // [0] MOS
-    temps[1] = static_cast<int16_t>(telemetry.balance_temperature * 10.0f);  // [1] Balance
-    temps[2] = static_cast<int16_t>(telemetry.t1_temperature * 10.0f);       // [2] T1
-    temps[3] = static_cast<int16_t>(telemetry.t2_temperature * 10.0f);       // [3] T2
-    temps[4] = static_cast<int16_t>(telemetry.t3_temperature * 10.0f);       // [4] T3
-    temps[5] = static_cast<int16_t>(telemetry.t4_temperature * 10.0f);       // [5] T4
+    const float sensorTemps[6] = {
+      telemetry.mos_temperature,
+      telemetry.balance_temperature,
+      telemetry.t1_temperature,
+      telemetry.t2_temperature,
+      telemetry.t3_temperature,
+      telemetry.t4_temperature
+    };
+
+    for (uint8_t i = 0; i < 6; i++) {
+      bool isValid = !isnan(sensorTemps[i]);
+
+      if (isValid) {
+        validBitmap |= static_cast<uint8_t>(1U << i);
+      }
+      temps[i] = isValid ? static_cast<int16_t>(sensorTemps[i] * 10.0f) : 0;
+    }
+
+    temp_buffer[0] = validBitmap;
     temps[6] = 0;  // [6] Reserved
     temps[7] = 0;  // [7] Reserved
 

src/sp140/bms.cpp

@@ -3,6 +3,71 @@
 #include "sp140/globals.h"
 #include "sp140/lvgl/lvgl_core.h"  // for spiBusMutex
 
+namespace {
+
+void logBmsCellProbeConnectionTransitions(const float sanitizedCellTemps[BMS_CELL_PROBE_COUNT]) {
+  static bool hasPreviousState = false;
+  static bool wasConnected[BMS_CELL_PROBE_COUNT] = {false, false, false, false};
+
+  for (uint8_t i = 0; i < BMS_CELL_PROBE_COUNT; i++) {
+    // Use already-sanitized telemetry values so connection detection and
+    // downstream behavior stay consistent.
+    const bool connected = !isnan(sanitizedCellTemps[i]);
+
+    if (!hasPreviousState) {
+      wasConnected[i] = connected;
+      continue;
+    }
+
+    if (connected != wasConnected[i]) {
+      USBSerial.printf("[BMS] T%u sensor %s (sanitized=%.1fC)\n",
+                       i + 1,
+                       connected ? "reconnected" : "disconnected",
+                       sanitizedCellTemps[i]);
+      wasConnected[i] = connected;
+    }
+  }
+
+  hasPreviousState = true;
+}
+
+void recomputeBmsTemperatureExtrema(STR_BMS_TELEMETRY_140& telemetry) {  // NOLINT(runtime/references)
+  const float allTemps[] = {
+    telemetry.mos_temperature,
+    telemetry.balance_temperature,
+    telemetry.t1_temperature,
+    telemetry.t2_temperature,
+    telemetry.t3_temperature,
+    telemetry.t4_temperature
+  };
+
+  bool hasValidReading = false;
+  float highest = NAN;
+  float lowest = NAN;
+
+  for (float temp : allTemps) {
+    // Disconnected probes are stored as NaN; exclude them from extrema.
+    if (isnan(temp)) {
+      continue;
+    }
+
+    if (!hasValidReading) {
+      highest = temp;
+      lowest = temp;
+      hasValidReading = true;
+      continue;
+    }
+
+    if (temp > highest) highest = temp;
+    if (temp < lowest) lowest = temp;
+  }
+
+  telemetry.highest_temperature = highest;
+  telemetry.lowest_temperature = lowest;
+}
+
+}  // namespace
+
 STR_BMS_TELEMETRY_140 bmsTelemetryData = {
   .bmsState = TelemetryState::NOT_CONNECTED
 };
@@ -51,10 +116,6 @@ void updateBMSData() {
   // Calculated highest cell minus lowest cell voltage
   bmsTelemetryData.voltage_differential = bms_can->getHighestCellVoltage() - bms_can->getLowestCellVoltage();
 
-  // Temperature readings
-  bmsTelemetryData.highest_temperature = bms_can->getHighestTemperature();
-  bmsTelemetryData.lowest_temperature = bms_can->getLowestTemperature();
-
   // Battery statistics
   bmsTelemetryData.battery_cycle = bms_can->getBatteryCycle();
   bmsTelemetryData.energy_cycle = bms_can->getEnergyCycle();
@@ -73,10 +134,25 @@ void updateBMSData() {
   // Populate individual temperature sensors
   bmsTelemetryData.mos_temperature = bms_can->getTemperature(0);      // BMS MOSFET
   bmsTelemetryData.balance_temperature = bms_can->getTemperature(1);  // BMS Balance resistors
-  bmsTelemetryData.t1_temperature = bms_can->getTemperature(2);       // Cell probe 1
-  bmsTelemetryData.t2_temperature = bms_can->getTemperature(3);       // Cell probe 2
-  bmsTelemetryData.t3_temperature = bms_can->getTemperature(4);       // Cell probe 3
-  bmsTelemetryData.t4_temperature = bms_can->getTemperature(5);       // Cell probe 4
+
+  const float rawCellTemps[BMS_CELL_PROBE_COUNT] = {
+    bms_can->getTemperature(2),
+    bms_can->getTemperature(3),
+    bms_can->getTemperature(4),
+    bms_can->getTemperature(5)
+  };
+  float sanitizedCellTemps[BMS_CELL_PROBE_COUNT];
+
+  sanitizeCellProbeTemps(rawCellTemps, sanitizedCellTemps);
+  bmsTelemetryData.t1_temperature = sanitizedCellTemps[0];  // Cell probe 1
+  bmsTelemetryData.t2_temperature = sanitizedCellTemps[1];  // Cell probe 2
+  bmsTelemetryData.t3_temperature = sanitizedCellTemps[2];  // Cell probe 3
+  bmsTelemetryData.t4_temperature = sanitizedCellTemps[3];  // Cell probe 4
+
+  // Emit transition logs to help field-debug intermittent probe wiring issues.
+  logBmsCellProbeConnectionTransitions(sanitizedCellTemps);
+  // Keep published high/low temperatures aligned with sanitized probe values.
+  recomputeBmsTemperatureExtrema(bmsTelemetryData);
 
   bmsTelemetryData.lastUpdateMs = millis();
   unsigned long dur = bmsTelemetryData.lastUpdateMs - tStart;

src/sp140/lvgl/lvgl_updates.cpp

@@ -334,11 +334,25 @@ void updateLvglMainScreen(
   float batteryPercent = unifiedBatteryData.soc;
   float totalVolts = unifiedBatteryData.volts;
   float lowestCellV = bmsTelemetry.lowest_cell_voltage;
-  // Calculate highest cell temperature from T1-T4 only (excluding MOSFET and balance temps)
-  float batteryTemp = bmsTelemetry.t1_temperature;
-  if (bmsTelemetry.t2_temperature > batteryTemp) batteryTemp = bmsTelemetry.t2_temperature;
-  if (bmsTelemetry.t3_temperature > batteryTemp) batteryTemp = bmsTelemetry.t3_temperature;
-  if (bmsTelemetry.t4_temperature > batteryTemp) batteryTemp = bmsTelemetry.t4_temperature;
+  // Calculate battery temp from connected T1-T4 cell probes only.
+  const float cellTemps[] = {
+    bmsTelemetry.t1_temperature,
+    bmsTelemetry.t2_temperature,
+    bmsTelemetry.t3_temperature,
+    bmsTelemetry.t4_temperature
+  };
+  float batteryTemp = NAN;
+  bool hasValidBatteryTemp = false;
+  for (float cellTemp : cellTemps) {
+    if (isnan(cellTemp)) {
+      continue;
+    }
+
+    if (!hasValidBatteryTemp || cellTemp > batteryTemp) {
+      batteryTemp = cellTemp;
+      hasValidBatteryTemp = true;
+    }
+  }
   float escTemp = escTelemetry.cap_temp;
   float motorTemp = escTelemetry.motor_temp;
   // Check if BMS or ESC is connected
@@ -660,7 +674,7 @@ void updateLvglMainScreen(
     lv_obj_remove_style(batt_temp_bg, &style_warning, 0);
     lv_obj_remove_style(batt_temp_bg, &style_critical, 0);
 
-    if (bmsTelemetry.bmsState == TelemetryState::CONNECTED) {
+    if (bmsTelemetry.bmsState == TelemetryState::CONNECTED && hasValidBatteryTemp) {
       lv_label_set_text_fmt(batt_temp_label, "%d", static_cast<int>(batteryTemp));
       if (batteryTemp >= bmsCellTempThresholds.critHigh) {
         lv_obj_add_style(batt_temp_bg, &style_critical, 0);
@@ -672,6 +686,7 @@ void updateLvglMainScreen(
         lv_obj_add_flag(batt_temp_bg, LV_OBJ_FLAG_HIDDEN);
       }
     } else {
+      // No valid cell probe connected: show "-" instead of a fake low reading.
       lv_label_set_text(batt_temp_label, "-");
       lv_obj_add_flag(batt_temp_bg, LV_OBJ_FLAG_HIDDEN);
     }

test/native_stubs/BMS_CAN.h

@@ -0,0 +1,3 @@
+#pragma once
+
+class BMS_CAN {};

test/native_stubs/SPI.h

@@ -0,0 +1,3 @@
+#pragma once
+
+class SPIClass {};