beslan/modbus
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

mbed trace

Browse Source
This commit is contained in:
Beslan
2022-01-25 06:48:12 +03:00
parent 4aba0072c3
commit bb39f6b11c
3 changed files with 92 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
Modbus.cpp
View File

@@ -188,13 +188,14 @@ void Modbus::slavePDU(uint8_t *frame) {
default:
exceptionResponse(fcode, EX_ILLEGAL_FUNCTION);
break;
}
}
void Modbus::successResponce(TAddress startreg, uint16_t numoutputs, FunctionCode fn) {
free(_frame);
_len = 5;
_frame = (uint8_t *) malloc(_len);
_len = 5U;
_frame = static_cast<uint8_t *> (malloc(_len));
_frame[0] = fn;
_frame[1] = startreg.address >> 8;
_frame[2] = startreg.address & 0x00FF;
@@ -204,7 +205,7 @@ void Modbus::successResponce(TAddress startreg, uint16_t numoutputs, FunctionCod
void Modbus::exceptionResponse(FunctionCode fn, ResultCode excode) {
free(_frame);
_len = 2;
_len = 2U;
_frame = (uint8_t *) malloc(_len);
_frame[0] = fn + 0x80;
_frame[1] = excode;
@@ -212,17 +213,15 @@ void Modbus::exceptionResponse(FunctionCode fn, ResultCode excode) {
}
void Modbus::getMultipleBits(uint8_t *frame, TAddress startreg, uint16_t numregs) {
uint8_t bitn = 0;
uint16_t i = 0;
uint8_t bitn = 0U;
uint16_t i = 0U;
while (numregs--) {
if (BIT_BOOL(Reg(startreg)))
bitSet(frame[i], bitn);
else
bitClear(frame[i], bitn);
if (BIT_BOOL(Reg(startreg))) { bitSet(frame[i], bitn); }
else { bitClear(frame[i], bitn); }
bitn++; //increment the bit index
if (bitn == 8) {
if (bitn == 8U) {
i++;
bitn = 0;
bitn = 0U;
}
startreg++; //increment the register
}
@@ -251,7 +250,7 @@ void Modbus::readBits(TAddress startreg, uint16_t numregs, FunctionCode fn) {
free(_frame);
//Determine the message length = function type, byte count and
//for each group of 8 registers the message length increases by 1
_len = 2 + numregs / 8;
_len = 2U + numregs / 8U;
if (numregs % 8)
_len++; //Add 1 to the message length for the partial byte.
_frame = (uint8_t *) malloc(_len);
@@ -260,8 +259,8 @@ void Modbus::readBits(TAddress startreg, uint16_t numregs, FunctionCode fn) {
return;
}
_frame[0] = fn;
_frame[1] = _len - 2; //byte count (_len - function code and byte count)
_frame[_len - 1] = 0; //Clean last probably partial byte
_frame[1] = _len - 2U; //byte count (_len - function code and byte count)
_frame[_len - 1U] = 0U; //Clean last probably partial byte
getMultipleBits(_frame + 2, startreg, numregs);
_reply = REPLY_NORMAL;
}
@@ -290,22 +289,22 @@ void Modbus::readWords(TAddress startreg, uint16_t numregs, FunctionCode fn) {
}
void Modbus::setMultipleBits(const uint8_t *frame, TAddress startreg, uint16_t numoutputs) {
uint8_t bitn = 0;
uint16_t i = 0;
uint8_t bitn = 0U;
uint16_t i = 0U;
while (numoutputs--) {
Reg(startreg, BIT_VAL(bitRead(frame[i], bitn)));
(void) Reg(startreg, BIT_VAL(bitRead(frame[i], bitn)));
bitn++; //increment the bit index
if (bitn == 8) {
if (bitn == 8U) {
i++;
bitn = 0;
bitn = 0U;
}
startreg++; //increment the register
}
}
void Modbus::setMultipleWords(const uint16_t *frame, TAddress startreg, uint16_t numregs) {
for (uint16_t i = 0; i < numregs; i++) {
Reg(startreg + i, __bswap_16(frame[i]));
for (uint16_t i = 0U; i < numregs; i++) {
(void) Reg(startreg + i, __bswap_16(frame[i]));
}
}
@@ -333,7 +332,7 @@ bool Modbus::onSet(TAddress address, cbModbus cb, uint16_t numregs) {
if (!cb) {
return removeOnGet(address);
}
while (numregs > 0) {
while (numregs > 0U) {
reg = searchRegister(address);
if (reg) {
_callbacks.push_back({TCallback::ON_SET, address, cb});
@@ -347,7 +346,7 @@ bool Modbus::onSet(TAddress address, cbModbus cb, uint16_t numregs) {
bool Modbus::removeOnSet(TAddress address, cbModbus cb, uint16_t numregs) {
while (numregs--) {
_callbacks.erase(remove_if(_callbacks.begin(), _callbacks.end(), [address, cb](TCallback entry) {
(void) _callbacks.erase(remove_if(_callbacks.begin(), _callbacks.end(), [address, cb](TCallback entry) {
return entry.type == TCallback::ON_SET && entry.address == address && (!cb || entry.cb == cb);
}), _callbacks.end());
address++;
@@ -357,7 +356,7 @@ bool Modbus::removeOnSet(TAddress address, cbModbus cb, uint16_t numregs) {
bool Modbus::removeOnGet(TAddress address, cbModbus cb, uint16_t numregs) {
while (numregs--) {
_callbacks.erase(remove_if(_callbacks.begin(), _callbacks.end(), [address, cb](TCallback entry) {
(void) _callbacks.erase(remove_if(_callbacks.begin(), _callbacks.end(), [address, cb](TCallback entry) {
return entry.type == TCallback::ON_GET && entry.address == address && (!cb || entry.cb == cb);
}), _callbacks.end());
address++;
@@ -367,7 +366,7 @@ bool Modbus::removeOnGet(TAddress address, cbModbus cb, uint16_t numregs) {
bool Modbus::readSlave(uint16_t address, uint16_t numregs, FunctionCode fn) {
free(_frame);
_len = 5;
_len = 5U;
_frame = (uint8_t *) malloc(_len);
_frame[0] = fn;
_frame[1] = address >> 8;
@@ -379,18 +378,18 @@ bool Modbus::readSlave(uint16_t address, uint16_t numregs, FunctionCode fn) {
bool Modbus::writeSlaveBits(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, bool *data) {
free(_frame);
_len = 6 + numregs / 8;
if (numregs % 8)
_len = 6U + numregs / 8U;
if (numregs % 8U)
_len++; //Add 1 to the message length for the partial byte.
_frame = (uint8_t *) malloc(_len);
if (_frame) {
_frame[0] = fn;
_frame[1] = to >> 8;
_frame[2] = to & 0x00FF;
_frame[3] = numregs >> 8;
_frame[4] = numregs & 0x00FF;
_frame[5] = _len - 6;
_frame[_len - 1] = 0; //Clean last probably partial byte
_frame[1] = to >> 8U;
_frame[2] = to & 0x00FFU;
_frame[3] = numregs >> 8U;
_frame[4] = numregs & 0x00FFU;
_frame[5] = _len - 6U;
_frame[_len - 1] = 0U; //Clean last probably partial byte
if (data) {
boolToBits(_frame + 6, data, numregs);
} else {
@@ -429,33 +428,33 @@ bool Modbus::writeSlaveWords(TAddress startreg, uint16_t to, uint16_t numregs, F
}
void Modbus::boolToBits(uint8_t *dst, const bool *src, uint16_t numregs) {
uint8_t bitn = 0;
uint16_t i = 0;
uint16_t j = 0;
uint8_t bitn = 0U;
uint16_t i = 0U;
uint16_t j = 0U;
while (numregs--) {
if (src[j])
bitSet(dst[i], bitn);
else
bitClear(dst[i], bitn);
bitn++; //increment the bit index
if (bitn == 8) {
if (bitn == 8U) {
i++;
bitn = 0;
bitn = 0U;
}
j++; //increment the register
}
}
void Modbus::bitsToBool(bool *dst, const uint8_t *src, uint16_t numregs) {
uint8_t bitn = 0;
uint16_t i = 0;
uint16_t j = 0;
uint8_t bitn = 0U;
uint16_t i = 0U;
uint16_t j = 0U;
while (numregs--) {
dst[j] = bitRead(src[i], bitn);
bitn++; //increment the bit index
if (bitn == 8) {
if (bitn == 8U) {
i++;
bitn = 0;
bitn = 0U;
}
j++; //increment the register
}
@@ -493,8 +492,8 @@ void Modbus::masterPDU(uint8_t *frame, const uint8_t *sourceFrame, TAddress star
case FC_READ_COILS:
case FC_READ_INPUT_STAT:
//field2 = numregs, frame[1] = data length, header len = 2
bytecount_calc = field2 / 8;
if (field2 % 8)
bytecount_calc = field2 / 8U;
if (field2 % 8U)
bytecount_calc++;
if (frame[1] != bytecount_calc) { // check if data size matches
_reply = EX_DATA_MISMACH;
@@ -513,6 +512,7 @@ void Modbus::masterPDU(uint8_t *frame, const uint8_t *sourceFrame, TAddress star
break;
default:
_reply = EX_GENERAL_FAILURE;
break;
}
}