diff --git a/.claude/settings.local.json b/.claude/settings.local.json index 25a2884a..cdaf95d9 100644 --- a/.claude/settings.local.json +++ b/.claude/settings.local.json @@ -3,8 +3,9 @@ "allow": [ "Bash(pio)", "Bash(pio run)", - "Bash(cpplint:*)" + "Bash(cpplint:*)", + "Bash(pio test:*)" ], "deny": [] } -} \ No newline at end of file +} diff --git a/inc/sp140/ble/bms_packet_codec.h b/inc/sp140/ble/bms_packet_codec.h new file mode 100644 index 00000000..9c6e885c --- /dev/null +++ b/inc/sp140/ble/bms_packet_codec.h @@ -0,0 +1,14 @@ +#ifndef INC_SP140_BLE_BMS_PACKET_CODEC_H_ +#define INC_SP140_BLE_BMS_PACKET_CODEC_H_ + +#include + +#include "sp140/structs.h" + +BLE_BMS_Telemetry_V1 buildBMSPackedTelemetryV1( + const STR_BMS_TELEMETRY_140& telemetry, uint8_t bms_id); + +BLE_BMS_Extended_Telemetry_V1 buildBMSExtendedTelemetryV1( + const STR_BMS_TELEMETRY_140& telemetry, uint8_t bms_id); + +#endif // INC_SP140_BLE_BMS_PACKET_CODEC_H_ diff --git a/inc/sp140/ble/bms_service.h b/inc/sp140/ble/bms_service.h index 45d940bc..6f4cee6e 100644 --- a/inc/sp140/ble/bms_service.h +++ b/inc/sp140/ble/bms_service.h @@ -10,5 +10,6 @@ void initBmsBleService(NimBLEServer* server); // Binary packed telemetry update (V1 protocol) // bms_id: 0-3 for multi-BMS support void updateBMSPackedTelemetry(const STR_BMS_TELEMETRY_140& telemetry, uint8_t bms_id = 0); +void updateBMSExtendedTelemetry(const STR_BMS_TELEMETRY_140& telemetry, uint8_t bms_id = 0); #endif // INC_SP140_BLE_BMS_SERVICE_H_ diff --git a/inc/sp140/structs.h b/inc/sp140/structs.h index 29715c09..9aa228ee 100644 --- a/inc/sp140/structs.h +++ b/inc/sp140/structs.h @@ -25,7 +25,7 @@ typedef struct { float inPWM; float outPWM; uint8_t statusFlag; - word checksum; + uint16_t checksum; unsigned long lastUpdateMs; // Timestamp of last telemetry update TelemetryState escState; // Current connection state uint16_t running_error; // Runtime error bitmask @@ -67,6 +67,7 @@ struct UnifiedBatteryData { }; #define BMS_CELLS_NUM 24 // Maximum number of cells supported +#define BMS_TEMPERATURE_SENSORS_NUM 6 // MOS, Balance, T1-T4 // BMS telemetry data typedef struct { @@ -138,6 +139,29 @@ typedef struct { uint32_t lastUpdateMs; // Timestamp of last update } BLE_BMS_Telemetry_V1; +// Binary packed Extended BMS telemetry for BLE transmission +// Contains summary fields + per-cell voltages + per-probe temperatures. +// Min/max/differential are omitted — clients compute from the arrays. +typedef struct { + uint8_t version; // Protocol version (1) + uint8_t bms_id; // BMS identifier (0-3 for multi-BMS) + uint8_t connection_state; // TelemetryState enum value + float soc; // State of charge (%) + float battery_voltage; // Total battery voltage (V) + float battery_current; // Battery current (A) + float power; // Power (kW) + uint8_t battery_fail_level; // Battery failure status + uint8_t is_charge_mos; // Charge MOSFET state (0/1) + uint8_t is_discharge_mos; // Discharge MOSFET state (0/1) + uint8_t is_charging; // Charging state (0/1) + uint32_t battery_cycle; // Battery cycle count + float energy_cycle; // Energy per cycle (kWh) + uint32_t lastUpdateMs; // Timestamp of last update + float cell_voltages[BMS_CELLS_NUM]; // Per-cell voltages (V) + // Temperature ordering: [mos, balance, t1, t2, t3, t4] + float temperatures[BMS_TEMPERATURE_SENSORS_NUM]; // Probe temperatures (deg C) +} BLE_BMS_Extended_Telemetry_V1; + // Binary packed ESC telemetry for BLE transmission (~46 bytes) typedef struct { uint8_t version; // Protocol version (1) @@ -167,6 +191,9 @@ typedef struct { uint32_t uptime_ms; // Time since boot (ms) } BLE_Controller_Telemetry_V1; +static_assert(sizeof(BLE_BMS_Extended_Telemetry_V1) <= 182, + "Extended BMS packet exceeds BLE payload budget for MTU 185"); + #pragma pack(pop) #endif // INC_SP140_STRUCTS_H_ diff --git a/src/sp140/ble/bms_packet_codec.cpp b/src/sp140/ble/bms_packet_codec.cpp new file mode 100644 index 00000000..25ecd7f6 --- /dev/null +++ b/src/sp140/ble/bms_packet_codec.cpp @@ -0,0 +1,76 @@ +#include "sp140/ble/bms_packet_codec.h" + +namespace { + +constexpr uint8_t kBmsTelemetryVersion = 1; + +#if defined(ARDUINO_ARCH_ESP32) +static_assert(sizeof(unsigned long) == 4, + "ESP32 build expects 32-bit unsigned long for telemetry timestamps"); +#endif + +void fillTemperatureArray( + const STR_BMS_TELEMETRY_140& telemetry, + float temperatures[BMS_TEMPERATURE_SENSORS_NUM]) { + // Order is fixed for client parsing: [mos, balance, t1, t2, t3, t4] + temperatures[0] = telemetry.mos_temperature; + temperatures[1] = telemetry.balance_temperature; + temperatures[2] = telemetry.t1_temperature; + temperatures[3] = telemetry.t2_temperature; + temperatures[4] = telemetry.t3_temperature; + temperatures[5] = telemetry.t4_temperature; +} + +} // namespace + +BLE_BMS_Telemetry_V1 buildBMSPackedTelemetryV1( + const STR_BMS_TELEMETRY_140& telemetry, uint8_t bms_id) { + BLE_BMS_Telemetry_V1 packet = {}; + packet.version = kBmsTelemetryVersion; + packet.bms_id = bms_id; + packet.connection_state = static_cast(telemetry.bmsState); + packet.soc = telemetry.soc; + packet.battery_voltage = telemetry.battery_voltage; + packet.battery_current = telemetry.battery_current; + packet.power = telemetry.power; + packet.highest_cell_voltage = telemetry.highest_cell_voltage; + packet.lowest_cell_voltage = telemetry.lowest_cell_voltage; + packet.highest_temperature = telemetry.highest_temperature; + packet.lowest_temperature = telemetry.lowest_temperature; + packet.voltage_differential = telemetry.voltage_differential; + packet.battery_fail_level = telemetry.battery_fail_level; + packet.is_charge_mos = telemetry.is_charge_mos ? 1 : 0; + packet.is_discharge_mos = telemetry.is_discharge_mos ? 1 : 0; + packet.is_charging = telemetry.is_charging ? 1 : 0; + packet.battery_cycle = telemetry.battery_cycle; + packet.energy_cycle = telemetry.energy_cycle; + packet.lastUpdateMs = static_cast(telemetry.lastUpdateMs); + + return packet; +} + +BLE_BMS_Extended_Telemetry_V1 buildBMSExtendedTelemetryV1( + const STR_BMS_TELEMETRY_140& telemetry, uint8_t bms_id) { + BLE_BMS_Extended_Telemetry_V1 packet = {}; + packet.version = kBmsTelemetryVersion; + packet.bms_id = bms_id; + packet.connection_state = static_cast(telemetry.bmsState); + packet.soc = telemetry.soc; + packet.battery_voltage = telemetry.battery_voltage; + packet.battery_current = telemetry.battery_current; + packet.power = telemetry.power; + packet.battery_fail_level = telemetry.battery_fail_level; + packet.is_charge_mos = telemetry.is_charge_mos ? 1 : 0; + packet.is_discharge_mos = telemetry.is_discharge_mos ? 1 : 0; + packet.is_charging = telemetry.is_charging ? 1 : 0; + packet.battery_cycle = telemetry.battery_cycle; + packet.energy_cycle = telemetry.energy_cycle; + packet.lastUpdateMs = static_cast(telemetry.lastUpdateMs); + + for (uint8_t i = 0; i < BMS_CELLS_NUM; ++i) { + packet.cell_voltages[i] = telemetry.cell_voltages[i]; + } + fillTemperatureArray(telemetry, packet.temperatures); + + return packet; +} diff --git a/src/sp140/ble/bms_service.cpp b/src/sp140/ble/bms_service.cpp index c9bb85a0..35547f21 100644 --- a/src/sp140/ble/bms_service.cpp +++ b/src/sp140/ble/bms_service.cpp @@ -4,6 +4,7 @@ #include "sp140/ble.h" #include "sp140/ble/ble_ids.h" +#include "sp140/ble/bms_packet_codec.h" namespace { @@ -11,6 +12,7 @@ NimBLEService* pBmsService = nullptr; // Binary packed telemetry characteristic (V1) NimBLECharacteristic* pBMSPackedTelemetry = nullptr; +NimBLECharacteristic* pBMSExtendedTelemetry = nullptr; } // namespace @@ -27,13 +29,24 @@ void initBmsBleService(NimBLEServer* server) { NimBLEUUID(BMS_PACKED_TELEMETRY_UUID), NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - // Initialize packed telemetry with zeros - BLE_BMS_Telemetry_V1 initialPacket = {}; - initialPacket.version = 1; + pBMSExtendedTelemetry = pBmsService->createCharacteristic( + NimBLEUUID(BMS_EXTENDED_TELEMETRY_UUID), + NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); + + // Initialize telemetry characteristics with a disconnected baseline. + STR_BMS_TELEMETRY_140 initialTelemetry = {}; + initialTelemetry.bmsState = TelemetryState::NOT_CONNECTED; + BLE_BMS_Telemetry_V1 initialPacket = buildBMSPackedTelemetryV1(initialTelemetry, 0); pBMSPackedTelemetry->setValue( reinterpret_cast(&initialPacket), sizeof(BLE_BMS_Telemetry_V1)); + BLE_BMS_Extended_Telemetry_V1 initialExtendedPacket = + buildBMSExtendedTelemetryV1(initialTelemetry, 0); + pBMSExtendedTelemetry->setValue( + reinterpret_cast(&initialExtendedPacket), + sizeof(BLE_BMS_Extended_Telemetry_V1)); + pBmsService->start(); } @@ -43,26 +56,7 @@ void updateBMSPackedTelemetry(const STR_BMS_TELEMETRY_140& telemetry, uint8_t bm return; } - BLE_BMS_Telemetry_V1 packet; - packet.version = 1; - packet.bms_id = bms_id; - packet.connection_state = static_cast(telemetry.bmsState); - packet.soc = telemetry.soc; - packet.battery_voltage = telemetry.battery_voltage; - packet.battery_current = telemetry.battery_current; - packet.power = telemetry.power; - packet.highest_cell_voltage = telemetry.highest_cell_voltage; - packet.lowest_cell_voltage = telemetry.lowest_cell_voltage; - packet.highest_temperature = telemetry.highest_temperature; - packet.lowest_temperature = telemetry.lowest_temperature; - packet.voltage_differential = telemetry.voltage_differential; - packet.battery_fail_level = telemetry.battery_fail_level; - packet.is_charge_mos = telemetry.is_charge_mos ? 1 : 0; - packet.is_discharge_mos = telemetry.is_discharge_mos ? 1 : 0; - packet.is_charging = telemetry.is_charging ? 1 : 0; - packet.battery_cycle = telemetry.battery_cycle; - packet.energy_cycle = telemetry.energy_cycle; - packet.lastUpdateMs = static_cast(telemetry.lastUpdateMs); + BLE_BMS_Telemetry_V1 packet = buildBMSPackedTelemetryV1(telemetry, bms_id); pBMSPackedTelemetry->setValue( reinterpret_cast(&packet), @@ -72,3 +66,19 @@ void updateBMSPackedTelemetry(const STR_BMS_TELEMETRY_140& telemetry, uint8_t bm pBMSPackedTelemetry->notify(); } } + +void updateBMSExtendedTelemetry(const STR_BMS_TELEMETRY_140& telemetry, uint8_t bms_id) { + if (pBmsService == nullptr || pBMSExtendedTelemetry == nullptr) { + return; + } + + BLE_BMS_Extended_Telemetry_V1 packet = + buildBMSExtendedTelemetryV1(telemetry, bms_id); + pBMSExtendedTelemetry->setValue( + reinterpret_cast(&packet), + sizeof(BLE_BMS_Extended_Telemetry_V1)); + + if (deviceConnected) { + pBMSExtendedTelemetry->notify(); + } +} diff --git a/src/sp140/sp140.ino b/src/sp140/sp140.ino index 736d243b..4bf14d83 100644 --- a/src/sp140/sp140.ino +++ b/src/sp140/sp140.ino @@ -379,6 +379,8 @@ void throttleTask(void *pvParameters) { void updateBLETask(void *pvParameters) { STR_BMS_TELEMETRY_140 newBmsTelemetry; + static unsigned long lastExtendedUpdateMs = 0; + const unsigned long EXTENDED_UPDATE_INTERVAL_MS = 500; // 2Hz while (true) { // Add error checking for queue @@ -393,6 +395,12 @@ void updateBLETask(void *pvParameters) { // Update packed binary telemetry (always enabled) updateBMSPackedTelemetry(newBmsTelemetry, 0); // bms_id=0 for primary BMS + const unsigned long now = millis(); + if (now - lastExtendedUpdateMs >= EXTENDED_UPDATE_INTERVAL_MS) { + updateBMSExtendedTelemetry(newBmsTelemetry, 0); // bms_id=0 for primary BMS + lastExtendedUpdateMs = now; + } + // Update controller telemetry at same 10Hz rate as BMS // Gather sensor data from barometer and ESP32 float altitude = getAltitude(deviceData); @@ -534,14 +542,15 @@ void bmsTask(void *pvParameters) { } } + if (bmsTelemetryQueue != NULL) { + xQueueOverwrite(bmsTelemetryQueue, &bmsTelemetryData); + } + if (bmsTelemetryData.bmsState == TelemetryState::CONNECTED) { unifiedBatteryData.volts = bmsTelemetryData.battery_voltage; unifiedBatteryData.amps = bmsTelemetryData.battery_current; unifiedBatteryData.soc = bmsTelemetryData.soc; unifiedBatteryData.power = bmsTelemetryData.power; - if (bmsTelemetryQueue != NULL) { - xQueueOverwrite(bmsTelemetryQueue, &bmsTelemetryData); - } } else if (escTelemetryData.escState == TelemetryState::CONNECTED) { unifiedBatteryData.volts = escTelemetryData.volts; unifiedBatteryData.amps = escTelemetryData.amps; diff --git a/test/test_ble_bms_packets/test_ble_bms_packets.cpp b/test/test_ble_bms_packets/test_ble_bms_packets.cpp new file mode 100644 index 00000000..5c83d6d2 --- /dev/null +++ b/test/test_ble_bms_packets/test_ble_bms_packets.cpp @@ -0,0 +1,128 @@ +#include + +#include + +#include "../native_stubs/Arduino.h" +#include "../../inc/sp140/ble/bms_packet_codec.h" +#include "../../src/sp140/ble/bms_packet_codec.cpp" + +namespace { + +STR_BMS_TELEMETRY_140 makeConnectedTelemetry() { + STR_BMS_TELEMETRY_140 telemetry = {}; + telemetry.bmsState = TelemetryState::CONNECTED; + telemetry.soc = 79.5f; + telemetry.battery_voltage = 95.2f; + telemetry.battery_current = 31.4f; + telemetry.power = 2.99f; + telemetry.highest_cell_voltage = 4.11f; + telemetry.lowest_cell_voltage = 3.94f; + telemetry.highest_temperature = 52.0f; + telemetry.lowest_temperature = 24.0f; + telemetry.energy_cycle = 4.8f; + telemetry.battery_cycle = 112; + telemetry.battery_fail_level = 2; + telemetry.voltage_differential = 0.17f; + telemetry.lastUpdateMs = 123456u; + telemetry.is_charging = true; + telemetry.is_charge_mos = true; + telemetry.is_discharge_mos = false; + for (uint8_t i = 0; i < BMS_CELLS_NUM; ++i) { + telemetry.cell_voltages[i] = 3.90f + (static_cast(i) * 0.01f); + } + telemetry.mos_temperature = 47.0f; + telemetry.balance_temperature = 39.0f; + telemetry.t1_temperature = 33.0f; + telemetry.t2_temperature = 34.0f; + telemetry.t3_temperature = 35.0f; + telemetry.t4_temperature = 36.0f; + return telemetry; +} + +} // namespace + +TEST(BMSPacketCodec, ConnectedTelemetryMapsToPackedAndExtended) { + STR_BMS_TELEMETRY_140 telemetry = makeConnectedTelemetry(); + + BLE_BMS_Telemetry_V1 packed = buildBMSPackedTelemetryV1(telemetry, 3); + BLE_BMS_Extended_Telemetry_V1 extended = buildBMSExtendedTelemetryV1(telemetry, 3); + + EXPECT_EQ(packed.version, 1); + EXPECT_EQ(packed.bms_id, 3); + EXPECT_EQ(packed.connection_state, static_cast(TelemetryState::CONNECTED)); + EXPECT_FLOAT_EQ(packed.soc, telemetry.soc); + EXPECT_FLOAT_EQ(packed.battery_voltage, telemetry.battery_voltage); + EXPECT_FLOAT_EQ(packed.battery_current, telemetry.battery_current); + EXPECT_FLOAT_EQ(packed.power, telemetry.power); + EXPECT_FLOAT_EQ(packed.energy_cycle, telemetry.energy_cycle); + + EXPECT_EQ(extended.version, 1); + EXPECT_EQ(extended.bms_id, 3); + EXPECT_EQ(extended.connection_state, static_cast(TelemetryState::CONNECTED)); + EXPECT_FLOAT_EQ(extended.soc, telemetry.soc); + EXPECT_FLOAT_EQ(extended.battery_voltage, telemetry.battery_voltage); + EXPECT_FLOAT_EQ(extended.battery_current, telemetry.battery_current); + EXPECT_FLOAT_EQ(extended.power, telemetry.power); + EXPECT_FLOAT_EQ(extended.cell_voltages[0], telemetry.cell_voltages[0]); + EXPECT_FLOAT_EQ(extended.cell_voltages[BMS_CELLS_NUM - 1], + telemetry.cell_voltages[BMS_CELLS_NUM - 1]); + EXPECT_FLOAT_EQ(extended.temperatures[0], telemetry.mos_temperature); + EXPECT_FLOAT_EQ(extended.temperatures[1], telemetry.balance_temperature); + EXPECT_FLOAT_EQ(extended.temperatures[2], telemetry.t1_temperature); + EXPECT_FLOAT_EQ(extended.temperatures[3], telemetry.t2_temperature); + EXPECT_FLOAT_EQ(extended.temperatures[4], telemetry.t3_temperature); + EXPECT_FLOAT_EQ(extended.temperatures[5], telemetry.t4_temperature); +} + +TEST(BMSPacketCodec, DisconnectedTelemetryKeepsLastKnownValues) { + STR_BMS_TELEMETRY_140 telemetry = makeConnectedTelemetry(); + telemetry.bmsState = TelemetryState::NOT_CONNECTED; + + BLE_BMS_Telemetry_V1 packed = buildBMSPackedTelemetryV1(telemetry, 0); + BLE_BMS_Extended_Telemetry_V1 extended = buildBMSExtendedTelemetryV1(telemetry, 0); + + EXPECT_EQ(packed.connection_state, static_cast(TelemetryState::NOT_CONNECTED)); + EXPECT_FLOAT_EQ(packed.soc, telemetry.soc); + EXPECT_FLOAT_EQ(packed.battery_voltage, telemetry.battery_voltage); + EXPECT_FLOAT_EQ(packed.battery_current, telemetry.battery_current); + EXPECT_FLOAT_EQ(packed.power, telemetry.power); + EXPECT_FLOAT_EQ(packed.highest_cell_voltage, telemetry.highest_cell_voltage); + EXPECT_FLOAT_EQ(packed.lowest_cell_voltage, telemetry.lowest_cell_voltage); + EXPECT_FLOAT_EQ(packed.highest_temperature, telemetry.highest_temperature); + EXPECT_FLOAT_EQ(packed.lowest_temperature, telemetry.lowest_temperature); + EXPECT_FLOAT_EQ(packed.voltage_differential, telemetry.voltage_differential); + EXPECT_FLOAT_EQ(packed.energy_cycle, telemetry.energy_cycle); + + EXPECT_EQ(extended.connection_state, static_cast(TelemetryState::NOT_CONNECTED)); + EXPECT_FLOAT_EQ(extended.soc, telemetry.soc); + EXPECT_FLOAT_EQ(extended.battery_voltage, telemetry.battery_voltage); + EXPECT_FLOAT_EQ(extended.battery_current, telemetry.battery_current); + EXPECT_FLOAT_EQ(extended.power, telemetry.power); + EXPECT_FLOAT_EQ(extended.energy_cycle, telemetry.energy_cycle); + for (uint8_t i = 0; i < BMS_CELLS_NUM; ++i) { + EXPECT_FLOAT_EQ(extended.cell_voltages[i], telemetry.cell_voltages[i]); + } + const float expectedTemps[BMS_TEMPERATURE_SENSORS_NUM] = { + telemetry.mos_temperature, + telemetry.balance_temperature, + telemetry.t1_temperature, + telemetry.t2_temperature, + telemetry.t3_temperature, + telemetry.t4_temperature, + }; + for (uint8_t i = 0; i < BMS_TEMPERATURE_SENSORS_NUM; ++i) { + EXPECT_FLOAT_EQ(extended.temperatures[i], expectedTemps[i]); + } +} + +TEST(BMSPacketCodec, StructSizeExpectations) { + EXPECT_EQ(sizeof(BLE_BMS_Telemetry_V1), static_cast(55)); + EXPECT_EQ(sizeof(BLE_BMS_Extended_Telemetry_V1), static_cast(155)); + EXPECT_LE(sizeof(BLE_BMS_Extended_Telemetry_V1), static_cast(182)); +} + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + RUN_ALL_TESTS(); + return 0; +}