alro65/AR-Autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
295efa2d83a6b9b365520cda6616fd3baf6f7eb0
AR-Autopilot/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp
T
alro65 295efa2d83 sprint-2: PID inner loop + Python rudder simulator 
End-to-end implementation per docs/sprint-2-plan.md.

Builds: pio run -e esp32-dev SUCCESS, RAM 6.8%, Flash 26.8% (351 KB).
Tests: pytest 129/129 green (110 Sprint 1 + 19 Sprint 2).

Python (arautopilot/studio/simulator/):

- rudder_dynamics.py: marine-realistic physical model of a hydraulic
  rudder actuator. Defaults tuned so 100 % PWM produces steady-state
  v_max ~5 deg/s, matching the brief's "typical 3-6 dps" for a 30 m
  yacht. Includes deadband, min-useful PWM snap, port/stbd asymmetry,
  end-stops, optional external torque, RunRecorder helper.
- pid_inner.py: pure-Python reference PID. Anti-windup via back-
  calculation, setpoint rate limit, setpoint deadband, derivative LPF,
  actuator non-linearity compensation. This module is the algorithmic
  source of truth; C++ firmware is a line-by-line port.

Firmware (firmware/ar_autopilot_v1/src/pid/):

- pid_inner.h: header-only C++17 controller, byte-equivalent port of
  pid_inner.py. Compiles on ESP32 toolchain AND on host g++/clang/MSVC
  (no Arduino dependencies) -- ready for native Unity cross-validation
  once a host compiler is installed.
- pid_inner_task.{h,cpp}: FreeRTOS task wrapper. 50 Hz on Core 1
  (real-time core). Subscribes to TWDT, bleeds integrator during
  STANDBY, surfaces telemetry + tunables via the Modbus slave.

Modbus map (regenerated from YAML):

- 6 new INPUT registers (40-45): setpoint, output, error, kp/ki/kd live
- 4 new HOLDING registers (16-19): writable setpoint + kp/ki/kd req
  (writes propagate atomically; zero kp rejected as ILLEGAL_DATA_VALUE)

Tests:

- test_rudder_simulator.py: 9 tests (zero-input rest, full deflection,
  end-stop saturation, deadband, min-useful snap, asymmetry, recorder
  API, invalid dt, end-stop velocity zeroing).
- test_pid_inner_python.py: 10 tests (positive/negative step response,
  setpoint deadband holds, anti-windup bounds under saturation,
  allowed=false bleeds integrator, actuator deadband + asymmetry
  compensation, output saturation, rate limit, disturbance rejection).

NOT in Sprint 2 (intentional per brief sec. 12):
  - Outer heading PID, gain scheduling by SOG, ROT feed-forward
    (those land in Sprint 3)
  - Cross-validation tests via ctypes (need host C++ compiler that
    this Windows machine lacks; algorithmic parity enforced by review)

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
2026-05-18 15:27:45 -04:00

459 lines
17 KiB
C++
Raw Blame History

// =============================================================================
// 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 "../system/ar_log.h"
#include "../system/task_config.h"
#include "modbus_registers.h"
#include "nmea2000_consumer.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;
};
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);
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;
}
// Sprint 1: battery voltage and actuator current wired in Sprint 6
// (Safety + alarms). For now, return 0.
case INPUT_BATTERY_VOLTAGE_X100:
case INPUT_ACTUATOR_CURRENT_X100:
return 0;
// ----- 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;
}
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: stubs in Sprint 1, wired in Sprint 6.
case DISCRETE_ALARM_OFF_COURSE:
case DISCRETE_ALARM_OFF_COURSE_SEVERE:
case DISCRETE_ALARM_RUDDER_NOT_RESP:
case DISCRETE_ALARM_HEADING_LOST:
case DISCRETE_ALARM_ACTUATOR_OVERCURR:
case DISCRETE_ALARM_VOLTAGE_LOW:
case DISCRETE_ALARM_LIMIT_REACHED:
case DISCRETE_ALARM_WATCHDOG_TRIPPED:
case DISCRETE_ALARM_VMS_CRITICAL:
case DISCRETE_ANY_ALARM_ACTIVE:
return false;
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;
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_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;
}
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;
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