544 lines
15 KiB
C++
544 lines
15 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();
|
||
|
||
}
|