From c872e679c6c93afa404230b05156777019dad63d Mon Sep 17 00:00:00 2001 From: Roar Fredriksen Date: Mon, 18 Sep 2017 20:07:38 +0200 Subject: [PATCH] Crc16 and DlmsReader ported from C# project. Working condition --- Code/ESPDebugger/Crc16.cpp | 37 ++++++++ Code/ESPDebugger/Crc16.h | 23 +++++ Code/ESPDebugger/DlmsReader.cpp | 154 +++++++++++++++++++++++++++++++ Code/ESPDebugger/DlmsReader.h | 42 +++++++++ Code/ESPDebugger/ESPDebugger.ino | 14 ++- 5 files changed, 266 insertions(+), 4 deletions(-) create mode 100644 Code/ESPDebugger/Crc16.cpp create mode 100644 Code/ESPDebugger/Crc16.h create mode 100644 Code/ESPDebugger/DlmsReader.cpp create mode 100644 Code/ESPDebugger/DlmsReader.h diff --git a/Code/ESPDebugger/Crc16.cpp b/Code/ESPDebugger/Crc16.cpp new file mode 100644 index 00000000..bfb7141f --- /dev/null +++ b/Code/ESPDebugger/Crc16.cpp @@ -0,0 +1,37 @@ +#include "Crc16.h" + +Crc16Class::Crc16Class() +{ + unsigned short value; + unsigned short temp; + for (unsigned short i = 0; i < 256; ++i) + { + value = 0; + temp = i; + for (byte j = 0; j < 8; ++j) + { + if (((value ^ temp) & 0x0001) != 0) + { + value = (ushort)((value >> 1) ^ polynomial); + } + else + { + value >>= 1; + } + temp >>= 1; + } + table[i] = value; + } +} + +unsigned short Crc16Class::ComputeChecksum(byte *data, int start, int length) +{ + ushort fcs = 0xffff; + for (int i = start; i < (start + length); i++) + { + byte index = (fcs ^ data[i]) & 0xff; + fcs = (ushort)((fcs >> 8) ^ table[index]); + } + fcs ^= 0xffff; + return fcs; +} diff --git a/Code/ESPDebugger/Crc16.h b/Code/ESPDebugger/Crc16.h new file mode 100644 index 00000000..406c0af2 --- /dev/null +++ b/Code/ESPDebugger/Crc16.h @@ -0,0 +1,23 @@ +#ifndef _CRC16_h +#define _CRC16_h + +#if defined(ARDUINO) && ARDUINO >= 100 + #include "arduino.h" +#else + #include "WProgram.h" +#endif + +class Crc16Class +{ + public: + Crc16Class(); + unsigned short ComputeChecksum(byte *data, int start, int length); + protected: + private: + const unsigned short polynomial = 0x8408; + unsigned short table[256]; +}; + +#endif + + diff --git a/Code/ESPDebugger/DlmsReader.cpp b/Code/ESPDebugger/DlmsReader.cpp new file mode 100644 index 00000000..cce664b4 --- /dev/null +++ b/Code/ESPDebugger/DlmsReader.cpp @@ -0,0 +1,154 @@ +#include "DlmsReader.h" + +DlmsReader::DlmsReader() +{ + //this->Clear(); +} + +void DlmsReader::Clear() +{ + this->position = 0; + this->dataLength = 0; + this->destinationAddressLength = 0; + this->sourceAddressLength = 0; + this->frameFormatType = 0; +} + +bool DlmsReader::Read(byte data) +{ + if (position == 0 && data != 0x7E) + { + // we haven't started yet, wait for the start flag (no need to capture any data yet) + return false; + } + else + { + // We have completed reading of one package, so clear and be ready for the next + if (dataLength > 0 && position >= dataLength + 2) + Clear(); + + // Check if we're about to run into a buffer overflow + if (position >= DLMS_READER_BUFFER_SIZE) + Clear(); + + // Check if this is a second start flag, which indicates the previous one was a stop from the last package + if (position == 1 && data == 0x7E) + { + // just return, we can keep the one byte we had in the buffer + return false; + } + + // We have started, so capture every byte + buffer[position++] = data; + + if (position == 1) + { + // This was the start flag, we're not done yet + return false; + } + else if (position == 2) + { + // Capture the Frame Format Type + frameFormatType = (byte)(data & 0xF0); + if (!IsValidFrameFormat(frameFormatType)) + Clear(); + return false; + } + else if (position == 3) + { + // Capture the length of the data package + dataLength = ((buffer[1] & 0x0F) << 8) | buffer[2]; + return false; + } + else if (destinationAddressLength == 0) + { + // Capture the destination address + destinationAddressLength = GetAddress(3, destinationAddress, 0, DLMS_READER_MAX_ADDRESS_SIZE); + if (destinationAddressLength > 3) + Clear(); + return false; + } + else if (sourceAddressLength == 0) + { + // Capture the source address + sourceAddressLength = GetAddress(3 + destinationAddressLength, sourceAddress, 0, DLMS_READER_MAX_ADDRESS_SIZE); + if (sourceAddressLength > 3) + Clear(); + return false; + } + else if (position == 4 + destinationAddressLength + sourceAddressLength + 2) + { + // Verify the header checksum + ushort headerChecksum = GetChecksum(position - 3); + if (headerChecksum != Crc16.ComputeChecksum(buffer, 1, position - 3)) + Clear(); + return false; + } + else if (position == dataLength + 1) + { + // Verify the data package checksum + ushort checksum = this->GetChecksum(position - 3); + if (checksum != Crc16.ComputeChecksum(buffer, 1, position - 3)) + Clear(); + return false; + } + else if (position == dataLength + 2) + { + // We're done, check the stop flag and signal we're done + if (data == 0x7E) + return true; + else + { + Clear(); + return false; + } + } + } + return false; +} + +bool DlmsReader::IsValidFrameFormat(byte frameFormatType) +{ + return frameFormatType == 0xA0; +} + +int DlmsReader::GetRawData(byte *dataBuffer, int start, int length) +{ + if (dataLength > 0 && position == dataLength + 2) + { + int headerLength = 3 + destinationAddressLength + sourceAddressLength + 2; + int bytesWritten = 0; + for (int i = headerLength + 1; i < dataLength - 1; i++) + { + dataBuffer[i + start - headerLength - 1] = buffer[i]; + bytesWritten++; + } + return bytesWritten; + } + else + return 0; +} + +int DlmsReader::GetAddress(int addressPosition, byte* addressBuffer, int start, int length) +{ + int addressBufferPos = start; + for (int i = addressPosition; i < position; i++) + { + addressBuffer[addressBufferPos++] = buffer[i]; + + // LSB=1 means this was the last address byte + if ((buffer[i] & 0x01) == 0x01) + break; + + // See if we've reached last byte, try again when we've got more data + else if (i == position - 1) + return 0; + } + return addressBufferPos - start; +} + +ushort DlmsReader::GetChecksum(int checksumPosition) +{ + return (ushort)(buffer[checksumPosition + 2] << 8 | + buffer[checksumPosition + 1]); +} diff --git a/Code/ESPDebugger/DlmsReader.h b/Code/ESPDebugger/DlmsReader.h new file mode 100644 index 00000000..c457602c --- /dev/null +++ b/Code/ESPDebugger/DlmsReader.h @@ -0,0 +1,42 @@ +#ifndef _DLMSREADER_h +#define _DLMSREADER_h + +#include "Crc16.h" + +#if defined(ARDUINO) && ARDUINO >= 100 + #include "arduino.h" +#else + #include "WProgram.h" +#endif + +#define DLMS_READER_BUFFER_SIZE 256 +#define DLMS_READER_MAX_ADDRESS_SIZE 5 + +class DlmsReader +{ + public: + DlmsReader(); + bool Read(byte data); + int GetRawData(byte *buffer, int start, int length); + + protected: + Crc16Class Crc16; + + private: + byte buffer[DLMS_READER_BUFFER_SIZE]; + int position; + int dataLength; + byte frameFormatType; + byte destinationAddress[DLMS_READER_MAX_ADDRESS_SIZE]; + byte destinationAddressLength; + byte sourceAddress[DLMS_READER_MAX_ADDRESS_SIZE]; + byte sourceAddressLength; + + void Clear(); + int GetAddress(int addressPosition, byte* buffer, int start, int length); + unsigned short GetChecksum(int checksumPosition); + bool IsValidFrameFormat(byte frameFormatType); + void WriteBuffer(); +}; + +#endif diff --git a/Code/ESPDebugger/ESPDebugger.ino b/Code/ESPDebugger/ESPDebugger.ino index 5ff1b0c7..62e2d24f 100644 --- a/Code/ESPDebugger/ESPDebugger.ino +++ b/Code/ESPDebugger/ESPDebugger.ino @@ -12,7 +12,10 @@ * Created 14. september 2017 by Roar Fredriksen */ +#include "DlmsReader.h" +#include "Crc16.h" +DlmsReader reader; byte buffer[512]; int bytesRead; @@ -37,11 +40,14 @@ void loop() { if (Serial.available()) { byte newByte = Serial.read(); - buffer[bytesRead++] = newByte; - bool completed = bytesRead > 1 && newByte == 0x7E; - - if (completed) + //if (newByte < 0x10) Serial1.print("0"); + //Serial1.print(newByte, HEX); + + if (reader.Read(newByte)) + { + bytesRead = reader.GetRawData(buffer, 0, 512); writeAndEmptyBuffer(); + } } }