alro65/AR-Autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
46dc0423a014dd599cca86b6bbab28c6dd05fe7b
AR-Autopilot/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp
T
alro65 45642fda0e sprint-8: EKF + adaptive tuner + HWID + SHA-256 audit hash-chain 
- heading_ekf.py: 2-state Kalman filter fusing PGN 127250 heading and
  127251 ROT with shortest-arc innovation and symmetric covariance update
- adaptive_tuner.py: gradient-descent outer-loop Kp/Ki adjuster bounded
  to ±adaptive_max_deviation_pct; oscillation vs steady-state detection
- hwid.py: HMAC-SHA256 activation token (verify side); hwid_from_mac_words
  converts three Modbus uint16 MAC words to 12-char hex HWID
- audit.py: SHA-256 hash-chain -- each JSONL line carries prev_hash and
  line_hash; verify_chain() detects tampering, deletion, insertion
- firmware/system/hwid.h+cpp: esp_efuse_mac_get_default wrapper + FNV-32
  hash + "AA:BB:CC:DD:EE:FF" formatter
- modbus_registers.yaml + generated .h/.py: HWID_MAC_01/23/45 at
  input addrs 9/10/11 (three 16-bit words = 6-byte MAC)
- modbus_slave.cpp: INPUT_HWID_MAC_01/23/45 cases read eFuse MAC
- main.cpp: logs HWID string + FNV-32 hash at boot (activation traceability)
- tests: 72 new tests (audit signing, EKF, adaptive tuner, HWID) -- 398 total

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-05-20 03:07:27 -04:00

639 lines
26 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
// =============================================================================
// modbus_slave.cpp -- eModbus-based RTU server (slave) implementation
// =============================================================================
//
// Sprint 1 scope:
// - Input registers (FC 0x04): firmware version, uptime, mode, heap,
// rudder angle/raw/valid, heading + ROT (stub for Sprint 1), battery
// and actuator current placeholders.
// - Discrete inputs (FC 0x02): engaged flag, all DIs, alarm bits.
// - Holding registers (FC 0x03/0x06/0x10): mode request, heading
// setpoint, brightness, alarm volume, dodge offset.
// - Coils (FC 0x01/0x05/0x0F): engage/disengage requests, ack-all,
// knob arm (reserved).
//
// In Sprint 1 the writes that try to change to a non-STANDBY mode are
// silently coerced (the modes::request_mode() helper rejects them with a
// warning).
// =============================================================================
#include "modbus_slave.h"
#include <Arduino.h>
#include <ModbusServerRTU.h>
#include "../hal/di_do.h"
#include "../hal/pinout.h"
#include "../hal/rudder_sensor.h"
#include "../modes/standby.h"
#include "../pid/pid_inner_task.h"
#include "../pid/pid_outer_task.h"
#include "../system/ar_log.h"
#include "../system/task_config.h"
#include "modbus_registers.h"
#include "nmea2000_consumer.h"
#include "../hal/knob_encoder.h"
#include "../safety/safety_monitor.h"
#include "../system/hwid.h"
namespace arautopilot::protocols::modbus {
namespace {
constexpr const char* TAG = "AR/MB";
// eModbus server with a 1000 ms timeout for client transactions.
ModbusServerRTU g_server(1000, PIN_RS485_DE);
bool g_running = false;
volatile uint32_t g_req_count = 0;
volatile uint32_t g_err_count = 0;
// ----- Storage for the writable side (coils + holding registers) -----------
// Discrete inputs and input registers are computed on the fly from the live
// telemetry helpers, so they don't need backing storage. Coils and holding
// registers do (eModbus expects deterministic reads of the values it last
// stored).
struct HoldingStorage {
uint16_t mode_request = 0;
int16_t heading_setpoint_x100 = 0;
uint16_t brightness_pct = 80;
uint16_t alarm_volume_pct = 60;
int16_t dodge_offset_deg_x100 = 0;
int16_t pid_inner_setpoint_req_x100 = 0;
uint16_t pid_inner_kp_req_x1000 = 0;
uint16_t pid_inner_ki_req_x1000 = 0;
uint16_t pid_inner_kd_req_x1000 = 0;
uint16_t pid_outer_heading_sp_req_x100 = 0;
uint16_t pid_outer_speed_kn_req_x10 = 150; // 15.0 kn default
uint16_t pid_outer_kp_req_x1000 = 0;
uint16_t pid_outer_ki_req_x1000 = 0;
uint16_t pid_outer_kd_req_x1000 = 0;
// Sprint 5: True Course + Track Keeping
uint16_t true_course_sp_x100 = 0;
uint16_t xte_gain_x1000 = 500; // 0.5 deg/m default
uint16_t xte_max_correction_x100 = 2000; // 20.0 deg default
};
HoldingStorage g_holding;
struct CoilStorage {
bool engage_request = false;
bool disengage_request = false;
bool ack_all = false;
bool knob_arm = false;
};
CoilStorage g_coils;
// ----- Helpers ---------------------------------------------------------------
uint16_t read_input_register(uint16_t addr) {
switch (addr) {
case INPUT_FW_VERSION_MAJOR: return 0;
case INPUT_FW_VERSION_MINOR: return 1;
case INPUT_FW_VERSION_PATCH: return 0;
case INPUT_SCHEMA_VERSION: return 0;
case INPUT_UPTIME_SECONDS_LO: return (uint16_t)((millis() / 1000U) & 0xFFFFU);
case INPUT_UPTIME_SECONDS_HI: return (uint16_t)(((millis() / 1000U) >> 16) & 0xFFFFU);
case INPUT_CURRENT_MODE: return (uint16_t)modes::current_mode();
case INPUT_FREE_HEAP_KB: return (uint16_t)(ESP.getFreeHeap() / 1024U);
case INPUT_MIN_FREE_HEAP_KB: return (uint16_t)(ESP.getMinFreeHeap() / 1024U);
// Sprint 8: Hardware ID (MAC eFuse, 3 × uint16)
case INPUT_HWID_MAC_01:
case INPUT_HWID_MAC_23:
case INPUT_HWID_MAC_45: {
uint8_t mac[6] = {};
system::hwid_get_mac(mac);
if (addr == INPUT_HWID_MAC_01) return (uint16_t)((mac[0] << 8) | mac[1]);
if (addr == INPUT_HWID_MAC_23) return (uint16_t)((mac[2] << 8) | mac[3]);
return (uint16_t)((mac[4] << 8) | mac[5]);
}
case INPUT_RUDDER_ANGLE_DEG_X100: {
auto r = hal::rudder_sensor_latest();
int v = (int)(r.angle_deg * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_RUDDER_RAW_ADC: {
auto r = hal::rudder_sensor_latest();
return (uint16_t)r.raw_adc;
}
case INPUT_RUDDER_VALID: {
auto r = hal::rudder_sensor_latest();
return r.valid ? 1 : 0;
}
case INPUT_HEADING_DEG_X100: {
auto n = nmea2000::nmea2000_latest();
int v = (int)(n.heading_deg * 100.0f);
if (v < 0) v = 0;
if (v > 35999) v = 35999;
return (uint16_t)v;
}
case INPUT_ROT_DPS_X100: {
auto n = nmea2000::nmea2000_latest();
int v = (int)(n.rate_of_turn_dps * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_HEADING_AGE_MS: {
auto n = nmea2000::nmea2000_latest();
uint32_t age = (millis() - n.heading_age_ms);
if (!n.heading_valid) age = 60000;
if (age > 60000) age = 60000;
return (uint16_t)age;
}
case INPUT_BATTERY_VOLTAGE_X100: {
// 1:6 voltage divider on AI2; 12-bit ADC, 3.3 V reference.
const int raw = analogRead(PIN_AI2_BATTERY_VOLTAGE);
const float vbat = (float)raw * (3.3f * 6.0f / 4095.0f);
int v = (int)(vbat * 100.0f);
if (v < 0) v = 0;
if (v > 32767) v = 32767;
return (uint16_t)v;
}
case INPUT_ACTUATOR_CURRENT_X100: {
// Hall-effect transducer on AI3, 50 A FS.
const int raw = analogRead(PIN_AI3_ACTUATOR_CURRENT);
const float iact = (float)raw * (3.3f * 50.0f / 4095.0f);
int v = (int)(iact * 100.0f);
if (v < 0) v = 0;
if (v > 32767) v = 32767;
return (uint16_t)v;
}
// ----- PID inner-loop telemetry (Sprint 2) -----
case INPUT_PID_INNER_SETPOINT_X100: {
int v = (int)(pid::pid_inner_setpoint_deg() * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_PID_INNER_OUTPUT_X100: {
int v = (int)(pid::pid_inner_last_output_pct() * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_PID_INNER_ERROR_X100: {
int v = (int)(pid::pid_inner_last_error_deg() * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_PID_INNER_KP_X1000:
case INPUT_PID_INNER_KI_X1000:
case INPUT_PID_INNER_KD_X1000: {
float kp, ki, kd;
pid::pid_inner_get_gains(kp, ki, kd);
float v;
if (addr == INPUT_PID_INNER_KP_X1000) v = kp;
else if (addr == INPUT_PID_INNER_KI_X1000) v = ki;
else v = kd;
int scaled = (int)(v * 1000.0f);
if (scaled < 0) scaled = 0;
if (scaled > 65535) scaled = 65535;
return (uint16_t)scaled;
}
// ----- PID outer-loop telemetry (Sprint 3) -----
case INPUT_PID_OUTER_HEADING_SP_X100: {
int v = (int)(pid::pid_outer_heading_setpoint_deg() * 100.0f);
if (v < 0) v = 0;
if (v > 35999) v = 35999;
return (uint16_t)v;
}
case INPUT_PID_OUTER_RUDDER_SP_X100: {
int v = (int)(pid::pid_outer_last_rudder_setpoint_deg() * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_PID_OUTER_ERROR_X100: {
int v = (int)(pid::pid_outer_last_error_deg() * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_PID_OUTER_SPEED_KN_X10: {
int v = (int)(pid::pid_outer_speed_kn() * 10.0f);
if (v < 0) v = 0;
if (v > 65535) v = 65535;
return (uint16_t)v;
}
case INPUT_PID_OUTER_KP_X1000:
case INPUT_PID_OUTER_KI_X1000:
case INPUT_PID_OUTER_KD_X1000: {
float kp, ki, kd;
pid::pid_outer_get_gains(kp, ki, kd);
float v;
if (addr == INPUT_PID_OUTER_KP_X1000) v = kp;
else if (addr == INPUT_PID_OUTER_KI_X1000) v = ki;
else v = kd;
int scaled = (int)(v * 1000.0f);
if (scaled < 0) scaled = 0;
if (scaled > 65535) scaled = 65535;
return (uint16_t)scaled;
}
// ----- Sprint 5: COG / SOG / XTE telemetry -----
case INPUT_COG_DEG_X100: {
auto cs = nmea2000::nmea2000_cog_sog();
if (!cs.valid) return 0;
int v = (int)(cs.cog_deg * 100.0f);
if (v < 0) v = 0;
if (v > 35999) v = 35999;
return (uint16_t)v;
}
case INPUT_SOG_KN_X10: {
auto cs = nmea2000::nmea2000_cog_sog();
if (!cs.valid) return 0;
int v = (int)(cs.sog_kn * 10.0f);
if (v < 0) v = 0;
if (v > 65535) v = 65535;
return (uint16_t)v;
}
case INPUT_COG_AGE_MS: {
auto cs = nmea2000::nmea2000_cog_sog();
uint32_t age = cs.valid ? (millis() - cs.age_ms) : 60000;
if (age > 60000) age = 60000;
return (uint16_t)age;
}
case INPUT_XTE_DM_SIGNED: {
auto nd = nmea2000::nmea2000_nav_data();
if (!nd.valid) return 0;
int v = (int)(nd.xte_m * 10.0f); // metres → decimetres
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_XTE_AGE_MS: {
auto nd = nmea2000::nmea2000_nav_data();
uint32_t age = nd.valid ? (millis() - nd.age_ms) : 60000;
if (age > 60000) age = 60000;
return (uint16_t)age;
}
case INPUT_DTW_M: {
auto nd = nmea2000::nmea2000_nav_data();
if (!nd.valid) return 0;
uint32_t dtw = (uint32_t)nd.dtw_m;
if (dtw > 65535) dtw = 65535;
return (uint16_t)dtw;
}
default:
return 0;
}
}
bool read_discrete(uint16_t addr) {
switch (addr) {
case DISCRETE_PILOT_ENGAGED: return !modes::is_standby();
case DISCRETE_DI_DISENGAGE_BUTTON: return hal::di_read(PIN_DI1_DISENGAGE_BUTTON);
case DISCRETE_DI_LIMIT_PORT: return hal::di_read(PIN_DI2_LIMIT_SWITCH_PORT);
case DISCRETE_DI_LIMIT_STBD: return hal::di_read(PIN_DI3_LIMIT_SWITCH_STBD);
case DISCRETE_DI_EXTERNAL_ALARM: return hal::di_read(PIN_DI4_EXTERNAL_ALARM);
case DISCRETE_DI_MANUAL_CONFIRM: return hal::di_read(PIN_DI5_MANUAL_CONFIRM);
case DISCRETE_ACTUATOR_POWER: return hal::do_state(PIN_DO3_ACTUATOR_POWER);
case DISCRETE_ACTUATOR_DRIVING_PORT:return hal::do_state(PIN_DO1_PUMP_PORT);
case DISCRETE_ACTUATOR_DRIVING_STBD:return hal::do_state(PIN_DO2_PUMP_STBD);
// Alarm bits: wired in Sprint 6 via safety_monitor.
case DISCRETE_ALARM_OFF_COURSE: return safety::safety_alarm_bits().off_course;
case DISCRETE_ALARM_OFF_COURSE_SEVERE: return safety::safety_alarm_bits().off_course_severe;
case DISCRETE_ALARM_RUDDER_NOT_RESP: return safety::safety_alarm_bits().rudder_not_resp;
case DISCRETE_ALARM_HEADING_LOST: return safety::safety_alarm_bits().heading_lost;
case DISCRETE_ALARM_ACTUATOR_OVERCURR: return safety::safety_alarm_bits().actuator_overcurr;
case DISCRETE_ALARM_VOLTAGE_LOW: return safety::safety_alarm_bits().voltage_low;
case DISCRETE_ALARM_LIMIT_REACHED: return safety::safety_alarm_bits().limit_reached;
case DISCRETE_ALARM_WATCHDOG_TRIPPED: return safety::safety_alarm_bits().watchdog_tripped;
case DISCRETE_ALARM_VMS_CRITICAL: return safety::safety_alarm_bits().vms_critical;
case DISCRETE_ANY_ALARM_ACTIVE: return safety::safety_alarm_bits().any();
default:
return false;
}
}
uint16_t read_holding(uint16_t addr) {
switch (addr) {
case HOLDING_MODE_REQUEST: return g_holding.mode_request;
case HOLDING_HEADING_SETPOINT_X100: return (uint16_t)g_holding.heading_setpoint_x100;
case HOLDING_BRIGHTNESS_PCT: return g_holding.brightness_pct;
case HOLDING_ALARM_VOLUME_PCT: return g_holding.alarm_volume_pct;
case HOLDING_DODGE_OFFSET_DEG_X100: return (uint16_t)g_holding.dodge_offset_deg_x100;
case HOLDING_PID_INNER_SETPOINT_REQ_X100: return (uint16_t)g_holding.pid_inner_setpoint_req_x100;
case HOLDING_PID_INNER_KP_REQ_X1000: return g_holding.pid_inner_kp_req_x1000;
case HOLDING_PID_INNER_KI_REQ_X1000: return g_holding.pid_inner_ki_req_x1000;
case HOLDING_PID_INNER_KD_REQ_X1000: return g_holding.pid_inner_kd_req_x1000;
case HOLDING_PID_OUTER_HEADING_SP_REQ_X100: return g_holding.pid_outer_heading_sp_req_x100;
case HOLDING_PID_OUTER_SPEED_KN_REQ_X10: return g_holding.pid_outer_speed_kn_req_x10;
case HOLDING_PID_OUTER_KP_REQ_X1000: return g_holding.pid_outer_kp_req_x1000;
case HOLDING_PID_OUTER_KI_REQ_X1000: return g_holding.pid_outer_ki_req_x1000;
case HOLDING_PID_OUTER_KD_REQ_X1000: return g_holding.pid_outer_kd_req_x1000;
case HOLDING_TRUE_COURSE_SP_X100: return g_holding.true_course_sp_x100;
case HOLDING_XTE_GAIN_X1000: return g_holding.xte_gain_x1000;
case HOLDING_XTE_MAX_CORRECTION_X100: return g_holding.xte_max_correction_x100;
default: return 0;
}
}
// Returns Modbus exception code (0 = ok). Side effect: stores value.
Modbus::Error write_holding(uint16_t addr, uint16_t value) {
switch (addr) {
case HOLDING_MODE_REQUEST:
if (value > 4) return Modbus::Error::ILLEGAL_DATA_VALUE;
g_holding.mode_request = value;
modes::request_mode((modes::Mode)value);
return Modbus::Error::SUCCESS;
case HOLDING_HEADING_SETPOINT_X100:
g_holding.heading_setpoint_x100 = (int16_t)value;
return Modbus::Error::SUCCESS;
case HOLDING_BRIGHTNESS_PCT:
if (value > 100) return Modbus::Error::ILLEGAL_DATA_VALUE;
g_holding.brightness_pct = value;
return Modbus::Error::SUCCESS;
case HOLDING_ALARM_VOLUME_PCT:
if (value > 100) return Modbus::Error::ILLEGAL_DATA_VALUE;
g_holding.alarm_volume_pct = value;
return Modbus::Error::SUCCESS;
case HOLDING_DODGE_OFFSET_DEG_X100:
g_holding.dodge_offset_deg_x100 = (int16_t)value;
return Modbus::Error::SUCCESS;
// ----- PID inner-loop tunables (Sprint 2) -----
case HOLDING_PID_INNER_SETPOINT_REQ_X100: {
int16_t sv = (int16_t)value;
g_holding.pid_inner_setpoint_req_x100 = sv;
pid::pid_inner_set_setpoint_deg((float)sv * 0.01f);
return Modbus::Error::SUCCESS;
}
case HOLDING_PID_OUTER_HEADING_SP_REQ_X100: {
if (value > 35999) return Modbus::Error::ILLEGAL_DATA_VALUE;
g_holding.pid_outer_heading_sp_req_x100 = value;
pid::pid_outer_set_heading_setpoint_deg((float)value * 0.01f);
return Modbus::Error::SUCCESS;
}
case HOLDING_PID_OUTER_SPEED_KN_REQ_X10: {
if (value > 800) return Modbus::Error::ILLEGAL_DATA_VALUE; // 80 kn cap
g_holding.pid_outer_speed_kn_req_x10 = value;
pid::pid_outer_set_speed_kn((float)value * 0.1f);
return Modbus::Error::SUCCESS;
}
case HOLDING_PID_OUTER_KP_REQ_X1000:
case HOLDING_PID_OUTER_KI_REQ_X1000:
case HOLDING_PID_OUTER_KD_REQ_X1000: {
if (addr == HOLDING_PID_OUTER_KP_REQ_X1000) {
g_holding.pid_outer_kp_req_x1000 = value;
} else if (addr == HOLDING_PID_OUTER_KI_REQ_X1000) {
g_holding.pid_outer_ki_req_x1000 = value;
} else {
g_holding.pid_outer_kd_req_x1000 = value;
}
float kp = (float)g_holding.pid_outer_kp_req_x1000 * 0.001f;
float ki = (float)g_holding.pid_outer_ki_req_x1000 * 0.001f;
float kd = (float)g_holding.pid_outer_kd_req_x1000 * 0.001f;
if (kp <= 0.0f) {
return Modbus::Error::ILLEGAL_DATA_VALUE;
}
pid::pid_outer_update_gains(kp, ki, kd);
return Modbus::Error::SUCCESS;
}
case HOLDING_PID_INNER_KP_REQ_X1000:
case HOLDING_PID_INNER_KI_REQ_X1000:
case HOLDING_PID_INNER_KD_REQ_X1000: {
// Update the requested-gain shadow, then push all three to the
// live controller. We do all three together so partial writes
// don't leave the gains inconsistent.
if (addr == HOLDING_PID_INNER_KP_REQ_X1000) {
g_holding.pid_inner_kp_req_x1000 = value;
} else if (addr == HOLDING_PID_INNER_KI_REQ_X1000) {
g_holding.pid_inner_ki_req_x1000 = value;
} else {
g_holding.pid_inner_kd_req_x1000 = value;
}
float kp = (float)g_holding.pid_inner_kp_req_x1000 * 0.001f;
float ki = (float)g_holding.pid_inner_ki_req_x1000 * 0.001f;
float kd = (float)g_holding.pid_inner_kd_req_x1000 * 0.001f;
// Refuse zero kp -- the rest of the algorithm assumes kp > 0
// for back-calculation anti-windup. If the operator writes 0
// we ignore it (leave whatever the firmware booted with).
if (kp <= 0.0f) {
return Modbus::Error::ILLEGAL_DATA_VALUE;
}
pid::pid_inner_update_gains(kp, ki, kd);
return Modbus::Error::SUCCESS;
}
// ----- Sprint 5: True Course + XTE parameters -----
case HOLDING_TRUE_COURSE_SP_X100: {
if (value > 35999) return Modbus::Error::ILLEGAL_DATA_VALUE;
g_holding.true_course_sp_x100 = value;
pid::pid_outer_set_cog_setpoint_deg((float)value * 0.01f);
return Modbus::Error::SUCCESS;
}
case HOLDING_XTE_GAIN_X1000: {
g_holding.xte_gain_x1000 = value;
pid::pid_outer_set_xte_gain((float)value * 0.001f);
return Modbus::Error::SUCCESS;
}
case HOLDING_XTE_MAX_CORRECTION_X100: {
if (value > 9000) return Modbus::Error::ILLEGAL_DATA_VALUE; // 90 deg cap
g_holding.xte_max_correction_x100 = value;
pid::pid_outer_set_xte_max_correction((float)value * 0.01f);
return Modbus::Error::SUCCESS;
}
default:
return Modbus::Error::ILLEGAL_DATA_ADDRESS;
}
}
Modbus::Error write_coil(uint16_t addr, bool value) {
switch (addr) {
case COIL_CMD_ENGAGE_REQUEST:
g_coils.engage_request = value;
// Sprint 1: requesting engagement does nothing yet -- modes
// engine is implemented in Sprint 3+. Just log and store.
if (value) {
AR_LOGI(TAG, "engage request received (no-op in Sprint 1)");
}
return Modbus::Error::SUCCESS;
case COIL_CMD_DISENGAGE_REQUEST:
g_coils.disengage_request = value;
if (value) {
modes::force_standby("modbus-disengage");
}
return Modbus::Error::SUCCESS;
case COIL_CMD_ACK_ALL_ALARMS:
g_coils.ack_all = value;
if (value) {
AR_LOGI(TAG, "ack-all-alarms received (no alarms in Sprint 1)");
}
return Modbus::Error::SUCCESS;
case COIL_CMD_KNOB_ARM:
g_coils.knob_arm = value;
hal::knob_encoder_set_armed(value);
return Modbus::Error::SUCCESS;
default:
return Modbus::Error::ILLEGAL_DATA_ADDRESS;
}
}
// ----- eModbus worker callbacks ---------------------------------------------
ModbusMessage on_read_input(ModbusMessage req) {
uint16_t start, count;
req.get(2, start);
req.get(4, count);
if (count == 0 ||
(uint32_t)start + count > (uint32_t)INPUT_MAX_ADDR + 1U ||
count > 125) {
++g_err_count;
ModbusMessage resp;
resp.setError(req.getServerID(), req.getFunctionCode(),
Modbus::Error::ILLEGAL_DATA_VALUE);
return resp;
}
ModbusMessage resp;
resp.add(req.getServerID(), req.getFunctionCode(),
(uint8_t)(count * 2));
for (uint16_t i = 0; i < count; ++i) {
resp.add(read_input_register(start + i));
}
++g_req_count;
return resp;
}
ModbusMessage on_read_discrete(ModbusMessage req) {
uint16_t start, count;
req.get(2, start);
req.get(4, count);
if (count == 0 ||
(uint32_t)start + count > (uint32_t)DISCRETE_MAX_ADDR + 1U ||
count > 2000) {
++g_err_count;
ModbusMessage resp;
resp.setError(req.getServerID(), req.getFunctionCode(),
Modbus::Error::ILLEGAL_DATA_VALUE);
return resp;
}
ModbusMessage resp;
uint8_t byte_count = (uint8_t)((count + 7) / 8);
resp.add(req.getServerID(), req.getFunctionCode(), byte_count);
for (uint8_t b = 0; b < byte_count; ++b) {
uint8_t v = 0;
for (uint8_t bit = 0; bit < 8; ++bit) {
uint16_t idx = b * 8U + bit;
if (idx < count && read_discrete(start + idx)) {
v |= (uint8_t)(1U << bit);
}
}
resp.add(v);
}
++g_req_count;
return resp;
}
ModbusMessage on_read_holding(ModbusMessage req) {
uint16_t start, count;
req.get(2, start);
req.get(4, count);
if (count == 0 ||
(uint32_t)start + count > (uint32_t)HOLDING_MAX_ADDR + 1U ||
count > 125) {
++g_err_count;
ModbusMessage resp;
resp.setError(req.getServerID(), req.getFunctionCode(),
Modbus::Error::ILLEGAL_DATA_VALUE);
return resp;
}
ModbusMessage resp;
resp.add(req.getServerID(), req.getFunctionCode(),
(uint8_t)(count * 2));
for (uint16_t i = 0; i < count; ++i) {
resp.add(read_holding(start + i));
}
++g_req_count;
return resp;
}
ModbusMessage on_write_holding(ModbusMessage req) {
uint16_t addr, value;
req.get(2, addr);
req.get(4, value);
auto err = write_holding(addr, value);
if (err != Modbus::Error::SUCCESS) {
++g_err_count;
ModbusMessage resp;
resp.setError(req.getServerID(), req.getFunctionCode(), err);
return resp;
}
// Echo per Modbus 0x06 convention.
ModbusMessage resp;
resp.add(req.getServerID(), req.getFunctionCode(), addr, value);
++g_req_count;
return resp;
}
ModbusMessage on_write_coil(ModbusMessage req) {
uint16_t addr, value;
req.get(2, addr);
req.get(4, value);
// 0xFF00 = ON, 0x0000 = OFF per Modbus spec.
if (value != 0x0000 && value != 0xFF00) {
++g_err_count;
ModbusMessage resp;
resp.setError(req.getServerID(), req.getFunctionCode(),
Modbus::Error::ILLEGAL_DATA_VALUE);
return resp;
}
auto err = write_coil(addr, value == 0xFF00);
if (err != Modbus::Error::SUCCESS) {
++g_err_count;
ModbusMessage resp;
resp.setError(req.getServerID(), req.getFunctionCode(), err);
return resp;
}
ModbusMessage resp;
resp.add(req.getServerID(), req.getFunctionCode(), addr, value);
++g_req_count;
return resp;
}
} // namespace
void modbus_slave_init() {
// Register worker callbacks BEFORE starting the server (eModbus
// requirement).
g_server.registerWorker(SLAVE_ID, READ_INPUT_REGISTER, &on_read_input);
g_server.registerWorker(SLAVE_ID, READ_DISCR_INPUT, &on_read_discrete);
g_server.registerWorker(SLAVE_ID, READ_HOLD_REGISTER, &on_read_holding);
g_server.registerWorker(SLAVE_ID, WRITE_HOLD_REGISTER, &on_write_holding);
g_server.registerWorker(SLAVE_ID, WRITE_COIL, &on_write_coil);
AR_LOGI(TAG,
"modbus_slave_init: slave_id=%u inputs=%u discretes=%u "
"holdings=%u coils=%u",
(unsigned)SLAVE_ID, (unsigned)INPUT_COUNT, (unsigned)DISCRETE_COUNT,
(unsigned)HOLDING_COUNT, (unsigned)COIL_COUNT);
}
void modbus_slave_start() {
// UART2 on the AR-NMEA-IO is wired to RS-485.
Serial2.begin(BAUDRATE, SERIAL_8N1, PIN_RS485_RX, PIN_RS485_TX);
g_server.begin(Serial2, AR_TASK_CORE_COMMS);
g_running = true;
AR_LOGI(TAG, "modbus_slave_start: listening on UART2 @ %u baud (RX=%d TX=%d DE=%d)",
(unsigned)BAUDRATE, PIN_RS485_RX, PIN_RS485_TX, PIN_RS485_DE);
}
bool modbus_slave_is_running() { return g_running; }
uint32_t modbus_slave_request_count() { return g_req_count; }
uint32_t modbus_slave_error_count() { return g_err_count; }
} // namespace arautopilot::protocols::modbus
Reference in New Issue View Git Blame Copy Permalink