sprint-1: firmware ESP32 base -- STANDBY + Modbus + NMEA 2000 + watchdog
End-to-end implementation of Sprint 1 per docs/sprint-1-plan.md.
Builds: pio run -e esp32-dev SUCCESS, RAM 6.7%, Flash 26.5% (347 KB).
Tests: pytest 110/110 green; pio test -e native deferred (needs host
C++ compiler -- none on this Windows machine).
Firmware (firmware/ar_autopilot_v1/):
- platformio.ini: 4 envs (esp32-dev release, esp32-debug, native unity
tests, check static analysis). NMEA2000-library@4.22, NMEA2000_esp32@
1.0, eModbus@1.7.4 pinned.
- main.cpp: boot in STANDBY, FreeRTOS task spawn, returns to scheduler.
- system/: ar_log.h facade, task_config.h (priorities/stacks/cores
central table), heartbeat (1 Hz LED + uptime).
- modes/: STANDBY-only state machine; non-STANDBY rejected.
- hal/: di_do.cpp (5 DI + 10 DO with debounce + last-state cache),
rudder_sensor.cpp (100 Hz ADC + 5-sample median filter, Core 1),
rudder_actuator.cpp (DO1/DO2/DO3 with three safety interlocks:
power-off, STANDBY mode, limit switch).
- safety/: TWDT @ 2 s panic-on-expire; 50 Hz safety task on Core 1
enforcing DI1 physical disengage button, DI4 external alarm,
both-limit-switch interlock.
- protocols/modbus_slave.cpp: eModbus RTU server on UART2 @ 38400 8N1,
slave ID 1. 17 inputs + 19 discretes + 5 holdings + 4 coils. Reads
pull live telemetry; writes validate range and route to handlers.
- protocols/nmea2000_consumer.cpp: stack open with CAN TX=GPIO3
RX=GPIO1, subscribed to PGN 127250 (Heading) + PGN 127251 (Rate of
Turn). 5 s staleness flag built in for Sprint 6 alarm wiring.
- filters/median.h: templated MedianFilter<T,N> (host testable).
Cross-cutting:
- modbus_registers.yaml: single source of truth for the Modbus register
map. 45 entries.
- tools/gen_modbus_registers.py: YAML -> C++ header + Python module
generator with --check for drift detection.
- arautopilot/shared/modbus_register_map.py: generated Python mirror,
imported by Studio + tools.
- arautopilot/tests/test_modbus_register_map.py: 30 tests covering
schema, address uniqueness, range, spot-checks, and drift detection
(fails if YAML edited without regenerating).
- firmware/ar_autopilot_v1/tools/modbus_client_test.py: manual Modbus
client for poking the slave from a PC with USB-RS485 dongle.
- firmware/ar_autopilot_v1/test/test_median_filter/test_median.cpp:
8 Unity tests of the median filter (host-side, no Arduino dependency).
- docs/firmware.md: full operator + integrator guide (toolchain, build,
flash, expected boot log, troubleshooting, Sprint 1 capability matrix).
Architecture note: opted for Arduino-on-ESP32 only instead of the
proposed dual Arduino-as-ESP-IDF-component setup. Rationale documented
in CHANGELOG and docs/firmware.md -- Arduino-on-ESP32 already provides
the FreeRTOS primitives we need; dual framework adds fragility without
benefit at Sprint 1 scope. Reconsider in Sprint 8 (OTA + secure boot).
NOT in Sprint 1 (intentional per brief sec. 12):
- PID loops (inner/outer)
- True Course / Track Keeping
- Full alarm catalogue beyond DI1/DI4
- Knob driver
- Studio GUI / dedicated display
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
@@ -0,0 +1,382 @@
|
||||
// =============================================================================
|
||||
// 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 "../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;
|
||||
};
|
||||
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;
|
||||
|
||||
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;
|
||||
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;
|
||||
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
Block a user