mirrors
/
Tasmota
mirror of https://github.com/arendst/Tasmota.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #16351 from jeroenst/ModbusBridgeWrite 

Modbus bridge write
This commit is contained in:
Theo Arends 2022-08-27 11:21:11 +02:00 committed by GitHub
parent be99f1183e 728c108294
commit 906d725534
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 387 additions and 175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
lib/lib_basic/TasmotaModbus-3.5.0/src/TasmotaModbus.cpp
View File

@ -19,8 +19,14 @@
Documentation about modbus protocol: https://www.modbustools.com/modbus.html
*/
#include "Arduino.h"
#include "TasmotaModbus.h"
extern void AddLog(uint32_t loglevel, PGM_P formatP, ...);
enum LoggingLevels {LOG_LEVEL_NONE, LOG_LEVEL_ERROR, LOG_LEVEL_INFO, LOG_LEVEL_DEBUG, LOG_LEVEL_DEBUG_MORE};
//#define TASMOTAMODBUSDEBUG
TasmotaModbus::TasmotaModbus(int receive_pin, int transmit_pin) : TasmotaSerial(receive_pin, transmit_pin, 1)
{
mb_address = 0;
@ -55,18 +61,21 @@ int TasmotaModbus::Begin(long speed, uint32_t config)
return result;
}
uint8_t TasmotaModbus::Send(uint8_t device_address, uint8_t function_code, uint16_t start_address, uint16_t register_count, uint16_t *registers)
uint8_t TasmotaModbus::Send(uint8_t device_address, uint8_t function_code, uint16_t start_address, uint16_t count, uint16_t *write_data)
{
uint8_t *frame;
uint8_t framepointer = 0;
uint16_t byte_count = count * 2; // In register mode count is nr of registers (2 bytes)
if ((function_code == 1) || (function_code == 2) || (function_code == 15)) byte_count = ((count-1) / 8) + 1; // In bitmode count is nr of bits
if (function_code < 7)
{
frame = (uint8_t *)malloc(8); // Addres(1), Function(1), Start/Coil Address(2), Registercount or Data (2), CRC(2)
}
else
{
frame = (uint8_t *)malloc(9 + (register_count * 2)); // Addres(1), Function(1), Start/Coil Address(2),Quantity of registers (2), Bytecount(1), Data(1..n), CRC(2)
frame = (uint8_t *)malloc(9 + byte_count); // Addres(1), Function(1), Start/Coil Address(2),Quantity of registers (2), Bytecount(1), Data(1..n), CRC(2)
}
mb_address = device_address; // Save address for receipt check
@ -77,43 +86,44 @@ uint8_t TasmotaModbus::Send(uint8_t device_address, uint8_t function_code, uint1
frame[framepointer++] = (uint8_t)(start_address); // LSB
if (function_code < 5)
{
frame[framepointer++] = (uint8_t)(register_count >> 8); // MSB
frame[framepointer++] = (uint8_t)(register_count); // LSB
frame[framepointer++] = (uint8_t)(count >> 8); // MSB
frame[framepointer++] = (uint8_t)(count); // LSB
}
else if ((function_code == 5) || (function_code == 6))
{
if (registers == NULL)
if (write_data == NULL)
{
free(frame);
return 13; // Register data not specified
}
if (register_count != 1)
if (count != 1)
{
free(frame);
return 12; // Wrong register count
}
frame[framepointer++] = (uint8_t)(registers[0] >> 8); // MSB
frame[framepointer++] = (uint8_t)(registers[0]); // LSB
frame[framepointer++] = (uint8_t)(write_data[0] >> 8); // MSB
frame[framepointer++] = (uint8_t)(write_data[0]); // LSB
}
else if ((function_code == 15) || (function_code == 16))
{
frame[framepointer++] = (uint8_t)(register_count >> 8); // MSB
frame[framepointer++] = (uint8_t)(register_count); // LSB
frame[framepointer++] = register_count * 2;
if (registers == NULL)
frame[framepointer++] = (uint8_t)(count >> 8); // MSB
frame[framepointer++] = (uint8_t)(count); // LSB
frame[framepointer++] = byte_count;
if (write_data == NULL)
{
free(frame);
return 13; // Register data not specified
}
if (register_count == 0)
if (count == 0)
{
free(frame);
return 12; // Wrong register count
}
for (int registerpointer = 0; registerpointer < register_count; registerpointer++)
for (uint16_t bytepointer = 0; bytepointer < byte_count; bytepointer++)
{
frame[framepointer++] = (uint8_t)(registers[registerpointer] >> 8); // MSB
frame[framepointer++] = (uint8_t)(registers[registerpointer]); // LSB
frame[framepointer++] = (uint8_t)(write_data[bytepointer/2] >> (bytepointer % 2 ? 0 : 8)); // MSB, LSB, MSB ....
}
}
else
@ -126,6 +136,18 @@ uint8_t TasmotaModbus::Send(uint8_t device_address, uint8_t function_code, uint1
frame[framepointer++] = (uint8_t)(crc);
frame[framepointer++] = (uint8_t)(crc >> 8);
#ifdef TASMOTAMODBUSDEBUG
uint8_t *buf;
uint16_t bufsize=(framepointer + 1) * 3;
buf = (uint8_t *)malloc(bufsize);
memset(buf, 0, bufsize);
uint16_t i;
for (i = 0; i < framepointer;i++)
snprintf((char *)&buf[i*3], (bufsize-i*3), "%02X ",frame[i]);
AddLog(LOG_LEVEL_DEBUG_MORE, PSTR("MBS: Serial Send: %s"), buf);
free(buf);
#endif
flush();
write(frame, framepointer);
free(frame);
@ -137,12 +159,13 @@ bool TasmotaModbus::ReceiveReady()
return (available() > 4);
}
uint8_t TasmotaModbus::ReceiveBuffer(uint8_t *buffer, uint8_t data_count)
uint8_t TasmotaModbus::ReceiveBuffer(uint8_t *buffer, uint8_t register_count, uint16_t byte_count)
{
mb_len = 0;
uint32_t timeout = millis() + 10;
uint8_t header_length = 3;
while ((mb_len < (data_count * 2) + header_length + 2) && (millis() < timeout)) {
if (byte_count == 0) byte_count = (register_count * 2);
while ((mb_len < byte_count + header_length + 2) && (millis() < timeout)) {
if (available()) {
uint8_t data = (uint8_t)read();
if (!mb_len) { // Skip leading data as provided by hardware serial
@ -152,20 +175,6 @@ uint8_t TasmotaModbus::ReceiveBuffer(uint8_t *buffer, uint8_t data_count)
} else {
buffer[mb_len++] = data;
if (3 == mb_len) {
if (buffer[1] & 0x80) { // 01 84 02 f2 f1
if (0 == buffer[2]) {
return 3; // 3 = Illegal Data Value,
}
return buffer[2]; // 1 = Illegal Function,
// 2 = Illegal Data Address,
// 3 = Illegal Data Value,
// 4 = Slave Error
// 5 = Acknowledge but not finished (no error)
// 6 = Slave Busy
// 8 = Memory Parity error
// 10 = Gateway Path Unavailable
// 11 = Gateway Target device failed to respond
}
if ((buffer[1] == 5) || (buffer[1] == 6) || (buffer[1] == 15) || (buffer[1] == 16)) header_length = 4; // Addr, Func, StartAddr
}
}
@ -175,6 +184,36 @@ uint8_t TasmotaModbus::ReceiveBuffer(uint8_t *buffer, uint8_t data_count)
}
}
#ifdef TASMOTAMODBUSDEBUG
// RX Logging
uint8_t *buf;
uint16_t bufsize=(mb_len + 1) * 3;
buf = (uint8_t *)malloc(bufsize);
memset(buf, 0, bufsize);
uint16_t i;
for (i = 0; i < mb_len;i++)
snprintf((char *)&buf[i*3], (bufsize-i*3), "%02X ",buffer[i]);
AddLog(LOG_LEVEL_DEBUG_MORE, PSTR("MBS: Serial Received: %s"), buf);
free(buf);
#endif
if (buffer[1] & 0x80)
{ // 01 84 02 f2 f1
if (0 == buffer[2])
{
return 3; // 3 = Illegal Data Value,
}
return buffer[2]; // 1 = Illegal Function,
// 2 = Illegal Data Address,
// 3 = Illegal Data Value,
// 4 = Slave Error
// 5 = Acknowledge but not finished (no error)
// 6 = Slave Busy
// 8 = Memory Parity error
// 10 = Gateway Path Unavailable
// 11 = Gateway Target device failed to respond
}
if (mb_len < 6) { return 7; } // 7 = Not enough data
/*

8
lib/lib_basic/TasmotaModbus-3.5.0/src/TasmotaModbus.h
View File

@ -53,8 +53,12 @@ class TasmotaModbus : public TasmotaSerial {
* 13 = Register data not specified
* 14 = To many registers
*/
uint8_t Send(uint8_t device_address, uint8_t function_code, uint16_t start_address, uint16_t register_count, uint16_t *registers = NULL);
uint8_t ReceiveBuffer(uint8_t *buffer, uint8_t register_count);
uint8_t Send(uint8_t device_address, uint8_t function_code, uint16_t start_address, uint16_t count, uint16_t *writeData = NULL);
// Count is nr of registers for function code 3,4,6 and 16, nr of coils/inputs for function code 1,2 and 15 and must be 1 for function code 5 and 6
uint8_t ReceiveBuffer(uint8_t *buffer, uint8_t register_count, uint16_t byte_count = 0);
// Bytecount is mandatory for functioncode 1 and 2 because they can return an odd number of bytes.
uint8_t Receive8BitRegister(uint8_t *value);
uint8_t Receive16BitRegister(uint16_t *value);
uint8_t Receive32BitRegister(float *value);

451
tasmota/tasmota_xdrv_driver/xdrv_63_modbus_bridge.ino
View File

@ -26,12 +26,23 @@
* When USE_MODBUS_BRIDGE_TCP is also defined, this bridge can also be used as an ModbusTCP
* bridge.
*
* Example Command:
* Example Commands:
* -- Read Coils --
* ModbusSend {"deviceaddress": 1, "functioncode": 1, "startaddress": 1, "type":"bit", "count":2}
*
* -- Read Input Register --
* ModbusSend {"deviceaddress": 1, "functioncode": 3, "startaddress": 1, "type":"uint16", "count":2}
*
*
* -- Write multiple coils --
* ModbusSend {"deviceaddress": 1, "functioncode": 15, "startaddress": 1, "type":"uint16", "count":4, "values":[1,2,3,4]}
* ModbusSend {"deviceaddress": 1, "functioncode": 15, "startaddress": 1, "type":"bit", "count":4, "values":[1,0,1,1]}
*
* Info for modbusBridgeTCPServer:
* https://ipc2u.com/articles/knowledge-base/detailed-description-of-the-modbus-tcp-protocol-with-command-examples/
*
* Info for modbus serial communications:
* https://ozeki.hu/p_5879-mobdbus-function-code-4-read-input-registers.html
* https://www.modbustools.com/modbus.html
* https://ipc2u.com/articles/knowledge-base/modbus-rtu-made-simple-with-detailed-descriptions-and-examples/
\*********************************************************************************************/
#define XDRV_63 63
@ -110,7 +121,7 @@ enum class ModbusBridgeFunctionCode
{
mb_undefined = 0,
mb_readCoilStatus = 1,
mb_readContactStatus = 2,
mb_readInputStatus = 2,
mb_readHoldingRegisters = 3,
mb_readInputRegisters = 4,
mb_writeSingleCoil = 5,
@ -150,6 +161,7 @@ struct ModbusBridge
ModbusBridgeType type = ModbusBridgeType::mb_undefined;
uint16_t dataCount = 0;
uint16_t byteCount = 0;
uint16_t startAddress = 0;
uint8_t deviceAddress = 0;
uint8_t count = 0;
@ -158,6 +170,24 @@ struct ModbusBridge
ModbusBridge modbusBridge;
/********************************************************************************************/
//
// Helper functions for data conversion between little and big endian
//
uint16_t swap_endian16(uint16_t num)
{
return (num>>8) | (num<<8);
}
uint32_t swap_endian32(uint32_t num)
{
return ((num>>24)&0xff) | // move byte 3 to byte 0
((num<<8)&0xff0000) | // move byte 1 to byte 2
((num>>8)&0xff00) | // move byte 2 to byte 1
((num<<24)&0xff000000); // byte 0 to byte 3
}
/********************************************************************************************/
//
// Applies serial configuration to modbus serial port
@ -216,8 +246,67 @@ void ModbusBridgeHandle(void)
if (data_ready)
{
uint8_t *buffer;
buffer = (uint8_t *)malloc(9 + (modbusBridge.dataCount * 2)); // Addres(1), Function(1), Length(1), Data(1..n), CRC(2)
uint32_t error = tasmotaModbus->ReceiveBuffer(buffer, modbusBridge.dataCount);
if (modbusBridge.byteCount == 0) modbusBridge.byteCount = modbusBridge.dataCount * 2;
buffer = (uint8_t *)malloc(9 + modbusBridge.byteCount); // Addres(1), Function(1), Length(1), Data(1..n), CRC(2)
memset(buffer, 0, 9 + modbusBridge.byteCount);
uint32_t error = tasmotaModbus->ReceiveBuffer(buffer, 0, modbusBridge.byteCount);
#ifdef USE_MODBUS_BRIDGE_TCP
for (uint32_t i = 0; i < nitems(modbusBridgeTCP.client_tcp); i++)
{
WiFiClient &client = modbusBridgeTCP.client_tcp[i];
if (client)
{
uint8_t header[8];
uint8_t nrOfBytes = 8;
header[0] = modbusBridgeTCP.tcp_transaction_id >> 8;
header[1] = modbusBridgeTCP.tcp_transaction_id;
header[2] = 0;
header[3] = 0;
header[6] = buffer[0]; // Send slave address
header[7] = buffer[1]; // Send function code
if (error)
{
header[4] = 0; // Message Length Hi-Byte
header[5] = 3; // Message Length Low-Byte
header[7] = buffer[1] | 0x80; // Send function code
header[8] = error;
nrOfBytes += 1;
client.write(header, 9);
}
else if (buffer[1] <= 2)
{
header[4] = modbusBridge.byteCount >> 8;
header[5] = modbusBridge.byteCount + 3;
header[8] = modbusBridge.byteCount;
client.write(header, 9);
nrOfBytes += 1;
client.write(buffer + 3, modbusBridge.byteCount); // Don't send CRC
nrOfBytes += modbusBridge.byteCount;
}
else if (buffer[1] <= 4)
{
header[4] = modbusBridge.byteCount >> 8;
header[5] = modbusBridge.byteCount + 3;
header[8] = modbusBridge.byteCount;
client.write(header, 9);
nrOfBytes += 1;
client.write(buffer + 3, modbusBridge.byteCount); // Don't send CRC
nrOfBytes += modbusBridge.byteCount;
}
else
{
header[4] = 0; // Message Length Hi-Byte
header[5] = 6; // Message Length Low-Byte
client.write(header, 8);
client.write(buffer + 2, 4); // Don't send CRC
nrOfBytes += 4;
}
client.flush();
AddLog(LOG_LEVEL_DEBUG, PSTR("MBS: MBRTCP from Modbus deviceAddress %d, writing %d bytes to client"), buffer[0], nrOfBytes);
}
}
#endif
if (error)
{
@ -226,33 +315,9 @@ void ModbusBridgeHandle(void)
return;
}
#ifdef USE_MODBUS_BRIDGE_TCP
for (uint32_t i = 0; i < nitems(modbusBridgeTCP.client_tcp); i++)
{
WiFiClient &client = modbusBridgeTCP.client_tcp[i];
if (client)
{
uint8_t MBAP_Header[7];
MBAP_Header[0] = modbusBridgeTCP.tcp_transaction_id >> 8;
MBAP_Header[1] = modbusBridgeTCP.tcp_transaction_id;
MBAP_Header[2] = 0;
MBAP_Header[3] = 0;
MBAP_Header[4] = ((modbusBridge.dataCount * 2) + 3) >> 8;
MBAP_Header[5] = (modbusBridge.dataCount * 2) + 3;
MBAP_Header[6] = buffer[0]; // Send slave address
client.write(MBAP_Header, 7);
client.write(buffer + 1, 1); // Send Functioncode
uint8_t bytecount[1];
bytecount[0] = modbusBridge.dataCount * 2;
client.write(bytecount, 1); // Send length of rtu data
client.write(buffer + 3, (modbusBridge.dataCount * 2)); // Don't send CRC
client.flush();
AddLog(LOG_LEVEL_DEBUG, PSTR("MBS: MBRTCP from Modbus deviceAddress %d, writing %d bytes to client"), buffer[0], (modbusBridge.dataCount * 2) + 9);
}
}
#endif
modbusBridge.byteCount = 0;
ModbusBridgeError errorcode = ModbusBridgeError::noerror;
if (modbusBridge.deviceAddress == 0)
{
#ifdef USE_MODBUS_BRIDGE_TCP
@ -278,17 +343,13 @@ void ModbusBridgeHandle(void)
}
else
{
if ((modbusBridge.type == ModbusBridgeType::mb_int8 || modbusBridge.type == ModbusBridgeType::mb_uint8)
&& ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2]))
if ((modbusBridge.type == ModbusBridgeType::mb_int8 || modbusBridge.type == ModbusBridgeType::mb_uint8) && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2]))
errorcode = ModbusBridgeError::wrongdataCount;
else if ((modbusBridge.type == ModbusBridgeType::mb_bit)
&& ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2]))
else if ((modbusBridge.type == ModbusBridgeType::mb_bit) && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2]))
errorcode = ModbusBridgeError::wrongdataCount;
else if ((modbusBridge.type == ModbusBridgeType::mb_int16 || modbusBridge.type == ModbusBridgeType::mb_uint16)
&& ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2]))
else if ((modbusBridge.type == ModbusBridgeType::mb_int16 || modbusBridge.type == ModbusBridgeType::mb_uint16) && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2]))
errorcode = ModbusBridgeError::wrongdataCount;
else if ((modbusBridge.type == ModbusBridgeType::mb_int32 || modbusBridge.type == ModbusBridgeType::mb_uint32 || modbusBridge.type == ModbusBridgeType::mb_float)
&& ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2]))
else if ((modbusBridge.type == ModbusBridgeType::mb_int32 || modbusBridge.type == ModbusBridgeType::mb_uint32 || modbusBridge.type == ModbusBridgeType::mb_float) && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2]))
errorcode = ModbusBridgeError::wrongdataCount;
}
}
@ -354,11 +415,13 @@ void ModbusBridgeHandle(void)
char svalue[MBR_MAX_VALUE_LENGTH + 1] = "";
if (modbusBridge.type == ModbusBridgeType::mb_float)
{
// Convert next 4 bytes to float
float value = 0;
if (buffer[1] < 3)
{
// In bit mode only convert returned bytes
if (buffer[2] - (count * 4))
((uint8_t *)&value)[0] = buffer[dataOffset + (count * 4)]; // Get int values
((uint8_t *)&value)[0] = buffer[dataOffset + (count * 4)]; // Get float values
if ((buffer[2] - (count * 4)) >> 1)
((uint8_t *)&value)[1] = buffer[dataOffset + 1 + (count * 4)];
if ((buffer[2] - (count * 4) - 1) >> 1)
@ -377,7 +440,17 @@ void ModbusBridgeHandle(void)
}
else if (modbusBridge.type == ModbusBridgeType::mb_bit)
{
uint8_t value = (uint8_t)(buffer[dataOffset + (count >> 3)]);
uint8_t bits_left = modbusBridge.count - ((count/8) * 8);
uint8_t value = 0;
if (bits_left < 8)
{
uint8_t bits_skip = 8 - bits_left;
value = (uint8_t)(buffer[dataOffset + ((count + bits_skip) >> 3)]);
}
else
{
value = (uint8_t)(buffer[dataOffset + (count >> 3)]);
}
snprintf(svalue, MBR_MAX_VALUE_LENGTH, "%d", ((value >> (count & 7)) & 1));
}
else
@ -389,17 +462,17 @@ void ModbusBridgeHandle(void)
if (buffer[1] < 3)
{
if (buffer[2] - (count * 4))
((uint8_t *)&value)[0] = buffer[dataOffset + (count * 4)]; // Get int values
if ((buffer[2] - (count * 4)) >> 1)
((uint8_t *)&value)[0] = buffer[dataOffset + (count * 4)]; // Get uint values
if (buffer[2] - ((count * 4) - 1))
((uint8_t *)&value)[1] = buffer[dataOffset + 1 + (count * 4)];
if ((buffer[2] - (count * 4) - 1) >> 1)
if (buffer[2] - ((count * 4) - 2))
((uint8_t *)&value)[2] = buffer[dataOffset + 2 + (count * 4)];
if ((buffer[2] - (count * 4)) >> 2)
if (buffer[2] - ((count * 4) - 3))
((uint8_t *)&value)[3] = buffer[dataOffset + 3 + (count * 4)];
}
else
{
((uint8_t *)&value)[3] = buffer[dataOffset + (count * 4)]; // Get int values
((uint8_t *)&value)[3] = buffer[dataOffset + (count * 4)]; // Get uint values
((uint8_t *)&value)[2] = buffer[dataOffset + 1 + (count * 4)];
((uint8_t *)&value)[1] = buffer[dataOffset + 2 + (count * 4)];
((uint8_t *)&value)[0] = buffer[dataOffset + 3 + (count * 4)];
@ -417,7 +490,7 @@ void ModbusBridgeHandle(void)
{
if (buffer[2] - (count * 2))
((uint8_t *)&value)[0] = buffer[dataOffset + (count * 2)];
if ((buffer[2] - (count * 2)) >> 1)
if (buffer[2] - ((count * 2) - 1))
((uint8_t *)&value)[1] = buffer[dataOffset + 1 + (count * 2)];
}
else
@ -439,12 +512,6 @@ void ModbusBridgeHandle(void)
else
snprintf(svalue, MBR_MAX_VALUE_LENGTH, "%u", value);
}
/*
else if (modbusBridge.type == ModbusBridgeType::mb_bit)
{
uint8_t value = (uint8_t)(buffer[dataOffset + count]);
snprintf(svalue, MBR_MAX_VALUE_LENGTH, "%d%d%d%d%d%d%d%d", ((value >> 7) & 1), ((value >> 6) & 1), ((value >> 5) & 1), ((value >> 4) & 1), ((value >> 3) & 1), ((value >> 2) & 1), ((value >> 1) & 1), (value & 1));
}*/
}
ResponseAppend_P(PSTR("%s"), svalue);
if (count < data_count - 1)
@ -576,18 +643,60 @@ void ModbusTCPHandle(void)
busy = true;
}
}
if (buf_len == 12)
if (buf_len >= 12)
{
uint8_t mbdeviceaddress = (uint8_t)modbusBridgeTCP.tcp_buf[6];
uint8_t mbfunctioncode = (uint8_t)modbusBridgeTCP.tcp_buf[7];
uint16_t mbstartaddress = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[8]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[9]));
modbusBridge.dataCount = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[10]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[11]));
uint16_t *writeData = NULL;
uint16_t count = 0;
modbusBridgeTCP.tcp_transaction_id = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[0]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[1]));
AddLog(LOG_LEVEL_DEBUG_MORE, PSTR("MBS: MBRTCP to Modbus Transactionid:%d, deviceAddress:%d, functionCode:%d, startAddress:%d, Count:%d"),
modbusBridgeTCP.tcp_transaction_id, mbdeviceaddress, mbfunctioncode, mbstartaddress, modbusBridge.dataCount);
if (mbfunctioncode <= 2)
{
count = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[10]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[11]));
modbusBridge.byteCount = ((count - 1) >> 3) + 1;
modbusBridge.dataCount = ((count - 1) >> 4) + 1;
}
else if (mbfunctioncode <= 4)
{
count = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[10]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[11]));
modbusBridge.byteCount = count * 2;
modbusBridge.dataCount = count;
}
else
{
// For functioncode 15 & 16 ignore bytecount, tasmotaModbus does calculate this
uint8_t dataStartByte = mbfunctioncode <= 6 ? 10 : 13;
uint16_t byteCount = (buf_len - dataStartByte);
modbusBridge.byteCount = 2;
modbusBridge.dataCount = 1;
tasmotaModbus->Send(mbdeviceaddress, mbfunctioncode, mbstartaddress, modbusBridge.dataCount);
writeData = (uint16_t *)malloc((byteCount / 2)+1);
if ((mbfunctioncode == 15) || (mbfunctioncode == 16)) count = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[10]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[11]));
else count = 1;
for (uint16_t dataPointer = 0; dataPointer < byteCount; dataPointer++)
{
if (dataPointer % 2 == 0)
{
writeData[dataPointer / 2] = (uint16_t)(((uint16_t)modbusBridgeTCP.tcp_buf[dataStartByte + dataPointer]) << 8);
}
else
{
writeData[dataPointer / 2] |= ((uint16_t)modbusBridgeTCP.tcp_buf[dataStartByte + dataPointer]);
}
}
}
AddLog(LOG_LEVEL_DEBUG_MORE, PSTR("MBS: MBRTCP to Modbus TransactionId:%d, deviceAddress:%d, functionCode:%d, startAddress:%d, count:%d, recvCount:%d, recvBytes:%d"),
modbusBridgeTCP.tcp_transaction_id, mbdeviceaddress, mbfunctioncode, mbstartaddress, count, modbusBridge.dataCount, modbusBridge.byteCount);
tasmotaModbus->Send(mbdeviceaddress, mbfunctioncode, mbstartaddress, count, writeData);
free(writeData);
}
}
yield(); // avoid WDT if heavy traffic
@ -603,6 +712,7 @@ void CmndModbusBridgeSend(void)
{
uint16_t *writeData = NULL;
uint8_t writeDataSize = 0;
bool bitMode = false;
ModbusBridgeError errorcode = ModbusBridgeError::noerror;
JsonParser parser(XdrvMailbox.data);
@ -615,7 +725,9 @@ void CmndModbusBridgeSend(void)
modbusBridge.startAddress = root.getULong(PSTR(D_JSON_MODBUS_START_ADDRESS), 0);
const char *stype = root.getStr(PSTR(D_JSON_MODBUS_TYPE), "uint8");
modbusBridge.count = root.getUInt(PSTR(D_JSON_MODBUS_COUNT), 1);
modbusBridge.count = root.getUInt(PSTR(D_JSON_MODBUS_COUNT), 1); // Number of bits or bytes to read / write
if ((functionCode == 1) || (functionCode == 2) || (functionCode == 15)) bitMode = true;
if (modbusBridge.deviceAddress == 0)
errorcode = ModbusBridgeError::wrongdeviceaddress;
@ -634,7 +746,8 @@ void CmndModbusBridgeSend(void)
if (strcmp(stype, "int8") == 0)
{
modbusBridge.type = ModbusBridgeType::mb_int8;
modbusBridge.dataCount = ((modbusBridge.count - 1)/ 2) + 1;
modbusBridge.dataCount = bitMode ? modbusBridge.count : ((modbusBridge.count - 1) / 2) + 1;
if (bitMode) modbusBridge.byteCount = (modbusBridge.count / 8) + 1;
}
else if (strcmp(stype, "int16") == 0)
{
@ -644,12 +757,12 @@ void CmndModbusBridgeSend(void)
else if (strcmp(stype, "int32") == 0)
{
modbusBridge.type = ModbusBridgeType::mb_int32;
modbusBridge.dataCount = 2 * modbusBridge.count;
modbusBridge.dataCount = bitMode ? modbusBridge.count : 2 * modbusBridge.count;
}
else if ((strcmp(stype, "uint8") == 0))
{
modbusBridge.type = ModbusBridgeType::mb_uint8;
modbusBridge.dataCount = ((modbusBridge.count - 1)/ 2) + 1;
modbusBridge.dataCount = bitMode ? modbusBridge.count : ((modbusBridge.count - 1) / 2) + 1;
}
else if ((strcmp(stype, "uint16") == 0) || (strcmp(stype, "") == 0)) // Default is uint16
{
@ -659,37 +772,86 @@ void CmndModbusBridgeSend(void)
else if (strcmp(stype, "uint32") == 0)
{
modbusBridge.type = ModbusBridgeType::mb_uint32;
modbusBridge.dataCount = 2 * modbusBridge.count;
modbusBridge.dataCount = bitMode ? modbusBridge.count : 2 * modbusBridge.count;
}
else if (strcmp(stype, "float") == 0)
{
modbusBridge.type = ModbusBridgeType::mb_float;
modbusBridge.dataCount = 2 * modbusBridge.count;
modbusBridge.dataCount = bitMode ? 2 * modbusBridge.count : modbusBridge.count;
}
else if (strcmp(stype, "raw") == 0)
{
modbusBridge.type = ModbusBridgeType::mb_raw;
modbusBridge.dataCount = modbusBridge.count;
modbusBridge.dataCount = bitMode ? modbusBridge.count : ((modbusBridge.count - 1) / 2) + 1;
}
else if (strcmp(stype, "hex") == 0)
{
modbusBridge.type = ModbusBridgeType::mb_hex;
modbusBridge.dataCount = modbusBridge.count;
modbusBridge.dataCount = bitMode ? modbusBridge.count : ((modbusBridge.count - 1) / 2) + 1;
}
else if (strcmp(stype, "bit") == 0)
{
modbusBridge.type = ModbusBridgeType::mb_bit;
modbusBridge.dataCount = ((modbusBridge.count - 1) / 16) + 1;
modbusBridge.dataCount = bitMode ? modbusBridge.count : ((modbusBridge.count - 1) / 16) + 1 ;
}
else
errorcode = ModbusBridgeError::wrongtype;
if (modbusBridge.dataCount > MBR_MAX_REGISTERS)
// If functioncode is 15, the count is not the number of registers but the number
// of bit to write, so calculate the number data bytes to write.
if (modbusBridge.functionCode == ModbusBridgeFunctionCode::mb_writeMultipleCoils)
{
modbusBridge.dataCount = modbusBridge.count;
}
// Prevent buffer overflow due to usage of to many registers
if ((!bitMode) && (modbusBridge.dataCount > MBR_MAX_REGISTERS))
errorcode = ModbusBridgeError::wrongcount;
// If write data is specified in JSON copy it into writeData array
if ((bitMode) && (modbusBridge.dataCount > MBR_MAX_REGISTERS * 8))
errorcode = ModbusBridgeError::wrongcount;
// Get Json data for writing
JsonParserArray jsonDataArray = root[PSTR(D_JSON_MODBUS_VALUES)].getArray();
if (jsonDataArray.isArray())
writeDataSize = jsonDataArray.size();
// Check if number of supplied data items is valid
switch (modbusBridge.functionCode)
{
case ModbusBridgeFunctionCode::mb_writeMultipleCoils:
// In writeMultipleCoil mode the amount of given data bits is less or equal to the count
switch (modbusBridge.type)
{
case ModbusBridgeType::mb_bit:
if (modbusBridge.count > writeDataSize) errorcode = ModbusBridgeError::wrongcount;
break;
case ModbusBridgeType::mb_uint8:
case ModbusBridgeType::mb_int8:
case ModbusBridgeType::mb_raw:
case ModbusBridgeType::mb_hex:
if (modbusBridge.count > writeDataSize * 8) errorcode = ModbusBridgeError::wrongcount;
break;
case ModbusBridgeType::mb_uint16:
case ModbusBridgeType::mb_int16:
if (modbusBridge.count > writeDataSize * 16) errorcode = ModbusBridgeError::wrongcount;
break;
case ModbusBridgeType::mb_uint32:
case ModbusBridgeType::mb_int32:
if (modbusBridge.count > writeDataSize * 32) errorcode = ModbusBridgeError::wrongcount;
break;
}
break;
case ModbusBridgeFunctionCode::mb_writeSingleRegister:
case ModbusBridgeFunctionCode::mb_writeSingleCoil:
if (modbusBridge.count != 1) errorcode = ModbusBridgeError::wrongcount;
break;
case ModbusBridgeFunctionCode::mb_writeMultipleRegisters:
if (modbusBridge.count != writeDataSize) errorcode = ModbusBridgeError::wrongcount;
break;
}
// If write data is specified in JSON copy it into writeData array
if ((errorcode == ModbusBridgeError::noerror) && (jsonDataArray.isArray()))
{
if (modbusBridge.dataCount > 40)
{
@ -697,73 +859,85 @@ void CmndModbusBridgeSend(void)
}
else
{
writeDataSize = jsonDataArray.size();
writeData = (uint16_t *)malloc(modbusBridge.dataCount);
if (modbusBridge.count != writeDataSize)
for (uint8_t jsonDataArrayPointer = 0; jsonDataArrayPointer < writeDataSize; jsonDataArrayPointer++)
{
errorcode = ModbusBridgeError::wrongcount;
}
else
{
writeData = (uint16_t *)malloc(writeDataSize * 2);
for (uint8_t jsonDataArrayPointer = 0; jsonDataArrayPointer < writeDataSize; jsonDataArrayPointer++)
if (errorcode != ModbusBridgeError::noerror) break;
switch (modbusBridge.type)
{
switch (modbusBridge.type)
case ModbusBridgeType::mb_bit:
{
// Initialize current data/register to 0
if (jsonDataArrayPointer % 16 == 0)
{
case ModbusBridgeType::mb_bit:
{
// Set 2 following bytes to 0
if (jsonDataArrayPointer % 16 == 0)
{
writeData[jsonDataArrayPointer/15] = 0;
}
// Swap low and high bytes according to modbus specification
uint16_t bitValue = (jsonDataArray[jsonDataArrayPointer].getUInt(0) == 1) ? 1 : 0;
uint8_t bitPointer = (jsonDataArrayPointer % 15) + 8;
if (bitPointer > 15) bitPointer -= 16;
writeData[jsonDataArrayPointer/15] += bitValue << bitPointer;
}
break;
case ModbusBridgeType::mb_int8:
if (jsonDataArrayPointer % 2) writeData[jsonDataArrayPointer / 2] += (int8_t)jsonDataArray[jsonDataArrayPointer].getInt(0);
else writeData[jsonDataArrayPointer] = (int8_t)jsonDataArray[jsonDataArrayPointer / 2].getInt(0) << 8;
break;
case ModbusBridgeType::mb_uint8:
if (jsonDataArrayPointer % 2) writeData[jsonDataArrayPointer / 2] += (uint8_t)jsonDataArray[jsonDataArrayPointer].getInt(0);
else writeData[jsonDataArrayPointer / 2] = (uint8_t)jsonDataArray[jsonDataArrayPointer].getInt(0) << 8;
break;
case ModbusBridgeType::mb_int16:
writeData[jsonDataArrayPointer] = (int16_t)jsonDataArray[jsonDataArrayPointer].getInt(0);
break;
case ModbusBridgeType::mb_uint16:
writeData[jsonDataArrayPointer] = (uint16_t)jsonDataArray[jsonDataArrayPointer].getUInt(0);
break;
case ModbusBridgeType::mb_float:
// TODO
errorcode = ModbusBridgeError::wrongtype;
break;
case ModbusBridgeType::mb_int32:
writeData[jsonDataArrayPointer++] = (int16_t)(jsonDataArray[jsonDataArrayPointer].getInt(0) >> 16);
writeData[jsonDataArrayPointer] = (uint16_t)jsonDataArray[jsonDataArrayPointer].getInt(0);
break;
case ModbusBridgeType::mb_uint32:
writeData[jsonDataArrayPointer++] = (uint16_t)(jsonDataArray[jsonDataArrayPointer].getUInt(0) >> 16);
writeData[jsonDataArrayPointer] = (uint16_t)jsonDataArray[jsonDataArrayPointer].getUInt(0);
break;
case ModbusBridgeType::mb_raw:
writeData[jsonDataArrayPointer] = (uint8_t)jsonDataArray[jsonDataArrayPointer*2].getUInt(0) << 8 + (uint8_t)jsonDataArray[(jsonDataArrayPointer*2)+1].getUInt(0);
break;
case ModbusBridgeType::mb_hex:
writeData[jsonDataArrayPointer] = (uint8_t)jsonDataArray[jsonDataArrayPointer*2].getUInt(0) << 8 + (uint8_t)jsonDataArray[(jsonDataArrayPointer*2)+1].getUInt(0);
break;
default:
errorcode = ModbusBridgeError::wrongtype;
break;
writeData[jsonDataArrayPointer / 15] = 0;
}
// Swap low and high bytes according to modbus specification
uint16_t bitValue = (jsonDataArray[jsonDataArrayPointer].getUInt(0) == 1) ? 1 : 0;
uint8_t bitPointer = (jsonDataArrayPointer % 16) + 8;
if (bitPointer > 15) bitPointer -= 16;
writeData[jsonDataArrayPointer / 16] += bitValue << bitPointer;
}
break;
case ModbusBridgeType::mb_int8:
if (jsonDataArrayPointer % 2)
writeData[jsonDataArrayPointer / 2] += (int8_t)jsonDataArray[jsonDataArrayPointer].getInt(0);
else
writeData[jsonDataArrayPointer / 2] = (int8_t)jsonDataArray[jsonDataArrayPointer / 2].getInt(0) << 8;
if (modbusBridge.dataCount != writeDataSize / 2) errorcode = ModbusBridgeError::wrongcount;
break;
case ModbusBridgeType::mb_hex:
case ModbusBridgeType::mb_raw:
case ModbusBridgeType::mb_uint8:
if (jsonDataArrayPointer % 2)
writeData[jsonDataArrayPointer / 2] += (uint8_t)jsonDataArray[jsonDataArrayPointer].getUInt(0);
else
writeData[jsonDataArrayPointer / 2] = (uint8_t)jsonDataArray[jsonDataArrayPointer].getUInt(0) << 8;
if (modbusBridge.dataCount != writeDataSize / 2) errorcode = ModbusBridgeError::wrongcount;
break;
case ModbusBridgeType::mb_int16:
writeData[jsonDataArrayPointer] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getInt(0))
: (int16_t)jsonDataArray[jsonDataArrayPointer].getInt(0);
break;
case ModbusBridgeType::mb_uint16:
writeData[jsonDataArrayPointer] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getUInt(0))
: (int16_t)jsonDataArray[jsonDataArrayPointer].getUInt(0);
break;
case ModbusBridgeType::mb_int32:
writeData[(jsonDataArrayPointer * 2)] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getInt(0))
: (int16_t)(jsonDataArray[jsonDataArrayPointer].getInt(0) >> 16);
writeData[(jsonDataArrayPointer * 2) + 1] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getInt(0) >> 16)
: (uint16_t)(jsonDataArray[jsonDataArrayPointer].getInt(0));
break;
case ModbusBridgeType::mb_uint32:
writeData[(jsonDataArrayPointer * 2)] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getUInt(0))
: (uint16_t)(jsonDataArray[jsonDataArrayPointer].getUInt(0) >> 16);
writeData[(jsonDataArrayPointer * 2) + 1] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getUInt(0) >> 16)
: (uint16_t)(jsonDataArray[jsonDataArrayPointer].getUInt(0));
break;
case ModbusBridgeType::mb_float:
// TODO
default:
errorcode = ModbusBridgeError::wrongtype;
break;
}
}
}
// Adapt data according to modbus protocol
if (modbusBridge.functionCode == ModbusBridgeFunctionCode::mb_writeSingleCoil)
{
writeData[0] = writeData[0] ? 0xFF00 : 0x0000; // High Byte
}
}
// Handle errorcode and exit function when an error has occured
@ -774,23 +948,18 @@ void CmndModbusBridgeSend(void)
return;
}
// Adapt data according to modbus protocol
for (uint8_t writeDataPointer = 0; writeDataPointer < modbusBridge.dataCount; writeDataPointer++)
{
// For function code 5, on and off are 0xFF00 and 0x0000, not 1 and 0
if (modbusBridge.functionCode == ModbusBridgeFunctionCode::mb_writeSingleCoil)
{
writeData[writeDataPointer] = writeData[writeDataPointer] ? 0xFF00 : 0x0000;
}
}
// If writing a single coil or single register, the register count is always 1. We also prevent writing data out of range
if ((modbusBridge.functionCode == ModbusBridgeFunctionCode::mb_writeSingleCoil) || (modbusBridge.functionCode == ModbusBridgeFunctionCode::mb_writeSingleRegister))
modbusBridge.dataCount = 1;
uint8_t error = tasmotaModbus->Send(modbusBridge.deviceAddress, (uint8_t)modbusBridge.functionCode, modbusBridge.startAddress, modbusBridge.dataCount, writeData);
if (error) AddLog(LOG_LEVEL_DEBUG, PSTR("MBS: MBR Driver send error %u"), error);
free(writeData);
if (error)
{
AddLog(LOG_LEVEL_DEBUG, PSTR("MBS: MBR Driver send error %u"), error);
return;
}
ResponseCmndDone();
}