diff --git a/inc/sp140/bms.h b/inc/sp140/bms.h index fcde6e9a..94342f8c 100644 --- a/inc/sp140/bms.h +++ b/inc/sp140/bms.h @@ -3,6 +3,7 @@ #include #include #include +#include #include "sp140/structs.h" @@ -10,6 +11,46 @@ #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; diff --git a/inc/sp140/structs.h b/inc/sp140/structs.h index 29715c09..043ac821 100644 --- a/inc/sp140/structs.h +++ b/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) diff --git a/src/sp140/ble/bms_service.cpp b/src/sp140/ble/bms_service.cpp index 4478473f..7929db10 100644 --- a/src/sp140/ble/bms_service.cpp +++ b/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(&temp_buffer[1]); - temps[0] = static_cast(telemetry.mos_temperature * 10.0f); // [0] MOS - temps[1] = static_cast(telemetry.balance_temperature * 10.0f); // [1] Balance - temps[2] = static_cast(telemetry.t1_temperature * 10.0f); // [2] T1 - temps[3] = static_cast(telemetry.t2_temperature * 10.0f); // [3] T2 - temps[4] = static_cast(telemetry.t3_temperature * 10.0f); // [4] T3 - temps[5] = static_cast(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(1U << i); + } + temps[i] = isValid ? static_cast(sensorTemps[i] * 10.0f) : 0; + } + + temp_buffer[0] = validBitmap; temps[6] = 0; // [6] Reserved temps[7] = 0; // [7] Reserved diff --git a/src/sp140/bms.cpp b/src/sp140/bms.cpp index 768bf71a..46302989 100644 --- a/src/sp140/bms.cpp +++ b/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; diff --git a/src/sp140/lvgl/lvgl_updates.cpp b/src/sp140/lvgl/lvgl_updates.cpp index 8897c670..5b22fe55 100644 --- a/src/sp140/lvgl/lvgl_updates.cpp +++ b/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(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); } diff --git a/test/native_stubs/BMS_CAN.h b/test/native_stubs/BMS_CAN.h new file mode 100644 index 00000000..473c7ddd --- /dev/null +++ b/test/native_stubs/BMS_CAN.h @@ -0,0 +1,3 @@ +#pragma once + +class BMS_CAN {}; diff --git a/test/native_stubs/SPI.h b/test/native_stubs/SPI.h new file mode 100644 index 00000000..f74ca657 --- /dev/null +++ b/test/native_stubs/SPI.h @@ -0,0 +1,3 @@ +#pragma once + +class SPIClass {}; diff --git a/test/test_simplemonitor/test_simplemonitor.cpp b/test/test_simplemonitor/test_simplemonitor.cpp index f3d6ed8f..f42060d8 100644 --- a/test/test_simplemonitor/test_simplemonitor.cpp +++ b/test/test_simplemonitor/test_simplemonitor.cpp @@ -1,4 +1,5 @@ #include +#include "sp140/bms.h" #include "sp140/simple_monitor.h" #include "sp140/monitor_config.h" @@ -237,6 +238,171 @@ TEST(SimpleMonitor, BMSTemperatureAlerts) { EXPECT_EQ(logger.entries.back().lvl, AlertLevel::CRIT_HIGH); } +TEST(SimpleMonitor, BMSCellTemperatureDisconnectedIsIgnored) { + FakeLogger logger; + float fakeTemp = 30.0f; + + SensorMonitor cellTempMon( + SensorID::BMS_T1_Temp, + SensorCategory::BMS, + bmsCellTempThresholds, + [&]() { return fakeTemp; }, + &logger); + + cellTempMon.check(); // Baseline OK + EXPECT_TRUE(logger.entries.empty()); + + // Disconnected probe reading should be treated as invalid (sanitized to NaN upstream). + fakeTemp = NAN; + cellTempMon.check(); + EXPECT_TRUE(logger.entries.empty()); + + // Remaining connected probes still report alerts normally. + fakeTemp = 54.0f; + cellTempMon.check(); + ASSERT_EQ(logger.entries.size(), 1u); + EXPECT_EQ(logger.entries.back().lvl, AlertLevel::WARN_HIGH); +} + +TEST(SimpleMonitor, BMSCellTemperatureDisconnectClearsActiveAlert) { + FakeLogger logger; + float fakeTemp = 55.0f; + + SensorMonitor cellTempMon( + SensorID::BMS_T2_Temp, + SensorCategory::BMS, + bmsCellTempThresholds, + [&]() { return fakeTemp; }, + &logger); + + // Enter warning state first. + cellTempMon.check(); + ASSERT_EQ(logger.entries.size(), 1u); + EXPECT_EQ(logger.entries.back().lvl, AlertLevel::WARN_HIGH); + + // Disconnect should clear the active alert. + fakeTemp = NAN; + cellTempMon.check(); + ASSERT_EQ(logger.entries.size(), 2u); + EXPECT_EQ(logger.entries.back().lvl, AlertLevel::OK); +} + +TEST(SimpleMonitor, BMSCellTempSanitizerDisconnectThreshold) { + EXPECT_TRUE(isnan(sanitizeBmsCellTempC(-40.0f))); + EXPECT_TRUE(isnan(sanitizeBmsCellTempC(-41.0f))); + EXPECT_FLOAT_EQ(sanitizeBmsCellTempC(-39.5f), -39.5f); +} + +TEST(SimpleMonitor, BMSCellTempSanitizerPreservesValidValues) { + EXPECT_FLOAT_EQ(sanitizeBmsCellTempC(0.0f), 0.0f); + EXPECT_FLOAT_EQ(sanitizeBmsCellTempC(25.0f), 25.0f); + 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;