brunotic
/
DreherTankController
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
DreherTankController/src/modbus/ModbusRTUSlave.cpp

504 lines
13 KiB
C++
Raw Blame History

#include "ModbusRTUSlave.h"
#include "utility/LinkedList.h"
ModbusRTUSlave::ModbusRTUSlave(HardwareSerial* serialport, const uint8_t controlPinArg)
: ser(serialport)
, controlPin(controlPinArg)
, isReading(true)
, rWords(new wordsList())
, rBits(new bitsList())
, rwWords(new wordsList())
, rwBits(new bitsList())
{
pinMode(controlPin, OUTPUT);
digitalWrite(controlPin, LOW);
}
void ModbusRTUSlave::begin(byte address, uint32_t baudrate)
{
if (connected) {
ser->end();
}
slaveAddress = address;
ser->begin(baudrate);
ResCnt=0;
connected = true;
}
void ModbusRTUSlave::end()
{
if (connected) {
ser->end();
connected = false;
}
}
bool ModbusRTUSlave::addCoilArea(u16 address, u8* values, int cnt)
{
if(getBitAddress(rwBits, address)==NULL)
{
rwBits->add(new ModbusRTUSlaveBitAddress(address, values, cnt));
return true;
}
return false;
}
bool ModbusRTUSlave::addDiscreteInputArea(u16 address, u8* values, int cnt)
{
if(getBitAddress(rBits, address)==NULL)
{
rBits->add(new ModbusRTUSlaveBitAddress(address, values, cnt));
return true;
}
return false;
}
bool ModbusRTUSlave::addHoldingRegisterArea(u16 address, u16* values, int cnt)
{
if(getWordAddress(rwWords, address)==NULL)
{
rwWords->add(new ModbusRTUSlaveWordAddress(address, values, cnt));
return true;
}
return false;
}
bool ModbusRTUSlave::addInputRegisterArea(u16 address, u16* values, int cnt)
{
if(getWordAddress(rWords, address)==NULL)
{
rWords->add(new ModbusRTUSlaveWordAddress(address, values, cnt));
return true;
}
return false;
}
// bool ModbusRTUSlave::addValidator(u16 reg, u16 min, u16 max, u16 step=5)
// {
// return validator.add(reg, min, max, step);
// }
ModbusRTUSlaveWordAddress* ModbusRTUSlave::getWordAddress(wordsList *words, u16 Addr)
{
ModbusRTUSlaveWordAddress* ret=NULL;
for(int i = 0; i < words->size(); i++)
{
ModbusRTUSlaveWordAddress* a = words->get(i);
if(a!=NULL && Addr >= a->addr && Addr < a->addr + a->len) ret=a;
}
return ret;
}
ModbusRTUSlaveBitAddress* ModbusRTUSlave::getBitAddress(bitsList *bits, u16 Addr)
{
ModbusRTUSlaveBitAddress* ret=NULL;
for(int i = 0; i < bits->size(); i++)
{
ModbusRTUSlaveBitAddress* a = bits->get(i);
if(a!=NULL && Addr >= a->addr && Addr < a->addr + (a->len*8)) ret=a;
}
return ret;
}
ModbusRTUSlaveWordAddress* ModbusRTUSlave::getWordAddress(wordsList *words, u16 Addr, u16 Len)
{
ModbusRTUSlaveWordAddress* ret=NULL;
for(int i = 0; i < words->size(); i++)
{
ModbusRTUSlaveWordAddress* a = words->get(i);
if(a!=NULL && Addr >= a->addr && Addr+Len <= a->addr + a->len) ret=a;
}
return ret;
}
ModbusRTUSlaveBitAddress* ModbusRTUSlave::getBitAddress(bitsList *bits, u16 Addr, u16 Len)
{
ModbusRTUSlaveBitAddress* ret=NULL;
for(int i = 0; i < bits->size(); i++)
{
ModbusRTUSlaveBitAddress* a = bits->get(i);
if(a!=NULL && Addr >= a->addr && Addr+Len <= a->addr + (a->len*8)) ret=a;
}
return ret;
}
void ModbusRTUSlave::process()
{
bool bvalid = true;
while(isDataAvail())
{
byte d = doRead();
lstResponse[ResCnt++]=d;
if(ResCnt>=4)
{
requestedSlave = lstResponse[0];
if(requestedSlave == slaveAddress)
{
fnCode = lstResponse[1];
requestedRegister = (lstResponse[2] << 8) | lstResponse[3];
receivedLastRequest = micros();
switch(fnCode)
{
case 1: // read coils
if(ResCnt >= 8)
{
readCoilOrDiscreteInput(rwBits, bvalid);
}
break;
case 2: // read discrete inputs
if(ResCnt >= 8)
{
readCoilOrDiscreteInput(rBits, bvalid);
}
break;
case 3: // read holding registers
if(ResCnt >= 8)
{
readHoldingOrInputRegister(rwWords, bvalid);
}
break;
case 4: // read input registers
if(ResCnt >= 8)
{
readHoldingOrInputRegister(rWords, bvalid);
}
break;
case 5: // write single coil
if(ResCnt >= 8)
{
u16 Data = (lstResponse[4] << 8) | lstResponse[5];
byte hi = 0xFF, lo = 0xFF;
getCRC(lstResponse,300, 0, 6, &hi, &lo);
ModbusRTUSlaveBitAddress *a = getBitAddress(rwBits, requestedRegister);
if (a != NULL && lstResponse[6] == hi && lstResponse[7] == lo)
{
u16 stidx = (requestedRegister - a->addr) / 8;
bool b = Data==0xFF00;
callbackCoil(requestedRegister, b);
bitWrite(a->values[stidx], (requestedRegister - a->addr)%8, b);
byte ret[8];
ret[0]=requestedSlave;
ret[1]=fnCode;
ret[2]=((requestedRegister&0xFF00)>>8);
ret[3]=((requestedRegister&0x00FF));
ret[4]=((Data&0xFF00)>>8);
ret[5]=((Data&0x00FF));
byte hi = 0xFF, lo = 0xFF;
getCRC(ret, 8, 0, 6, &hi, &lo);
ret[6]=hi;
ret[7]=lo;
doWrite(ret, 8);
ResCnt=0;
}
else bvalid = false;
}
break;
case 6: // write single register
if(ResCnt >= 8)
{
u16 Data = (lstResponse[4] << 8) | lstResponse[5];
byte hi = 0xFF, lo = 0xFF;
getCRC(lstResponse,300, 0, 6, &hi, &lo);
ModbusRTUSlaveWordAddress *a = getWordAddress(rwWords, requestedRegister);
if (a != NULL && lstResponse[6] == hi && lstResponse[7] == lo)
{
u16 stidx = requestedRegister - a->addr;
a->values[stidx] = callbackRegister(requestedRegister, Data);
byte ret[8];
ret[0]=requestedSlave;
ret[1]=fnCode;
ret[2]=((requestedRegister&0xFF00)>>8);
ret[3]=((requestedRegister&0x00FF));
ret[4]=((Data&0xFF00)>>8);
ret[5]=((Data&0x00FF));
byte hi = 0xFF, lo = 0xFF;
getCRC(ret, 8, 0, 6, &hi, &lo);
ret[6]=hi;
ret[7]=lo;
doWrite(ret, 8);
ResCnt=0;
}
else bvalid = false;
}
break;
case 15: // write multiple coils
if(ResCnt >= 7)
{
u16 Length = (lstResponse[4] << 8) | lstResponse[5];
u8 ByteCount = lstResponse[6];
if(ResCnt >= 9+ByteCount)
{
byte hi = 0xFF, lo = 0xFF;
getCRC(lstResponse,300, 0, 7 + ByteCount, &hi, &lo);
if(lstResponse[(9 + ByteCount - 2)] == hi && lstResponse[(9 + ByteCount - 1)] == lo)
{
ModbusRTUSlaveBitAddress *a = getBitAddress(rwBits, requestedRegister, Length);
if (a != NULL)
{
bool b;
u16 offset = 0;
u16 stidx = (requestedRegister - a->addr) / 8;
int ng=(requestedRegister - a->addr) % 8;
int ns=stidx;
for(int i=7; i<7+ByteCount;i++)
{
byte val = lstResponse[i];
for(int j=0;j<8;j++)
{
b = bitRead(val,j);
callbackCoil(requestedRegister + offset, b);
bitWrite(a->values[ns], ng++, b);
if(ng==8){ns++;ng=0;}
offset++;
}
}
if(bvalid)
{
byte ret[8];
ret[0]=requestedSlave;
ret[1]=fnCode;
ret[2]=((requestedRegister&0xFF00)>>8);
ret[3]=((requestedRegister&0x00FF));
ret[4]=((Length&0xFF00)>>8);
ret[5]=((Length&0x00FF));
byte hi = 0xFF, lo = 0xFF;
getCRC(ret, 8, 0, 6, &hi, &lo);
ret[6]=hi;
ret[7]=lo;
doWrite(ret, 8);
ResCnt=0;
}
}
}
else bvalid=false;
}
}
break;
case 16: // write multiple registers
if(ResCnt >= 7)
{
u16 Length = (lstResponse[4] << 8) | lstResponse[5];
u8 ByteCount = lstResponse[6];
if(ResCnt >= 9+ByteCount)
{
byte hi = 0xFF, lo = 0xFF;
getCRC(lstResponse,300, 0, 7 + ByteCount, &hi, &lo);
if(lstResponse[(9 + ByteCount - 2)] == hi && lstResponse[(9 + ByteCount - 1)] == lo)
{
for(int i=7; i<7+ByteCount;i+=2)
{
u16 data = callbackRegister(requestedRegister, lstResponse[i] << 8 | lstResponse[i+1]);
ModbusRTUSlaveWordAddress *a = getWordAddress(rwWords, requestedRegister + ((i-7)/2));
if (a != NULL) { a->values[(requestedRegister + ((i-7)/2)) - a->addr] = data; }
else { bvalid=false; break; }
}
if(bvalid)
{
byte ret[8];
ret[0]=requestedSlave;
ret[1]=fnCode;
ret[2]=((requestedRegister&0xFF00)>>8);
ret[3]=((requestedRegister&0x00FF));
ret[4]=((Length&0xFF00)>>8);
ret[5]=((Length&0x00FF));
byte hi = 0xFF, lo = 0xFF;
getCRC(ret, 8, 0, 6, &hi, &lo);
ret[6]=hi;
ret[7]=lo;
doWrite(ret, 8);
ResCnt=0;
}
}
else bvalid=false;
}
}
break;
}
}
else bvalid = false;
}
lastrecv = millis();
}
if(!bvalid && ResCnt>0) ResCnt=0;
if(ResCnt>0 && (millis()-lastrecv > 200 || millis() < lastrecv)) ResCnt=0;
}
/*
void ModbusRTUSlave::getCRC(LinkedList<byte>* pby, int startindex, int nSize, byte* byFirstReturn, byte* bySecondReturn)
{
int uIndex;
byte uchCRCHi = 0xff;
byte uchCRCLo = 0xff;
for (int i = startindex; i < startindex + nSize && i<pby->size(); i++)
{
uIndex = uchCRCHi ^ pby->get(i);
uchCRCHi = uchCRCLo ^ auchCRCHi[uIndex];
uchCRCLo = auchCRCLo[uIndex];
}
(*byFirstReturn) = uchCRCHi;
(*bySecondReturn) = uchCRCLo;
}
*/
void ModbusRTUSlave::getCRC(byte* pby, int arsize, int startindex, int nSize, byte* byFirstReturn, byte* bySecondReturn)
{
int uIndex;
byte uchCRCHi = 0xff;
byte uchCRCLo = 0xff;
for (int i = startindex; i < startindex + nSize && i<arsize; i++)
{
uIndex = uchCRCHi ^ pby[i];
uchCRCHi = uchCRCLo ^ auchCRCHi[uIndex];
uchCRCLo = auchCRCLo[uIndex];
}
(*byFirstReturn) = uchCRCHi;
(*bySecondReturn) = uchCRCLo;
}
void ModbusRTUSlave::switchToReadingIfNotReadingNow()
{
if (!isReading)
{
ser->flush();
digitalWrite(controlPin, LOW);
isReading = true;
}
}
bool ModbusRTUSlave::isDataAvail()
{
switchToReadingIfNotReadingNow();
return ser->available();
}
int ModbusRTUSlave::doRead()
{
switchToReadingIfNotReadingNow();
return ser->read();
}
void ModbusRTUSlave::doWrite(byte* buffer, int const length)
{
if (isReading)
{
digitalWrite(controlPin, HIGH);
isReading = false;
}
if (waitWithAnswerMicroS) {
unsigned long t = micros() - receivedLastRequest;
if (t < waitWithAnswerMicroS)
delayMicroseconds(waitWithAnswerMicroS - t);
}
ser->write(buffer, length);
}
void ModbusRTUSlave::readCoilOrDiscreteInput(bitsList *bits, bool &valid)
{
u16 Length = (lstResponse[4] << 8) | lstResponse[5];
byte hi = 0xFF, lo = 0xFF;
getCRC(lstResponse,300, 0, 6, &hi, &lo);
ModbusRTUSlaveBitAddress *a = getBitAddress(bits, requestedRegister, Length);
if (Length > 0 && a != NULL && lstResponse[6] == hi && lstResponse[7] == lo)
{
u16 stidx = (requestedRegister - a->addr) / 8;
u16 nlen = ((Length-1) / 8)+1;
byte dat[nlen];
memset(dat,0,nlen);
int ng=(requestedRegister - a->addr) % 8;
int ns=stidx;
for(int i=0;i<nlen;i++)
{
byte val=0;
for(int j=0;j<8;j++)
{
if(bitRead(a->values[ns], ng++)) bitSet(val,j);
if(ng==8){ns++;ng=0;}
}
dat[i]=val;
}
byte ret[3+nlen+2];
ret[0]=requestedSlave;
ret[1]=fnCode;
ret[2]=nlen;
for(int i=0;i<nlen;i++) ret[3+i]=dat[i];
byte hi = 0xFF, lo = 0xFF;
getCRC(ret, 3+nlen+2, 0, 3+nlen, &hi, &lo);
ret[3+nlen]=hi;
ret[3+nlen+1]=lo;
doWrite(ret, 3+nlen+2);
ResCnt=0;
}
else valid = false;
}
void ModbusRTUSlave::readHoldingOrInputRegister(wordsList *words, bool &valid)
{
u16 Length = (lstResponse[4] << 8) | lstResponse[5];
byte hi = 0xFF, lo = 0xFF;
getCRC(lstResponse,300, 0, 6, &hi, &lo);
ModbusRTUSlaveWordAddress *a = getWordAddress(words, requestedRegister, Length);
if (Length > 0 && a != NULL && lstResponse[6] == hi && lstResponse[7] == lo)
{
u16 stidx = requestedRegister - a->addr;
u16 nlen = Length * 2;
byte ret[3+nlen+2];
ret[0]=requestedSlave;
ret[1]=fnCode;
ret[2]=nlen;
for(int i=stidx;i<stidx+Length;i++)
{
ret[3+((i-stidx)*2)+0]=((a->values[i] & 0xFF00) >> 8);
ret[3+((i-stidx)*2)+1]=((a->values[i] & 0xFF));
}
byte hi = 0xFF, lo = 0xFF;
getCRC(ret, 3+nlen+2, 0, 3+nlen, &hi, &lo);
ret[3+nlen]=hi;
ret[3+nlen+1]=lo;
doWrite(ret, 3+nlen+2);
ResCnt=0;
}
else valid = false;
}
bool getBit(u8* area, int index)
{
u16 stidx = index / 8;
return bitRead(area[stidx], index%8);
}
void setBit(u8* area, int index, bool value)
{
u16 stidx = index / 8;
bitWrite(area[stidx], index%8, value);
}
ModbusRTUSlaveBitAddress::ModbusRTUSlaveBitAddress(u16 address, u8* value, int cnt)
{
addr = address;
values = value;
len = cnt;
}
ModbusRTUSlaveWordAddress::ModbusRTUSlaveWordAddress(u16 address, u16* value, int cnt)
{
addr = address;
values = value;
len = cnt;
}
Reference in New Issue View Git Blame Copy Permalink