mirror of
https://github.com/dalathegreat/Battery-Emulator.git
synced 2025-10-03 17:59:27 +02:00
554 lines
15 KiB
C++
554 lines
15 KiB
C++
// =================================================================================================
|
|
// eModbus: Copyright 2020, 2021 by Michael Harwerth, Bert Melis and the contributors to eModbus
|
|
// MIT license - see license.md for details
|
|
// =================================================================================================
|
|
|
|
#include "CoilData.h"
|
|
#undef LOCAL_LOG_LEVEL
|
|
#include "Logging.h"
|
|
|
|
// Constructor: optional size in bits, optional initial value for all bits
|
|
// Maximum size is 2000 coils (=250 bytes)
|
|
CoilData::CoilData(uint16_t size, bool initValue) :
|
|
CDsize(0),
|
|
CDbyteSize(0),
|
|
CDbuffer(nullptr) {
|
|
// Limit the size to 2000 (Modbus rules)
|
|
if (size > 2000) size = 2000;
|
|
// Do we have a size?
|
|
if (size) {
|
|
// Calculate number of bytes needed
|
|
CDbyteSize = byteIndex(size - 1) + 1;
|
|
// Allocate and init buffer
|
|
CDbuffer = new uint8_t[CDbyteSize];
|
|
memset(CDbuffer, initValue ? 0xFF : 0, CDbyteSize);
|
|
if (initValue) {
|
|
CDbuffer[CDbyteSize - 1] &= CDfilter[bitIndex(size - 1)];
|
|
}
|
|
CDsize = size;
|
|
}
|
|
}
|
|
|
|
// Alternate constructor, taking a "1101..." bit image char array to init
|
|
CoilData::CoilData(const char *initVector) :
|
|
CDsize(0),
|
|
CDbyteSize(0),
|
|
CDbuffer(nullptr) {
|
|
// Init with bit image array.
|
|
setVector(initVector);
|
|
}
|
|
|
|
// Destructor: take care of cleaning up
|
|
CoilData::~CoilData() {
|
|
if (CDbuffer) {
|
|
delete CDbuffer;
|
|
}
|
|
}
|
|
|
|
// Assignment operator
|
|
CoilData& CoilData::operator=(const CoilData& m) {
|
|
// Remove old data
|
|
if (CDbuffer) {
|
|
delete CDbuffer;
|
|
}
|
|
// Are coils in source?
|
|
if (m.CDsize > 0) {
|
|
// Yes. Allocate new buffer and copy data
|
|
CDbuffer = new uint8_t[m.CDbyteSize];
|
|
memcpy(CDbuffer, m.CDbuffer, m.CDbyteSize);
|
|
CDsize = m.CDsize;
|
|
CDbyteSize = m.CDbyteSize;
|
|
} else {
|
|
// No, leave buffer empty
|
|
CDsize = 0;
|
|
CDbyteSize = 0;
|
|
CDbuffer = nullptr;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
// Copy constructor
|
|
CoilData::CoilData(const CoilData& m) :
|
|
CDsize(0),
|
|
CDbyteSize(0),
|
|
CDbuffer(nullptr) {
|
|
// Has the source coils at all?
|
|
if (m.CDsize > 0) {
|
|
// Yes. Allocate new buffer and copy data
|
|
CDbuffer = new uint8_t[m.CDbyteSize];
|
|
memcpy(CDbuffer, m.CDbuffer, m.CDbyteSize);
|
|
CDsize = m.CDsize;
|
|
CDbyteSize = m.CDbyteSize;
|
|
}
|
|
}
|
|
|
|
#ifndef NO_MOVE
|
|
// Move constructor
|
|
CoilData::CoilData(CoilData&& m) {
|
|
// Copy all data
|
|
CDbuffer = m.CDbuffer;
|
|
CDsize = m.CDsize;
|
|
CDbyteSize = m.CDbyteSize;
|
|
// Then clear source
|
|
m.CDbuffer = nullptr;
|
|
m.CDsize = 0;
|
|
m.CDbyteSize = 0;
|
|
}
|
|
|
|
// Move assignment
|
|
CoilData& CoilData::operator=(CoilData&& m) {
|
|
// Remove buffer, if already allocated
|
|
if (CDbuffer) {
|
|
delete CDbuffer;
|
|
}
|
|
// Are there coils in the source at all?
|
|
if (m.CDsize > 0) {
|
|
// Yes. Copy over all data
|
|
CDbuffer = m.CDbuffer;
|
|
CDsize = m.CDsize;
|
|
CDbyteSize = m.CDbyteSize;
|
|
// Then clear source
|
|
m.CDbuffer = nullptr;
|
|
m.CDsize = 0;
|
|
m.CDbyteSize = 0;
|
|
} else {
|
|
// No, leave object empty.
|
|
CDbuffer = nullptr;
|
|
CDsize = 0;
|
|
CDbyteSize = 0;
|
|
}
|
|
return *this;
|
|
}
|
|
#endif
|
|
|
|
// Comparison operators
|
|
bool CoilData::operator==(const CoilData& m) {
|
|
// Self-compare is always true
|
|
if (this == &m) return true;
|
|
// Different sizes are never equal
|
|
if (CDsize != m.CDsize) return false;
|
|
// Compare the data
|
|
if (CDsize > 0 && memcmp(CDbuffer, m.CDbuffer, CDbyteSize)) return false;
|
|
return true;
|
|
}
|
|
|
|
// Inequality: invert the result of the equality comparison
|
|
bool CoilData::operator!=(const CoilData& m) {
|
|
return !(*this == m);
|
|
}
|
|
|
|
// Assignment of a bit image char array to re-init
|
|
CoilData& CoilData::operator=(const char *initVector) {
|
|
// setVector() may be unsuccessful - then data is deleted!
|
|
setVector(initVector);
|
|
return *this;
|
|
}
|
|
|
|
// If used as vector<uint8_t>, return a complete slice
|
|
CoilData::operator vector<uint8_t> const () {
|
|
// Create new vector to return
|
|
vector<uint8_t> retval;
|
|
if (CDsize > 0) {
|
|
// Copy over all buffer content
|
|
retval.assign(CDbuffer, CDbuffer + CDbyteSize);
|
|
}
|
|
// return the copy (or an empty vector)
|
|
return retval;
|
|
}
|
|
|
|
// slice: return a CoilData object with coils shifted leftmost
|
|
// will return empty object if illegal parameters are detected
|
|
CoilData CoilData::slice(uint16_t start, uint16_t length) {
|
|
CoilData retval;
|
|
|
|
// Any slice of an empty coilset is an empty coilset ;)
|
|
if (CDsize == 0) return retval;
|
|
|
|
// If start is beyond the available coils, return empty slice
|
|
if (start > CDsize) return retval;
|
|
|
|
// length default is all up to the end
|
|
if (length == 0) length = CDsize - start;
|
|
|
|
// Does the requested slice fit in the buffer?
|
|
if ((start + length) <= CDsize) {
|
|
// Yes, it does. Extend return object
|
|
retval = CoilData(length);
|
|
|
|
// Loop over all requested bits
|
|
for (uint16_t i = start; i < start + length; ++i) {
|
|
if (CDbuffer[byteIndex(i)] & (1 << bitIndex(i))) {
|
|
retval.set(i - start, true);
|
|
}
|
|
}
|
|
}
|
|
return retval;
|
|
}
|
|
|
|
// operator[]: return value of a single coil
|
|
bool CoilData::operator[](uint16_t index) const {
|
|
if (index < CDsize) {
|
|
return (CDbuffer[byteIndex(index)] & (1 << bitIndex(index))) ? true : false;
|
|
}
|
|
// Wrong parameter -> always return false
|
|
return false;
|
|
}
|
|
|
|
// set functions to change coil value(s)
|
|
// Will return true if done, false if impossible (wrong address or data)
|
|
|
|
// set #1: alter one single coil
|
|
bool CoilData::set(uint16_t index, bool value) {
|
|
// Within coils?
|
|
if (index < CDsize) {
|
|
// Yes. Determine affected byte and bit therein
|
|
uint16_t by = byteIndex(index);
|
|
uint8_t mask = 1 << bitIndex(index);
|
|
|
|
// Stamp out bit
|
|
CDbuffer[by] &= ~mask;
|
|
// If required, set it to 1 now
|
|
if (value) {
|
|
CDbuffer[by] |= mask;
|
|
}
|
|
return true;
|
|
}
|
|
// Wrong parameter -> always return false
|
|
return false;
|
|
}
|
|
|
|
// set #2: alter a group of coils, overwriting it by the bits from vector newValue
|
|
bool CoilData::set(uint16_t start, uint16_t length, vector<uint8_t> newValue) {
|
|
// Does the vector contain enough data for the specified size?
|
|
if (newValue.size() >= (size_t)(byteIndex(length - 1) + 1)) {
|
|
// Yes, we safely may call set #3 with it
|
|
return set(start, length, newValue.data());
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// set #3: alter a group of coils, overwriting it by the bits from uint8_t buffer newValue
|
|
// **** Watch out! ****
|
|
// This may be a potential risk if newValue is pointing to an array shorter than required.
|
|
// Then heap data behind the array may be used to set coils!
|
|
bool CoilData::set(uint16_t start, uint16_t length, uint8_t *newValue) {
|
|
// Does the requested slice fit in the buffer?
|
|
if (length && (start + length) <= CDsize) {
|
|
// Yes, it does.
|
|
// Prepare pointers to the source byte and the bit within
|
|
uint8_t *cp = newValue;
|
|
uint8_t bitPtr = 0;
|
|
|
|
// Loop over all bits to be set
|
|
for (uint16_t i = start; i < start + length; i++) {
|
|
// Get affected byte
|
|
uint8_t by = byteIndex(i);
|
|
// Calculate single-bit mask in target byte
|
|
uint8_t mask = 1 << bitIndex(i);
|
|
// Stamp out bit
|
|
CDbuffer[by] &= ~mask;
|
|
// is source bit set?
|
|
if (*cp & (1 << bitPtr)) {
|
|
// Yes. Set it in target as well
|
|
CDbuffer[by] |= mask;
|
|
}
|
|
// Advance source bit ptr
|
|
bitPtr++;
|
|
// Overflow?
|
|
if (bitPtr >= 8) {
|
|
// Yes. move pointers to first bit in next source byte
|
|
bitPtr = 0;
|
|
cp++;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// set #4: alter a group of coils, overwriting it by the coils in another CoilData object
|
|
// Setting stops when either target storage or source coils are exhausted
|
|
bool CoilData::set(uint16_t index, const CoilData& c) {
|
|
// if source object is empty, return false
|
|
if (c.empty()) return false;
|
|
|
|
// If target is empty, or index is beyond coils, return false
|
|
if (CDsize == 0 || index >= CDsize) return false;
|
|
|
|
// Take the minimum of remaining coils after index and the length of c
|
|
uint16_t length = CDsize - index;
|
|
if (c.coils() < length) length = c.coils();
|
|
|
|
// Loop over all coils to be copied
|
|
for (uint16_t i = index; i < index + length; ++i) {
|
|
set(i, c[i - index]);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// set #5: alter a group of coils, overwriting it by a bit image array
|
|
// Setting stops when either target storage or source bits are exhausted
|
|
bool CoilData::set(uint16_t index, const char *initVector) {
|
|
// if target is empty or index is beyond coils, return false
|
|
if (CDsize == 0 || index >= CDsize) return false;
|
|
|
|
// We do a single pass on the bit image array, until it ends or the target is exhausted
|
|
const char *cp = initVector; // pointer to source array
|
|
bool skipFlag = false; // Signal next character irrelevant
|
|
|
|
while (*cp && index < CDsize) {
|
|
switch (*cp) {
|
|
case '1': // A valid 1 bit
|
|
case '0': // A valid 0 bit
|
|
// Shall we ignore it?
|
|
if (skipFlag) {
|
|
// Yes. just reset the ignore flag
|
|
skipFlag = false;
|
|
} else {
|
|
// No, we can set it. First stamp out the existing bit
|
|
CDbuffer[byteIndex(index)] &= ~(1 << bitIndex(index));
|
|
// Do we have a 1 bit here?
|
|
if (*cp == '1') {
|
|
// Yes. set it in coil storage
|
|
CDbuffer[byteIndex(index)] |= (1 << bitIndex(index));
|
|
}
|
|
index++;
|
|
}
|
|
break;
|
|
case '_': // Skip next
|
|
skipFlag = true;
|
|
break;
|
|
default: // anything else
|
|
skipFlag = false;
|
|
break;
|
|
}
|
|
cp++;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// Comparison against bit image array
|
|
bool CoilData::operator==(const char *initVector) {
|
|
const char *cp = initVector; // pointer to source array
|
|
bool skipFlag = false; // Signal next character irrelevant
|
|
uint16_t index = 0;
|
|
|
|
// We do a single pass on the bit image array, until it ends or the target is exhausted
|
|
while (*cp && index < CDsize) {
|
|
switch (*cp) {
|
|
case '1': // A valid 1 bit
|
|
case '0': // A valid 0 bit
|
|
// Shall we ignore it?
|
|
if (skipFlag) {
|
|
// Yes. just reset the ignore flag
|
|
skipFlag = false;
|
|
} else {
|
|
// No, we can compare it
|
|
uint8_t value = CDbuffer[byteIndex(index)] & (1 << bitIndex(index));
|
|
// Do we have a 1 bit here?
|
|
if (*cp == '1') {
|
|
// Yes. Is the source different? Then we can stop
|
|
if (value == 0) return false;
|
|
} else {
|
|
// No, it is a 0. Different?
|
|
if (value) return false;
|
|
}
|
|
index++;
|
|
}
|
|
break;
|
|
case '_': // Skip next
|
|
skipFlag = true;
|
|
break;
|
|
default: // anything else
|
|
skipFlag = false;
|
|
break;
|
|
}
|
|
cp++;
|
|
}
|
|
// So far everything was equal, but we may have more bits in the image array!
|
|
if (*cp) {
|
|
// There is more. Check for more valid bits
|
|
while (*cp) {
|
|
switch (*cp) {
|
|
case '1': // A valid 1 bit
|
|
case '0': // A valid 0 bit
|
|
// Shall we ignore it?
|
|
if (skipFlag) {
|
|
// Yes. just reset the ignore flag
|
|
skipFlag = false;
|
|
} else {
|
|
// No, a valid bit that exceeds the target coils count
|
|
return false;
|
|
}
|
|
break;
|
|
case '_': // Skip next
|
|
skipFlag = true;
|
|
break;
|
|
default: // anything else
|
|
skipFlag = false;
|
|
break;
|
|
}
|
|
cp++;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool CoilData::operator!=(const char *initVector) {
|
|
return !(*this == initVector);
|
|
}
|
|
|
|
// Init all coils by a readable bit image array
|
|
bool CoilData::setVector(const char *initVector) {
|
|
uint16_t length = 0; // resulting bit pattern length
|
|
const char *cp = initVector; // pointer to source array
|
|
bool skipFlag = false; // Signal next character irrelevant
|
|
|
|
// Do a first pass to count all valid bits in array
|
|
while (*cp) {
|
|
switch (*cp) {
|
|
case '1': // A valid 1 bit
|
|
case '0': // A valid 0 bit
|
|
// Shall we ignore it?
|
|
if (skipFlag) {
|
|
// Yes. just reset the ignore flag
|
|
skipFlag = false;
|
|
} else {
|
|
// No, we can count it
|
|
length++;
|
|
}
|
|
break;
|
|
case '_': // Skip next
|
|
skipFlag = true;
|
|
break;
|
|
default: // anything else
|
|
skipFlag = false;
|
|
break;
|
|
}
|
|
cp++;
|
|
}
|
|
|
|
// If there are coils already, trash them.
|
|
if (CDbuffer) {
|
|
delete CDbuffer;
|
|
}
|
|
CDsize = 0;
|
|
CDbyteSize = 0;
|
|
|
|
// Did we count a manageable number?
|
|
if (length && length <= 2000) {
|
|
// Yes. Init the coils
|
|
CDsize = length;
|
|
CDbyteSize = byteIndex(length - 1) + 1;
|
|
// Allocate new coil storage
|
|
CDbuffer = new uint8_t[CDbyteSize];
|
|
memset(CDbuffer, 0, CDbyteSize);
|
|
|
|
// Prepare second loop
|
|
uint16_t ptr = 0; // bit pointer in coil storage
|
|
skipFlag = false;
|
|
cp = initVector;
|
|
|
|
// Do a second pass, converting 1 and 0 into coils (bits)
|
|
// loop as above, only difference is setting the bits
|
|
while (*cp) {
|
|
switch (*cp) {
|
|
case '1':
|
|
case '0':
|
|
if (skipFlag) {
|
|
skipFlag = false;
|
|
} else {
|
|
// Do we have a 1 bit here?
|
|
if (*cp == '1') {
|
|
// Yes. set it in coil storage
|
|
CDbuffer[byteIndex(ptr)] |= (1 << bitIndex(ptr));
|
|
}
|
|
// advance bit pointer in any case 0 or 1
|
|
ptr++;
|
|
}
|
|
break;
|
|
case '_':
|
|
skipFlag = true;
|
|
break;
|
|
default:
|
|
skipFlag = false;
|
|
break;
|
|
}
|
|
cp++;
|
|
}
|
|
// We had content, so return true
|
|
return true;
|
|
}
|
|
// No valid bits found, return false
|
|
return false;
|
|
}
|
|
|
|
// init: set all coils to 1 or 0 (default)
|
|
void CoilData::init(bool value) {
|
|
if (CDsize > 0) {
|
|
memset(CDbuffer, value ? 0xFF : 0, CDbyteSize);
|
|
// Stamp out overhang bits
|
|
CDbuffer[CDbyteSize - 1] &= CDfilter[bitIndex(CDsize - 1)];
|
|
}
|
|
}
|
|
|
|
// Return number of coils set to 1 (or not)
|
|
// Uses Brian Kernighan's algorithm!
|
|
uint16_t CoilData::coilsSetON() const {
|
|
uint16_t count = 0;
|
|
|
|
// Do we have coils at all?
|
|
if (CDbyteSize) {
|
|
// Yes. Loop over all bytes summing up the '1' bits
|
|
for (uint8_t i = 0; i < CDbyteSize; ++i) {
|
|
uint8_t by = CDbuffer[i];
|
|
while (by) {
|
|
by &= by - 1; // this clears the LSB-most set bit
|
|
count++;
|
|
}
|
|
}
|
|
}
|
|
return count;
|
|
}
|
|
|
|
uint16_t CoilData::coilsSetOFF() const {
|
|
return CDsize - coilsSetON();
|
|
}
|
|
|
|
#if !IS_LINUX
|
|
// Not for Linux for the Print reference!
|
|
|
|
// Print out a coil storage in readable form to ease debugging
|
|
void CoilData::print(const char *label, Print& s) {
|
|
uint8_t bitptr = 0;
|
|
uint8_t labellen = strlen(label);
|
|
uint8_t pos = labellen;
|
|
|
|
// Put out the label
|
|
s.print(label);
|
|
|
|
// Print out all coils as "1" or "0"
|
|
for (uint16_t i = 0; i < CDsize; ++i) {
|
|
s.print((CDbuffer[byteIndex(i)] & (1 << bitptr)) ? "1" : "0");
|
|
pos++;
|
|
// Have a blank after every group of 4
|
|
if (i % 4 == 3) {
|
|
// Have a line break if > 80 characters, including the last group of 4
|
|
if (pos >= 80) {
|
|
s.println("");
|
|
pos = 0;
|
|
// Leave a nice empty space below the label
|
|
while (pos++ < labellen) {
|
|
s.print(" ");
|
|
}
|
|
} else {
|
|
s.print(" ");
|
|
pos++;
|
|
}
|
|
}
|
|
bitptr++;
|
|
bitptr &= 0x07;
|
|
}
|
|
s.println("");
|
|
}
|
|
#endif
|