mirror of
https://github.com/DCC-EX/CommandStation-EX.git
synced 2025-04-21 12:31:19 +02:00
393 lines
11 KiB
C++
393 lines
11 KiB
C++
/*
|
|
* © 2024, Travis Farmer. All rights reserved.
|
|
* © 2024, Chris Bulliner. All rights reserved. https://github.com/CMB27
|
|
*
|
|
* This file is part of DCC++EX API
|
|
*
|
|
* This is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* It is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "IO_Modbus.h"
|
|
#include "defines.h"
|
|
|
|
|
|
void Modbus::setTransactionId(uint16_t transactionId) {
|
|
_setRegister(tcp, 0, transactionId);
|
|
}
|
|
|
|
void Modbus::setProtocolId(uint16_t protocolId) {
|
|
_setRegister(tcp, 2, protocolId);
|
|
}
|
|
|
|
void Modbus::setLength(uint16_t length) {
|
|
if (length < 3 || length > 254) _setRegister(tcp, 4, 0);
|
|
else _setRegister(tcp, 4, length);
|
|
}
|
|
|
|
void Modbus::setUnitId(uint8_t unitId) {
|
|
tcp[6] = unitId;
|
|
}
|
|
|
|
void Modbus::setFunctionCode(uint8_t functionCode) {
|
|
pdu[0] = functionCode;
|
|
}
|
|
|
|
void Modbus::setDataRegister(uint8_t index, uint16_t value) {
|
|
_setRegister(data, index, value);
|
|
}
|
|
|
|
|
|
|
|
void Modbus::setRtuLen(uint16_t rtuLen) {
|
|
setLength(rtuLen - 2);
|
|
}
|
|
|
|
void Modbus::setTcpLen(uint16_t tcpLen) {
|
|
setLength(tcpLen - 6);
|
|
}
|
|
|
|
void Modbus::setPduLen(uint16_t pduLen) {
|
|
setLength(pduLen + 1);
|
|
}
|
|
|
|
void Modbus::setDataLen(uint16_t dataLen) {
|
|
setLength(dataLen + 2);
|
|
}
|
|
|
|
|
|
|
|
uint16_t Modbus::getTransactionId() {
|
|
return _getRegister(tcp, 0);
|
|
}
|
|
|
|
uint16_t Modbus::getProtocolId() {
|
|
return _getRegister(tcp, 2);
|
|
}
|
|
|
|
uint16_t Modbus::getLength() {
|
|
uint16_t length = _getRegister(tcp, 4);
|
|
if (length < 3 || length > 254) return 0;
|
|
else return length;
|
|
}
|
|
|
|
uint8_t Modbus::getUnitId() {
|
|
return tcp[6];
|
|
}
|
|
|
|
uint8_t Modbus::getFunctionCode() {
|
|
return pdu[0];
|
|
}
|
|
|
|
uint16_t Modbus::getDataRegister(uint8_t index) {
|
|
return _getRegister(data, index);
|
|
}
|
|
|
|
|
|
|
|
uint16_t Modbus::getRtuLen() {
|
|
uint16_t len = getLength();
|
|
if (len == 0) return 0;
|
|
else return len + 2;
|
|
}
|
|
|
|
uint16_t Modbus::getTcpLen() {
|
|
uint16_t len = getLength();
|
|
if (len == 0) return 0;
|
|
else return len + 6;
|
|
}
|
|
|
|
uint16_t Modbus::getPduLen() {
|
|
uint16_t len = getLength();
|
|
if (len == 0) return 0;
|
|
else return len - 1;
|
|
}
|
|
|
|
uint16_t Modbus::getDataLen() {
|
|
uint16_t len = getLength();
|
|
if (len == 0) return 0;
|
|
else return len - 2;
|
|
}
|
|
|
|
|
|
|
|
void Modbus::updateCrc(uint8_t *buf, uint16_t len) {
|
|
uint16_t crc = _calculateCrc(buf, len);
|
|
buf[len] = lowByte(crc);
|
|
buf[len + 1] = highByte(crc);
|
|
}
|
|
|
|
bool Modbus::crcGood(uint8_t *buf, uint16_t len) {
|
|
uint16_t aduCrc = buf[len] | (buf[len + 1] << 8);
|
|
uint16_t calculatedCrc = _calculateCrc(buf, len);
|
|
if (aduCrc == calculatedCrc) return true;
|
|
else return false;
|
|
}
|
|
|
|
void Modbus::_setRegister(uint8_t *buf, uint16_t index, uint16_t value) {
|
|
buf[index] = highByte(value);
|
|
buf[index + 1] = lowByte(value);
|
|
}
|
|
|
|
uint16_t Modbus::_getRegister(uint8_t *buf, uint16_t index) {
|
|
return (buf[index] << 8) | buf[index + 1];
|
|
}
|
|
|
|
uint16_t Modbus::_calculateCrc(uint8_t *buf, uint16_t len) {
|
|
uint16_t value = 0xFFFF;
|
|
for (uint16_t i = 0; i < len; i++) {
|
|
value ^= (uint16_t)buf[i];
|
|
for (uint8_t j = 0; j < 8; j++) {
|
|
bool lsb = value & 1;
|
|
value >>= 1;
|
|
if (lsb == true) value ^= 0xA001;
|
|
}
|
|
}
|
|
return value;
|
|
}
|
|
|
|
|
|
|
|
uint16_t div8RndUp(uint16_t value) {
|
|
return (value + 7) >> 3;
|
|
}
|
|
|
|
void Modbus::clearRxBuffer() {
|
|
unsigned long startMicros = micros();
|
|
do {
|
|
if (_serialD->available() > 0) {
|
|
startMicros = micros();
|
|
_serialD->read();
|
|
}
|
|
} while (micros() - startMicros < _frameTimeout);
|
|
}
|
|
|
|
|
|
/************************************************************
|
|
* Modbus implementation
|
|
************************************************************/
|
|
|
|
|
|
// Constructor for Modbus
|
|
Modbus::Modbus(uint8_t busNo, HardwareSerial &serial, unsigned long baud, uint16_t cycleTimeMS, int8_t txPin, int waitA, int waitB) {
|
|
_baud = baud;
|
|
_serialD = &serial;
|
|
_txPin = txPin;
|
|
_busNo = busNo;
|
|
_cycleTime = cycleTimeMS * 1000UL; // convert from milliseconds to microseconds.
|
|
_waitA = waitA;
|
|
_waitB = waitB;
|
|
if (_waitA < 3) _waitA = 3;
|
|
if (_waitB < 2) _waitB = 2;
|
|
// Add device to HAL device chain
|
|
IODevice::addDevice(this);
|
|
|
|
// Add bus to Modbus chain.
|
|
_nextBus = _busList;
|
|
_busList = this;
|
|
}
|
|
|
|
// Main loop function for Modbus.
|
|
// Work through list of nodes. For each node, in separate loop entries
|
|
// When the slot time has finished, move on to the next device.
|
|
void Modbus::_loop(unsigned long currentMicros) {
|
|
_currentMicros = currentMicros;
|
|
|
|
if (_currentNode == NULL) {
|
|
_currentNode = _nodeListStart;
|
|
|
|
}
|
|
|
|
if (_currentMicros - _cycleStartTime < _cycleTime) return;
|
|
_cycleStartTime = _currentMicros;
|
|
if (_currentNode == NULL) return;
|
|
|
|
bool flagOK = true;
|
|
#if defined(MODBUS_STM_COMM)
|
|
ArduinoPins::fastWriteDigital(MODBUS_STM_COMM,HIGH);
|
|
#endif
|
|
|
|
if (taskCnt > 0) {
|
|
// run through tasks
|
|
int* taskData[25];
|
|
getNextTask(taskData);
|
|
switch((int) taskData[0]) {
|
|
case 0:
|
|
// protection for pulling empty task
|
|
break;
|
|
case 1: // configure pin
|
|
if (taskData[4] == (int*) CONFIGURE_INPUT) {
|
|
uint8_t pullup = (uint8_t) taskData[6];
|
|
uint8_t outBuffer[6] = {EXIODPUP, (uint8_t) taskData[0], (uint8_t)taskData[3], pullup};
|
|
uint8_t responseBuffer[3];
|
|
updateCrc(outBuffer,sizeof(outBuffer)-2);
|
|
if (_txPin != VPIN_NONE) ArduinoPins::fastWriteDigital(_txPin, HIGH);
|
|
_serialD->write(outBuffer, sizeof(outBuffer));
|
|
_serialD->flush();
|
|
if (_txPin != VPIN_NONE) ArduinoPins::fastWriteDigital(_txPin, LOW);
|
|
unsigned long startMillis = millis();
|
|
if (!_serialD->available()) {
|
|
if (waitReceive == true && _waitCounter > _waitA) {
|
|
|
|
}
|
|
if (millis() - startMillis >= 500) return;
|
|
}
|
|
uint16_t len = 0;
|
|
unsigned long startMicros = micros();
|
|
do {
|
|
if (_serialD->available()) {
|
|
startMicros = micros();
|
|
responseBuffer[len] = _serialD->read();
|
|
len++;
|
|
}
|
|
} while (micros() - startMicros <= 500 && len < 256);
|
|
if (crcGood(responseBuffer,sizeof(responseBuffer)-2)) {
|
|
if (responseBuffer[0] == EXIORDY) {
|
|
} else {
|
|
DIAG(F("EXIOMB Vpin %u cannot be used as a digital input pin"), (int)taskData[2]);
|
|
}
|
|
}
|
|
} else if (taskData[3] == (int*) CONFIGURE_ANALOGINPUT) {
|
|
// TODO: Consider moving code from _configureAnalogIn() to here and remove _configureAnalogIn
|
|
// from IODevice class definition. Not urgent, but each virtual function defined
|
|
// means increasing the RAM requirement of every HAL device driver, whether it's relevant
|
|
// to the driver or not.
|
|
}
|
|
break;
|
|
case 2: // configure analog in
|
|
uint8_t commandBuffer[5] = {EXIOENAN, (uint8_t) taskData[0], (uint8_t) taskData[3]};
|
|
uint8_t responseBuffer[3];
|
|
updateCrc(commandBuffer,sizeof(commandBuffer)-2);
|
|
if (_txPin != VPIN_NONE) ArduinoPins::fastWriteDigital(_txPin, HIGH);
|
|
_serialD->write(commandBuffer, sizeof(commandBuffer));
|
|
_serialD->flush();
|
|
if (_txPin != VPIN_NONE) ArduinoPins::fastWriteDigital(_txPin, LOW);
|
|
unsigned long startMillis = millis();
|
|
if (!_serialD->available()) {
|
|
if (waitReceive == true && _waitCounter > _waitA) {
|
|
|
|
}
|
|
if (millis() - startMillis >= 500) return;
|
|
}
|
|
uint16_t len = 0;
|
|
unsigned long startMicros = micros();
|
|
do {
|
|
if (_serialD->available()) {
|
|
startMicros = micros();
|
|
responseBuffer[len] = _serialD->read();
|
|
len++;
|
|
}
|
|
} while (micros() - startMicros <= 500 && len < 256);
|
|
|
|
if (crcGood(responseBuffer,sizeof(responseBuffer)-2)) {
|
|
if (responseBuffer[0] != EXIORDY) {
|
|
DIAG(F("EX-IOExpanderMB: Vpin %u on node %d cannot be used as an analogue input pin"), (int) taskData[2], (int) taskData[0]);
|
|
}
|
|
}
|
|
break;
|
|
case 3: // write pin
|
|
uint8_t digitalOutBuffer[6];
|
|
uint8_t responseBuffer[3];
|
|
digitalOutBuffer[0] = EXIOWRD;
|
|
digitalOutBuffer[1] = (uint8_t) taskData[0];
|
|
digitalOutBuffer[2] = (uint8_t) taskData[3];
|
|
digitalOutBuffer[3] = value;
|
|
uint8_t status = I2CManager.read(_I2CAddress, responseBuffer, 1, digitalOutBuffer, 3);
|
|
if (status != I2C_STATUS_OK) {
|
|
reportError(status);
|
|
} else {
|
|
if (responseBuffer[0] != EXIORDY) {
|
|
DIAG(F("Vpin %u cannot be used as a digital output pin"), (int)vpin);
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
// receive states
|
|
|
|
}
|
|
|
|
if (error == MODBUS_RTU_MASTER_WAITING) {
|
|
if (_waitCounter > _waitA) { // retry after 10 cycles of waiting, or user setting waitA.
|
|
_waitCounter = 0;
|
|
_waitCounterB++;
|
|
} else {
|
|
_waitCounter++;
|
|
}
|
|
if (_waitCounterB > _waitB) { // move on to next node if fails 10 times, or user setting waitB.
|
|
_waitCounter = 0;
|
|
_waitCounterB = 0;
|
|
_operationCount = 0;
|
|
|
|
_currentNode = _currentNode->getNext();
|
|
}
|
|
} else {
|
|
_waitCounter = 0;
|
|
_waitCounterB = 0;
|
|
}
|
|
|
|
if (error == MODBUS_RTU_MASTER_SUCCESS) { // should have the effect of retrying same opperation until success
|
|
if (_operationCount < 3) { // unless it fails waitB and moves on to next node. may even
|
|
_operationCount++; // improve error recovery...
|
|
} else {
|
|
_operationCount = 0;
|
|
_currentNode = _currentNode->getNext();
|
|
}
|
|
}
|
|
#if defined(MODBUS_STM_OK)
|
|
if (flagOK == true) {
|
|
ArduinoPins::fastWriteDigital(MODBUS_STM_OK,HIGH);
|
|
} else {
|
|
ArduinoPins::fastWriteDigital(MODBUS_STM_OK,LOW);
|
|
}
|
|
#endif
|
|
#if defined(MODBUS_STM_FAIL)
|
|
if (flagOK == false) {
|
|
ArduinoPins::fastWriteDigital(MODBUS_STM_FAIL,HIGH);
|
|
} else {
|
|
ArduinoPins::fastWriteDigital(MODBUS_STM_FAIL,LOW);
|
|
}
|
|
#endif
|
|
#if defined(MODBUS_STM_COMM)
|
|
ArduinoPins::fastWriteDigital(MODBUS_STM_COMM,LOW);
|
|
#endif
|
|
|
|
}
|
|
|
|
// Link to chain of Modbus instances
|
|
Modbus *Modbus::_busList = NULL;
|
|
|
|
|
|
/************************************************************
|
|
* Modbusnode implementation
|
|
************************************************************/
|
|
|
|
// Constructor for Modbusnode object
|
|
Modbusnode::Modbusnode(VPIN firstVpin, int nPins, uint8_t busNo, uint8_t nodeID) {
|
|
_firstVpin = firstVpin;
|
|
_nPins = nPins;
|
|
_busNo = busNo;
|
|
_nodeID = nodeID;
|
|
if (_nodeID > 255) _nodeID = 255;
|
|
|
|
// Add this device to HAL device list
|
|
IODevice::addDevice(this);
|
|
_display();
|
|
// Add Modbusnode to Modbus object.
|
|
Modbus *bus = Modbus::findBus(_busNo);
|
|
if (bus != NULL) {
|
|
bus->addNode(this);
|
|
return;
|
|
}
|
|
|
|
}
|