Normalize BMS -40C cell probe handling (ignore any 2)

inc/sp140/bms.h

@@ -12,16 +12,45 @@
 #define MCP_BAUDRATE 250000
 
 // BMS cell probe disconnect handling.
-// Requirement: a raw value of exactly -40C indicates a disconnected probe.
-// Use strict '>' so -40.0 is treated as disconnected, not valid.
+// 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;

src/sp140/bms.cpp

@@ -5,27 +5,25 @@
 
 namespace {
 
-constexpr uint8_t kBmsCellProbeCount = 4;
-
-void logBmsCellProbeConnectionTransitions(const float rawCellTemps[kBmsCellProbeCount]) {
+void logBmsCellProbeConnectionTransitions(const float sanitizedCellTemps[BMS_CELL_PROBE_COUNT]) {
   static bool hasPreviousState = false;
-  static bool wasConnected[kBmsCellProbeCount] = {false, false, false, false};
+  static bool wasConnected[BMS_CELL_PROBE_COUNT] = {false, false, false, false};
 
-  for (uint8_t i = 0; i < kBmsCellProbeCount; i++) {
-    // Reuse the same sanitizer used for telemetry storage so detection and
+  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(sanitizeBmsCellTempC(rawCellTemps[i]));
+    const bool connected = !isnan(sanitizedCellTemps[i]);
 
     if (!hasPreviousState) {
       wasConnected[i] = connected;
       continue;
     }
 
     if (connected != wasConnected[i]) {
-      USBSerial.printf("[BMS] T%u sensor %s (raw=%.1fC)\n",
+      USBSerial.printf("[BMS] T%u sensor %s (sanitized=%.1fC)\n",
                        i + 1,
                        connected ? "reconnected" : "disconnected",
-                       rawCellTemps[i]);
+                       sanitizedCellTemps[i]);
       wasConnected[i] = connected;
     }
   }
@@ -137,20 +135,22 @@ void updateBMSData() {
   bmsTelemetryData.mos_temperature = bms_can->getTemperature(0);      // BMS MOSFET
   bmsTelemetryData.balance_temperature = bms_can->getTemperature(1);  // BMS Balance resistors
 
-  const float rawCellTemps[kBmsCellProbeCount] = {
+  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];
 
-  bmsTelemetryData.t1_temperature = sanitizeBmsCellTempC(rawCellTemps[0]);  // Cell probe 1
-  bmsTelemetryData.t2_temperature = sanitizeBmsCellTempC(rawCellTemps[1]);  // Cell probe 2
-  bmsTelemetryData.t3_temperature = sanitizeBmsCellTempC(rawCellTemps[2]);  // Cell probe 3
-  bmsTelemetryData.t4_temperature = sanitizeBmsCellTempC(rawCellTemps[3]);  // Cell probe 4
+  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(rawCellTemps);
+  logBmsCellProbeConnectionTransitions(sanitizedCellTemps);
   // Keep published high/low temperatures aligned with sanitized probe values.
   recomputeBmsTemperatureExtrema(bmsTelemetryData);
 

src/sp140/lvgl/lvgl_updates.cpp

@@ -344,14 +344,12 @@ void updateLvglMainScreen(
   float batteryTemp = NAN;
   bool hasValidBatteryTemp = false;
   for (float cellTemp : cellTemps) {
-    // sanitizeBmsCellTempC() converts disconnected probes (-40C) to NaN.
-    const float sanitizedCellTemp = sanitizeBmsCellTempC(cellTemp);
-    if (isnan(sanitizedCellTemp)) {
+    if (isnan(cellTemp)) {
       continue;
     }
 
-    if (!hasValidBatteryTemp || sanitizedCellTemp > batteryTemp) {
-      batteryTemp = sanitizedCellTemp;
+    if (!hasValidBatteryTemp || cellTemp > batteryTemp) {
+      batteryTemp = cellTemp;
       hasValidBatteryTemp = true;
     }
   }

test/test_simplemonitor/test_simplemonitor.cpp

@@ -299,6 +299,110 @@ TEST(SimpleMonitor, BMSCellTempSanitizerPreservesValidValues) {
   EXPECT_TRUE(isnan(sanitizeBmsCellTempC(NAN)));
 }
 
+TEST(SimpleMonitor, BMSCellProbeSanitizerIgnoresUpToTwoDisconnectedSentinels) {
+  const float rawTemps[BMS_CELL_PROBE_COUNT] = {-40.0f, -39.0f, -40.0f, 12.0f};
+  float sanitizedTemps[BMS_CELL_PROBE_COUNT] = {};
+
+  sanitizeCellProbeTemps(rawTemps, sanitizedTemps);
+
+  EXPECT_TRUE(isnan(sanitizedTemps[0]));
+  EXPECT_FLOAT_EQ(sanitizedTemps[1], -39.0f);
+  EXPECT_TRUE(isnan(sanitizedTemps[2]));
+  EXPECT_FLOAT_EQ(sanitizedTemps[3], 12.0f);
+}
+
+TEST(SimpleMonitor, BMSCellProbeSanitizerKeepsThirdDisconnectedSentinelAsValidTemp) {
+  const float rawTemps[BMS_CELL_PROBE_COUNT] = {-40.0f, -40.0f, -40.0f, 25.0f};
+  float sanitizedTemps[BMS_CELL_PROBE_COUNT] = {};
+
+  sanitizeCellProbeTemps(rawTemps, sanitizedTemps);
+
+  EXPECT_TRUE(isnan(sanitizedTemps[0]));
+  EXPECT_TRUE(isnan(sanitizedTemps[1]));
+  EXPECT_FLOAT_EQ(sanitizedTemps[2], -40.0f);
+  EXPECT_FLOAT_EQ(sanitizedTemps[3], 25.0f);
+}
+
+TEST(SimpleMonitor, BMSCellProbeSanitizerKeepsThirdAndFourthDisconnectedSentinelsAsValidTemps) {
+  const float rawTemps[BMS_CELL_PROBE_COUNT] = {-40.0f, -40.0f, -40.0f, -40.0f};
+  float sanitizedTemps[BMS_CELL_PROBE_COUNT] = {};
+
+  sanitizeCellProbeTemps(rawTemps, sanitizedTemps);
+
+  EXPECT_TRUE(isnan(sanitizedTemps[0]));
+  EXPECT_TRUE(isnan(sanitizedTemps[1]));
+  EXPECT_FLOAT_EQ(sanitizedTemps[2], -40.0f);
+  EXPECT_FLOAT_EQ(sanitizedTemps[3], -40.0f);
+}
+
+TEST(SimpleMonitor, BMSCellProbeSanitizerTreatsBelowDisconnectedAndNaNAsInvalid) {
+  const float rawTemps[BMS_CELL_PROBE_COUNT] = {-41.0f, -39.0f, -40.0f, NAN};
+  float sanitizedTemps[BMS_CELL_PROBE_COUNT] = {};
+
+  sanitizeCellProbeTemps(rawTemps, sanitizedTemps);
+
+  EXPECT_TRUE(isnan(sanitizedTemps[0]));
+  EXPECT_FLOAT_EQ(sanitizedTemps[1], -39.0f);
+  EXPECT_TRUE(isnan(sanitizedTemps[2]));
+  EXPECT_TRUE(isnan(sanitizedTemps[3]));
+}
+
+TEST(SimpleMonitor, BMSCellProbeSentinelTransitionFromTwoToThreeToTwoTriggersAndClearsAlert) {
+  FakeLogger logger;
+  float sanitizedTemps[BMS_CELL_PROBE_COUNT] = {NAN, NAN, NAN, NAN};
+
+  SensorMonitor t1Mon(
+    SensorID::BMS_T1_Temp,
+    SensorCategory::BMS,
+    bmsCellTempThresholds,
+    [&]() { return sanitizedTemps[0]; },
+    &logger);
+  SensorMonitor t2Mon(
+    SensorID::BMS_T2_Temp,
+    SensorCategory::BMS,
+    bmsCellTempThresholds,
+    [&]() { return sanitizedTemps[1]; },
+    &logger);
+  SensorMonitor t3Mon(
+    SensorID::BMS_T3_Temp,
+    SensorCategory::BMS,
+    bmsCellTempThresholds,
+    [&]() { return sanitizedTemps[2]; },
+    &logger);
+  SensorMonitor t4Mon(
+    SensorID::BMS_T4_Temp,
+    SensorCategory::BMS,
+    bmsCellTempThresholds,
+    [&]() { return sanitizedTemps[3]; },
+    &logger);
+
+  const auto evaluateCycle = [&](const float rawTemps[BMS_CELL_PROBE_COUNT]) {
+    sanitizeCellProbeTemps(rawTemps, sanitizedTemps);
+    t1Mon.check();
+    t2Mon.check();
+    t3Mon.check();
+    t4Mon.check();
+  };
+
+  // Two disconnected sentinels are ignored - no low-temp alert.
+  const float twoSentinels[BMS_CELL_PROBE_COUNT] = {-40.0f, -40.0f, 5.0f, 6.0f};
+  evaluateCycle(twoSentinels);
+  EXPECT_TRUE(logger.entries.empty());
+
+  // Third sentinel becomes a real temperature and must trigger a low-temp alert.
+  const float threeSentinels[BMS_CELL_PROBE_COUNT] = {-40.0f, -40.0f, -40.0f, 6.0f};
+  evaluateCycle(threeSentinels);
+  ASSERT_EQ(logger.entries.size(), 1u);
+  EXPECT_EQ(logger.entries[0].id, SensorID::BMS_T3_Temp);
+  EXPECT_EQ(logger.entries[0].lvl, AlertLevel::CRIT_LOW);
+
+  // Returning to only two sentinels clears that low-temp alert cleanly.
+  evaluateCycle(twoSentinels);
+  ASSERT_EQ(logger.entries.size(), 2u);
+  EXPECT_EQ(logger.entries[1].id, SensorID::BMS_T3_Temp);
+  EXPECT_EQ(logger.entries[1].lvl, AlertLevel::OK);
+}
+
 TEST(SimpleMonitor, MonitorIDAccess) {
     FakeLogger logger;