544 lines
14 KiB
C++
544 lines
14 KiB
C++
/*
|
|
* ModbusRtu.cpp
|
|
*
|
|
* Created on: 5 èþë. 2020 ã.
|
|
* Author: LeonidTitov
|
|
*/
|
|
|
|
#include "../common/Crc.hpp"
|
|
|
|
#include "ModbusRtu.hh"
|
|
|
|
// Îæèäàíèå çàïðîñà
|
|
void driver::modbus::ModBusPdu::processReq() {
|
|
|
|
if ( mAdu.getFrm ( deqFrmReq ) ) {
|
|
|
|
//  ñëó÷àè íîâîãî çàïðîñà ïîëíûé ñáðîñ ñîñòîÿíèÿ
|
|
mMapTaskRd.clear();
|
|
mMapTaskWr.clear();
|
|
|
|
deqFrmRes.clear();
|
|
|
|
mExcCode = NoException;
|
|
|
|
mIsBroadcast = !deqFrmReq[0];// Ôëàã øèðîêîâåùàòåëüíîãî çàïðîñà
|
|
mFuncCode = deqFrmReq[1];// Ñîõðàíÿåì êîä çàïðîñà
|
|
|
|
const uint16_t shift = getDataTblShift ( (FuncCode)mFuncCode );
|
|
|
|
switch ( mFuncCode ) {
|
|
case ReadCoil:
|
|
case ReadDiscreteInput:
|
|
case ReadHoldingReg:
|
|
case ReadInputReg: {
|
|
|
|
uint16_t addrBeg = shift + ( deqFrmReq[2] << 8 ) + deqFrmReq[3];
|
|
uint16_t addrEnd = addrBeg + ( deqFrmReq[4] << 8 ) + deqFrmReq[5];
|
|
|
|
if( addrEnd - addrBeg > getLimitQuantity( (FuncCode)mFuncCode ) ) {
|
|
mExcCode = IllegalDataValue;
|
|
break;
|
|
}
|
|
|
|
for ( uint16_t addrReg = addrBeg; addrReg < addrEnd; addrReg++ ) {
|
|
|
|
mMapTaskRd.insert ( make_pair( addrReg, 0 ) );
|
|
|
|
}
|
|
|
|
} break;
|
|
case WriteSingleCoil:
|
|
case WriteSingleReg: {
|
|
|
|
uint16_t addr = shift + ( deqFrmReq[2] << 8 ) + deqFrmReq[3];
|
|
uint16_t value = ( deqFrmReq[4] << 8 ) + deqFrmReq[5];
|
|
|
|
mMapTaskWr.insert( make_pair( addr, value ) );
|
|
|
|
} break;
|
|
case WriteMultipleCoil: {
|
|
|
|
const size_t valShift = 7; // Ñìåùåíèå äàííûõ â ïàêåòå çàïðîñà
|
|
|
|
uint16_t addrBeg = shift + ( deqFrmReq[2] << 8 ) + deqFrmReq[3];
|
|
uint16_t addrEnd = addrBeg + ( deqFrmReq[4] << 8 ) + deqFrmReq[5];
|
|
|
|
if( addrEnd - addrBeg > getLimitQuantity( (FuncCode)mFuncCode ) ) {
|
|
mExcCode = IllegalDataValue;
|
|
break;
|
|
}
|
|
|
|
const uint16_t bitOn = 0xFF00; // Èíäèêàòîð âêëþ÷åííîãî áèòà
|
|
const uint16_t bitOff = 0x0000; // Èíäèêàòîð âûêëþ÷åííîãî áèòà
|
|
|
|
for( uint16_t iBit = 0, addrReg = addrBeg;
|
|
addrReg < addrEnd; addrReg++, iBit++ ) {
|
|
|
|
// Âû÷èñëåíèå äèñêðåòíîãî çíà÷åíèÿ
|
|
uint16_t value = deqFrmReq[valShift + iBit / 8] &
|
|
(1u << iBit % 8) ? bitOn: bitOff;
|
|
mMapTaskWr.insert( make_pair( addrReg, value ) );
|
|
|
|
}
|
|
|
|
} break;
|
|
case WriteMultipleReg: {
|
|
|
|
const size_t valShift = 7; // Ñìåùåíèå äàííûõ â ïàêåòå çàïðîñà
|
|
|
|
uint16_t addrBeg = shift + ( deqFrmReq[2] << 8 ) + deqFrmReq[3];
|
|
uint16_t addrEnd = addrBeg + ( deqFrmReq[4] << 8 ) + deqFrmReq[5];
|
|
|
|
if( addrEnd - addrBeg > getLimitQuantity( (FuncCode)mFuncCode ) ) {
|
|
mExcCode = IllegalDataValue;
|
|
break;
|
|
}
|
|
|
|
for( uint16_t iVal = valShift, addrReg = addrBeg;
|
|
addrReg < addrEnd; addrReg++, iVal += 2 ) {
|
|
|
|
// Âû÷èñëåíèå àíàëîãîâîãî çíà÷åíèÿ
|
|
uint16_t value = ( deqFrmReq[iVal] << 8 ) + deqFrmReq[iVal + 1];
|
|
mMapTaskWr.insert( make_pair( addrReg, value ) );
|
|
|
|
}
|
|
|
|
} break;
|
|
default: {
|
|
|
|
// Íå ïîääåðæèâàåìûé êîä îïåðàöèè
|
|
mExcCode = IllegalFunc;
|
|
|
|
}
|
|
}
|
|
|
|
// Óñòàíîâêà ïðèçíàêà îøèáêè
|
|
if ( mExcCode != NoException ) {
|
|
mFuncCode += 0x80;
|
|
} else
|
|
packet_counter++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void driver::modbus::ModBusPdu::processResp() {
|
|
|
|
if ( ( mFuncCode != DummyCode
|
|
&& mMapTaskRd.empty()
|
|
&& mMapTaskWr.empty() ) || mFuncCode > 0x80 ) {
|
|
|
|
deqFrmRes.clear();
|
|
deqFrmRes.push_back( mFuncCode );// Íà÷àëî îòâåòíîãî ïàêåòà êîä îïåðàöèè
|
|
|
|
switch ( mFuncCode ) {
|
|
case ReadCoil:
|
|
case ReadDiscreteInput: {
|
|
|
|
const size_t num_results = mMapTaskRes.size();
|
|
const size_t nrBytes = num_results ? ( ( num_results - 1 ) / 8 ) + 1 : 0;
|
|
|
|
deqFrmRes.push_back( nrBytes );// Ñîõðàíÿåì êîë-âî áàéò äàííûõ
|
|
|
|
for ( size_t i = 0; i < nrBytes; i ++ ) {
|
|
|
|
deqFrmRes.push_back( 0 );// Ïðåäóñòàíîâêà off çíà÷åíèé
|
|
|
|
}
|
|
|
|
const uint16_t bitOn = 0xFF00;// Èíäèêàòîð âêëþ÷åííîãî áèòà
|
|
const size_t valShift = 2;// Ñìåùåíèå äàííûõ â ïàêåòå îòâåòà
|
|
|
|
// Ôèêñèðóåì ïîëó÷åííûå äàííûå
|
|
size_t iBit = 0;
|
|
for( MapPolyAlloc::const_iterator it = mMapTaskRes.begin(); it != mMapTaskRes.end(); it++, iBit++ ) {
|
|
|
|
deqFrmRes[valShift + iBit / 8] |=
|
|
it->second == bitOn ? (1u << iBit % 8) : 0;
|
|
|
|
}
|
|
|
|
} break;
|
|
case ReadHoldingReg:
|
|
case ReadInputReg: {
|
|
|
|
const size_t nrBytes = mMapTaskRes.size() * 2;
|
|
|
|
// Ñîõðàíÿåì êîë-âî áàéò äàííûõ
|
|
deqFrmRes.push_back( nrBytes );
|
|
|
|
// Ôèêñèðóåì ïîëó÷åííûå äàííûå
|
|
for( MapPolyAlloc::const_iterator it = mMapTaskRes.begin(); it != mMapTaskRes.end(); it++ ) {
|
|
|
|
deqFrmRes.push_back( it->second >> 8 ); // HiByte
|
|
deqFrmRes.push_back( it->second & 0xFF ); // LoByte
|
|
|
|
}
|
|
|
|
} break;
|
|
case WriteSingleCoil:
|
|
case WriteSingleReg: {
|
|
|
|
uint16_t shift = getDataTblShift( (FuncCode)mFuncCode );
|
|
|
|
// Àäðåñ ïåðâîãî ðåãèñòðà
|
|
uint16_t addrBeg = mMapTaskRes.begin()->first - shift;
|
|
deqFrmRes.push_back( addrBeg >> 8 ); // HiByte
|
|
deqFrmRes.push_back( addrBeg & 0xFF ); // LoByte
|
|
|
|
// Çàïèñàííîå çíà÷åíèå ðåãèñòðà
|
|
uint16_t value = mMapTaskRes.begin()->second;
|
|
deqFrmRes.push_back( value >> 8 ); // HiByte
|
|
deqFrmRes.push_back( value & 0xFF ); // LoByte
|
|
|
|
} break;
|
|
case WriteMultipleCoil:
|
|
case WriteMultipleReg: {
|
|
|
|
uint16_t shift = getDataTblShift( (FuncCode)mFuncCode );
|
|
|
|
// Àäðåñ ïåðâîãî ðåãèñòðà
|
|
uint16_t addrBeg = mMapTaskRes.begin()->first - shift;
|
|
deqFrmRes.push_back( addrBeg >> 8 ); // HiByte
|
|
deqFrmRes.push_back( addrBeg & 0xFF ); // LoByte
|
|
|
|
// Êîëè÷åñòâî çàïèñàííûõ ðåãèñòðîâ
|
|
uint16_t nrReg = mMapTaskRes.size();
|
|
deqFrmRes.push_back( nrReg >> 8 ); // HiByte
|
|
deqFrmRes.push_back( nrReg & 0xFF ); // LoByte
|
|
|
|
} break;
|
|
default: {
|
|
|
|
// Îáðàáîòêà èñêëþ÷èòåëüíûõ ñèòóàöèé
|
|
deqFrmRes.push_back( mExcCode );
|
|
|
|
} break;
|
|
}
|
|
|
|
// Îòïðàâêà ïàêåòà
|
|
if( !mIsBroadcast && !mAdu.putFrm( deqFrmRes ) ) {
|
|
|
|
// TODO Îáðàáîòêà îòêàçà îòïðàâêè
|
|
|
|
}
|
|
|
|
mFuncCode = DummyCode;// Çàïðîñ çàâåðøåí
|
|
|
|
//todo: Äîáàâèë î÷èñòêó ìàïû ïîñëå èñïîëüçîâàíèÿ.
|
|
mMapTaskRes.clear();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
bool driver::modbus::ModBusAdu::getFrm( DequePolyAlloc & deqFrm ) {
|
|
|
|
if( !deqFrm.empty() ) {
|
|
deqFrm.clear();
|
|
}
|
|
|
|
static char modbus_frame_word = 0;
|
|
|
|
while (mIUartPort.receive(&modbus_frame_word, 1)) {
|
|
deqFrm.push_front(modbus_frame_word);
|
|
}
|
|
|
|
if( deqFrm.size() > 2 ) {
|
|
ModbusCrc::crc_t crcTmp;
|
|
DequePolyAlloc::const_iterator itFrm = deqFrm.begin();
|
|
|
|
const std::size_t szFrmData = deqFrm.size() - 2;
|
|
|
|
for ( std::size_t i = 0; i < szFrmData; i++, itFrm++ ) {
|
|
crcTmp = common::crc::crcCalc<ModbusCrc>(&itFrm[0], 1, crcTmp);
|
|
}
|
|
|
|
const ModbusCrc::res_t received_crc_low = *itFrm;
|
|
itFrm++;
|
|
const ModbusCrc::res_t received_crc_high = *itFrm;
|
|
const ModbusCrc::res_t received_crc =
|
|
(received_crc_high << 8) | (received_crc_low & 0xFF);
|
|
|
|
if((received_crc == (ModbusCrc::res_t) crcTmp)
|
|
&& ((mAddr == deqFrm.front()) || !deqFrm.front()))
|
|
{
|
|
//Óäàëÿåì äàííûå CRC
|
|
deqFrm.pop_back();
|
|
deqFrm.pop_back();
|
|
crc_error_flag = false;
|
|
} else {
|
|
deqFrm.clear ();
|
|
crc_error_flag = true;
|
|
}
|
|
}
|
|
|
|
return !deqFrm.empty();
|
|
|
|
}
|
|
|
|
bool driver::modbus::ModBusAdu::putFrm( DequePolyAlloc & deqFrm ) {
|
|
|
|
deqFrm.push_front(mAddr);// Âñòàâêà àäðåñà óñòðîéñòâà
|
|
|
|
ModbusCrc::crc_t crcTmp;
|
|
VecPolyAlloc vecFrm(mpMemRes);// Âûõîäíîé áóôåð
|
|
DequePolyAlloc::const_iterator itFrm = deqFrm.begin();
|
|
|
|
while (itFrm != deqFrm.end()) {
|
|
// Âñòàâêà äàííûõ ôðýéìà
|
|
crcTmp = common::crc::crcCalc<ModbusCrc>(&itFrm[0], 1, crcTmp);
|
|
vecFrm.push_back(*itFrm);
|
|
itFrm++;
|
|
}
|
|
|
|
const ModbusCrc::res_t output_crc = crcTmp;
|
|
|
|
// Âñòàâêà êîíòðîëüíîé ñóììû
|
|
vecFrm.push_back(output_crc & 0xFF);
|
|
vecFrm.push_back(output_crc >> 8);
|
|
|
|
return mIUartPort.send(&vecFrm.front(), vecFrm.size());
|
|
|
|
}
|
|
|
|
void driver::modbus::ModBusPdu::setExceptCode( ExceptCode code ) {
|
|
|
|
mExcCode = code;
|
|
mFuncCode |= 0x80;
|
|
|
|
}
|
|
|
|
bool driver::modbus::ModBusPdu::isException() const {
|
|
|
|
return mExcCode != NoException;
|
|
|
|
}
|
|
|
|
uint16_t driver::modbus::ModBusPdu::getLimitQuantity( FuncCode code ) const {
|
|
|
|
uint16_t limit = 0;
|
|
|
|
switch( code ) {
|
|
|
|
case ReadDiscreteInput:
|
|
case WriteSingleCoil:
|
|
case WriteMultipleCoil:
|
|
case ReadCoil: {
|
|
|
|
limit = 0x7D0;
|
|
|
|
} break;
|
|
|
|
case ReadInputReg:
|
|
case WriteSingleReg:
|
|
case WriteMultipleReg:
|
|
case ReadHoldingReg: {
|
|
|
|
limit = 0x7D;
|
|
|
|
} break;
|
|
|
|
default: break;
|
|
}
|
|
|
|
return limit;
|
|
|
|
}
|
|
|
|
uint16_t driver::modbus::ModBusPdu::getPacketCounter() const {
|
|
|
|
return packet_counter;
|
|
|
|
}
|
|
|
|
driver::modbus::ModBusPdu::ExceptCode driver::modbus::ModBusPdu::getExceptCode() const {
|
|
|
|
return mExcCode;
|
|
|
|
}
|
|
|
|
uint16_t driver::modbus::ModBusPdu::getDataTblShift( FuncCode code ) const {
|
|
|
|
uint16_t shift = 0;
|
|
|
|
switch ( code ) {
|
|
|
|
case WriteSingleCoil:
|
|
case WriteMultipleCoil:
|
|
case ReadCoil: {
|
|
|
|
// ×òåíèå äèñêðåòíîãî âûâîäà (DO)
|
|
shift = DataTableBegin::DO;
|
|
|
|
} break;
|
|
|
|
case ReadDiscreteInput: {
|
|
|
|
// ×òåíèå äèñêðåòíîãî âûâîäà (DI)
|
|
shift = DataTableBegin::DI;
|
|
|
|
} break;
|
|
|
|
case WriteSingleReg:
|
|
case WriteMultipleReg:
|
|
case ReadHoldingReg: {
|
|
|
|
// ×òåíèå àíàëîãîâîãî âûâîäà (AO)
|
|
shift = DataTableBegin::AO;
|
|
|
|
} break;
|
|
|
|
case ReadInputReg: {
|
|
|
|
// ×òåíèå àíàëîãîâîãî âûâîäà (AI)
|
|
shift = DataTableBegin::AI;
|
|
|
|
} break;
|
|
|
|
default: {
|
|
|
|
shift = 0;
|
|
|
|
} break;
|
|
|
|
}
|
|
|
|
return shift;
|
|
|
|
}
|
|
|
|
void driver::modbus::ModBusDispatch::add( uint16_t nReg, IModBusDispatch & disp ) {
|
|
|
|
mBase.insert( make_pair( nReg, &disp ) );
|
|
|
|
}
|
|
|
|
void driver::modbus::ModBusDispatch::del( uint16_t nReg ) {
|
|
|
|
mBase.erase( nReg );
|
|
|
|
}
|
|
|
|
void driver::modbus::ModBusDispatch::process() {
|
|
|
|
// Ïîëó÷åíèå çàïðîñîâ
|
|
mPdu.processReq();
|
|
|
|
dispatch( TaskRead, mPdu.mMapTaskRd );
|
|
dispatch( TaskWrite, mPdu.mMapTaskWr );
|
|
|
|
mPdu.processResp();
|
|
|
|
}
|
|
|
|
void driver::modbus::ModBusDispatch::dispatch( TaskType typeOp,
|
|
MapPolyAlloc & mapTasks ) {
|
|
|
|
if( !mapTasks.empty() ) {
|
|
|
|
// Ñïèñîê ðåãèñòðîâ çàêîí÷èâøèõ äåéñòâèÿ
|
|
DequeU16PolyAlloc deqTaskDone( mPdu.mAdu.mpMemRes );
|
|
|
|
for( MapPolyAlloc::iterator it = mapTasks.begin();
|
|
it != mapTasks.end();
|
|
it++ ) {
|
|
|
|
const uint16_t nReg = it->first;
|
|
|
|
if( mBase.count( nReg ) ) {
|
|
|
|
const IModBusDispatch::Status sts =
|
|
typeOp == TaskRead ? mBase[nReg]->read( it->second ) :
|
|
mBase[nReg]->write( it->second );
|
|
|
|
switch ( sts ) {
|
|
case IModBusDispatch::Done: {
|
|
// Ñîõðàíÿåì ðåçóëüòàò
|
|
mPdu.mMapTaskRes.insert( *it );
|
|
deqTaskDone.push_back( nReg );
|
|
|
|
} break;
|
|
case IModBusDispatch::Failure: {
|
|
|
|
mPdu.setExceptCode( ModBusPdu::ServerDeviceFail );
|
|
|
|
} break;
|
|
case IModBusDispatch::Pending: {
|
|
|
|
mPdu.setExceptCode( ModBusPdu::ServerDeviceBusy );
|
|
|
|
} break;
|
|
default: {
|
|
|
|
} break;
|
|
}
|
|
} else {
|
|
// Îøèáêà, òàêîé ðåãèñò íå çàðåãèñòðèðîâàí
|
|
mPdu.setExceptCode( ModBusPdu::IllegalDataAddr );
|
|
|
|
}
|
|
|
|
if ( mPdu.isException() )
|
|
break;
|
|
|
|
}
|
|
|
|
// Óäàëÿåì âûïîëíåííûå çàäà÷è èç ñïèñêà
|
|
for( DequeU16PolyAlloc::const_iterator it = deqTaskDone.begin();
|
|
it != deqTaskDone.end();
|
|
it++ ) {
|
|
|
|
mapTasks.erase( *it );
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
void driver::modbus::ModBusRtu::process() {
|
|
|
|
if (mPort.isRecieptionError()) {
|
|
mPort.reset();
|
|
}
|
|
|
|
mDisp.process();
|
|
|
|
}
|
|
|
|
void driver::modbus::ModBusRtu::add(uint16_t nReg, IModBusDispatch &disp) {
|
|
|
|
mDisp.add( nReg, disp );
|
|
|
|
}
|
|
|
|
void driver::modbus::ModBusRtu::del(uint16_t nReg) {
|
|
|
|
mDisp.del( nReg );
|
|
|
|
}
|
|
|
|
bool driver::modbus::ModBusAdu::isCrcError() const {
|
|
|
|
return crc_error_flag;
|
|
|
|
}
|
|
|
|
bool driver::modbus::ModBusRtu::isCrcError() const {
|
|
|
|
return mAdu.isCrcError();
|
|
|
|
}
|
|
|
|
bool driver::modbus::ModBusRtu::isException() const {
|
|
|
|
return mPdu.isException();
|
|
|
|
}
|
|
|
|
float driver::modbus::ModBusRtu::getPacketCounter() const {
|
|
|
|
return mPdu.getPacketCounter();
|
|
|
|
}
|