380 lines
14 KiB
C++
380 lines
14 KiB
C++
#include "RS485_driver.h"
|
|
#include <algorithm>
|
|
#include <cstring>
|
|
#include <endian.h>
|
|
|
|
uint8_t data[][8] = {
|
|
// ESP32-S3-POE-ETH-8DI-8RO Control Command (RS485 receiving data)
|
|
{0x06, 0x05, 0x00, 0x01, 0x55, 0x00, 0xA2, 0xED}, // ESP32-S3-POE-ETH-8DI-8RO CH1 Toggle
|
|
{0x06, 0x05, 0x00, 0x02, 0x55, 0x00, 0x52, 0xED}, // ESP32-S3-POE-ETH-8DI-8RO CH2 Toggle
|
|
{0x06, 0x05, 0x00, 0x03, 0x55, 0x00, 0x03, 0x2D}, // ESP32-S3-POE-ETH-8DI-8RO CH3 Toggle
|
|
{0x06, 0x05, 0x00, 0x04, 0x55, 0x00, 0xB2, 0xEC}, // ESP32-S3-POE-ETH-8DI-8RO CH4 Toggle
|
|
{0x06, 0x05, 0x00, 0x05, 0x55, 0x00, 0xE3, 0x2C}, // ESP32-S3-POE-ETH-8DI-8RO CH5 Toggle
|
|
{0x06, 0x05, 0x00, 0x06, 0x55, 0x00, 0x13, 0x2C}, // ESP32-S3-POE-ETH-8DI-8RO CH6 Toggle
|
|
{0x06, 0x05, 0x00, 0x07, 0x55, 0x00, 0x42, 0xEC}, // ESP32-S3-POE-ETH-8DI-8RO CH7 Toggle
|
|
{0x06, 0x05, 0x00, 0x08, 0x55, 0x00, 0x72, 0xEF}, // ESP32-S3-POE-ETH-8DI-8RO CH8 Toggle
|
|
{0x06, 0x05, 0x00, 0xFF, 0xFF, 0x00, 0xBD, 0xBD}, // ESP32-S3-POE-ETH-8DI-8RO ALL ON
|
|
{0x06, 0x05, 0x00, 0xFF, 0x00, 0x00, 0xFC, 0x4D}, // ESP32-S3-POE-ETH-8DI-8RO ALL OFF
|
|
};
|
|
uint8_t Send_Data[][8] = {
|
|
// Modbus RTU Relay Control Command (RS485 send data)
|
|
{0x01, 0x05, 0x00, 0x00, 0x55, 0x00, 0xF2, 0x9A}, // Modbus RTU Relay CH1 Toggle
|
|
{0x01, 0x05, 0x00, 0x01, 0x55, 0x00, 0xA3, 0x5A}, // Modbus RTU Relay CH2 Toggle
|
|
{0x01, 0x05, 0x00, 0x02, 0x55, 0x00, 0x53, 0x5A}, // Modbus RTU Relay CH3 Toggle
|
|
{0x01, 0x05, 0x00, 0x03, 0x55, 0x00, 0x02, 0x9A}, // Modbus RTU Relay CH4 Toggle
|
|
{0x01, 0x05, 0x00, 0x04, 0x55, 0x00, 0xB3, 0x5B}, // Modbus RTU Relay CH5 Toggle
|
|
{0x01, 0x05, 0x00, 0x05, 0x55, 0x00, 0xE2, 0x9B}, // Modbus RTU Relay CH6 Toggle
|
|
{0x01, 0x05, 0x00, 0x06, 0x55, 0x00, 0x12, 0x9B}, // Modbus RTU Relay CH7 Toggle
|
|
{0x01, 0x05, 0x00, 0x07, 0x55, 0x00, 0x43, 0x5B}, // Modbus RTU Relay CH8 Toggle
|
|
{0x01, 0x05, 0x00, 0xFF, 0xFF, 0xFF, 0xFC, 0x4A}, // Modbus RTU Relay ALL ON
|
|
{0x01, 0x05, 0x00, 0xFF, 0x00, 0x00, 0xFD, 0xFA}, // Modbus RTU Relay ALL OFF
|
|
};
|
|
|
|
namespace drivers
|
|
{
|
|
|
|
////////////////////////////////
|
|
//////////// RS485 ////////////
|
|
////////////////////////////////
|
|
|
|
RS485::RS485(const uint32_t baud, const SerialConfig conf)
|
|
{
|
|
log_i("Init serial port 1");
|
|
m_serial = std::make_unique<HardwareSerial>(PORT); // RS485 is hardwired to serial port 1
|
|
m_serial->begin(baud, conf);
|
|
m_serial->setMode(UART_MODE_RS485_HALF_DUPLEX);
|
|
}
|
|
|
|
const bool RS485::write(const std::vector<uint8_t> data)
|
|
{
|
|
return data.size() == m_serial->write(data.data(), data.size());
|
|
}
|
|
|
|
const bool RS485::readAll(std::vector<uint8_t> &data)
|
|
{
|
|
const uint32_t avail(m_serial->available());
|
|
data.resize(avail);
|
|
return data.size() == m_serial->readBytes(data.data(), avail);
|
|
}
|
|
|
|
const bool RS485::readN(const uint16_t nBytes, std::vector<uint8_t> &data)
|
|
{
|
|
data.resize(nBytes);
|
|
return data.size() == m_serial->readBytes(data.data(), nBytes);
|
|
}
|
|
|
|
const bool RS485::readUntil(const uint8_t ch, std::vector<uint8_t> &data)
|
|
{
|
|
const uint32_t avail(m_serial->available());
|
|
data.resize(avail);
|
|
m_serial->readBytesUntil(ch, data.data(), avail);
|
|
data.shrink_to_fit();
|
|
return true;
|
|
}
|
|
|
|
////////////////////////////////
|
|
//////////// MODBUS ////////////
|
|
////////////////////////////////
|
|
|
|
MODBUS::MODBUS(const uint32_t baud, const SerialConfig conf) : RS485::RS485(baud, conf)
|
|
{
|
|
log_i("Init MODBUS Master Mode");
|
|
}
|
|
|
|
// Func 0x01
|
|
const bool MODBUS::readCoils(const uint8_t device, const uint16_t reg, const uint16_t num, std::vector<bool> &coils)
|
|
{
|
|
constexpr uint8_t func = 0x01;
|
|
log_d("Read coils: dev[%02x], reg[%04x], num[%d]", device, reg, num);
|
|
return readBinary(func, device, reg, num, coils);
|
|
}
|
|
|
|
// Func 0x02
|
|
const bool MODBUS::readInputs(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<bool> &inputs)
|
|
{
|
|
constexpr uint8_t func = 0x01;
|
|
log_d("Read multi inputs: dev[%02x], reg[%04x], num[%d]", device, reg, num);
|
|
return readBinary(func, device, reg, num, inputs);
|
|
}
|
|
|
|
// Func 0x03
|
|
const bool MODBUS::readHoldingRegisters(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<uint16_t> &values)
|
|
{
|
|
constexpr uint8_t func = 0x03;
|
|
log_d("Read multi holding registers: dev[%02x], reg[%04x], num[%d]", device, reg, num);
|
|
return readInteger(func, device, reg, num, values);
|
|
}
|
|
|
|
// Func 0x04
|
|
const bool MODBUS::readInputRegisters(const uint8_t device, const uint16_t reg, const uint8_t num, std::vector<uint16_t> &values)
|
|
{
|
|
constexpr uint8_t func = 0x04;
|
|
log_d("Read multi input registers: dev[%02x], reg[%04x], num[%d]", device, reg, num);
|
|
return readInteger(func, device, reg, num, values);
|
|
}
|
|
|
|
// Func 0x05
|
|
const bool MODBUS::writeCoil(const uint8_t device, const uint16_t coil, const bool value)
|
|
{
|
|
constexpr uint8_t func = 0x05;
|
|
log_d("Write single coil: dev[%02x], reg[%04x], val[...]", device, reg);
|
|
return writeBinary(device, func, coil, 1, {value});
|
|
}
|
|
|
|
// Func 0x06
|
|
const bool MODBUS::writeRegister(const uint8_t device, const uint16_t reg, const uint16_t value)
|
|
{
|
|
constexpr uint8_t func = 0x06;
|
|
log_d("Write single register: dev[%02x], reg[%04x], val[%04x]", device, reg, value);
|
|
return writeInteger(device, func, reg, 1, {value});
|
|
}
|
|
|
|
// Func 0x0F
|
|
const bool MODBUS::writeCoils(const uint8_t device, const uint16_t coils, const std::vector<bool> &values)
|
|
{
|
|
constexpr uint8_t func = 0x0F;
|
|
log_d("Write multi coils: dev[%02x], reg[%04x], val[...]", device, reg);
|
|
return writeBinary(device, func, coils, values.size(), values);
|
|
}
|
|
|
|
// Func 0x10
|
|
const bool MODBUS::writeRegisters(const uint8_t device, const uint16_t reg, const std::vector<uint16_t> &values)
|
|
{
|
|
constexpr uint8_t func = 0x10;
|
|
log_d("Write multi registers: dev[%02x], reg[%04x], val[...]", device, reg);
|
|
return writeInteger(device, func, reg, values.size(), values);
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////
|
|
/////////////////////// Utility Functions ///////////////////////
|
|
/////////////////////////////////////////////////////////////////
|
|
|
|
const bool MODBUS::readBinary(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t bits, std::vector<bool> &out)
|
|
{
|
|
if (!write(singleRequest(device, func, reg, bits)))
|
|
{
|
|
log_e("Failed send readBinary command");
|
|
return false;
|
|
}
|
|
const uint16_t nRespDataBytes = 1 + (uint16_t)(bits / 8); // 1 bit for every coil, if not 8 mutiple padded with zeroes
|
|
const uint16_t expectedRespLen = (RESP_HEADER_SIZE + RESP_CRC_SIZE) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
|
|
std::vector<uint8_t> response;
|
|
if (!readN(expectedRespLen, response))
|
|
{
|
|
log_e("Failed receive readBinary response");
|
|
return false;
|
|
}
|
|
|
|
// element 2 of response has the response data bytes expected
|
|
if (response.at(2) != nRespDataBytes)
|
|
{
|
|
log_e("Failed receive, data to short: bytes[%d], expected[%d]", response.at(2), nRespDataBytes);
|
|
return false;
|
|
}
|
|
|
|
// compute crc of current message
|
|
if (!verifyCrc(response))
|
|
return false;
|
|
|
|
// extract coils data from data portion of response
|
|
out.clear();
|
|
out.reserve(bits);
|
|
uint16_t bitNum(0);
|
|
|
|
// get response data bytes excluding header and crc
|
|
const std::vector<uint8_t> respData(response.begin() + RESP_HEADER_SIZE, response.end() - RESP_CRC_SIZE);
|
|
for (auto it = respData.begin(); it < respData.end(); it++)
|
|
{
|
|
const auto v = *it;
|
|
for (uint8_t j(0); j < 8 && bitNum < bits; j++)
|
|
{
|
|
const auto cv((0x01 << j) && v);
|
|
out.push_back(cv);
|
|
bitNum++;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
const bool MODBUS::readInteger(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t num, std::vector<uint16_t> &out)
|
|
{
|
|
if (!write(singleRequest(device, func, reg, num)))
|
|
{
|
|
log_e("Failed send readInteger command");
|
|
return false;
|
|
}
|
|
const uint16_t nRespDataBytes = num * sizeof(uint16_t);
|
|
const uint16_t expectedRespLen = (RESP_HEADER_SIZE + RESP_CRC_SIZE) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
|
|
std::vector<uint8_t> response;
|
|
if (!readN(expectedRespLen, response))
|
|
{
|
|
log_e("Failed receive readInteger response");
|
|
return false;
|
|
}
|
|
|
|
// element 2 of response has the response data bytes expected
|
|
if (response.at(2) != nRespDataBytes)
|
|
{
|
|
log_e("Failed receive, data to short: bytes[%d], expected[%d]", response.at(2), nRespDataBytes);
|
|
return false;
|
|
}
|
|
|
|
// compute crc of current message
|
|
if (!verifyCrc(response))
|
|
return false;
|
|
|
|
// extract coils data from data portion of response
|
|
out.clear();
|
|
out.reserve(nRespDataBytes);
|
|
// get response data bytes excluding header and crc
|
|
const std::vector<uint8_t> respData(response.begin() + RESP_HEADER_SIZE, response.end() - RESP_CRC_SIZE);
|
|
for (auto i(0); i < nRespDataBytes; i++)
|
|
{
|
|
const uint8_t hi(respData.at(i * sizeof(uint16_t)));
|
|
const uint8_t lo(respData.at(1 + i * sizeof(uint16_t)));
|
|
const uint16_t val(0xFFFF & ((hi << 8) | lo));
|
|
out.push_back(be16toh(val));
|
|
}
|
|
return true;
|
|
}
|
|
|
|
const bool MODBUS::writeBinary(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t bits, const std::vector<bool> &in)
|
|
{
|
|
std::vector<uint16_t> bitsOut;
|
|
if (bits <= 1) // if single coil value must be 0x00FF[00] for on[off]
|
|
{
|
|
bitsOut.push_back(htobe16(in.front() ? 0x00FF : 0x0000));
|
|
}
|
|
else // if multiple coils value is 0x01 shifted for the number of coil intended
|
|
{
|
|
const uint16_t numRegisters((uint16_t)(bits / 16) + 1);
|
|
bitsOut.resize(numRegisters, 0);
|
|
for (uint16_t i(0); i < in.size(); i++)
|
|
{
|
|
if (!in[i]) // if value is false skip
|
|
continue;
|
|
const uint16_t curReg(i / 16);
|
|
bitsOut[curReg] |= 0x01 << i % 16;
|
|
}
|
|
}
|
|
|
|
if (!write(multiRequest(device, func, reg, bits, bitsOut)))
|
|
{
|
|
log_e("Failed send writeBinary command");
|
|
return false;
|
|
}
|
|
|
|
const uint16_t expectedRespLen(sizeof(resp_t));
|
|
std::vector<uint8_t> response;
|
|
if (!readN(expectedRespLen, response))
|
|
{
|
|
log_e("Failed receive writeBinary response");
|
|
return false;
|
|
}
|
|
|
|
// compute crc of current message
|
|
if (!verifyCrc(response))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
const bool MODBUS::writeInteger(const uint8_t func, const uint8_t device, const uint16_t reg, const uint16_t num, const std::vector<uint16_t> &in)
|
|
{
|
|
if (!write(multiRequest(device, func, reg, num, in)))
|
|
{
|
|
log_e("Failed send writeInteger command");
|
|
return false;
|
|
}
|
|
|
|
const uint16_t expectedRespLen(sizeof(resp_t));
|
|
std::vector<uint8_t> response;
|
|
if (!readN(expectedRespLen, response))
|
|
{
|
|
log_e("Failed receive writeInteger response");
|
|
return false;
|
|
}
|
|
|
|
// compute crc of current message
|
|
if (!verifyCrc(response))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
const std::vector<uint8_t> MODBUS::singleRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t data)
|
|
{
|
|
req_t header;
|
|
header.device = device;
|
|
header.func = func;
|
|
header.reg = htobe16(reg);
|
|
header.data = htobe16(data);
|
|
|
|
const uint8_t headerBytes(sizeof(req_t));
|
|
const uint8_t crcBytes(sizeof(crc_t));
|
|
|
|
// compute crc for header + data
|
|
m_crc.reset();
|
|
m_crc.add((uint8_t *)&header, headerBytes); // exclude last two bytes of crc
|
|
const uint16_t crc(htole16(m_crc.calc()));
|
|
|
|
std::vector<uint8_t> dataOut(headerBytes + crcBytes, 0);
|
|
std::memcpy(dataOut.data(), &header, headerBytes);
|
|
std::memcpy(dataOut.data() + headerBytes, &crc, crcBytes);
|
|
return dataOut;
|
|
}
|
|
|
|
const std::vector<uint8_t> MODBUS::multiRequest(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t qty, const std::vector<uint16_t> &data)
|
|
{
|
|
req_multi_t header;
|
|
header.device = device;
|
|
header.func = func;
|
|
header.reg = htobe16(reg);
|
|
header.qty = htobe16(qty);
|
|
header.bytes = (uint8_t)(data.size() * sizeof(uint16_t)); // 8 bit value
|
|
|
|
// convert uint16_t values from host endianness to big endian
|
|
std::vector<uint16_t> dataBe;
|
|
dataBe.reserve(data.size());
|
|
std::for_each(data.begin(), data.end(), [&dataBe](auto v)
|
|
{ dataBe.push_back(htobe16(v)); });
|
|
|
|
const uint8_t headerBytes(sizeof(req_multi_t));
|
|
const uint8_t dataBytes(sizeof(uint16_t) * dataBe.size());
|
|
const uint8_t crcBytes(sizeof(crc_t));
|
|
|
|
// compute crc for header + data
|
|
m_crc.reset();
|
|
m_crc.add((uint8_t *)&header, headerBytes); // add the request excluding the CRC code
|
|
m_crc.add((uint8_t *)dataBe.data(), dataBytes);
|
|
const uint16_t crc(htole16(m_crc.calc()));
|
|
|
|
std::vector<uint8_t> dataOut;
|
|
dataOut.resize(headerBytes + dataBytes + crcBytes); // header message + data values + crc code
|
|
std::memcpy(dataOut.data(), &header, headerBytes); // copy message
|
|
std::memcpy(dataOut.data() + headerBytes, dataBe.data(), dataBytes); // copy data
|
|
std::memcpy(dataOut.data() + headerBytes + dataBytes, &crc, crcBytes); // copy crc
|
|
return dataOut;
|
|
}
|
|
|
|
const bool MODBUS::verifyCrc(const std::vector<uint8_t> &data)
|
|
{
|
|
// compute crc of current message
|
|
m_crc.reset();
|
|
m_crc.add(data.data(), data.size());
|
|
const uint16_t computedCrc(m_crc.calc());
|
|
// extract crc from response
|
|
const uint16_t size(data.size());
|
|
const uint8_t crcLo(data.at(size - 2));
|
|
const uint8_t crcHi(data.at(size - 1));
|
|
|
|
// verify crc code
|
|
if (highByte(computedCrc) != crcHi || lowByte(computedCrc) != crcLo)
|
|
{
|
|
log_e("Failed verify CRC code: comp[%04x], rec[%04x]", computedCrc, 0xFFFF & ((crcHi << 8) | crcLo));
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
} |