From 1d08a75357a2b3585bf67f9867d5b0fa54021a38 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20=C3=96ster?= Date: Thu, 5 Dec 2024 23:58:43 +0200 Subject: [PATCH] Refactor CRC handling, check incoming messages --- include/leafbms.h | 2 + include/leafinv.h | 3 +- src/leafbms.cpp | 217 ++++++++++++++++++++++++++-------------------- src/leafinv.cpp | 37 +++----- 4 files changed, 142 insertions(+), 117 deletions(-) diff --git a/include/leafbms.h b/include/leafbms.h index 74cd9c1b..6301a0a8 100644 --- a/include/leafbms.h +++ b/include/leafbms.h @@ -25,6 +25,8 @@ class LeafBMS: public BMS public: void SetCanInterface(CanHardware* can) override; void DecodeCAN(int id, uint8_t * data) override; +private: + bool isMessageCorrupt(uint8_t *data); }; #endif // LEAFBMS_H diff --git a/include/leafinv.h b/include/leafinv.h index 8b2c762d..dbaf1fe2 100644 --- a/include/leafinv.h +++ b/include/leafinv.h @@ -39,9 +39,8 @@ class LeafINV: public Inverter float GetMotorSpeed() { return speed; } int GetInverterState() { return error; } void SetCanInterface(CanHardware* c); - private: - static void nissan_crc(uint8_t *data, uint8_t polynomial); + uint8_t nissan_crc(uint8_t *data); static int8_t fahrenheit_to_celsius(uint16_t fahrenheit); uint32_t lastRecv; int16_t speed; diff --git a/src/leafbms.cpp b/src/leafbms.cpp index b1b69ef3..9633bd30 100644 --- a/src/leafbms.cpp +++ b/src/leafbms.cpp @@ -44,107 +44,140 @@ void LeafBMS::DecodeCAN(int id, uint8_t * data) { uint8_t* bytes = (uint8_t*)data; - if (id == 0x1DB) - { - float cur = uint16_t(bytes[0] << 3) + uint16_t(bytes[1] >>5); - if(cur>1023)cur -=2047; //check if negative - uint16_t udc = uint16_t(bytes[2] << 2) + uint16_t(bytes[3] >>6); - //bool interlock = (bytes[3] & (1 << 3)) >> 3; - //bool full = (bytes[3] & (1 << 4)) >> 4; - - if (Param::GetInt(Param::ShuntType) == 0)//Only populate if no shunt is used - { - float BattCur = cur / 2; - float BattVoltage = udc / 2; - Param::SetFloat(Param::idc, BattCur); - if(BattVoltage < 450)Param::SetFloat(Param::udc2, BattVoltage); - if(BattVoltage > 200)Param::SetFloat(Param::udcsw, BattVoltage - 20); //Set for precharging based on actual voltage - float kw = (BattVoltage*BattCur)/1000;//get power from isa sensor and post to parameter database - Param::SetFloat(Param::power, kw); + switch (id) { + case 0x1DB:{ + if (isMessageCorrupt(bytes)) { + //Message content malformed, abort reading data from it! Raise flag! + break; + } + float cur = uint16_t(bytes[0] << 3) + uint16_t(bytes[1] >>5); + if(cur>1023)cur -=2047; //check if negative + uint16_t udc = uint16_t(bytes[2] << 2) + uint16_t(bytes[3] >>6); + //bool interlock = (bytes[3] & (1 << 3)) >> 3; + //bool full = (bytes[3] & (1 << 4)) >> 4; + + if (Param::GetInt(Param::ShuntType) == 0)//Only populate if no shunt is used + { + float BattCur = cur / 2; + float BattVoltage = udc / 2; + Param::SetFloat(Param::idc, BattCur); + if(BattVoltage < 450)Param::SetFloat(Param::udc2, BattVoltage); + if(BattVoltage > 200)Param::SetFloat(Param::udcsw, BattVoltage - 20); //Set for precharging based on actual voltage + float kw = (BattVoltage*BattCur)/1000;//get power from isa sensor and post to parameter database + Param::SetFloat(Param::power, kw); + } + break; } - } - else if (id == 0x1DC) - { - float dislimit = uint16_t(bytes[0] << 2) + uint16_t(bytes[1] >>6); - dislimit = dislimit*0.25; //Kw discharge limit - float chglimit = uint16_t((bytes[1] & 0x3F) << 4) + uint16_t(bytes[2] >>4); - chglimit = chglimit*0.25; //Kw charge limit - float chargelimit = uint16_t((bytes[2] & 0x0F) << 6) + uint16_t(bytes[3] >>2); - chargelimit = chargelimit*0.1; //Kw charger limit - - chargelimit = chargelimit*1000 / Param::GetFloat(Param::udc2);//Transform into Amps - //Param::SetFixed(Param::dislim, dislimit / 4); - - Param::SetFloat(Param::BMS_ChargeLim, chargelimit); - - Param::SetInt(Param::BMS_MaxInput, chglimit); - Param::SetInt(Param::BMS_MaxOutput, dislimit); - } - else if (id == 0x55B) - { - float soc = uint16_t(bytes[0] << 2) + uint16_t(bytes[1] >> 6); - if (Param::GetInt(Param::ShuntType) == 0)//Only populate if no shunt is used - { - soc = soc*0.1; - Param::SetFloat(Param::SOC, soc); + case 0x1DC: { + if (isMessageCorrupt(bytes)) { + //Message content malformed, abort reading data from it! Raise flag! + break; + } + float dislimit = uint16_t(bytes[0] << 2) + uint16_t(bytes[1] >>6); + dislimit = dislimit*0.25; //Kw discharge limit + float chglimit = uint16_t((bytes[1] & 0x3F) << 4) + uint16_t(bytes[2] >>4); + chglimit = chglimit*0.25; //Kw charge limit + float chargelimit = uint16_t((bytes[2] & 0x0F) << 6) + uint16_t(bytes[3] >>2); + chargelimit = chargelimit*0.1; //Kw charger limit + + chargelimit = chargelimit*1000 / Param::GetFloat(Param::udc2);//Transform into Amps + //Param::SetFixed(Param::dislim, dislimit / 4); + + Param::SetFloat(Param::BMS_ChargeLim, chargelimit); + Param::SetInt(Param::BMS_MaxInput, chglimit); + Param::SetInt(Param::BMS_MaxOutput, dislimit); + break; } + case 0x55B: { + if (isMessageCorrupt(bytes)) { + //Message content malformed, abort reading data from it! Raise flag! + break; + } + float soc = uint16_t(bytes[0] << 2) + uint16_t(bytes[1] >> 6); + if (Param::GetInt(Param::ShuntType) == 0)//Only populate if no shunt is used + { + soc = soc*0.1; + Param::SetFloat(Param::SOC, soc); + } - uint16_t IsoTemp = uint16_t(bytes[4] << 2) + uint16_t(bytes[5] >> 6); + uint16_t IsoTemp = uint16_t(bytes[4] << 2) + uint16_t(bytes[5] >> 6); - Param::SetInt(Param::BMS_IsoMeas,IsoTemp); - } - else if (id == 0x5BC) - { - /* - int soh = bytes[4] >> 1; - int cond = (bytes[6] >> 5) + ((bytes[5] & 0x3) << 3); - /nt limres = bytes[5] >> 5; + Param::SetInt(Param::BMS_IsoMeas,IsoTemp); + break; + } + case 0x5BC: { + /* + int soh = bytes[4] >> 1; + int cond = (bytes[6] >> 5) + ((bytes[5] & 0x3) << 3); + /nt limres = bytes[5] >> 5; + + //Param::SetInt(Param::limreason, limres); - //Param::SetInt(Param::limreason, limres); + //Only acquire quick charge remaining time + if (cond == 0) + { + int time = bytes[7] + ((bytes[6] & 0x1F) << 8); - //Only acquire quick charge remaining time - if (cond == 0) - { - int time = bytes[7] + ((bytes[6] & 0x1F) << 8); + //Param::SetInt(Param::chgtime, time); + } + - //Param::SetInt(Param::chgtime, time); + //Param::SetInt(Param::soh, soh); + */ + //0x5BC only contains average battery temperature on ZE0 + if (LEAF_battery_Type == ZE0_BATTERY) { + temperature = (bytes[3] - 40); + Param::SetInt(Param::BMS_Tavg, temperature); + } + break; } - - - //Param::SetInt(Param::soh, soh); - */ - //0x5BC only contains average battery temperature on ZE0 - if (LEAF_battery_Type == ZE0_BATTERY) { - temperature = (bytes[3] - 40); - Param::SetInt(Param::BMS_Tavg, temperature); + case 0x5C0: { + //This temperature only works for 2013-2017 AZE0 LEAF packs, the mux is different on other generations + if (LEAF_battery_Type == AZE0_BATTERY) { + if ((bytes[0] >> 6) == 1) { // Mux signalling MAX value + temperature = ((bytes[2] / 2) - 40); //Effectively has only 7-bit precision, bottom bit is always 0 + Param::SetInt(Param::BMS_Tavg, temperature); + } + } + break; } - } - else if (id == 0x5C0) - { - //This temperature only works for 2013-2017 AZE0 LEAF packs, the mux is different on other generations - if (LEAF_battery_Type == AZE0_BATTERY) { - if ((bytes[0] >> 6) == 1) { // Mux signalling MAX value - temperature = ((bytes[2] / 2) - 40); //Effectively has only 7-bit precision, bottom bit is always 0 - Param::SetInt(Param::BMS_Tavg, temperature); + case 0x59E: { + //AZE0 2013-2017 or ZE1 2018-2023 battery detected + //Only detect as AZE0 if not already set as ZE1 + if (LEAF_battery_Type != ZE1_BATTERY) { + LEAF_battery_Type = AZE0_BATTERY; + } + break; } - } - } - else if (id == 0x59E) - { - //AZE0 2013-2017 or ZE1 2018-2023 battery detected - //Only detect as AZE0 if not already set as ZE1 - if (LEAF_battery_Type != ZE1_BATTERY) { - LEAF_battery_Type = AZE0_BATTERY; - } - } - else if (id == 0x1C2) - { - //ZE1 2018-2023 battery detected! - LEAF_battery_Type = ZE1_BATTERY; - } - else if (id == 0x1ED) - { - //ZE1 62kWh battery detected! - LEAF_battery_Type = ZE1_BATTERY; + case 0x1C2: { + //ZE1 2018-2023 battery detected! + LEAF_battery_Type = ZE1_BATTERY; + break; + } + case 0x1ED: { + //ZE1 62kWh battery detected! + LEAF_battery_Type = ZE1_BATTERY; + break; + } + default: + break; } } + +bool LeafBMS::isMessageCorrupt(uint8_t *data) +{ + uint8_t crc = 0; + uint8_t polynomial = 0x85; + + for (int b = 0; b < 8; b++) { + uint8_t byte = (b == 7) ? 0 : data[b]; // Treat 8th byte as 0 during calculation. + for (int i = 7; i >= 0; i--) { + uint8_t bit = ((byte & (1 << i)) > 0) ? 1 : 0; + if (crc >= 0x80) + crc = (uint8_t)(((crc << 1) + bit) ^ polynomial); + else + crc = (uint8_t)((crc << 1) + bit); + } + } + return crc != data[7]; +} \ No newline at end of file diff --git a/src/leafinv.cpp b/src/leafinv.cpp index 3b1dddbb..c47f371f 100644 --- a/src/leafinv.cpp +++ b/src/leafinv.cpp @@ -161,9 +161,7 @@ void LeafINV::Task10Ms() bytes[4] = weird_d34_values[mprun10][1];//0xC0; bytes[5] = 0x00; // Always 0x00 (LeafLogs, canmsgs) bytes[6] = mprun10; // A 2-bit counter - - // Extra CRC - nissan_crc(bytes, 0x85);//not sure if this is really working or just making me look like a muppet. + bytes[7] = nissan_crc(bytes); can->Send(0x11A, (uint32_t*)bytes, 8); @@ -303,9 +301,7 @@ void LeafINV::Task10Ms() // 2016-24kWh-ev-on-drive-park-off.pcap #12101 / 15.63s // outFrame.data.bytes[6] = 0x01; //byte 6 brake signal - - // Extra CRC - nissan_crc(bytes, 0x85); + bytes[7] = nissan_crc(bytes); can->Send(0x1D4, (uint32_t*)bytes, 8);//send on can1 @@ -335,9 +331,7 @@ void LeafINV::Task10Ms() bytes[4] = 0x40; //SOC for dash in Leaf. fixed val. bytes[5] = 0x00; bytes[6] = mprun10; - - // Extra CRC in byte 7 - nissan_crc(bytes, 0x85); + bytes[7] = nissan_crc(bytes); can->Send(0x1DB, (uint32_t*)bytes, 8); } @@ -357,8 +351,7 @@ void LeafINV::Task10Ms() bytes[4]=0x00;//may not need pairing code crap here...and we don't:) bytes[5]=0x00; bytes[6]=mprun10; - // Extra CRC in byte 7 - nissan_crc(bytes, 0x85); + bytes[7] = nissan_crc(bytes); can->Send(0x1DC, (uint32_t*)bytes, 8); } @@ -456,8 +449,7 @@ void LeafINV::Task100Ms() bytes[4] = 0xDF; bytes[5] = 0xC0; bytes[6] = ((0x1 << 4) | (mprun100)); - // Extra CRC in byte 7 - nissan_crc(bytes, 0x85); + bytes[7] = nissan_crc(bytes); can->Send(0x55b, (uint32_t*)bytes, 8); @@ -505,21 +497,20 @@ int8_t LeafINV::fahrenheit_to_celsius(uint16_t fahrenheit) -void LeafINV::nissan_crc(uint8_t *data, uint8_t polynomial) +uint8_t LeafINV::nissan_crc(uint8_t *data) { - // We want to process 8 bytes with the 8th byte being zero - data[7] = 0; uint8_t crc = 0; - for(int b=0; b<8; b++) - { - for(int i=7; i>=0; i--) - { - uint8_t bit = ((data[b] &(1 << i)) > 0) ? 1 : 0; - if(crc >= 0x80) + uint8_t polynomial = 0x85; + + for (int b = 0; b < 8; b++) { + uint8_t byte = (b == 7) ? 0 : data[b]; // Treat 8th byte as 0 during calculation. + for (int i = 7; i >= 0; i--) { + uint8_t bit = ((byte & (1 << i)) > 0) ? 1 : 0; + if (crc >= 0x80) crc = (uint8_t)(((crc << 1) + bit) ^ polynomial); else crc = (uint8_t)((crc << 1) + bit); } } - data[7] = crc; + return crc; }