alro65/AR-Autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

sprint-1: firmware ESP32 base -- STANDBY + Modbus + NMEA 2000 + watchdog

Browse Source 
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:
Alvaro Enrique Romero Donado
2026-05-18 10:45:56 -04:00
parent 1d7dd63327
commit 65860948b4
31 changed files with 3310 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
firmware/ar_autopilot_v1/src/protocols/modbus_registers.h
+148
View File
@@ -0,0 +1,148 @@
// =============================================================================
// modbus_registers.h -- AR-Autopilot Modbus RTU register map
// =============================================================================
//
// AUTO-GENERATED. Do not edit by hand.
// Source: firmware/ar_autopilot_v1/modbus_registers.yaml
// Regenerate with: python tools/gen_modbus_registers.py
// =============================================================================
#pragma once
#include <cstdint>
namespace arautopilot::protocols::modbus {
constexpr const char* MAP_SCHEMA_VERSION = "0.1.0";
constexpr uint8_t SLAVE_ID = 1;
constexpr uint32_t BAUDRATE = 38400;
constexpr char PARITY = 'N';
constexpr uint8_t DATA_BITS = 8;
constexpr uint8_t STOP_BITS = 1;
// ----- Discrete inputs (read-only bits) -----
constexpr uint16_t DISCRETE_COUNT = 19;
constexpr uint16_t DISCRETE_MAX_ADDR = 25;
// 1 if the pilot is currently engaged (any mode other than STANDBY)
constexpr uint16_t DISCRETE_PILOT_ENGAGED = 0;
// Live state of the physical Engage/Disengage push-button
constexpr uint16_t DISCRETE_DI_DISENGAGE_BUTTON = 1;
// Port-side rudder mechanical end-stop reached
constexpr uint16_t DISCRETE_DI_LIMIT_PORT = 2;
// Starboard-side rudder mechanical end-stop reached
constexpr uint16_t DISCRETE_DI_LIMIT_STBD = 3;
// External critical alarm (VMS / genset) asserted
constexpr uint16_t DISCRETE_DI_EXTERNAL_ALARM = 4;
// Manual confirmation switch asserted (emergency override)
constexpr uint16_t DISCRETE_DI_MANUAL_CONFIRM = 5;
// Master actuator power relay (DO3) commanded ON
constexpr uint16_t DISCRETE_ACTUATOR_POWER = 8;
// Actuator direction output: driving to port
constexpr uint16_t DISCRETE_ACTUATOR_DRIVING_PORT = 9;
// Actuator direction output: driving to starboard
constexpr uint16_t DISCRETE_ACTUATOR_DRIVING_STBD = 10;
// Off-course warning active
constexpr uint16_t DISCRETE_ALARM_OFF_COURSE = 16;
// Severe off-course alarm (auto-disengage)
constexpr uint16_t DISCRETE_ALARM_OFF_COURSE_SEVERE = 17;
// Rudder command sent but no feedback motion
constexpr uint16_t DISCRETE_ALARM_RUDDER_NOT_RESP = 18;
// NMEA 2000 heading PGN not received >5 s
constexpr uint16_t DISCRETE_ALARM_HEADING_LOST = 19;
// Actuator current over threshold
constexpr uint16_t DISCRETE_ALARM_ACTUATOR_OVERCURR = 20;
// Supply voltage below safe threshold
constexpr uint16_t DISCRETE_ALARM_VOLTAGE_LOW = 21;
// Rudder reached mechanical end-stop
constexpr uint16_t DISCRETE_ALARM_LIMIT_REACHED = 22;
// Firmware watchdog fired -- controller reset
constexpr uint16_t DISCRETE_ALARM_WATCHDOG_TRIPPED = 23;
// VMS reported blackout or electrical fault
constexpr uint16_t DISCRETE_ALARM_VMS_CRITICAL = 24;
// OR of all alarm bits (convenience)
constexpr uint16_t DISCRETE_ANY_ALARM_ACTIVE = 25;
// ----- Coils (read-write bits) -----
constexpr uint16_t COIL_COUNT = 4;
constexpr uint16_t COIL_MAX_ADDR = 3;
// Rising edge requests pilot engagement (subject to interlocks)
constexpr uint16_t COIL_CMD_ENGAGE_REQUEST = 0;
// Rising edge forces pilot to STANDBY
constexpr uint16_t COIL_CMD_DISENGAGE_REQUEST = 1;
// Rising edge acknowledges every active alarm
constexpr uint16_t COIL_CMD_ACK_ALL_ALARMS = 2;
// Knob arming request (Sprint 7+)
constexpr uint16_t COIL_CMD_KNOB_ARM = 3;
// ----- Input registers (read-only words) -----
constexpr uint16_t INPUT_COUNT = 17;
constexpr uint16_t INPUT_MAX_ADDR = 33;
// Firmware major version
constexpr uint16_t INPUT_FW_VERSION_MAJOR = 0;
// Firmware minor version
constexpr uint16_t INPUT_FW_VERSION_MINOR = 1;
// Firmware patch version
constexpr uint16_t INPUT_FW_VERSION_PATCH = 2;
// Modbus map schema version (0=v0.1.0)
constexpr uint16_t INPUT_SCHEMA_VERSION = 3;
// Uptime seconds, low 16 bits
// unit=s
constexpr uint16_t INPUT_UPTIME_SECONDS_LO = 4;
// Uptime seconds, high 16 bits
// unit=s
constexpr uint16_t INPUT_UPTIME_SECONDS_HI = 5;
// Current AutopilotMode (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)
constexpr uint16_t INPUT_CURRENT_MODE = 6;
// Current free heap, KiB
// unit=KiB
constexpr uint16_t INPUT_FREE_HEAP_KB = 7;
// Minimum free heap since boot
// unit=KiB
constexpr uint16_t INPUT_MIN_FREE_HEAP_KB = 8;
// Filtered rudder angle, deg * 100 (-3500..+3500)
// unit=deg, scale=0.01
constexpr uint16_t INPUT_RUDDER_ANGLE_DEG_X100 = 16;
// Raw ADC reading after median filter (0..4095)
// unit=counts
constexpr uint16_t INPUT_RUDDER_RAW_ADC = 17;
// 1 if median filter has filled (>=5 samples)
constexpr uint16_t INPUT_RUDDER_VALID = 18;
// Current heading from NMEA 2000 PGN 127250, deg*100 (0..35999)
// unit=deg, scale=0.01
constexpr uint16_t INPUT_HEADING_DEG_X100 = 24;
// Rate of turn from PGN 127251, deg/s*100 (signed int16)
// unit=deg/s, scale=0.01
constexpr uint16_t INPUT_ROT_DPS_X100 = 25;
// Milliseconds since the last heading update (0..60000)
// unit=ms
constexpr uint16_t INPUT_HEADING_AGE_MS = 26;
// System battery voltage, V*100
// unit=V, scale=0.01
constexpr uint16_t INPUT_BATTERY_VOLTAGE_X100 = 32;
// Actuator current, A*100
// unit=A, scale=0.01
constexpr uint16_t INPUT_ACTUATOR_CURRENT_X100 = 33;
// ----- Holding registers (read-write words) -----
constexpr uint16_t HOLDING_COUNT = 5;
constexpr uint16_t HOLDING_MAX_ADDR = 8;
// Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)
constexpr uint16_t HOLDING_MODE_REQUEST = 0;
// Desired heading, deg*100
// unit=deg, scale=0.01
constexpr uint16_t HOLDING_HEADING_SETPOINT_X100 = 1;
// Display brightness 0..100
// unit=%
constexpr uint16_t HOLDING_BRIGHTNESS_PCT = 2;
// Alarm volume 0..100
// unit=%
constexpr uint16_t HOLDING_ALARM_VOLUME_PCT = 3;
// Dodge mode heading offset, deg*100 (signed int16)
// unit=deg, scale=0.01
constexpr uint16_t HOLDING_DODGE_OFFSET_DEG_X100 = 8;
} // namespace arautopilot::protocols::modbus
firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp
+382
View File
@@ -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
firmware/ar_autopilot_v1/src/protocols/modbus_slave.h
+45
View File
@@ -0,0 +1,45 @@
// =============================================================================
// modbus_slave.h -- Modbus RTU server (slave) interface
// =============================================================================
//
// Sprint 1: exposes the read-only telemetry registers (input registers and
// discrete inputs) and accepts writes to the holding registers and coils
// defined in modbus_registers.h (auto-generated).
//
// Reads come straight from the live telemetry helpers
// (modes::current_mode(), hal::rudder_sensor_latest(), etc.).
//
// Writes are validated and routed to the corresponding command handler
// (mode change, alarm ack, etc.). Invalid values are rejected with
// ILLEGAL_DATA_VALUE per Modbus convention.
//
// All Modbus internals (RX task, response framing, RS-485 DE/RE control)
// are delegated to the eModbus library, which spawns its own tasks on
// Core 0.
// =============================================================================
#pragma once
#include <cstdint>
namespace arautopilot::protocols::modbus {
/// Initialise the RS-485 UART, configure the Modbus server, and register
/// every read/write callback. Must be called from setup() after di_init()
/// and rudder_sensor_init().
void modbus_slave_init();
/// Start the eModbus server. After this returns, the slave is listening on
/// the configured UART/baudrate and replies asynchronously.
void modbus_slave_start();
/// True after modbus_slave_start() has successfully initialised the server.
bool modbus_slave_is_running();
/// Number of valid requests processed since boot (cheap performance counter).
uint32_t modbus_slave_request_count();
/// Number of malformed requests / illegal-data-value rejections since boot.
uint32_t modbus_slave_error_count();
} // namespace arautopilot::protocols::modbus
firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp
+178
View File
@@ -0,0 +1,178 @@
// =============================================================================
// nmea2000_consumer.cpp -- NMEA 2000 backbone consumer
// =============================================================================
#include "nmea2000_consumer.h"
#include <Arduino.h>
// Override the default ESP32 CAN pins (16/4) to match our AR-NMEA-IO board.
#include "../hal/pinout.h"
#define ESP32_CAN_TX_PIN ((gpio_num_t)PIN_CAN_TX)
#define ESP32_CAN_RX_PIN ((gpio_num_t)PIN_CAN_RX)
#include <NMEA2000_CAN.h>
#include <N2kMessages.h>
#include "../system/ar_log.h"
#include "../system/task_config.h"
namespace arautopilot::protocols::nmea2000 {
namespace {
constexpr const char* TAG = "AR/N2K";
constexpr uint32_t STALE_THRESHOLD_MS = 5000;
portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
HeadingSnapshot g_snap{
.heading_deg = 0.0f,
.is_true = false,
.rate_of_turn_dps = 0.0f,
.heading_age_ms = 0,
.rot_age_ms = 0,
.heading_valid = false,
.rot_valid = false,
};
float rad_to_deg_pos(float rad) {
float d = rad * (180.0f / (float)M_PI);
// Normalise to 0..360.
while (d < 0.0f) d += 360.0f;
while (d >= 360.0f) d -= 360.0f;
return d;
}
float rad_to_deg_signed(float rad) {
return rad * (180.0f / (float)M_PI);
}
void HandleHeading(const tN2kMsg& msg) {
unsigned char sid;
double heading = 0.0, deviation = 0.0, variation = 0.0;
tN2kHeadingReference ref = N2khr_Unavailable;
if (!ParseN2kHeading(msg, sid, heading, deviation, variation, ref)) {
return;
}
if (heading > 1e8) {
// N2k "unavailable" markers come through as huge doubles.
return;
}
const float heading_deg = rad_to_deg_pos((float)heading);
const bool is_true = (ref == N2khr_true);
const uint32_t now = millis();
portENTER_CRITICAL(&g_mux);
g_snap.heading_deg = heading_deg;
g_snap.is_true = is_true;
g_snap.heading_age_ms = now;
g_snap.heading_valid = true;
portEXIT_CRITICAL(&g_mux);
AR_LOGV(TAG, "PGN 127250 heading=%.2f deg (%s)", heading_deg,
is_true ? "true" : "magnetic");
}
void HandleROT(const tN2kMsg& msg) {
unsigned char sid;
double rot = 0.0;
if (!ParseN2kRateOfTurn(msg, sid, rot)) {
return;
}
if (rot > 1e8 || rot < -1e8) {
return;
}
const float rot_dps = rad_to_deg_signed((float)rot);
const uint32_t now = millis();
portENTER_CRITICAL(&g_mux);
g_snap.rate_of_turn_dps = rot_dps;
g_snap.rot_age_ms = now;
g_snap.rot_valid = true;
portEXIT_CRITICAL(&g_mux);
AR_LOGV(TAG, "PGN 127251 ROT=%.3f deg/s", rot_dps);
}
// One global dispatcher because NMEA2000.SetMsgHandler() takes a single
// callback that we have to filter by PGN ourselves.
void MessageHandler(const tN2kMsg& msg) {
switch (msg.PGN) {
case 127250L:
HandleHeading(msg);
break;
case 127251L:
HandleROT(msg);
break;
default:
// Sprint 1: ignore everything else.
break;
}
}
void RxTask(void* /*pv*/) {
AR_LOGI(TAG, "nmea2000_consumer task started on core %d", xPortGetCoreID());
for (;;) {
NMEA2000.ParseMessages();
// Update validity flags based on age.
const uint32_t now = millis();
portENTER_CRITICAL(&g_mux);
if (g_snap.heading_valid && (now - g_snap.heading_age_ms) > STALE_THRESHOLD_MS) {
g_snap.heading_valid = false;
}
if (g_snap.rot_valid && (now - g_snap.rot_age_ms) > STALE_THRESHOLD_MS) {
g_snap.rot_valid = false;
}
portEXIT_CRITICAL(&g_mux);
// 100 Hz polling is plenty -- the CAN driver buffers incoming frames.
vTaskDelay(pdMS_TO_TICKS(10));
}
}
} // namespace
void nmea2000_consumer_init() {
NMEA2000.SetProductInformation(
"AR-AP-1", // Manufacturer's Model serial code
100, // Manufacturer's product code
"AR-Autopilot Controller v1", // Model ID
AR_FW_VERSION, // SW version
"AR-NMEA-IO v1.0" // Model version
);
NMEA2000.SetDeviceInformation(
1, // Unique number
150, // Device function: Autopilot
40, // Device class: Steering and Control surfaces
2046 // Manufacturer code: reserved/test (real one needs NMEA registration)
);
NMEA2000.SetMode(tNMEA2000::N2km_ListenAndNode, 25);
NMEA2000.EnableForward(false);
NMEA2000.SetMsgHandler(&MessageHandler);
if (NMEA2000.Open()) {
AR_LOGI(TAG,
"nmea2000_consumer_init: stack open, CAN TX=%d RX=%d, "
"subscribed to PGN 127250 + 127251",
PIN_CAN_TX, PIN_CAN_RX);
} else {
AR_LOGE(TAG,
"nmea2000_consumer_init: NMEA2000.Open() failed (CAN driver "
"init error). System will continue but no N2K traffic.");
}
}
void nmea2000_consumer_start_task() {
xTaskCreatePinnedToCore(RxTask, "n2k_rx", AR_TASK_STACK_N2K_RX, nullptr,
AR_TASK_PRIO_N2K_RX, nullptr, AR_TASK_CORE_COMMS);
}
HeadingSnapshot nmea2000_latest() {
HeadingSnapshot copy;
portENTER_CRITICAL(&g_mux);
copy = g_snap;
portEXIT_CRITICAL(&g_mux);
return copy;
}
bool nmea2000_is_stale() {
const auto s = nmea2000_latest();
return !s.heading_valid;
}
} // namespace arautopilot::protocols::nmea2000
firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h
+46
View File
@@ -0,0 +1,46 @@
// =============================================================================
// nmea2000_consumer.h -- NMEA 2000 backbone consumer
// =============================================================================
//
// Sprint 1 subscribes to PGN 127250 (Vessel Heading) and PGN 127251
// (Rate of Turn) from the boat's NMEA 2000 backbone. The latest values
// are stashed in a thread-safe snapshot that the Modbus slave (and later
// the PID outer loop) read from.
//
// Later sprints will also subscribe to PGN 129025/129029 (Position),
// 129026 (COG/SOG), 129284 (Navigation Data), 127257 (Attitude).
//
// This module owns the NMEA2000 instance and its dedicated FreeRTOS task
// pinned to Core 0.
// =============================================================================
#pragma once
#include <cstdint>
namespace arautopilot::protocols::nmea2000 {
struct HeadingSnapshot {
float heading_deg; ///< 0..360, magnetic or true depending on source
bool is_true; ///< true if reference is "true north", false if magnetic
float rate_of_turn_dps; ///< signed deg/s; positive = turning to starboard
uint32_t heading_age_ms; ///< millis() at the last 127250 update
uint32_t rot_age_ms; ///< millis() at the last 127251 update
bool heading_valid; ///< true if the heading is fresh (<5 s old)
bool rot_valid; ///< true if the ROT is fresh (<5 s old)
};
/// Initialise the NMEA2000 stack with our PGN handlers.
void nmea2000_consumer_init();
/// Spawn the FreeRTOS task that pumps NMEA2000.ParseMessages() forever.
void nmea2000_consumer_start_task();
/// Thread-safe read of the latest heading + ROT snapshot.
HeadingSnapshot nmea2000_latest();
/// True if either heading_age_ms or rot_age_ms exceeds the stale-threshold
/// (default 5 s, brief section 7).
bool nmea2000_is_stale();
} // namespace arautopilot::protocols::nmea2000