718 lines
20 KiB
Plaintext
718 lines
20 KiB
Plaintext
/*@!Encoding:1252*/
|
|
// Additionally include ModbusTcpCommon.cin or ModbusUdpCommon.cin
|
|
includes
|
|
{
|
|
#include "ModbusStructs.cin"
|
|
}
|
|
|
|
variables
|
|
{
|
|
const word gMaxPacketLength = __size_of(struct ModbusReqWriteRegisters);
|
|
|
|
msTimer gtRobin; // Timer that sends the packets and watches for timeouts
|
|
word gTxID = 0x0000; // Transaction Identifier for Modbus. Used as index for gQueue
|
|
|
|
// Global storage for pending and sent requests, associated by TxID
|
|
struct QueueElement
|
|
{
|
|
byte FuncCode;
|
|
word TimeoutTicks;
|
|
byte Timeouts;
|
|
word Length;
|
|
byte Buffer[gMaxPacketLength];
|
|
};
|
|
struct QueueElement gQueuePending[long, 2];
|
|
struct QueueElement gQueueSent[long, 2];
|
|
struct QueueElement gQueueAck[long, 2];
|
|
|
|
char ModbusExceptions[11][72] = {
|
|
"Illegal func code (0x01). The function code is unknown by the server",
|
|
"Illegal data address (0x02). Please check your configuration",
|
|
"Illegal data value (0x03)",
|
|
"Server failure (0x04). The server failed during execution",
|
|
"Acknowledge (0x05). The server needs more time to generate the response",
|
|
"Server busy (0x06). The request could not be accepted",
|
|
"",
|
|
"",
|
|
"",
|
|
"Gateway problem (0x0A). Gateway paths not available",
|
|
"Gateway problem (0x0B). The targeted device failed to respond"
|
|
};
|
|
}
|
|
|
|
void ModbusInit()
|
|
{
|
|
char ip[16];
|
|
sysGetVariableString("%BUS_TYPE%%CHANNEL%::%NODE_NAME%::Config", "IP", ip, elCount(ip));
|
|
|
|
writeDbg(MbInfo, "Connecting to %s:%d", ip, @sysvar::Config::Modbus::Port);
|
|
ModbusConnectTo(ip, @sysvar::Config::Modbus::Port);
|
|
}
|
|
|
|
|
|
|
|
// REGION: ModbusReadBits -------------------------------------------------------------
|
|
/// <ModbusReadBits>
|
|
void ModbusReadBits(word address, word count)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqRead);
|
|
const byte funcCode = 0x01;
|
|
byte buffer[length];
|
|
struct ModbusReqRead mbr;
|
|
|
|
mbr.Address = address;
|
|
mbr.Count = count;
|
|
|
|
writeDbg(MbDebug, "Sending 'Read Bits' (0x01) command. Addr: 0x%04X, Count: %d", address, count);
|
|
|
|
memcpy_h2n(buffer, mbr);
|
|
ModbusSend(buffer, length, funcCode);
|
|
}
|
|
|
|
/// <ModbusReadBits>
|
|
void OnModbusReceiveBits(struct ModbusApHeader mbap, byte buffer[])
|
|
{
|
|
struct ModbusResReceiveBits mbres;
|
|
struct ModbusReqRead mbreq;
|
|
byte bitStatus[1968];
|
|
word numBits;
|
|
byte i, j;
|
|
|
|
memcpy_n2h(mbres, buffer);
|
|
memcpy_n2h(mbreq, gQueueAck[mbap.TxID].Buffer);
|
|
|
|
writeDbg(MbDebug, "Received %d bits from 0x%04X", mbreq.Count, mbreq.Address);
|
|
|
|
for (i = 0; i < mbres.ByteCount; i++)
|
|
{
|
|
for (j = 0; j < 8; j++)
|
|
{
|
|
bitStatus[8*i+j] = (mbres.Data[i] >> j) & 0x01;
|
|
}
|
|
}
|
|
|
|
OnModbusReadBitsSuccess(mbres, bitStatus, mbreq);
|
|
}
|
|
|
|
/// <ModbusReadBits>
|
|
void OnModbusReceiveBitsException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while reading bits: %s", ModbusExceptions[ex-1]);
|
|
OnModbusReadBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusReadRegisters -------------------------------------------------------
|
|
/// <ModbusReadRegisters>
|
|
void ModbusReadRegisters(word address, word count) // 16 bit
|
|
{
|
|
const byte length = __size_of(struct ModbusReqRead);
|
|
const byte funcCode = 0x03;
|
|
byte buffer[length];
|
|
struct ModbusReqRead mbr;
|
|
|
|
mbr.Address = address; // [2] Start address
|
|
mbr.Count = count; // [2] Number of items; 1:max 125=0x7D
|
|
|
|
writeDbg(MbDebug, "Sending 'Read Registers' (0x03) command. Addr: 0x%04X, Count: %d", address, count);
|
|
|
|
memcpy_h2n(buffer, mbr);
|
|
ModbusSend(buffer, length, funcCode);
|
|
}
|
|
|
|
/// <ModbusReadRegisters>
|
|
void OnModbusReceiveRegisters(struct ModbusApHeader mbap, byte buffer[])
|
|
{
|
|
struct ModbusResReceiveRegisters mbres;
|
|
struct ModbusReqRead mbreq;
|
|
|
|
memcpy_n2h(mbres, buffer);
|
|
memcpy_n2h(mbreq, gQueueAck[mbap.TxID].Buffer);
|
|
|
|
writeDbg(MbDebug, "Received %d registers from 0x%04X", mbreq.Count, mbreq.Address);
|
|
|
|
OnModbusReadRegistersSuccess(mbres, mbreq);
|
|
}
|
|
|
|
/// <ModbusReadRegisters>
|
|
void OnModbusReceiveRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while reading registers: %s", ModbusExceptions[ex-1]);
|
|
OnModbusReadRegistersFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusWriteBit -------------------------------------------------------------
|
|
/// <ModbusWriteBit>
|
|
void ModbusWriteBit(word address, byte value)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqWriteSingle);
|
|
const byte funcCode = 0x05;
|
|
byte buffer[length];
|
|
struct ModbusReqWriteSingle mbw;
|
|
|
|
if (value >= 1)
|
|
value = 0xFF;
|
|
|
|
mbw.Address = address; // [2] Output address
|
|
mbw.Value = value << 8; // [2] Output value (0x0000: Off, 0xFF00: On)
|
|
|
|
writeDbg(Debug, "Sending 'Write Bit' (0x05) command. Addr: 0x%04X, Value: 0x%02X", address, value);
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, length, funcCode);
|
|
}
|
|
|
|
/// <ModbusWriteBit>
|
|
void OnModbusConfirmBit(struct ModbusApHeader mbap, byte buffer[])
|
|
{
|
|
struct ModbusResConfirmSingle mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbDebug, "Set bit at 0x%04X to %d", mbc.Address, mbc.Value);
|
|
|
|
OnModbusWriteBitSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteBit>
|
|
void OnModbusConfirmBitException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while writing bit: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteBitFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusWriteRegister ------------------------------------------------------
|
|
/// <ModbusWriteRegister>
|
|
void ModbusWriteRegister(word address, int value)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqWriteSingle);
|
|
const byte funcCode = 0x06;
|
|
byte buffer[length];
|
|
struct ModbusReqWriteSingle mbw;
|
|
|
|
mbw.Address = address; // [2] Output address
|
|
mbw.Value = value; // [2] Output value
|
|
|
|
writeDbg(MbDebug, "Sending 'Write Register' (0x06) command. Addr: 0x%04X, Value: 0x%02X", address, value);
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, length, funcCode);
|
|
}
|
|
|
|
/// <ModbusWriteRegister>
|
|
void OnModbusConfirmRegister(struct ModbusApHeader mbap, byte buffer[])
|
|
{
|
|
struct ModbusResConfirmSingle mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbDebug, "Set register at 0x%04X to %d", mbc.Address, mbc.Value);
|
|
|
|
OnModbusWriteRegisterSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteRegister>
|
|
void OnModbusConfirmRegisterException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while writing register: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteRegisterFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusWriteBits ----------------------------------------------------------
|
|
/// <ModbusWriteBits>
|
|
void ModbusWriteBits(word address, word count, byte values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqWriteBits);
|
|
const byte funcCode = 0x0F;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqWriteBits mbw;
|
|
byte dataLength;
|
|
byte overallLength;
|
|
word i;
|
|
|
|
dataLength = _ceil(count / 8.0);
|
|
overallLength = maxLength - 1968/8 + dataLength;
|
|
|
|
mbw.Address = address; // [2] Output address
|
|
mbw.Count = count; // [2] Number of items; 1:max 1968=0x7B0
|
|
mbw.ByteCount = dataLength; // [1] Number of bytes; = ceil(count/8)
|
|
memcpy(mbw.Data, values, dataLength); // this is 1 memcpy too much -.-
|
|
|
|
writeDbg(MbDebug, "Sending 'Write Bits' (0x0F) command. Addr: 0x%04X, Count: %d", address, count);
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, overallLength, funcCode);
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
void ModbusWriteBitsB(word address, word count, byte values[])
|
|
{
|
|
byte buffer[2]; // length
|
|
word length;
|
|
dword ellCount;
|
|
dword i;
|
|
dword j;
|
|
char str1[20*2], str2[20*3];
|
|
|
|
length = (word)_ceil(count / 8.0);
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: count: %d; length: %d", count, length);
|
|
|
|
if (count % 8 != 0)
|
|
length--;
|
|
|
|
for (i = 0; i < length; i++)
|
|
{
|
|
buffer[i] = 0;
|
|
for (j = 0; j < 8; j++)
|
|
{
|
|
buffer[i] |= (values[i*8 + j] & 0x01) << j;
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: j: %d; indx: %d; value: %d; mask: %X", j, i*8 + j, values[i*8 + j], (0x01 << j));
|
|
}
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: %d: %X", i, buffer[i]);
|
|
}
|
|
for (j = 0; j < count % 8; j++) // wont be executed if there is no remainder
|
|
{
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: j: %d; indx: %d; value: %d; mask: %X", j, (length-1)*8 + j, values[(length-1)*8 + j], (0x01 << j));
|
|
buffer[length] |= (values[(length)*8 + j] & 0x01) << j;
|
|
}
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: %d: %X", length-1, buffer[length-1]);
|
|
|
|
hbin_to_strhex(values, str1);
|
|
bin_to_strhex(buffer, str2);
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: Encoded %s to %s", str1, str2);
|
|
ModbusWriteBits(address, count, buffer);
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
void OnModbusConfirmBits(struct ModbusApHeader mbap, byte buffer[])
|
|
{
|
|
struct ModbusResConfirmMultiple mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbDebug, "Updated &d bits at 0x%04X", mbc.Count, mbc.Address);
|
|
|
|
OnModbusWriteBitsSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
void OnModbusConfirmBitsException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while writing bits: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusWriteRegisters -------------------------------------------------------
|
|
/// <ModbusWriteRegisters>
|
|
void ModbusWriteRegisters(word address, word count, word values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqWriteRegisters);
|
|
const byte funcCode = 0x10;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqWriteRegisters mbw;
|
|
byte dataLength;
|
|
word overallLength;
|
|
word i;
|
|
|
|
dataLength = 2 * count;
|
|
overallLength = maxLength - 2*123 + dataLength;
|
|
|
|
mbw.Address = address; // [2] Output address
|
|
mbw.Count = count; // [2] Number of items; 1:max 123=0x7B
|
|
mbw.ByteCount = dataLength; // [1] Number of bytes; = 2 * count
|
|
|
|
for (i = 0; i < dataLength; i++)
|
|
mbw.Data[i] = values[i];
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, overallLength, funcCode);
|
|
}
|
|
|
|
/// <ModbusWriteRegisters>
|
|
void OnModbusConfirmRegisters(struct ModbusApHeader mbap, byte buffer[])
|
|
{
|
|
struct ModbusResConfirmMultiple mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbDebug, "Updated &d registers at 0x%04X", mbc.Count, mbc.Address);
|
|
|
|
OnModbusWriteRegistersSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteRegisters>
|
|
void OnModbusConfirmRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while writing registers: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteRegistersFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusWriteMasks ------------------------------------------------------------
|
|
/// <ModbusWriteMasks>
|
|
void ModbusWriteMasks(word address, word and, word or)
|
|
{
|
|
const word length = __size_of(struct ModbusReqWriteMasks);
|
|
const byte funcCode = 0x16;
|
|
byte buffer[length];
|
|
struct ModbusReqWriteMasks mbw;
|
|
|
|
mbw.Address = address; // [2] Output address
|
|
mbw.And = and; // [2] AND mask
|
|
mbw.Or = or; // [2] OR mask
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, length, funcCode);
|
|
}
|
|
|
|
/// <ModbusWriteMasks>
|
|
void OnModbusConfirmMasks(struct ModbusApHeader mbap, byte buffer[])
|
|
{
|
|
struct ModbusResConfirmMasks mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbDebug, "Applied masks at 0x%04X", mbc.Address);
|
|
|
|
OnModbusWriteMasksSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteMasks>
|
|
void OnModbusConfirmMasksException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while applying masks: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteMasksFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusReadWriteRegisters -------------------------------------------------------
|
|
/// <ModbusReadWriteRegisters>
|
|
void ModbusReadWriteRegisters(word readAddress, word readCount, word writeAddress, word writeCount, int values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqReadWriteRegisters);
|
|
const byte funcCode = 0x17;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqReadWriteRegisters mbw;
|
|
byte dataLength;
|
|
word overallLength;
|
|
word i;
|
|
|
|
dataLength = 2 * writeCount;
|
|
overallLength = maxLength - 2*121 + dataLength;
|
|
|
|
mbw.ReadAddress = readAddress; // [2] Input address
|
|
mbw.ReadCount = readCount; // [2] Number of items; 1:max 125=0x7D
|
|
mbw.WriteAddress = writeAddress;// [2] Output address
|
|
mbw.WriteCount = writeCount; // [2] Number of items; 1:max 121=0x79
|
|
mbw.ByteCount = dataLength; // [1] Number of bytes; = 2 * count
|
|
|
|
for (i = 0; i < dataLength; i++)
|
|
mbw.Data[i] = values[i];
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, overallLength, funcCode);
|
|
}
|
|
|
|
/// <ModbusReadWriteRegisters>
|
|
void OnModbusReceiveConfirmRegisters(struct ModbusApHeader mbap, byte buffer[])
|
|
{
|
|
struct ModbusResReceiveRegisters mbres;
|
|
struct ModbusReqRead mbreq;
|
|
|
|
memcpy_n2h(mbres, buffer);
|
|
memcpy_n2h(mbreq, gQueueAck[mbap.TxID].Buffer);
|
|
|
|
writeDbg(MbDebug, "Wrote some registers and received %d registers from 0x%04X", mbreq.Count, mbreq.Address);
|
|
|
|
OnModbusReadRegistersSuccess(mbres, mbreq);
|
|
}
|
|
|
|
/// <ModbusReadWriteRegisters>
|
|
void OnModbusReceiveConfirmRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while reading and writing registers: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteRegistersFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// ------------------------------------------------------------------------------------
|
|
// REGION: OnModbusReceive ------------------------------------------------------------
|
|
|
|
/// <-OnModbusReceive>
|
|
void OnModbusReceive2(long packet)
|
|
{
|
|
struct ModbusApHeader mbap;
|
|
byte buffer[gMaxPacketLength];
|
|
long size; // packet size in bits
|
|
//char str[3*20];
|
|
byte header[__size_of(struct ModbusApHeader)];
|
|
|
|
size = EthGetThisData(0, gMaxPacketLength, buffer);
|
|
EthGetTokenData(packet, "modbus", "header", __size_of(struct ModbusApHeader), header); // TODO: EthGetTokenData does not handle endianness :( /////////////////////////////////////////////////
|
|
memcpy_n2h(mbap, header);
|
|
|
|
//bin_to_strhex(header, str);
|
|
//writeDbg(MbError, "Header: %s", str);
|
|
//writeDbg(MbError, "Header Length: 0x%X", mbap.Length);
|
|
OnModbusReceive2OnePacket(buffer, size, mbap);
|
|
}
|
|
|
|
/// <-OnModbusReceive>
|
|
void OnModbusReceive2OnePacket(byte buffer[], long size, struct ModbusApHeader mbap)
|
|
{
|
|
// Test transaction identifier?
|
|
// Test unit/device identifier?
|
|
word i; // counter
|
|
|
|
if (mbap.Length-2 != size)
|
|
{
|
|
writeDbg(ConnError, "OnModbusReceive2OnePAcket: Packet payload length (%d) is different from specified length (%d)!", size, mbap.Length-2);
|
|
return;
|
|
}
|
|
|
|
if (mbap.Protocol != 0) // Protocol is not Modbus (0x0000). Wayne.
|
|
{
|
|
writeDbg(ConnDebug, "OnModbusReceive2OnePacket: packet is no Modbus packet: Protocol = %d", mbap.Protocol);
|
|
return;
|
|
}
|
|
// MBAP Header is OK :) Go on
|
|
|
|
if (!gQueueSent.ContainsKey(mbap.TxID)) // We don't wait for this message!
|
|
return;
|
|
|
|
//write("Received TxID: %d", mbap.TxID);
|
|
memcpy(gQueueAck[mbap.TxID], gQueueSent[mbap.TxID]);
|
|
gQueueSent.Remove(mbap.TxID);
|
|
|
|
if (mbap.FuncCode > 0x80) // Oh no, we got a exception!
|
|
{
|
|
OnModbusReceive2Exceptions(buffer[0], mbap);
|
|
|
|
gQueueAck.Remove(mbap.TxID); // Remove from acknowledge queue
|
|
|
|
return;
|
|
}
|
|
|
|
// Let's give the PDU to the corresponding function
|
|
switch (mbap.FuncCode)
|
|
{
|
|
case 0x01:
|
|
case 0x02:
|
|
OnModbusReceiveBits(mbap, buffer);
|
|
break;
|
|
case 0x03:
|
|
case 0x04:
|
|
OnModbusReceiveRegisters(mbap, buffer);
|
|
break;
|
|
case 0x05:
|
|
OnModbusConfirmBit(mbap, buffer);
|
|
break;
|
|
case 0x06:
|
|
OnModbusConfirmRegister(mbap, buffer);
|
|
break;
|
|
case 0x0F:
|
|
OnModbusConfirmBits(mbap, buffer);
|
|
break;
|
|
case 0x10:
|
|
OnModbusConfirmRegisters(mbap, buffer);
|
|
break;
|
|
case 0x16:
|
|
OnModbusConfirmMasks(mbap, buffer);
|
|
break;
|
|
case 0x17:
|
|
OnModbusReceiveConfirmRegisters(mbap, buffer);
|
|
break;
|
|
default:
|
|
writeDbg(MbError, "OnModbusReceive2OnePacket: We received funcCode 0x%X!?", mbap.FuncCode);
|
|
}
|
|
|
|
gQueueAck.Remove(mbap.TxID); // Remove from acknowledge queue
|
|
}
|
|
|
|
/// <-OnModbusReceive>
|
|
void OnModbusReceive2Exceptions(byte exCode, struct ModbusApHeader mbap)
|
|
{
|
|
enum ModbusException ex;
|
|
ex = (enum ModbusException)exCode;
|
|
|
|
switch (mbap.FuncCode)
|
|
{
|
|
case 0x81:
|
|
case 0x82:
|
|
OnModbusReceiveBitsException(mbap, ex);
|
|
break;
|
|
case 0x83:
|
|
case 0x84:
|
|
OnModbusReceiveRegistersException(mbap, ex);
|
|
break;
|
|
case 0x85:
|
|
OnModbusConfirmBitException(mbap, ex);
|
|
break;
|
|
case 0x86:
|
|
OnModbusConfirmRegisterException(mbap, ex);
|
|
break;
|
|
case 0x8F:
|
|
OnModbusConfirmBitsException(mbap, ex);
|
|
break;
|
|
case 0x90:
|
|
OnModbusConfirmRegistersException(mbap, ex);
|
|
break;
|
|
case 0x96:
|
|
OnModbusConfirmMasksException(mbap, ex);
|
|
break;
|
|
case 0x97:
|
|
OnModbusReceiveConfirmRegistersException(mbap, ex);
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
// ------------------------------------------------------------------------------------
|
|
// REGION: ModbusSend -----------------------------------------------------------------
|
|
/// <-ModbusSend>
|
|
void ModbusSend(byte buffer[], word length, byte funcCode)
|
|
{
|
|
struct QueueElement qe;
|
|
word TxID;
|
|
|
|
TxID = gTxID++;
|
|
|
|
qe.FuncCode = funcCode;
|
|
qe.Length = length;
|
|
memcpy(qe.Buffer, buffer, length);
|
|
|
|
memcpy(gQueuePending[TxID], qe);
|
|
writeDbg(ConnDebug, "Appended packet 0x%04X to pending queue", TxID);
|
|
|
|
if (gQueuePending.Size() == 1 && gQueueSent.Size() == 0 && gSocketState == OK) // start timer if connection established
|
|
ModbusStartQueue();
|
|
}
|
|
|
|
void ModbusStartQueue()
|
|
{
|
|
writeDbg(ConnDebug, "Starting Timer gtRobin");
|
|
setTimerCyclic(gtRobin, 1);
|
|
}
|
|
|
|
/// <-ModbusSend>
|
|
on timer gtRobin
|
|
{
|
|
struct ModbusApHeader mbap;
|
|
enum ModbusRequestError reqError;
|
|
|
|
writeDbg(ConnDebug, "gtRobin: Queue Sent: %d, Queue Pending: %d, Queue Ack: %d", gQueueSent.Size(), gQueuePending.Size(), gQueueAck.Size());
|
|
|
|
// First: check timeouts = packets that were sent in previous run and not removed by response
|
|
for (long TxID : gQueueSent)
|
|
{
|
|
if (++gQueueSent[TxID].TimeoutTicks < @sysvar::Config::Modbus::RequestTimeout) // not timed out yet
|
|
continue;
|
|
// timed out!
|
|
if (++gQueueSent[TxID].Timeouts < @sysvar::Config::Modbus::MaxTransmissionCount) // if we may resend it
|
|
{
|
|
writeDbg(ConnInfo, "Packet 0x%04X timed out! Retrying...", TxID);
|
|
gQueueSent[TxID].TimeoutTicks = 0;
|
|
EthSetTokenInt(gPacket, "modbus", "TxId", TxID);
|
|
EthSetTokenInt(gPacket, "modbus", "FuncCode", gQueueSent[TxID].FuncCode);
|
|
ModbusSnd(gQueueSent[TxID].Buffer, gQueueSent[TxID].Length); // resend it
|
|
reqError = Timeout;
|
|
ModbusRecv();
|
|
}
|
|
else // we will NOT resend it
|
|
{
|
|
writeDbg(ConnWarning, "Packet 0x%04X timed out! Giving up", TxID);
|
|
reqError = FinalTimeout;
|
|
}
|
|
|
|
memcpy_n2h(mbap, gQueueSent[TxID].Buffer);
|
|
switch(mbap.FuncCode) // throw an "error" in each case
|
|
{
|
|
case ReadBits1:
|
|
case ReadBits2:
|
|
OnModbusReadBitsFailed(reqError, None, mbap);
|
|
break;
|
|
case ReadRegisters1:
|
|
case ReadRegisters2:
|
|
OnModbusReadRegistersFailed(reqError, None, mbap);
|
|
break;
|
|
case WriteBit:
|
|
OnModbusWriteBitFailed(reqError, None, mbap);
|
|
break;
|
|
case WriteRegister:
|
|
OnModbusWriteRegisterFailed(reqError, None, mbap);
|
|
break;
|
|
case WriteBits:
|
|
OnModbusWriteBitsFailed(reqError, None, mbap);
|
|
break;
|
|
case WriteRegisters:
|
|
OnModbusWriteRegistersFailed(reqError, None, mbap);
|
|
break;
|
|
case MaskRegister:
|
|
OnModbusWriteMasksFailed(reqError, None, mbap);
|
|
break;
|
|
case ReadWriteRegisters:
|
|
OnModbusReadWriteRegistersFailed(reqError, None, mbap);
|
|
break;
|
|
}
|
|
|
|
if (reqError == FinalTimeout) // remove the packet from queue
|
|
gQueueSent.Remove(TxID); // wait until here to let the methods access the request
|
|
}
|
|
|
|
// Second: send new packets
|
|
for (long TxID : gQueuePending)
|
|
{
|
|
if (gQueueSent.Size() > 4) // Wago 750-881 cannot handle more than 5 messages at a time :(
|
|
continue;
|
|
|
|
EthSetTokenInt(gPacket, "modbus", "TxId", TxID);
|
|
EthSetTokenInt(gPacket, "modbus", "FuncCode", gQueuePending[TxID].FuncCode);
|
|
// if packet was sent or the socket is not currently being opened
|
|
if (ModbusSnd(gQueuePending[TxID].Buffer, gQueuePending[TxID].Length) == 0 || gSocketState != NULL)
|
|
{
|
|
memcpy(gQueueSent[TxID], gQueuePending[TxID]); // move packet to sent queue
|
|
gQueuePending.Remove(TxID);
|
|
ModbusRecv();
|
|
}
|
|
}
|
|
|
|
if (gSocketState == ERROR || gQueueSent.Size() == 0 && gQueuePending.Size() == 0) // Stop timer to reduce latency of first packet
|
|
{
|
|
writeDbg(ConnDebug, "Stopping Timer gtRobin");
|
|
this.Cancel();
|
|
}
|
|
} |