projects/evDash
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Processing data in byte form

Browse Source
This commit is contained in:
Ján Mátik
2020-12-22 19:57:47 +01:00
parent c3421c1b91
commit 5bec1cfd3f
4 changed files with 353 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
CarBmwI3.cpp
View File

@@ -32,7 +32,9 @@ void CarBmwI3::activateCommandQueue() {
// 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;
}
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 (liveData->commandRequest.equals("22DDBC")) {
liveData->params.socPerc = liveData->hexToDecFromResponse(6, 10, 2, false) / 10.0;
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
}

223
CommObd2Can.cpp
View File

@@ -3,6 +3,8 @@
#include "LiveData.h"
#include <mcp_CAN.h>
//#include <string.h>
/**
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,6 +137,30 @@ void CommObd2Can::sendPID(const uint16_t pid, const String& cmd) {
uint8_t txBuf[8] = { 0 }; // init with zeroes
String tmpStr;
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++) {
@@ -135,6 +170,7 @@ void CommObd2Can::sendPID(const uint16_t pid, const String& cmd) {
txBuf[i + 1] = liveData->hexToDec(tmpStr, 1, false);
}
}
}
lastPid = pid;
const uint8_t sndStat = CAN->sendMsgBuf(pid, 0, 8, txBuf); // 11 bit
@@ -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,6 +216,34 @@ void CommObd2Can::sendFlowControlFrame() {
Serial.println("");
}
//static void mockupReceiveCanBuf(INT32U *id, INT8U *len, INT8U buf[])
//{
// static uint8_t counter = 0;
//
// std::unordered_map<uint8_t, std::vector<uint8_t>> 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
*/
@@ -183,6 +254,7 @@ uint8_t CommObd2Can::receivePID() {
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);
@@ -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;
}
/**

8
CommObd2Can.h
View File

@@ -5,6 +5,8 @@
#include <mcp_can.h>
#include <memory>
#include <vector>
#include <unordered_map>
class CommObd2Can : public CommInterface {
@@ -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<uint8_t> mergedData;
typedef std::vector<uint8_t> frameData_t;
std::unordered_map<uint16_t, frameData_t> 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();
};

2
LiveData.h
View File

@@ -7,6 +7,7 @@
#include <sys/time.h>
#include <BLEDevice.h>
#include "config.h"
#include <vector>
// SUPPORTED CARS
#define CAR_KIA_ENIRO_2020_64 0
@@ -203,6 +204,7 @@ class LiveData {
String commandQueue[300];
String responseRow;
String responseRowMerged;
std::vector<uint8_t> vResponseRowMerged;
uint16_t commandQueueIndex;
bool canSendNextAtCommand = false;
String commandRequest = "";