From 2a8261496d77947c4e6b00ecfc1cc36d01c168a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=A1n=20M=C3=A1tik?= Date: Wed, 23 Dec 2020 12:09:28 +0100 Subject: [PATCH] Processing data in binary (byte) form --- CarBmwI3.cpp | 58 +++++++++++++++-------------- CommObd2Can.cpp | 98 +++++++++++++++++++++---------------------------- CommObd2Can.h | 22 ++++++++--- LiveData.h | 1 + 4 files changed, 88 insertions(+), 91 deletions(-) diff --git a/CarBmwI3.cpp b/CarBmwI3.cpp index 55ea946..94324ea 100644 --- a/CarBmwI3.cpp +++ b/CarBmwI3.cpp @@ -1,5 +1,6 @@ #include "CarBmwI3.h" #include +#include /** activateliveData->commandQueue @@ -57,6 +58,7 @@ void CarBmwI3::activateCommandQueue() { liveData->commandQueueLoopFrom = commandQueueLoopFrom; liveData->commandQueueCount = commandQueue.size(); liveData->rxBuffOffset = 1; // there is one additional byte in received packets compared to other cars + liveData->expectedMinimalPacketLength = 7; // to filter occasional 5-bytes long packets } /** @@ -84,8 +86,14 @@ void CarBmwI3::parseRowMerged() }; Header_t* pHeader = (Header_t*)liveData->vResponseRowMerged.data(); - uint8_t* pPayload = pHeader->pData; + + const uint16_t payloadLength = liveData->vResponseRowMerged.size() - sizeof(Header_t); + + // create reversed payload to get little endian order of data + std::vector payloadReversed(pHeader->pData, pHeader->pData + payloadLength); + std::reverse(payloadReversed.begin(), payloadReversed.end()); + Serial.print("--extracted PID: "); Serial.println(pHeader->getPid()); Serial.print("--payload length: "); Serial.println(payloadLength); @@ -97,15 +105,14 @@ void CarBmwI3::parseRowMerged() case 0xDD69: { struct DD69_t { - uint8_t batAmp[4]; uint8_t unknown[4]; - int32_t getBatAmpRaw() { return 0x1000000 * batAmp[0] + 0x10000 * batAmp[1] + 0x100 * batAmp[2] + batAmp[3]; } + uint32_t batAmp; }; if (payloadLength == sizeof(DD69_t)) { - DD69_t* ptr = (DD69_t*)pHeader->pData; + DD69_t* ptr = (DD69_t*)payloadReversed.data(); - liveData->params.batPowerAmp = ptr->getBatAmpRaw() / 100.0; //liveData->hexToDecFromResponse(6, 14, 4, true) / 100.0; + liveData->params.batPowerAmp = ptr->batAmp / 100.0; //liveData->hexToDecFromResponse(6, 14, 4, true) / 100.0; liveData->params.batPowerKw = (liveData->params.batPowerAmp * liveData->params.batVoltage) / 1000.0; if (liveData->params.batPowerKw < 0) // Reset charging start time @@ -123,14 +130,13 @@ void CarBmwI3::parseRowMerged() case 0xDDB4: { struct DDB4_t { - uint8_t batVoltage[2]; - uint16_t getBatVoltage() { return 0x100 * batVoltage[0] + batVoltage[1]; }; + uint16_t batVoltage; }; if (payloadLength == sizeof(DDB4_t)) { // HV_SPANNUNG_BATTERIE - DDB4_t* ptr = (DDB4_t*)pHeader->pData; + DDB4_t* ptr = (DDB4_t*)payloadReversed.data(); - liveData->params.batVoltage = ptr->getBatVoltage() / 100.0; + liveData->params.batVoltage = ptr->batVoltage / 100.0; liveData->params.batPowerKw = (liveData->params.batPowerAmp * liveData->params.batVoltage) / 1000.0; if (liveData->params.batPowerKw < 0) // Reset charging start time liveData->params.chargingStartTime = liveData->params.currentTime; @@ -141,25 +147,22 @@ void CarBmwI3::parseRowMerged() case 0xDDC0: { struct DDC0_t { - uint8_t tempMin[2]; - uint8_t tempMax[2]; - uint8_t tempAvg[2]; uint8_t unknown[2]; - int16_t getTempMin() { return 0x100 * tempMin[0] + tempMin[1]; }; - int16_t getTempMax() { return 0x100 * tempMax[0] + tempMax[1]; }; - int16_t getTempAvg() { return 0x100 * tempAvg[0] + tempAvg[1]; }; + uint16_t tempAvg; + uint16_t tempMax; + uint16_t tempMin; }; if (payloadLength == sizeof(DDC0_t)) { - DDC0_t* ptr = (DDC0_t*)pHeader->pData; + DDC0_t* ptr = (DDC0_t*)payloadReversed.data(); - liveData->params.batMinC = ptr->getTempMin() / 100.0; - liveData->params.batTempC = ptr->getTempAvg() / 100.0; - liveData->params.batMaxC = ptr->getTempMax() / 100.0; + liveData->params.batMinC = ptr->tempMin / 100.0; + liveData->params.batTempC = ptr->tempAvg / 100.0; + liveData->params.batMaxC = ptr->tempMax / 100.0; - //Serial.print("----batMinC: "); Serial.println(liveData->params.batMinC); - //Serial.print("----batTemp: "); Serial.println(liveData->params.batTempC); - //Serial.print("----batMaxC: "); Serial.println(liveData->params.batMaxC); + Serial.print("----batMinC: "); Serial.println(liveData->params.batMinC); + Serial.print("----batTemp: "); Serial.println(liveData->params.batTempC); + Serial.print("----batMaxC: "); Serial.println(liveData->params.batMaxC); } } break; @@ -167,17 +170,16 @@ void CarBmwI3::parseRowMerged() case 0xDDBC: { struct DDBC_t { - uint8_t soc[2]; - uint8_t socMax[2]; - uint8_t socMin[2]; uint8_t unknown[2]; - uint16_t getSoc() { return 0x100 * soc[0] + soc[1]; }; + uint16_t socMin; + uint16_t socMax; + uint16_t soc; }; if (payloadLength == sizeof(DDBC_t)) { - DDBC_t* ptr = (DDBC_t*)pHeader->pData; + DDBC_t* ptr = (DDBC_t*)payloadReversed.data(); - liveData->params.socPerc = ptr->getSoc() / 10.0; + liveData->params.socPerc = ptr->soc / 10.0; } } break; diff --git a/CommObd2Can.cpp b/CommObd2Can.cpp index d5d635b..5c1d59b 100644 --- a/CommObd2Can.cpp +++ b/CommObd2Can.cpp @@ -216,34 +216,6 @@ void CommObd2Can::sendFlowControlFrame() { Serial.println(""); } -//static void mockupReceiveCanBuf(INT32U *id, INT8U *len, INT8U buf[]) -//{ -// static uint8_t counter = 0; -// -// std::unordered_map> packets = { -// { 0, { 0xF1, 0x05, 0x62, 0xDD, 0xB4, 0x92, 0xC2 } }, -// { 1, { 0xF1, 0x10, 34, 0xDD, 0xB4, 0x92, 0xC2 } }, -// { 2, { 0xF1, 0x21, 0xA0, 0xA1, 0xA2, 0xA3, 0xA4 } }, -// { 3, { 0xF1, 0x22, 0xB0, 0xB1, 0xB2, 0xB3, 0xB4 } }, -// { 4, { 0xF1, 0x23, 0xC0, 0xC1, 0xC2, 0xC3, 0xC4 } }, -// { 5, { 0xF1, 0x24, 0xD0, 0xD1, 0xD2, 0xD3, 0xD4 } }, -// { 6, { 0xF1, 0x25, 0xE0, 0xE1, 0xE2, 0xE3, 0xE4 } }, -// { 7, { 0xF1, 0x26, 0xF0, 0xF1, 0xF2, 0xF3, 0xF4 } } -// }; -// -// if (counter >= packets.size()) -// counter = 0; -// -// *id = 0x607; -// *len = packets[counter].size(); -// //memset(buf, 0, 7); -// -// -// memcpy(buf, packets[counter].data(), 7); -// -// counter++; -//} - /** Receive PID */ @@ -273,8 +245,10 @@ uint8_t CommObd2Can::receivePID() { } } - if (rxLen == 5) { - Serial.println(" [Ignoring 5 bytes long packet]"); + // Check if this packet shall be discarded due to its length. + // If liveData->expectedPacketLength is set to 0, accept any length. + if(liveData->expectedMinimalPacketLength != 0 && rxLen < liveData->expectedMinimalPacketLength) { + Serial.println(" [Ignored packet]"); return 0xff; } @@ -315,17 +289,34 @@ static void buffer2string(String& out_targetString, uint8_t* in_pBuffer, const u } +CommObd2Can::enFrame_t CommObd2Can::getFrameType(const uint8_t firstByte) { + const uint8_t frameType = (firstByte & 0xf0) >> 4; // frame type is in bits 7 to 4 + switch(frameType) { + case 0: + return enFrame_t::single; + case 1: + return enFrame_t::first; + case 2: + return enFrame_t::consecutive; + default: + return enFrame_t::unknown; + } +} + + + /** Process can frame on byte level + https://en.wikipedia.org/wiki/ISO_15765-2 */ bool CommObd2Can::processFrameBytes() { uint8_t* pDataStart = rxBuf + liveData->rxBuffOffset; // set pointer to data start based on specific offset of car - const uint8_t frameType = (*pDataStart & 0xf0) >> 4; + const auto frameType = getFrameType(*pDataStart); const uint8_t frameLenght = rxLen - liveData->rxBuffOffset; switch (frameType) { - case 0: // Single frame + case enFrame_t::single: // Single frame { struct SingleFrame_t { @@ -339,11 +330,16 @@ bool CommObd2Can::processFrameBytes() { rxRemaining = 0; - Serial.print("---Processing SingleFrame payload: "); printHexBuffer(pSingleFrame->pData, pSingleFrame->size, true); + Serial.print("----Processing SingleFrame payload: "); printHexBuffer(pSingleFrame->pData, pSingleFrame->size, true); + + // single frame - process directly + buffer2string(liveData->responseRowMerged, mergedData.data(), mergedData.size()); + liveData->vResponseRowMerged.assign(mergedData.begin(), mergedData.end()); + processMergedResponse(); } break; - case 1: // First frame + case enFrame_t::first: // First frame { struct FirstFrame_t { @@ -367,11 +363,11 @@ bool CommObd2Can::processFrameBytes() { dataRows[0].assign(pFirstFrame->pData, pFirstFrame->pData + framePayloadSize); rxRemaining -= framePayloadSize; - Serial.print("---Processing FirstFrame payload: "); printHexBuffer(pFirstFrame->pData, framePayloadSize, true); + Serial.print("----Processing FirstFrame payload: "); printHexBuffer(pFirstFrame->pData, framePayloadSize, true); } break; - case 2: // Consecutive frame + case enFrame_t::consecutive: // Consecutive frame { struct ConsecutiveFrame_t { @@ -388,43 +384,31 @@ bool CommObd2Can::processFrameBytes() { dataRows[pConseqFrame->index].assign(pConseqFrame->pData, pConseqFrame->pData + framePayloadSize); rxRemaining -= framePayloadSize; - Serial.print("---Processing ConsecFrame payload: "); printHexBuffer(pConseqFrame->pData, framePayloadSize, true); + Serial.print("----Processing ConsecFrame payload: "); printHexBuffer(pConseqFrame->pData, framePayloadSize, true); } break; default: - Serial.print("Unknown frame type within CommObd2Can::processFrameBytes(): "); Serial.println(frameType); + Serial.print("Unknown frame type within CommObd2Can::processFrameBytes(): "); Serial.println((uint8_t)frameType); return false; break; - } + } // \switch (frameType) - - if (frameType == 0) - { - // single frame - process directly - buffer2string(liveData->responseRowMerged, mergedData.data(), mergedData.size()); - liveData->vResponseRowMerged.assign(mergedData.begin(), mergedData.end()); - processMergedResponse(); - } - else if (rxRemaining <= 0) - { + // Merge data if all data was received + if (rxRemaining <= 0) { // multiple frames and no data remaining - merge everything to single packet - //for(const auto& row : dataRows) - for (int i = 0; i < dataRows.size(); i++) - { - Serial.print("---merging packet index "); + for (int i = 0; i < dataRows.size(); i++) { + Serial.print("------merging packet index "); Serial.print(i); Serial.print(" with length "); Serial.println(dataRows[i].size()); mergedData.insert(mergedData.end(), dataRows[i].begin(), dataRows[i].end()); - } - buffer2string(liveData->responseRowMerged, mergedData.data(), mergedData.size()); - liveData->vResponseRowMerged.assign(mergedData.begin(), mergedData.end()); + buffer2string(liveData->responseRowMerged, mergedData.data(), mergedData.size()); // output for string parsing + liveData->vResponseRowMerged.assign(mergedData.begin(), mergedData.end()); // output for binary parsing processMergedResponse(); - } return true; diff --git a/CommObd2Can.h b/CommObd2Can.h index 7d896f8..7ec8b81 100644 --- a/CommObd2Can.h +++ b/CommObd2Can.h @@ -13,7 +13,7 @@ class CommObd2Can : public CommInterface { protected: const uint8_t pinCanInt = 15; const uint8_t pinCanCs = 12; - std::unique_ptr CAN; + std::unique_ptr CAN; long unsigned int rxId; unsigned char rxLen = 0; uint8_t rxBuf[32]; @@ -22,22 +22,32 @@ class CommObd2Can : public CommInterface { char msgString[128]; // Array to store serial string uint16_t lastPid; unsigned long lastDataSent = 0; - + std::vector mergedData; - typedef std::vector frameData_t; - std::unordered_map dataRows; - + std::unordered_map> dataRows; + + enum class enFrame_t + { + single = 0, + first = 1, + consecutive = 2, + unknown = 9 + }; + public: void connectDevice() override; void disconnectDevice() override; void scanDevices() override; void mainLoop() override; void executeCommand(String cmd) override; - // + + private: void sendPID(const uint16_t pid, const String& cmd); void sendFlowControlFrame(); uint8_t receivePID(); + enFrame_t getFrameType(const uint8_t firstByte); bool processFrameBytes(); bool processFrame(); void processMergedResponse(); + }; diff --git a/LiveData.h b/LiveData.h index 42a6dc9..0e8a64c 100644 --- a/LiveData.h +++ b/LiveData.h @@ -230,6 +230,7 @@ class LiveData { // Canbus uint8_t rxBuffOffset = 0; // offset of processing received data, in some cars needs to be set to 1, like in BMW i3 + uint8_t expectedMinimalPacketLength = 0; // what length of packet should be accepted. Set to 0 to accept any length // Params PARAMS_STRUC params; // Realtime sensor values