Refactor Modbus inverter handling into superclass, use std::map instead of static array

Software/src/inverter/BYD-MODBUS.cpp

@@ -2,22 +2,9 @@
 #include "../datalayer/datalayer.h"
 #include "../devboard/hal/hal.h"
 #include "../devboard/utils/events.h"
-#include "../lib/eModbus-eModbus/RTUutils.h"
-#include "../lib/eModbus-eModbus/scripts/mbServerFCs.h"
 
 // For modbus register definitions, see https://gitlab.com/pelle8/inverter_resources/-/blob/main/byd_registers_modbus_rtu.md
 
-#define HISTORY_LENGTH 3  // Amount of samples(minutes) that needs to match for register to be considered stale
-static unsigned long previousMillis60s = 0;  // will store last time a 60s event occured
-static uint32_t user_configured_max_discharge_W = 0;
-static uint32_t user_configured_max_charge_W = 0;
-static uint32_t max_discharge_W = 0;
-static uint32_t max_charge_W = 0;
-static uint16_t register_401_history[HISTORY_LENGTH] = {0};
-static uint8_t history_index = 0;
-static uint8_t bms_char_dis_status = STANDBY;
-static bool all_401_values_equal = false;
-
 void BydModbusInverter::update_values() {
   verify_temperature();
   verify_inverter_modbus();
@@ -41,14 +28,20 @@ void BydModbusInverter::handle_static_data() {
   static uint16_t i = 100;
   for (uint8_t arr_idx = 0; arr_idx < sizeof(data_array_pointers) / sizeof(uint16_t*); arr_idx++) {
     uint16_t data_size = data_sizes[arr_idx];
-    memcpy(&mbPV[i], data_array_pointers[arr_idx], data_size);
+    for (int j = 0; j < data_size / sizeof(uint16_t); j++) {
+      mbPV[i + j] = data_array_pointers[arr_idx][j];
+    }
     i += data_size / sizeof(uint16_t);
   }
   static uint16_t init_p201[13] = {0, 0, 0, MAX_POWER, MAX_POWER, 0, 0, 53248, 10, 53248, 10, 0, 0};
-  memcpy(&mbPV[200], init_p201, sizeof(init_p201));
+  for (int i = 0; i < sizeof(init_p201) / sizeof(uint16_t); i++) {
+    mbPV[200 + i] = init_p201[i];
+  }
   static uint16_t init_p301[24] = {0,  0,  128, 0, 0,  0,     0, 0, 0,  2000,  0,   2000,
                                    75, 95, 0,   0, 16, 22741, 0, 0, 13, 52064, 230, 9900};
-  memcpy(&mbPV[300], init_p301, sizeof(init_p301));
+  for (int i = 0; i < sizeof(init_p301) / sizeof(uint16_t); i++) {
+    mbPV[300 + i] = init_p301[i];
+  }
 }
 
 void BydModbusInverter::handle_update_data_modbusp201_byd() {
@@ -150,10 +143,8 @@ bool BydModbusInverter::setup(void) {  // Performs one time setup at startup ove
   // Init Static data to the RTU Modbus
   handle_static_data();
 
-#if HAS_FREERTOS
   // Init Serial2 connected to the RTU Modbus
   RTUutils::prepareHardwareSerial(Serial2);
-#endif
 
   auto rx_pin = esp32hal->RS485_RX_PIN();
   auto tx_pin = esp32hal->RS485_TX_PIN();
@@ -163,16 +154,9 @@ bool BydModbusInverter::setup(void) {  // Performs one time setup at startup ove
   }
 
   Serial2.begin(9600, SERIAL_8N1, rx_pin, tx_pin);
-#if HAS_FREERTOS
-  // Register served function code worker for server
-  MBserver->registerWorker(MBTCP_ID, READ_HOLD_REGISTER, &FC03);
-  MBserver->registerWorker(MBTCP_ID, WRITE_HOLD_REGISTER, &FC06);
-  MBserver->registerWorker(MBTCP_ID, WRITE_MULT_REGISTERS, &FC16);
-  MBserver->registerWorker(MBTCP_ID, R_W_MULT_REGISTERS, &FC23);
 
   // Start ModbusRTU background task
-  ((ModbusServerRTU*)MBserver)->begin(Serial2, esp32hal->MODBUS_CORE());
+  MBserver.begin(Serial2, esp32hal->MODBUS_CORE());
-#endif
 
   return true;
 }

Software/src/inverter/BYD-MODBUS.h

@@ -5,10 +5,12 @@
 #define SELECTED_INVERTER_CLASS BydModbusInverter
 #endif
 
+#include "../devboard/utils/types.h"
 #include "ModbusInverterProtocol.h"
 
 class BydModbusInverter : public ModbusInverterProtocol {
  public:
+  BydModbusInverter() : ModbusInverterProtocol(21) {}
   const char* name() override { return Name; }
   bool setup() override;
   void update_values();
@@ -23,6 +25,18 @@ class BydModbusInverter : public ModbusInverterProtocol {
 
   //BYD Modbus specific value
   const uint16_t MAX_POWER = 40960;
+
+  static const int HISTORY_LENGTH =
+      3;  // Amount of samples(minutes) that needs to match for register to be considered stale
+  unsigned long previousMillis60s = 0;  // will store last time a 60s event occured
+  uint32_t user_configured_max_discharge_W = 0;
+  uint32_t user_configured_max_charge_W = 0;
+  uint32_t max_discharge_W = 0;
+  uint32_t max_charge_W = 0;
+  uint16_t register_401_history[HISTORY_LENGTH] = {0};
+  uint8_t history_index = 0;
+  uint8_t bms_char_dis_status = STANDBY;
+  bool all_401_values_equal = false;
 };
 
 #endif

Software/src/inverter/ModbusInverterProtocol.cpp

@@ -1,10 +1,181 @@
 #include "ModbusInverterProtocol.h"
+#include "../devboard/utils/logging.h"
 #include "../lib/eModbus-eModbus/ModbusServerRTU.h"
 
-static const int MB_RTU_NUM_VALUES = 13100;
+// Creates a ModbusRTU server instance with 2000ms timeout
-uint16_t mbPV[MB_RTU_NUM_VALUES];  // Process variable memory
+ModbusInverterProtocol::ModbusInverterProtocol(int serverId) : MBserver(2000) {
+  _serverId = serverId;
 
-ModbusInverterProtocol::ModbusInverterProtocol() {
+  MBserver.registerWorker(_serverId, READ_HOLD_REGISTER,
-  mbPV = ::mbPV;
+                          [this](ModbusMessage request) -> ModbusMessage { return FC03(request); });
-  MBserver = new ModbusServerRTU(2000);
+  MBserver.registerWorker(_serverId, WRITE_HOLD_REGISTER,
+                          [this](ModbusMessage request) -> ModbusMessage { return FC06(request); });
+  MBserver.registerWorker(_serverId, WRITE_MULT_REGISTERS,
+                          [this](ModbusMessage request) -> ModbusMessage { return FC16(request); });
+  MBserver.registerWorker(_serverId, R_W_MULT_REGISTERS,
+                          [this](ModbusMessage request) -> ModbusMessage { return FC23(request); });
+}
+
+ModbusInverterProtocol::~ModbusInverterProtocol() {
+  MBserver.unregisterWorker(_serverId, READ_HOLD_REGISTER);
+  MBserver.unregisterWorker(_serverId, WRITE_HOLD_REGISTER);
+  MBserver.unregisterWorker(_serverId, WRITE_MULT_REGISTERS);
+  MBserver.unregisterWorker(_serverId, R_W_MULT_REGISTERS);
+}
+
+// Server function to handle FC 0x03
+ModbusMessage ModbusInverterProtocol::FC03(ModbusMessage request) {
+  ModbusMessage response;  // The Modbus message we are going to give back
+  uint16_t addr = 0;       // Start address
+  uint16_t words = 0;      // # of words requested
+  request.get(2, addr);    // read address from request
+  request.get(4, words);   // read # of words from request
+
+  // Address overflow?
+  if ((addr + words) > MBPV_MAX) {
+    // Yes - send respective error response
+    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_ADDRESS);
+#ifdef DEBUG_LOG
+    logging.printf("Modbus FC03 error: illegal request addr=%d words=%d\n", addr, words);
+#endif
+    return response;
+  }
+
+  // Set up response
+  response.add(request.getServerID(), request.getFunctionCode(), (uint8_t)(words * 2));
+  for (uint8_t i = 0; i < words; ++i) {
+    // send increasing data values
+    response.add((uint16_t)(mbPV[addr + i]));
+  }
+
+  // #ifdef DEBUG_LOG
+  //     logging.printf("Modbus FC03 response: %d %d\n", addr, mbPV[addr]);
+  // #endif
+
+  return response;
+}
+
+// Server function to handle FC 0x06
+ModbusMessage ModbusInverterProtocol::FC06(ModbusMessage request) {
+  ModbusMessage response;  // The Modbus message we are going to give back
+  uint16_t addr = 0;       // Start address
+  uint16_t val = 0;        // value to write
+  request.get(2, addr);    // read address from request
+  request.get(4, val);     // read # of words from request
+
+  // Address overflow?
+  if ((addr) > MBPV_MAX) {
+    // Yes - send respective error response
+    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_ADDRESS);
+#ifdef DEBUG_LOG
+    logging.printf("Modbus FC06 error: illegal request addr=%d val=%d\n", addr, val);
+#endif
+    return response;
+  }
+
+  // Do the write
+  mbPV[addr] = val;
+
+  // Set up response
+  response.add(request.getServerID(), request.getFunctionCode(), mbPV[addr]);
+  return response;
+}
+
+// Server function to handle FC 0x10 (FC16)
+ModbusMessage ModbusInverterProtocol::FC16(ModbusMessage request) {
+  ModbusMessage response;  // The Modbus message we are going to give back
+  uint16_t addr = 0;       // Start address
+  uint16_t words = 0;      // total words to write
+  uint8_t bytes = 0;       // # of data bytes in request
+  uint16_t val = 0;        // value to be written
+  request.get(2, addr);    // read address from request
+  request.get(4, words);   // read # of words from request
+  request.get(6, bytes);   // read # of data bytes from request (seems redundant with # of words)
+
+  // # of registers proper?
+  if ((bytes != (words * 2))  // byte count in request must match # of words in request
+      || (words > 123))       // can't support more than this in request packet
+  {                           // Yes - send respective error response
+    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_VALUE);
+#ifdef DEBUG_LOG
+    logging.printf("Modbus FC16 error: bad registers addr=%d words=%d bytes=%d\n", addr, words, bytes);
+#endif
+    return response;
+  }
+  // Address overflow?
+  if ((addr + words) > MBPV_MAX) {
+    // Yes - send respective error response
+    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_ADDRESS);
+#ifdef DEBUG_LOG
+    logging.printf("Modbus FC16 error: overflow addr=%d words=%d\n", addr, words);
+#endif
+    return response;
+  }
+
+  // Do the writes
+  for (uint8_t i = 0; i < words; ++i) {
+    request.get(7 + (i * 2), val);  //data starts at byte 6 in request packet
+    mbPV[addr + i] = val;
+  }
+
+  // Set up response
+  response.add(request.getServerID(), request.getFunctionCode(), addr, words);
+  return response;
+}
+
+// Server function to handle FC 0x17 (FC23)
+ModbusMessage ModbusInverterProtocol::FC23(ModbusMessage request) {
+  ModbusMessage response;        // The Modbus message we are going to give back
+  uint16_t read_addr = 0;        // Start address for read
+  uint16_t read_words = 0;       // # of words requested for read
+  uint16_t write_addr = 0;       // Start address for write
+  uint16_t write_words = 0;      // total words to write
+  uint8_t write_bytes = 0;       // # of data bytes in write request
+  uint16_t write_val = 0;        // value to be written
+  request.get(2, read_addr);     // read address from request
+  request.get(4, read_words);    // read # of words from request
+  request.get(6, write_addr);    // read address from request
+  request.get(8, write_words);   // read # of words from request
+  request.get(10, write_bytes);  // read # of data bytes from request (seems redundant with # of words)
+
+  // ERROR CHECKS
+  // # of registers proper?
+  if ((write_bytes != (write_words * 2))  // byte count in request must match # of words in request
+      || (write_words > 121)              // can't fit more than this in the packet for FC23
+      || (read_words > 125))              // can't fit more than this in the response packet
+  {                                       // Yes - send respective error response
+    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_VALUE);
+#ifdef DEBUG_LOG
+    logging.printf("Modbus FC23 error: bad registers write_addr=%d write_words=%d write_bytes=%d read_words=%d\n",
+                   write_addr, write_words, write_bytes, read_words);
+#endif
+    return response;
+  }
+  // Address overflow?
+  if (((write_addr + write_words) > MBPV_MAX) ||
+      ((read_addr + read_words) > MBPV_MAX)) {  // Yes - send respective error response
+    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_ADDRESS);
+#ifdef DEBUG_LOG
+    logging.printf("Modbus FC23 error: overflow write_addr=%d write_words=%d read_addr=%d read_words=%d\n", write_addr,
+                   write_words, read_addr, read_words);
+#endif
+    return response;
+  }
+
+  //WRITE SECTION  - write is done before read for FC23
+  // Do the writes
+  for (uint8_t i = 0; i < write_words; ++i) {
+    request.get(11 + (i * 2), write_val);  //data starts at byte 6 in request packet
+    mbPV[write_addr + i] = write_val;
+  }
+
+  // READ SECTION
+  // Set up response
+  response.add(request.getServerID(), request.getFunctionCode(), (uint8_t)(read_words * 2));
+  for (uint8_t i = 0; i < read_words; ++i) {
+    // send increasing data values
+    response.add((uint16_t)(mbPV[read_addr + i]));
+  }
+
+  return response;
 }

Software/src/inverter/ModbusInverterProtocol.h

@@ -1,12 +1,14 @@
 #ifndef MODBUS_INVERTER_PROTOCOL_H
 #define MODBUS_INVERTER_PROTOCOL_H
 
-#include <HardwareSerial.h>
+#include "../lib/eModbus-eModbus/ModbusMessage.h"
-#include <stdint.h>
+#include "../lib/eModbus-eModbus/ModbusServerRTU.h"
-#include "../lib/eModbus-eModbus/ModbusServer.h"
 #include "InverterProtocol.h"
 
-extern uint16_t mbPV[];
+#include <HardwareSerial.h>
+
+#include <stdint.h>
+#include <map>
 
 // The abstract base class for all Modbus inverter protocols
 class ModbusInverterProtocol : public InverterProtocol {
@@ -14,15 +16,22 @@ class ModbusInverterProtocol : public InverterProtocol {
   InverterInterfaceType interface_type() { return InverterInterfaceType::Modbus; }
 
  protected:
-  // Create a ModbusRTU server instance with 2000ms timeout
+  ModbusInverterProtocol(int serverId);
-  ModbusInverterProtocol();
+  ~ModbusInverterProtocol();
 
-  static const int MB_RTU_NUM_VALUES = 13100;
+  ModbusMessage FC03(ModbusMessage request);
+  ModbusMessage FC06(ModbusMessage request);
+  ModbusMessage FC16(ModbusMessage request);
+  ModbusMessage FC23(ModbusMessage request);
 
-  // Modbus register file
+  // The highest Modbus register we allow reads/writes from
-  uint16_t* mbPV;
+  static const int MBPV_MAX = 30000;
+  // The Modbus server ID we respond to
+  int _serverId;
+  // The Modbus registers themselves
+  std::map<uint16_t, uint16_t> mbPV;
 
-  ModbusServer* MBserver;
+  ModbusServerRTU MBserver;
 };
 
 #endif

Software/src/lib/eModbus-eModbus/scripts/README.md

@@ -1 +0,0 @@
-The scripts in this folder originate from [eModbus discussion 147](https://github.com/eModbus/eModbus/discussions/147).

Software/src/lib/eModbus-eModbus/scripts/mbServerFCs.cpp

@@ -1,173 +0,0 @@
-#include "mbServerFCs.h"
-#include "../Logging.h"
-
-//modbus register memory - declared in main.cpp
-
-extern uint16_t mbPV[MBPV_MAX];
-
-// Server function to handle FC 0x03
-ModbusMessage FC03(ModbusMessage request) {
-  //Serial.println(request);
-  ModbusMessage response;  // The Modbus message we are going to give back
-  uint16_t addr = 0;       // Start address
-  uint16_t words = 0;      // # of words requested
-  request.get(2, addr);    // read address from request
-  request.get(4, words);   // read # of words from request
-  char debugString[1000];
-
-  LOG_D("FC03 received: read %d words @ %d\r\n", words, addr);
-
-  // Address overflow?
-  if ((addr + words) > MBPV_MAX) {
-    // Yes - send respective error response
-    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_ADDRESS);
-    LOG_D("ERROR - ILLEGAL DATA ADDRESS\r\n");
-    return response;
-  }
-
-  // Set up response
-  response.add(request.getServerID(), request.getFunctionCode(), (uint8_t)(words * 2));
-  sprintf(debugString, "Read : ");
-  for (uint8_t i = 0; i < words; ++i) {
-    // send increasing data values
-    response.add((uint16_t)(mbPV[addr + i]));
-    sprintf(debugString + strlen(debugString), "%i ", mbPV[addr + i]);
-  }
-  LOG_V("%s\r\n", debugString);
-
-  return response;
-}
-
-// Server function to handle FC 0x06
-ModbusMessage FC06(ModbusMessage request) {
-  //Serial.println(request);
-  ModbusMessage response;  // The Modbus message we are going to give back
-  uint16_t addr = 0;       // Start address
-  uint16_t val = 0;        // value to write
-  request.get(2, addr);    // read address from request
-  request.get(4, val);     // read # of words from request
-
-  LOG_D("FC06 received: write 1 word @ %d\r\n", addr);
-
-  // Address overflow?
-  if ((addr) > MBPV_MAX) {
-    // Yes - send respective error response
-    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_ADDRESS);
-    LOG_D("ERROR - ILLEGAL DATA ADDRESS\r\n");
-    return response;
-  }
-
-  // Do the write
-  mbPV[addr] = val;
-  LOG_V("Write : %i", val);
-
-  // Set up response
-  response.add(request.getServerID(), request.getFunctionCode(), mbPV[addr]);
-  return response;
-}
-
-// Server function to handle FC 0x10 (FC16)
-ModbusMessage FC16(ModbusMessage request) {
-  //Serial.println(request);
-  ModbusMessage response;  // The Modbus message we are going to give back
-  uint16_t addr = 0;       // Start address
-  uint16_t words = 0;      // total words to write
-  uint8_t bytes = 0;       // # of data bytes in request
-  uint16_t val = 0;        // value to be written
-  request.get(2, addr);    // read address from request
-  request.get(4, words);   // read # of words from request
-  request.get(6, bytes);   // read # of data bytes from request (seems redundant with # of words)
-  char debugString[1000];
-
-  LOG_D("FC16 received: write %d words @ %d\r\n", words, addr);
-
-  // # of registers proper?
-  if ((bytes != (words * 2))  // byte count in request must match # of words in request
-      || (words > 123))       // can't support more than this in request packet
-  {                           // Yes - send respective error response
-    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_VALUE);
-    LOG_D("ERROR - ILLEGAL DATA VALUE\r\n");
-    return response;
-  }
-  // Address overflow?
-  if ((addr + words) > MBPV_MAX) {
-    // Yes - send respective error response
-    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_ADDRESS);
-    LOG_D("ERROR - ILLEGAL DATA ADDRESS\r\n");
-    return response;
-  }
-
-  // Do the writes
-  sprintf(debugString, "Write : ");
-  for (uint8_t i = 0; i < words; ++i) {
-    request.get(7 + (i * 2), val);  //data starts at byte 6 in request packet
-    mbPV[addr + i] = val;
-    sprintf(debugString + strlen(debugString), "%i ", mbPV[addr + i]);
-  }
-  LOG_V("%s\r\n", debugString);
-
-  // Set up response
-  response.add(request.getServerID(), request.getFunctionCode(), addr, words);
-  return response;
-}
-
-// Server function to handle FC 0x17 (FC23)
-ModbusMessage FC23(ModbusMessage request) {
-  //Serial.println(request);
-  ModbusMessage response;        // The Modbus message we are going to give back
-  uint16_t read_addr = 0;        // Start address for read
-  uint16_t read_words = 0;       // # of words requested for read
-  uint16_t write_addr = 0;       // Start address for write
-  uint16_t write_words = 0;      // total words to write
-  uint8_t write_bytes = 0;       // # of data bytes in write request
-  uint16_t write_val = 0;        // value to be written
-  request.get(2, read_addr);     // read address from request
-  request.get(4, read_words);    // read # of words from request
-  request.get(6, write_addr);    // read address from request
-  request.get(8, write_words);   // read # of words from request
-  request.get(10, write_bytes);  // read # of data bytes from request (seems redundant with # of words)
-  char debugString[1000];
-
-  LOG_D("FC23 received: write %d @ %d, read %d @ %d\r\n", write_words, write_addr, read_words, read_addr);
-
-  // ERROR CHECKS
-  // # of registers proper?
-  if ((write_bytes != (write_words * 2))  // byte count in request must match # of words in request
-      || (write_words > 121)              // can't fit more than this in the packet for FC23
-      || (read_words > 125))              // can't fit more than this in the response packet
-  {                                       // Yes - send respective error response
-    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_VALUE);
-    LOG_D("ERROR - ILLEGAL DATA VALUE\r\n");
-    return response;
-  }
-  // Address overflow?
-  if (((write_addr + write_words) > MBPV_MAX) ||
-      ((read_addr + read_words) > MBPV_MAX)) {  // Yes - send respective error response
-    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_ADDRESS);
-    LOG_D("ERROR - ILLEGAL DATA ADDRESS\r\n");
-    return response;
-  }
-
-  //WRITE SECTION  - write is done before read for FC23
-  // Do the writes
-  sprintf(debugString, "Write: ");
-  for (uint8_t i = 0; i < write_words; ++i) {
-    request.get(11 + (i * 2), write_val);  //data starts at byte 6 in request packet
-    mbPV[write_addr + i] = write_val;
-    sprintf(debugString + strlen(debugString), "%i ", mbPV[write_addr + i]);
-  }
-  LOG_V("%s\r\n", debugString);
-
-  // READ SECTION
-  // Set up response
-  sprintf(debugString, "Read: ");
-  response.add(request.getServerID(), request.getFunctionCode(), (uint8_t)(read_words * 2));
-  for (uint8_t i = 0; i < read_words; ++i) {
-    // send increasing data values
-    response.add((uint16_t)(mbPV[read_addr + i]));
-    sprintf(debugString + strlen(debugString), "%i ", mbPV[read_addr + i]);
-  }
-  LOG_V("%s\r\n", debugString);
-
-  return response;
-}

Software/src/lib/eModbus-eModbus/scripts/mbServerFCs.h

@@ -1,9 +0,0 @@
-#include "../ModbusServerRTU.h"
-
-#define MBTCP_ID 21  // modbus TCP server ID
-#define MBPV_MAX 30000
-
-ModbusMessage FC03(ModbusMessage request);
-ModbusMessage FC06(ModbusMessage request);
-ModbusMessage FC16(ModbusMessage request);
-ModbusMessage FC23(ModbusMessage request);