448 lines
15 KiB
C++
448 lines
15 KiB
C++
#include "RS485_driver.h"
|
|
#include <algorithm>
|
|
#include <cstring>
|
|
#include <endian.h>
|
|
|
|
#include "utils.h"
|
|
|
|
namespace drivers
|
|
{
|
|
|
|
////////////////////////////////
|
|
//////////// RS485 ////////////
|
|
////////////////////////////////
|
|
|
|
RS485::RS485(const uint32_t baud, const SerialConfig conf) : m_serial(Serial1)
|
|
{
|
|
LOG_INFO("Init serial port 1");
|
|
// RS485 is hardwired to serial port 1
|
|
m_serial.begin(baud, conf, 18, 17);
|
|
m_serial.setTimeout(1000);
|
|
m_serial.flush();
|
|
}
|
|
|
|
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());
|
|
if (avail == 0)
|
|
return true;
|
|
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)
|
|
{
|
|
std::vector<uint8_t> buf;
|
|
buf.resize(nBytes);
|
|
if (m_serial.readBytes(buf.data(), nBytes) == nBytes)
|
|
{
|
|
data = std::move(buf);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
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)
|
|
{
|
|
std::vector<uint8_t> garbage;
|
|
readAll(garbage);
|
|
LOG_INFO("Init MODBUS Master Mode");
|
|
m_crc.reset(CRC16_MODBUS_POLYNOME, CRC16_MODBUS_INITIAL, CRC16_MODBUS_XOR_OUT, CRC16_MODBUS_REV_IN, CRC16_MAXIM_REV_OUT);
|
|
m_lastAccess = millis();
|
|
m_lastDevice = 0;
|
|
}
|
|
|
|
// Get transaction lock
|
|
std::unique_lock<std::mutex> MODBUS::getLock()
|
|
{
|
|
return std::unique_lock<std::mutex>(m_mutex);
|
|
}
|
|
|
|
std::mutex &MODBUS::getMutex()
|
|
{
|
|
return m_mutex;
|
|
}
|
|
|
|
void MODBUS::delayAccess(const uint8_t device)
|
|
{
|
|
if (device == m_lastDevice)
|
|
return;
|
|
auto now = millis();
|
|
if ((now - m_lastAccess) < c_minDelay) // fixed milliseconds delay between commands to different devices
|
|
{
|
|
LOG_WARN("MODBUS access delay", (now - m_lastAccess), "device", device);
|
|
delay(now - m_lastAccess);
|
|
}
|
|
m_lastDevice = device;
|
|
m_lastAccess = millis();
|
|
}
|
|
|
|
// 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;
|
|
delayAccess(device);
|
|
LOG_DEBUG("Read coils: dev[", device, "], reg[", reg, "], num[", num, "]");
|
|
return readBinary(device, func, 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 = 0x02;
|
|
delayAccess(device);
|
|
LOG_DEBUG("Read multi inputs: dev[", device, "], reg[", reg, "], num[", num, "]");
|
|
return readBinary(device, func, 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;
|
|
delayAccess(device);
|
|
LOG_DEBUG("Read multi holding registers: dev[", device, "], reg[", reg, "], num[", num, "]");
|
|
return readInteger(device, func, 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;
|
|
delayAccess(device);
|
|
LOG_DEBUG("Read multi input registers: dev[", device, "], reg[", reg, "], num[", num, "]");
|
|
return readInteger(device, func, 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;
|
|
delayAccess(device);
|
|
LOG_DEBUG("Write single coil: dev[", device, "], coil[", coil, "], value[", value ? "true" : "false", "]");
|
|
return writeBinary(device, func, coil, {value});
|
|
}
|
|
|
|
// Func 0x06
|
|
const bool MODBUS::writeRegister(const uint8_t device, const uint16_t reg, const uint16_t value)
|
|
{
|
|
constexpr uint8_t func = 0x06;
|
|
delayAccess(device);
|
|
LOG_DEBUG("Write single register: dev[", device, "], reg[", reg, "], value[", value, "]");
|
|
return writeInteger(device, func, reg, {value}, false);
|
|
}
|
|
|
|
// Func 0x0F
|
|
const bool MODBUS::writeCoils(const uint8_t device, const uint16_t coils, const std::vector<bool> &values)
|
|
{
|
|
constexpr uint8_t func = 0x0F;
|
|
delayAccess(device);
|
|
LOG_DEBUG("Write multi coils: dev[", device, "], start[", coils, "], num[", values.size(), "]");
|
|
return writeBinary(device, func, coils, 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;
|
|
delayAccess(device);
|
|
LOG_DEBUG("Write multi registers: dev[", device, "], start[", reg, "], num[", values.size(), "]");
|
|
return writeInteger(device, func, reg, values, true);
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////
|
|
/////////////////////// Utility Functions ///////////////////////
|
|
/////////////////////////////////////////////////////////////////
|
|
|
|
const bool MODBUS::readBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t bits, std::vector<bool> &out)
|
|
{
|
|
if (!write(singleRequest(device, func, reg, bits)))
|
|
{
|
|
LOG_ERROR("Failed send readBinary command");
|
|
return false;
|
|
}
|
|
const uint16_t nRespDataBytes = (uint16_t)ceil(bits / 8.0f); // 1 bit for every coil, if not 8 mutiple padded with zeroes
|
|
const uint16_t expectedRespLen = (c_respHeaderSize + c_respCrcSize) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
|
|
std::vector<uint8_t> response;
|
|
if (!readN(expectedRespLen, response))
|
|
{
|
|
LOG_ERROR("Failed receive readBinary response, expected[", expectedRespLen, "], received[", response.size(), "]");
|
|
return false;
|
|
}
|
|
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
|
|
printBytes("readBinary Response", response);
|
|
#endif
|
|
|
|
// element 2 of response has the response data bytes expected
|
|
const uint8_t actualRespLen(response.at(2));
|
|
if (actualRespLen != nRespDataBytes)
|
|
{
|
|
LOG_ERROR("Failed receive, data to short: actual[", actualRespLen, "], expected[", 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() + c_respHeaderSize, response.end() - sizeof(crc_t));
|
|
for (auto it = respData.begin(); it < respData.end(); it++)
|
|
{
|
|
for (uint8_t j(0); j < 8 && bitNum < bits; j++)
|
|
{
|
|
const bool cv((0x01 << j) & *it);
|
|
out.push_back(cv);
|
|
bitNum++;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
const bool MODBUS::readInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const uint16_t num, std::vector<uint16_t> &out)
|
|
{
|
|
if (!write(singleRequest(device, func, reg, num)))
|
|
{
|
|
LOG_ERROR("Failed send readInteger command");
|
|
return false;
|
|
}
|
|
const uint16_t nRespDataBytes = num * sizeof(uint16_t);
|
|
const uint16_t expectedRespLen = (c_respHeaderSize + sizeof(crc_t)) + nRespDataBytes; // device + function + nbytes + data[] + crc(16b)
|
|
std::vector<uint8_t> response;
|
|
if (!readN(expectedRespLen, response))
|
|
{
|
|
LOG_ERROR("Failed receive readInteger response, expected[", expectedRespLen, "], received[", response.size(), "]");
|
|
return false;
|
|
}
|
|
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
|
|
printBytes("readInteger Response", response);
|
|
#endif
|
|
|
|
// element 2 of response has the response data bytes expected
|
|
const uint8_t actualRespLen(response.at(2));
|
|
if (actualRespLen != nRespDataBytes)
|
|
{
|
|
LOG_ERROR("Failed receive, data to short: actual[", actualRespLen, "], expected[", 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 / sizeof(uint16_t));
|
|
// get response data bytes excluding header and crc
|
|
const std::vector<uint8_t> respData(response.begin() + c_respHeaderSize, response.end() - c_respCrcSize);
|
|
for (auto it = respData.begin(); it < respData.end(); it++)
|
|
{
|
|
const uint8_t lo(*it++);
|
|
const uint8_t hi(*it);
|
|
const uint16_t val(0xFFFF & ((hi << 8) | lo));
|
|
out.push_back(be16toh(val));
|
|
}
|
|
return true;
|
|
}
|
|
|
|
const bool MODBUS::writeBinary(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector<bool> &in)
|
|
{
|
|
const uint16_t bits(in.size());
|
|
std::vector<uint8_t> bitsOut;
|
|
if (bits == 1) // if single coil value must be 0x00FF[00] for on[off]
|
|
{
|
|
if (!write(singleRequest(device, func, reg, in.front() ? 0xFF00 : 0x0000)))
|
|
{
|
|
LOG_ERROR("Failed send writeSingleBinary command");
|
|
return false;
|
|
}
|
|
}
|
|
else // if multiple coils value is 0x01 shifted for the number of coil intended
|
|
{
|
|
const uint16_t numBytes((uint16_t)ceil(bits / 8.0f));
|
|
bitsOut.resize(numBytes, 0x00);
|
|
for (uint16_t i(0); i < in.size(); i++)
|
|
{
|
|
if (!in[i]) // if value is false skip
|
|
continue;
|
|
bitsOut[i / 8] |= 0x01 << i % 8;
|
|
}
|
|
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
|
|
LOG_DEBUG("\nnumBytes", numBytes);
|
|
printBool("bitsOut", in);
|
|
printBytes("bitsOut", bitsOut);
|
|
#endif
|
|
if (!write(multiRequest(device, func, reg, bits, bitsOut)))
|
|
{
|
|
LOG_ERROR("Failed send writeMultiBinary command");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
const uint16_t expectedRespLen(sizeof(resp_t) + sizeof(crc_t));
|
|
std::vector<uint8_t> response;
|
|
if (!readN(expectedRespLen, response))
|
|
{
|
|
LOG_ERROR("Failed receive writeBinary response, expected[", expectedRespLen, "], received[", response.size(), "]");
|
|
return false;
|
|
}
|
|
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
|
|
printBytes("writeBinary Response", response);
|
|
#endif
|
|
|
|
// compute crc of current message
|
|
if (!verifyCrc(response))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
const bool MODBUS::writeInteger(const uint8_t device, const uint8_t func, const uint16_t reg, const std::vector<uint16_t> &in, const bool multi)
|
|
{
|
|
const uint16_t num(in.size());
|
|
if (!multi)
|
|
{
|
|
if (!write(singleRequest(device, func, reg, in[0])))
|
|
{
|
|
LOG_ERROR("Failed send writeSingleInteger command");
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// build data vector for request, inverting bytes if necessary
|
|
std::vector<uint8_t> requestData;
|
|
requestData.resize(in.size() * sizeof(uint16_t), 0xff);
|
|
auto it = requestData.begin();
|
|
for (auto inV : in)
|
|
{
|
|
const uint16_t beV(htobe16(inV));
|
|
*(it++) = lowByte(beV);
|
|
*(it++) = highByte(beV);
|
|
}
|
|
if (!write(multiRequest(device, func, reg, num, requestData)))
|
|
{
|
|
LOG_ERROR("Failed send writeMultiInteger command");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
const uint16_t expectedRespLen(sizeof(resp_t) + sizeof(crc_t));
|
|
std::vector<uint8_t> response;
|
|
if (!readN(expectedRespLen, response))
|
|
{
|
|
LOG_ERROR("Failed receive writeInteger response, expected[", expectedRespLen, "], received[", response.size(), "]");
|
|
return false;
|
|
}
|
|
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
|
|
printBytes("writeInteger Response", response);
|
|
#endif
|
|
|
|
// 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.restart();
|
|
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);
|
|
|
|
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
|
|
printBytes("singleRequest", dataOut);
|
|
#endif
|
|
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<uint8_t> &data)
|
|
{
|
|
req_multi_t header;
|
|
header.device = device;
|
|
header.func = func;
|
|
header.reg = htobe16(reg);
|
|
header.qty = htobe16(qty);
|
|
header.bytes = data.size(); // 8 bit value
|
|
|
|
// const uint8_t headerBytes(sizeof(req_multi_t)); // sizeof not working because of memory padding
|
|
const uint8_t headerBytes(7);
|
|
const uint8_t dataBytes(data.size());
|
|
const uint8_t crcBytes(sizeof(crc_t));
|
|
|
|
// compute crc for header + data
|
|
m_crc.restart();
|
|
m_crc.add((uint8_t *)&header, headerBytes); // add the request excluding the CRC code
|
|
m_crc.add((uint8_t *)data.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, data.data(), dataBytes); // copy data
|
|
std::memcpy(dataOut.data() + headerBytes + dataBytes, &crc, crcBytes); // copy crc
|
|
|
|
#ifdef DEBUGLOG_DEFAULT_LOG_LEVEL_TRACE
|
|
printBytes("multiRequest", dataOut);
|
|
#endif
|
|
return dataOut;
|
|
}
|
|
|
|
const bool MODBUS::verifyCrc(const std::vector<uint8_t> &data)
|
|
{
|
|
// compute crc of current message
|
|
m_crc.restart();
|
|
m_crc.add(data.data(), data.size() - sizeof(crc_t));
|
|
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));
|
|
const uint16_t receivedCrc(0xFFFF & ((crcHi << 8) | crcLo));
|
|
|
|
// verify crc code
|
|
if (highByte(computedCrc) != crcHi || lowByte(computedCrc) != crcLo)
|
|
{
|
|
LOG_ERROR("Failed verify CRC code: comp[", computedCrc, "], rec[", receivedCrc, "]");
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
} |