From 5bec1cfd3f9c78caa1ad7aa8ac068474aa7e34e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=A1n=20M=C3=A1tik?= Date: Tue, 22 Dec 2020 19:57:47 +0100 Subject: [PATCH] Processing data in byte form --- CarBmwI3.cpp | 123 +++++++++++++++++++++--- CommObd2Can.cpp | 241 +++++++++++++++++++++++++++++++++++++++++++++--- CommObd2Can.h | 10 +- LiveData.h | 2 + 4 files changed, 353 insertions(+), 23 deletions(-) diff --git a/CarBmwI3.cpp b/CarBmwI3.cpp index 5270743..55ea946 100644 --- a/CarBmwI3.cpp +++ b/CarBmwI3.cpp @@ -31,8 +31,10 @@ void CarBmwI3::activateCommandQueue() { // Loop from (BMW i3) // BMS "ATSH6F1", - + + "22DDC0", // TEMPERATUREN "22DD69", // HV_STORM + "22DD6C", // KUEHLKREISLAUF_TEMP "22DDB4", // HV_SPANNUNG "22DDBC" // SOC @@ -67,24 +69,123 @@ void CarBmwI3::parseRowMerged() Serial.print("--currentAtshRequest: "); Serial.println(liveData->currentAtshRequest); Serial.print("--commandRequest: "); Serial.println(liveData->commandRequest); Serial.print("--mergedLength: "); Serial.println(liveData->responseRowMerged.length()); + Serial.print("--mergedVectorLength: "); Serial.println(liveData->vResponseRowMerged.size()); if (liveData->responseRowMerged.length() <= 6) { Serial.println("--too short data, skiping processing"); } + + struct Header_t + { + uint8_t startChar; + uint8_t pid[2]; + uint8_t pData[]; + + uint16_t getPid() { return 256 * pid[0] + pid[1]; }; + }; + + Header_t* pHeader = (Header_t*)liveData->vResponseRowMerged.data(); + uint8_t* pPayload = pHeader->pData; + const uint16_t payloadLength = liveData->vResponseRowMerged.size() - sizeof(Header_t); + Serial.print("--extracted PID: "); Serial.println(pHeader->getPid()); + Serial.print("--payload length: "); Serial.println(payloadLength); + // BMS if (liveData->currentAtshRequest.equals("ATSH6F1")) { - if (liveData->commandRequest.equals("22DD69")) { - liveData->params.batPowerAmp = - liveData->hexToDecFromResponse(6, 14, 4, true) / 100.0; - } - - if (liveData->commandRequest.equals("22DDB4")) { // HV_SPANNUNG_BATTERIE - liveData->params.batVoltage = liveData->hexToDecFromResponse(6, 10, 2, false) / 100.0; - } - if (liveData->commandRequest.equals("22DDBC")) { - liveData->params.socPerc = liveData->hexToDecFromResponse(6, 10, 2, false) / 10.0; + switch (pHeader->getPid()) { + 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]; } + }; + + if (payloadLength == sizeof(DD69_t)) { + DD69_t* ptr = (DD69_t*)pHeader->pData; + + liveData->params.batPowerAmp = ptr->getBatAmpRaw() / 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 + liveData->params.chargingStartTime = liveData->params.currentTime; + } } - } + break; + + case 0xDD6C: + { + + } + break; + + case 0xDDB4: + { + struct DDB4_t { + uint8_t batVoltage[2]; + uint16_t getBatVoltage() { return 0x100 * batVoltage[0] + batVoltage[1]; }; + }; + + if (payloadLength == sizeof(DDB4_t)) { // HV_SPANNUNG_BATTERIE + DDB4_t* ptr = (DDB4_t*)pHeader->pData; + + liveData->params.batVoltage = ptr->getBatVoltage() / 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; + } + } + break; + + 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]; }; + }; + + if (payloadLength == sizeof(DDC0_t)) { + DDC0_t* ptr = (DDC0_t*)pHeader->pData; + + liveData->params.batMinC = ptr->getTempMin() / 100.0; + liveData->params.batTempC = ptr->getTempAvg() / 100.0; + liveData->params.batMaxC = ptr->getTempMax() / 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); + } + } + break; + + 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]; }; + }; + + if (payloadLength == sizeof(DDBC_t)) { + DDBC_t* ptr = (DDBC_t*)pHeader->pData; + + liveData->params.socPerc = ptr->getSoc() / 10.0; + } + } + break; + + + } // switch + + } // ATSH6F1 } diff --git a/CommObd2Can.cpp b/CommObd2Can.cpp index c6bf9b1..d5d635b 100644 --- a/CommObd2Can.cpp +++ b/CommObd2Can.cpp @@ -3,6 +3,8 @@ #include "LiveData.h" #include +//#include + /** Connect CAN adapter */ @@ -28,6 +30,15 @@ void CommObd2Can::connectDevice() { return; } + if (liveData->settings.carType == CAR_BMW_I3_2014) { + //initialise mask and filter to allow only receipt of 0x7xx CAN IDs + CAN->init_Mask(0, 0, 0x07000000); // Init first mask... + CAN->init_Mask(1, 0, 0x07000000); // Init second mask... + for (uint8_t i = 0; i < 6; ++i) { + CAN->init_Filt(i, 0, 0x06000000); //Init filters + } + } + if (MCP2515_OK != CAN->setMode(MCP_NORMAL)) { // Set operation mode to normal so the MCP2515 sends acks to received data. Serial.println("Error: CAN->setMode(MCP_NORMAL) failed"); board->displayMessage(" > CAN init failed", ""); @@ -126,13 +137,38 @@ void CommObd2Can::sendPID(const uint16_t pid, const String& cmd) { uint8_t txBuf[8] = { 0 }; // init with zeroes String tmpStr; - txBuf[0] = cmd.length() / 2; - - for (uint8_t i = 0; i < 7; i++) { - tmpStr = cmd; - tmpStr = tmpStr.substring(i * 2, ((i + 1) * 2)); - if (tmpStr != "") { - txBuf[i + 1] = liveData->hexToDec(tmpStr, 1, false); + if (liveData->settings.carType == CAR_BMW_I3_2014) + { + struct Packet_t + { + uint8_t startChar; + uint8_t length; + uint8_t data[6]; + }; + + Packet_t* pPacket = (Packet_t*)txBuf; + + pPacket->startChar = 0x07; + pPacket->length = cmd.length() / 2; + + for (uint8_t i = 0; i < sizeof(pPacket->data); i++) { + tmpStr = cmd; + tmpStr = tmpStr.substring(i * 2, ((i + 1) * 2)); + if (tmpStr != "") { + pPacket->data[i] = liveData->hexToDec(tmpStr, 1, false); + } + } + } + else + { + txBuf[0] = cmd.length() / 2; + + for (uint8_t i = 0; i < 7; i++) { + tmpStr = cmd; + tmpStr = tmpStr.substring(i * 2, ((i + 1) * 2)); + if (tmpStr != "") { + txBuf[i + 1] = liveData->hexToDec(tmpStr, 1, false); + } } } @@ -159,6 +195,13 @@ void CommObd2Can::sendPID(const uint16_t pid, const String& cmd) { void CommObd2Can::sendFlowControlFrame() { uint8_t txBuf[8] = { 0x30, requestFramesCount /*request count*/, 14 /*ms between frames*/ , 0, 0, 0, 0, 0 }; + + // insert 0x07 into beginning for BMW i3 + if (liveData->settings.carType == CAR_BMW_I3_2014) { + memmove(txBuf + 1, txBuf, 7); + txBuf[0] = 0x07; + } + const uint8_t sndStat = CAN->sendMsgBuf(lastPid, 0, 8, txBuf); // 11 bit if (sndStat == CAN_OK) { Serial.print("Flow control frame sent "); @@ -173,17 +216,46 @@ 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 */ uint8_t CommObd2Can::receivePID() { - + if (!digitalRead(pinCanInt)) // If CAN0_INT pin is low, read receive buffer { lastDataSent = millis(); Serial.print(" CAN READ "); CAN->readMsgBuf(&rxId, &rxLen, rxBuf); // Read data: len = data length, buf = data byte(s) - +// mockupReceiveCanBuf(&rxId, &rxLen, rxBuf); + if ((rxId & 0x80000000) == 0x80000000) // Determine if ID is standard (11 bits) or extended (29 bits) sprintf(msgString, "Extended ID: 0x%.8lX DLC: %1d Data:", (rxId & 0x1FFFFFFF), rxLen); else @@ -201,14 +273,161 @@ uint8_t CommObd2Can::receivePID() { } } + if (rxLen == 5) { + Serial.println(" [Ignoring 5 bytes long packet]"); + return 0xff; + } + Serial.println(); - processFrame(); + processFrameBytes(); + //processFrame(); } else { //Serial.println(" CAN NOT READ "); return 0xff; } - return rxBuf[0]; + return rxBuf[0 + liveData->rxBuffOffset]; +} + +static void printHexBuffer(uint8_t* pData, const uint16_t length, const bool bAddNewLine) +{ + char str[8] = { 0 }; + + for (uint8_t i = 0; i < length; i++) { + sprintf(str, " 0x%.2X", pData[i]); + Serial.print(str); + } + + if (bAddNewLine) { + Serial.println(); + } +} + +static void buffer2string(String& out_targetString, uint8_t* in_pBuffer, const uint16_t in_length) +{ + char str[8] = { 0 }; + + for (uint16_t i = 0; i < in_length; i++) { + sprintf(str, "%.2X", in_pBuffer[i]); + out_targetString += str; + } + + +} + +/** + Process can frame on byte level + */ +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 uint8_t frameLenght = rxLen - liveData->rxBuffOffset; + + switch (frameType) { + case 0: // Single frame + { + struct SingleFrame_t + { + uint8_t size : 4; + uint8_t frameType : 4; + uint8_t pData[]; + }; + + SingleFrame_t* pSingleFrame = (SingleFrame_t*)pDataStart; + mergedData.assign(pSingleFrame->pData, pSingleFrame->pData + pSingleFrame->size); + + rxRemaining = 0; + + Serial.print("---Processing SingleFrame payload: "); printHexBuffer(pSingleFrame->pData, pSingleFrame->size, true); + } + break; + + case 1: // First frame + { + struct FirstFrame_t + { + uint8_t sizeMSB : 4; + uint8_t frameType : 4; + uint8_t sizeLSB : 8; + uint8_t pData[]; + + uint16_t lengthOfFullPacket() { return (256 * sizeMSB) + sizeLSB; } + + }; + + FirstFrame_t* pFirstFrame = (FirstFrame_t*)pDataStart; + + rxRemaining = pFirstFrame->lengthOfFullPacket(); // length of complete data + + mergedData.clear(); + dataRows.clear(); + + const uint8_t framePayloadSize = frameLenght - sizeof(FirstFrame_t); // remove one byte of header + dataRows[0].assign(pFirstFrame->pData, pFirstFrame->pData + framePayloadSize); + rxRemaining -= framePayloadSize; + + Serial.print("---Processing FirstFrame payload: "); printHexBuffer(pFirstFrame->pData, framePayloadSize, true); + } + break; + + case 2: // Consecutive frame + { + struct ConsecutiveFrame_t + { + uint8_t index : 4; + uint8_t frameType : 4; + uint8_t pData[]; + }; + + const uint8_t structSize = sizeof(ConsecutiveFrame_t); + Serial.print("[debug] sizeof(ConsecutiveFrame_t) is expected to be 1 and it's "); Serial.println(structSize); + + ConsecutiveFrame_t* pConseqFrame = (ConsecutiveFrame_t*)pDataStart; + const uint8_t framePayloadSize = frameLenght - sizeof(ConsecutiveFrame_t); // remove one byte of header + dataRows[pConseqFrame->index].assign(pConseqFrame->pData, pConseqFrame->pData + framePayloadSize); + rxRemaining -= framePayloadSize; + + Serial.print("---Processing ConsecFrame payload: "); printHexBuffer(pConseqFrame->pData, framePayloadSize, true); + } + break; + + default: + Serial.print("Unknown frame type within CommObd2Can::processFrameBytes(): "); Serial.println(frameType); + return false; + break; + } + + + 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) + { + // 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 "); + 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()); + processMergedResponse(); + + } + + return true; } /** diff --git a/CommObd2Can.h b/CommObd2Can.h index cbb352a..7d896f8 100644 --- a/CommObd2Can.h +++ b/CommObd2Can.h @@ -5,13 +5,15 @@ #include #include +#include +#include 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]; @@ -20,6 +22,11 @@ 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; + public: void connectDevice() override; void disconnectDevice() override; @@ -30,6 +37,7 @@ class CommObd2Can : public CommInterface { void sendPID(const uint16_t pid, const String& cmd); void sendFlowControlFrame(); uint8_t receivePID(); + bool processFrameBytes(); bool processFrame(); void processMergedResponse(); }; diff --git a/LiveData.h b/LiveData.h index 456b17f..42a6dc9 100644 --- a/LiveData.h +++ b/LiveData.h @@ -7,6 +7,7 @@ #include #include #include "config.h" +#include // SUPPORTED CARS #define CAR_KIA_ENIRO_2020_64 0 @@ -203,6 +204,7 @@ class LiveData { String commandQueue[300]; String responseRow; String responseRowMerged; + std::vector vResponseRowMerged; uint16_t commandQueueIndex; bool canSendNextAtCommand = false; String commandRequest = "";