544 lines
14 KiB
C++
544 lines
14 KiB
C++
#include "bms.h"
|
|
|
|
#include <Arduino.h>
|
|
#include <NimBLEDevice.h>
|
|
#include "logger.h"
|
|
#include "app_config.h"
|
|
|
|
// BMS address is loaded from appConfig.bms.address.
|
|
static const char* BMS_SERVICE_UUID = "ff00";
|
|
static const char* BMS_NOTIFY_UUID = "ff01";
|
|
static const char* BMS_WRITE_UUID = "ff02";
|
|
|
|
static NimBLEClient* client = nullptr;
|
|
static NimBLERemoteCharacteristic* notifyChar = nullptr;
|
|
static NimBLERemoteCharacteristic* writeChar = nullptr;
|
|
|
|
static uint8_t responseBuffer[128];
|
|
static size_t responseLength = 0;
|
|
static bool responseReady = false;
|
|
|
|
static unsigned long lastReadAttempt = 0;
|
|
|
|
static unsigned long lastBmsConnectAttempt = 0;
|
|
static unsigned long bmsConnectBackoffMs = 0;
|
|
static int bmsConnectFailCount = 0;
|
|
static unsigned long bmsReconnectPausedUntil = 0;
|
|
static bool bmsSetupMode = false;
|
|
static const unsigned long READ_INTERVAL_MS = 5000;
|
|
|
|
BmsData bmsData;
|
|
|
|
static BleScanResult bleScanResults[MAX_BLE_SCAN_RESULTS];
|
|
static int bleScanResultCount = 0;
|
|
|
|
static uint8_t JBD_STATUS_REQUEST[] = {
|
|
0xDD, 0xA5, 0x03, 0x00, 0xFF, 0xFD, 0x77
|
|
};
|
|
|
|
static uint8_t JBD_CELL_REQUEST[] = {
|
|
0xDD, 0xA5, 0x04, 0x00, 0xFF, 0xFC, 0x77
|
|
};
|
|
|
|
static uint16_t readU16(const uint8_t* data, int index) {
|
|
return ((uint16_t)data[index] << 8) | data[index + 1];
|
|
}
|
|
|
|
static int16_t readS16(const uint8_t* data, int index) {
|
|
return (int16_t)readU16(data, index);
|
|
}
|
|
|
|
static void resetResponseBuffer() {
|
|
responseLength = 0;
|
|
responseReady = false;
|
|
memset(responseBuffer, 0, sizeof(responseBuffer));
|
|
}
|
|
|
|
static void handleNotify(
|
|
NimBLERemoteCharacteristic* characteristic,
|
|
uint8_t* data,
|
|
size_t length,
|
|
bool isNotify
|
|
) {
|
|
if (responseLength + length > sizeof(responseBuffer)) {
|
|
resetResponseBuffer();
|
|
return;
|
|
}
|
|
|
|
memcpy(responseBuffer + responseLength, data, length);
|
|
responseLength += length;
|
|
|
|
if (responseLength > 0 && responseBuffer[responseLength - 1] == 0x77) {
|
|
responseReady = true;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
class BleScanCallbacks : public NimBLEScanCallbacks {
|
|
void onResult(const NimBLEAdvertisedDevice* device) override {
|
|
if (bleScanResultCount >= MAX_BLE_SCAN_RESULTS) {
|
|
return;
|
|
}
|
|
|
|
String address = device->getAddress().toString().c_str();
|
|
|
|
for (int i = 0; i < bleScanResultCount; i++) {
|
|
if (bleScanResults[i].address == address) {
|
|
bleScanResults[i].rssi = device->getRSSI();
|
|
|
|
if (bleScanResults[i].name.length() == 0 && device->haveName()) {
|
|
bleScanResults[i].name = device->getName().c_str();
|
|
}
|
|
|
|
return;
|
|
}
|
|
}
|
|
|
|
String name = "";
|
|
if (device->haveName()) {
|
|
name = device->getName().c_str();
|
|
}
|
|
|
|
bleScanResults[bleScanResultCount].address = address;
|
|
bleScanResults[bleScanResultCount].name = name;
|
|
bleScanResults[bleScanResultCount].rssi = device->getRSSI();
|
|
|
|
Serial.print(logTimestamp());
|
|
Serial.print(" BLE: found ");
|
|
Serial.print(bleScanResultCount + 1);
|
|
Serial.print(") ");
|
|
|
|
if (name.length() > 0) {
|
|
Serial.print(name);
|
|
} else {
|
|
Serial.print("(no name)");
|
|
}
|
|
|
|
Serial.print(" | ");
|
|
Serial.print(address);
|
|
Serial.print(" | RSSI ");
|
|
Serial.println(device->getRSSI());
|
|
|
|
bleScanResultCount++;
|
|
}
|
|
};
|
|
|
|
static BleScanCallbacks bleScanCallbacks;
|
|
|
|
void pauseBmsReconnect(uint32_t pauseMs) {
|
|
bmsReconnectPausedUntil = millis() + pauseMs;
|
|
}
|
|
|
|
|
|
void enterBmsSetupMode() {
|
|
Serial.print(logTimestamp());
|
|
Serial.println(" BMS setup: entering setup mode");
|
|
|
|
bmsSetupMode = true;
|
|
pauseBmsReconnect(3600000UL);
|
|
|
|
if (client && client->isConnected()) {
|
|
Serial.print(logTimestamp());
|
|
Serial.println(" BMS setup: disconnecting BMS");
|
|
client->disconnect();
|
|
}
|
|
|
|
if (client) {
|
|
NimBLEDevice::deleteClient(client);
|
|
client = nullptr;
|
|
}
|
|
|
|
notifyChar = nullptr;
|
|
writeChar = nullptr;
|
|
bmsData.connected = false;
|
|
|
|
Serial.print(logTimestamp());
|
|
Serial.println(" BMS setup: ready for BLE scans");
|
|
}
|
|
|
|
void exitBmsSetupMode() {
|
|
Serial.print(logTimestamp());
|
|
Serial.println(" BMS setup: exiting setup mode");
|
|
|
|
bmsSetupMode = false;
|
|
bmsReconnectPausedUntil = 0;
|
|
}
|
|
|
|
bool isBmsSetupMode() {
|
|
return bmsSetupMode;
|
|
}
|
|
|
|
int getBleScanResultCount() {
|
|
return bleScanResultCount;
|
|
}
|
|
|
|
const BleScanResult* getBleScanResult(int index) {
|
|
if (index < 0 || index >= bleScanResultCount) {
|
|
return nullptr;
|
|
}
|
|
|
|
return &bleScanResults[index];
|
|
}
|
|
|
|
int scanBleDevices(uint32_t scanSeconds) {
|
|
Serial.print(logTimestamp());
|
|
Serial.println(" BLE: Starting setup scan...");
|
|
|
|
if (!bmsSetupMode) {
|
|
Serial.print(logTimestamp());
|
|
Serial.println(" BLE: auto-entering BMS setup mode for scan");
|
|
enterBmsSetupMode();
|
|
}
|
|
|
|
bleScanResultCount = 0;
|
|
|
|
Serial.print(logTimestamp());
|
|
Serial.println(" BLE: waiting 5 seconds before scan");
|
|
delay(5000);
|
|
|
|
NimBLEScan* scan = NimBLEDevice::getScan();
|
|
scan->setActiveScan(true);
|
|
scan->setInterval(100);
|
|
scan->setWindow(99);
|
|
scan->clearResults();
|
|
|
|
Serial.print(logTimestamp());
|
|
Serial.print(" BLE: scanning for ");
|
|
Serial.print(scanSeconds);
|
|
Serial.println(" seconds...");
|
|
|
|
unsigned long scanStart = millis();
|
|
|
|
NimBLEScanResults results = scan->getResults(scanSeconds, false);
|
|
|
|
unsigned long elapsed = (millis() - scanStart) / 1000;
|
|
|
|
Serial.print(logTimestamp());
|
|
Serial.print(" BLE: scan returned after ");
|
|
Serial.print(elapsed);
|
|
Serial.println(" seconds");
|
|
|
|
int resultCount = results.getCount();
|
|
|
|
Serial.print(logTimestamp());
|
|
Serial.print(" BLE: raw devices seen: ");
|
|
Serial.println(resultCount);
|
|
|
|
for (int i = 0; i < resultCount && bleScanResultCount < MAX_BLE_SCAN_RESULTS; i++) {
|
|
const NimBLEAdvertisedDevice* device = results.getDevice(i);
|
|
|
|
String address = device->getAddress().toString().c_str();
|
|
|
|
bool alreadySeen = false;
|
|
for (int existing = 0; existing < bleScanResultCount; existing++) {
|
|
if (bleScanResults[existing].address == address) {
|
|
alreadySeen = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (alreadySeen) {
|
|
continue;
|
|
}
|
|
|
|
String name = "";
|
|
if (device->haveName()) {
|
|
name = device->getName().c_str();
|
|
}
|
|
|
|
bleScanResults[bleScanResultCount].address = address;
|
|
bleScanResults[bleScanResultCount].name = name;
|
|
bleScanResults[bleScanResultCount].rssi = device->getRSSI();
|
|
bleScanResultCount++;
|
|
}
|
|
|
|
scan->clearResults();
|
|
|
|
Serial.print(logTimestamp());
|
|
Serial.print(" BLE: devices found: ");
|
|
Serial.println(bleScanResultCount);
|
|
|
|
if (bleScanResultCount == 0) {
|
|
Serial.println("No BLE devices found.");
|
|
return 0;
|
|
}
|
|
|
|
Serial.println("BLE devices:");
|
|
|
|
for (int i = 0; i < bleScanResultCount; i++) {
|
|
Serial.print(i + 1);
|
|
Serial.print(") ");
|
|
|
|
if (bleScanResults[i].name.length() > 0) {
|
|
Serial.print(bleScanResults[i].name);
|
|
} else {
|
|
Serial.print("(no name)");
|
|
}
|
|
|
|
Serial.print(" | ");
|
|
Serial.print(bleScanResults[i].address);
|
|
Serial.print(" | RSSI ");
|
|
Serial.println(bleScanResults[i].rssi);
|
|
}
|
|
|
|
Serial.println("Use: select bms <number>");
|
|
Serial.println("Use: exit setup when finished");
|
|
|
|
return bleScanResultCount;
|
|
}
|
|
|
|
static bool connectBms() {
|
|
if (client && client->isConnected()) {
|
|
bmsData.connected = true;
|
|
return true;
|
|
}
|
|
|
|
if (client && !client->isConnected()) {
|
|
Serial.println("BMS: Disconnected, resetting client");
|
|
NimBLEDevice::deleteClient(client);
|
|
client = nullptr;
|
|
notifyChar = nullptr;
|
|
writeChar = nullptr;
|
|
bmsData.connected = false;
|
|
}
|
|
|
|
Serial.println("BMS: Connecting...");
|
|
|
|
if (!appConfig.bms.enabled || appConfig.bms.address.length() == 0) {
|
|
bmsData.connected = false;
|
|
return false;
|
|
}
|
|
|
|
NimBLEAddress address(
|
|
std::string(appConfig.bms.address.c_str()),
|
|
appConfig.bms.addressType == "random" ? BLE_ADDR_RANDOM : BLE_ADDR_PUBLIC
|
|
);
|
|
|
|
client = NimBLEDevice::createClient();
|
|
|
|
if (!client->connect(address)) {
|
|
Serial.println("BMS: Connect failed");
|
|
bmsData.connected = false;
|
|
NimBLEDevice::deleteClient(client);
|
|
client = nullptr;
|
|
return false;
|
|
}
|
|
|
|
NimBLERemoteService* service = client->getService(BMS_SERVICE_UUID);
|
|
if (!service) {
|
|
Serial.println("BMS: Service ff00 not found");
|
|
client->disconnect();
|
|
bmsData.connected = false;
|
|
return false;
|
|
}
|
|
|
|
notifyChar = service->getCharacteristic(BMS_NOTIFY_UUID);
|
|
writeChar = service->getCharacteristic(BMS_WRITE_UUID);
|
|
|
|
if (!notifyChar || !writeChar) {
|
|
Serial.println("BMS: Characteristics not found");
|
|
client->disconnect();
|
|
bmsData.connected = false;
|
|
return false;
|
|
}
|
|
|
|
if (!notifyChar->subscribe(true, handleNotify)) {
|
|
Serial.println("BMS: Notify subscribe failed");
|
|
client->disconnect();
|
|
bmsData.connected = false;
|
|
return false;
|
|
}
|
|
|
|
Serial.println("BMS: Connected");
|
|
bmsData.connected = true;
|
|
return true;
|
|
}
|
|
|
|
static bool requestPacket(uint8_t* command, size_t commandLength) {
|
|
resetResponseBuffer();
|
|
|
|
bool ok = writeChar->writeValue(command, commandLength, false);
|
|
if (!ok) {
|
|
Serial.println("BMS: Write failed");
|
|
bmsData.connected = false;
|
|
if (client) {
|
|
client->disconnect();
|
|
}
|
|
return false;
|
|
}
|
|
|
|
unsigned long start = millis();
|
|
while (!responseReady && millis() - start < 2000) {
|
|
delay(10);
|
|
}
|
|
|
|
if (!responseReady) {
|
|
Serial.println("BMS: Read timeout");
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool parseBasicInfo() {
|
|
if (responseLength < 40) {
|
|
Serial.println("BMS: Basic response too short");
|
|
return false;
|
|
}
|
|
|
|
if (responseBuffer[0] != 0xDD || responseBuffer[1] != 0x03) {
|
|
Serial.println("BMS: Unexpected basic response header");
|
|
return false;
|
|
}
|
|
|
|
uint16_t voltageRaw = readU16(responseBuffer, 4);
|
|
int16_t currentRaw = readS16(responseBuffer, 6);
|
|
uint16_t remainingRaw = readU16(responseBuffer, 8);
|
|
uint16_t capacityRaw = readU16(responseBuffer, 10);
|
|
uint16_t cyclesRaw = readU16(responseBuffer, 12);
|
|
|
|
uint8_t socRaw = responseBuffer[23];
|
|
|
|
uint8_t offset = 24;
|
|
uint8_t cellCount = responseBuffer[offset + 1];
|
|
uint8_t ntcCount = responseBuffer[offset + 2];
|
|
|
|
int tempOffset = offset + 3;
|
|
float tempF = 0;
|
|
|
|
if (ntcCount > 0 && responseLength > (size_t)(tempOffset + 1)) {
|
|
uint16_t tempRaw = readU16(responseBuffer, tempOffset);
|
|
float tempC = (tempRaw - 2731) / 10.0;
|
|
tempF = tempC * 9.0 / 5.0 + 32.0;
|
|
}
|
|
|
|
bmsData.voltage = voltageRaw / 100.0;
|
|
bmsData.current = currentRaw / 100.0;
|
|
bmsData.remainingAh = remainingRaw / 100.0;
|
|
bmsData.capacityAh = capacityRaw / 100.0;
|
|
bmsData.cycleCount = cyclesRaw;
|
|
bmsData.soc = socRaw;
|
|
bmsData.cellCount = cellCount;
|
|
bmsData.ntcCount = ntcCount;
|
|
bmsData.temperatureF = tempF;
|
|
bmsData.valid = true;
|
|
|
|
if (currentLogLevel >= LOG_DEBUG) {
|
|
Serial.print("BMS: SOC ");
|
|
Serial.print(bmsData.soc);
|
|
Serial.print("%, V ");
|
|
Serial.print(bmsData.voltage);
|
|
Serial.print(", A ");
|
|
Serial.print(bmsData.current);
|
|
Serial.print(", Ah ");
|
|
Serial.println(bmsData.remainingAh);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool parseCellVoltages() {
|
|
if (responseLength < 7) {
|
|
Serial.println("BMS: Cell response too short");
|
|
return false;
|
|
}
|
|
|
|
if (responseBuffer[0] != 0xDD || responseBuffer[1] != 0x04) {
|
|
Serial.println("BMS: Unexpected cell response header");
|
|
return false;
|
|
}
|
|
|
|
uint8_t payloadLength = responseBuffer[3];
|
|
int cellsReported = payloadLength / 2;
|
|
|
|
if (cellsReported > BMS_MAX_CELLS) {
|
|
cellsReported = BMS_MAX_CELLS;
|
|
}
|
|
|
|
if (cellsReported <= 0) {
|
|
Serial.println("BMS: No cell voltages reported");
|
|
return false;
|
|
}
|
|
|
|
float minV = 99.0;
|
|
float maxV = 0.0;
|
|
|
|
for (int i = 0; i < cellsReported; i++) {
|
|
uint16_t mv = readU16(responseBuffer, 4 + (i * 2));
|
|
float volts = mv / 1000.0;
|
|
|
|
bmsData.cellVoltages[i] = volts;
|
|
|
|
if (volts < minV) {
|
|
minV = volts;
|
|
}
|
|
|
|
if (volts > maxV) {
|
|
maxV = volts;
|
|
}
|
|
}
|
|
|
|
bmsData.cellCount = cellsReported;
|
|
bmsData.cellMinVoltage = minV;
|
|
bmsData.cellMaxVoltage = maxV;
|
|
bmsData.cellDeltaMv = (int)((maxV - minV) * 1000.0 + 0.5);
|
|
bmsData.cellsValid = true;
|
|
|
|
if (currentLogLevel >= LOG_DEBUG) {
|
|
Serial.print("BMS: Cells ");
|
|
Serial.print(cellsReported);
|
|
Serial.print(", min ");
|
|
Serial.print(minV, 3);
|
|
Serial.print("V, max ");
|
|
Serial.print(maxV, 3);
|
|
Serial.print("V, delta ");
|
|
Serial.print(bmsData.cellDeltaMv);
|
|
Serial.println("mV");
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void initBms() {
|
|
NimBLEDevice::init("OverlandController");
|
|
NimBLEDevice::setPower(ESP_PWR_LVL_P9);
|
|
resetResponseBuffer();
|
|
|
|
lastReadAttempt = 0;
|
|
lastBmsConnectAttempt = 0;
|
|
bmsConnectBackoffMs = 0;
|
|
bmsConnectFailCount = 0;
|
|
}
|
|
|
|
void updateBms() {
|
|
if (bmsSetupMode) {
|
|
return;
|
|
}
|
|
|
|
if ((long)(bmsReconnectPausedUntil - millis()) > 0) {
|
|
return;
|
|
}
|
|
|
|
if (millis() - lastReadAttempt < READ_INTERVAL_MS) {
|
|
return;
|
|
}
|
|
|
|
lastReadAttempt = millis();
|
|
|
|
if (!connectBms()) {
|
|
return;
|
|
}
|
|
|
|
if (requestPacket(JBD_STATUS_REQUEST, sizeof(JBD_STATUS_REQUEST))) {
|
|
parseBasicInfo();
|
|
}
|
|
|
|
delay(100);
|
|
|
|
if (requestPacket(JBD_CELL_REQUEST, sizeof(JBD_CELL_REQUEST))) {
|
|
parseCellVoltages();
|
|
}
|
|
}
|
|
|