yetangitu/Battery-Emulator
1
0
Fork 0
mirror of https://github.com/dalathegreat/Battery-Emulator.git synced 2025-10-06 03:50:13 +02:00
Code Issues Projects Releases Packages Wiki Activity

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

Browse source
This commit is contained in:
Jonny 2025-08-06 20:25:57 +01:00
parent b2e9515d44
commit 9eac0a90b5
7 changed files with 219 additions and 224 deletions
Download patch file Download diff file Expand all files Collapse all files

36
Software/src/inverter/BYD-MODBUS.cpp
View file

@ -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());
#endif
MBserver.begin(Serial2, esp32hal->MODBUS_CORE());
return true;
}

14
Software/src/inverter/BYD-MODBUS.h
View file

@ -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

181
Software/src/inverter/ModbusInverterProtocol.cpp
View file

@ -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;
uint16_t mbPV[MB_RTU_NUM_VALUES]; // Process variable memory
// Creates a ModbusRTU server instance with 2000ms timeout
ModbusInverterProtocol::ModbusInverterProtocol(int serverId) : MBserver(2000) {
_serverId = serverId;
ModbusInverterProtocol::ModbusInverterProtocol() {
mbPV = ::mbPV;
MBserver = new ModbusServerRTU(2000);
MBserver.registerWorker(_serverId, READ_HOLD_REGISTER,
[this](ModbusMessage request) -> ModbusMessage { return FC03(request); });
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;
}

29
Software/src/inverter/ModbusInverterProtocol.h
View file

@ -1,12 +1,14 @@
#ifndef MODBUS_INVERTER_PROTOCOL_H
#define MODBUS_INVERTER_PROTOCOL_H
#include <HardwareSerial.h>
#include <stdint.h>
#include "../lib/eModbus-eModbus/ModbusServer.h"
#include "../lib/eModbus-eModbus/ModbusMessage.h"
#include "../lib/eModbus-eModbus/ModbusServerRTU.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();
ModbusInverterProtocol(int serverId);
~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
uint16_t* mbPV;
// The highest Modbus register we allow reads/writes from
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

1
Software/src/lib/eModbus-eModbus/scripts/README.md
View file

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

173
Software/src/lib/eModbus-eModbus/scripts/mbServerFCs.cpp
View file

@ -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;
}

9
Software/src/lib/eModbus-eModbus/scripts/mbServerFCs.h
View file

@ -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);