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:
2026-05-18 10:45:56 -04:00
parent 1d7dd63327
commit 65860948b4
31 changed files with 3310 additions and 0 deletions
+158
View File
@@ -0,0 +1,158 @@
// =============================================================================
// di_do.cpp -- generic DI/DO implementation
// =============================================================================
#include "di_do.h"
#include <Arduino.h>
#include <array>
#include "../system/ar_log.h"
#include "pinout.h"
namespace arautopilot::hal {
namespace {
constexpr const char* TAG = "AR/HAL";
// Tunable: how many consecutive identical samples constitute a confirmed
// state for a DI. At a 50 Hz poll rate, 2 -> ~40 ms debounce.
constexpr uint8_t DI_DEBOUNCE_SAMPLES = 2;
struct DiSlot {
uint8_t pin;
bool state; // last confirmed (debounced) state
bool prev_state; // state on the *previous* poll, for edge detection
uint8_t stable_count; // consecutive identical raw reads
bool raw_last; // last raw read
};
constexpr std::size_t DI_COUNT = 5;
std::array<DiSlot, DI_COUNT> g_di_slots{
DiSlot{PIN_DI1_DISENGAGE_BUTTON, false, false, 0, false},
DiSlot{PIN_DI2_LIMIT_SWITCH_PORT, false, false, 0, false},
DiSlot{PIN_DI3_LIMIT_SWITCH_STBD, false, false, 0, false},
DiSlot{PIN_DI4_EXTERNAL_ALARM, false, false, 0, false},
DiSlot{PIN_DI5_MANUAL_CONFIRM, false, false, 0, false},
};
struct DoSlot {
uint8_t pin;
bool state;
};
constexpr std::size_t DO_COUNT = 10;
std::array<DoSlot, DO_COUNT> g_do_slots{
DoSlot{PIN_DO1_PUMP_PORT, false}, DoSlot{PIN_DO2_PUMP_STBD, false},
DoSlot{PIN_DO3_ACTUATOR_POWER, false}, DoSlot{PIN_DO4_BUZZER, false},
DoSlot{PIN_DO5_ENGAGED_LAMP, false}, DoSlot{PIN_DO6_RESERVED, false},
DoSlot{PIN_DO7_RESERVED, false}, DoSlot{PIN_DO8_RESERVED, false},
DoSlot{PIN_DO9_RESERVED, false}, DoSlot{PIN_DO10_RESERVED, false},
};
DiSlot* find_di(uint8_t pin) {
for (auto& slot : g_di_slots) {
if (slot.pin == pin) {
return &slot;
}
}
return nullptr;
}
DoSlot* find_do(uint8_t pin) {
for (auto& slot : g_do_slots) {
if (slot.pin == pin) {
return &slot;
}
}
return nullptr;
}
} // namespace
void di_init() {
for (auto& slot : g_di_slots) {
pinMode(slot.pin, INPUT_PULLUP);
// INPUT_PULLUP means a closed-to-ground switch reads LOW=pressed.
// Our logical "true" = "pressed/active", so we invert in di_poll().
slot.state = false;
slot.prev_state = false;
slot.stable_count = 0;
slot.raw_last = false;
}
AR_LOGI(TAG, "di_init: %u digital inputs configured (INPUT_PULLUP)",
(unsigned)g_di_slots.size());
}
void di_poll() {
for (auto& slot : g_di_slots) {
// Active-low (INPUT_PULLUP + switch-to-GND).
bool raw = (digitalRead(slot.pin) == LOW);
slot.prev_state = slot.state;
if (raw == slot.raw_last) {
if (slot.stable_count < DI_DEBOUNCE_SAMPLES) {
++slot.stable_count;
}
if (slot.stable_count >= DI_DEBOUNCE_SAMPLES) {
slot.state = raw;
}
} else {
slot.stable_count = 0;
}
slot.raw_last = raw;
}
}
bool di_read(uint8_t pin) {
auto* slot = find_di(pin);
return slot != nullptr ? slot->state : false;
}
bool di_rising_edge(uint8_t pin) {
auto* slot = find_di(pin);
return slot != nullptr && slot->state && !slot->prev_state;
}
bool di_falling_edge(uint8_t pin) {
auto* slot = find_di(pin);
return slot != nullptr && !slot->state && slot->prev_state;
}
void do_init() {
for (auto& slot : g_do_slots) {
pinMode(slot.pin, OUTPUT);
digitalWrite(slot.pin, LOW);
slot.state = false;
}
AR_LOGI(TAG, "do_init: %u digital outputs configured (driven LOW)",
(unsigned)g_do_slots.size());
}
bool do_write(uint8_t pin, bool high) {
auto* slot = find_do(pin);
if (slot == nullptr) {
AR_LOGW(TAG, "do_write: unknown pin %d", (int)pin);
return false;
}
if (slot->state == high) {
return false;
}
digitalWrite(slot->pin, high ? HIGH : LOW);
slot->state = high;
return true;
}
bool do_state(uint8_t pin) {
auto* slot = find_do(pin);
return slot != nullptr ? slot->state : false;
}
void do_all_off() {
for (auto& slot : g_do_slots) {
digitalWrite(slot.pin, LOW);
slot.state = false;
}
AR_LOGW(TAG, "do_all_off: all digital outputs driven LOW");
}
} // namespace arautopilot::hal
+57
View File
@@ -0,0 +1,57 @@
// =============================================================================
// di_do.h -- generic digital-input / digital-output helpers
// =============================================================================
//
// Thin layer above Arduino digitalRead / digitalWrite that:
// - Tracks the *logical* state of every input/output so other tasks can
// query without re-reading the pin.
// - Debounces DIs in software (configurable, default 20 ms).
// - Caches the last commanded DO state so we never write the same value
// twice (cheap optimisation for slow open-drain outputs).
//
// Pins handled by this module are the ones listed in hal/pinout.h.
// =============================================================================
#pragma once
#include <cstdint>
namespace arautopilot::hal {
// ----- Digital inputs -------------------------------------------------------
/// Initialise all DI pins declared in pinout.h with INPUT_PULLUP.
/// Must be called once during setup() before any di_read() call.
void di_init();
/// Sample every DI once, run the debounce filter, and store the result.
/// Intended to be called from the safety task at a fixed rate (50 Hz).
void di_poll();
/// Return the last debounced logical state of a DI pin.
/// Pin must be one of the PIN_DIx_* constants from pinout.h.
bool di_read(uint8_t pin);
/// Return true if the pin changed state on the most recent di_poll() call
/// (edge-detect). Useful for the disengage button.
bool di_rising_edge(uint8_t pin);
bool di_falling_edge(uint8_t pin);
// ----- Digital outputs ------------------------------------------------------
/// Initialise all DO pins declared in pinout.h as OUTPUT and drive them LOW.
/// Must be called once during setup() before any do_write() call.
void do_init();
/// Set a DO pin to the given logical state. No-op if the pin was already in
/// that state. Returns true if the write actually happened.
/// Pin must be one of the PIN_DOx_* constants from pinout.h.
bool do_write(uint8_t pin, bool high);
/// Return the last commanded state of a DO pin.
bool do_state(uint8_t pin);
/// Drive every DO LOW. Called by the safety system on emergency disengage.
void do_all_off();
} // namespace arautopilot::hal
@@ -0,0 +1,115 @@
// =============================================================================
// rudder_actuator.cpp -- rudder actuator driver (Sprint 1 stub)
// =============================================================================
#include "rudder_actuator.h"
#include <Arduino.h>
#include "../modes/standby.h"
#include "../system/ar_log.h"
#include "di_do.h"
#include "pinout.h"
namespace arautopilot::hal {
namespace {
constexpr const char* TAG = "AR/HAL";
bool g_power_on = false;
int8_t g_last_command_pct = 0;
} // namespace
void rudder_actuator_init() {
// do_init() in di_do.cpp already configured DO1..DO10 as outputs LOW.
// Just make sure our local state matches and assert the safe defaults.
do_write(PIN_DO1_PUMP_PORT, false);
do_write(PIN_DO2_PUMP_STBD, false);
do_write(PIN_DO3_ACTUATOR_POWER, false);
g_power_on = false;
g_last_command_pct = 0;
AR_LOGI(TAG, "rudder_actuator_init: power OFF, both directions LOW");
}
void rudder_actuator_power_on() {
do_write(PIN_DO1_PUMP_PORT, false); // ensure direction outputs LOW before
do_write(PIN_DO2_PUMP_STBD, false); // we energise the relay
do_write(PIN_DO3_ACTUATOR_POWER, true);
g_power_on = true;
AR_LOGI(TAG, "rudder_actuator: master power ON");
}
void rudder_actuator_power_off() {
do_write(PIN_DO1_PUMP_PORT, false);
do_write(PIN_DO2_PUMP_STBD, false);
do_write(PIN_DO3_ACTUATOR_POWER, false);
g_power_on = false;
g_last_command_pct = 0;
AR_LOGW(TAG, "rudder_actuator: master power OFF (interlock or operator)");
}
bool rudder_actuator_is_powered() { return g_power_on; }
bool rudder_actuator_at_limit() {
return di_read(PIN_DI2_LIMIT_SWITCH_PORT) ||
di_read(PIN_DI3_LIMIT_SWITCH_STBD);
}
bool rudder_command(int8_t pwm_pct) {
// ---- Safety interlocks -------------------------------------------------
// Interlock #1: master power must be on. Without DO3, the H-bridge or
// pump relays are isolated and the direction outputs do nothing -- but
// we refuse anyway so the firmware state is consistent.
if (!g_power_on) {
if (pwm_pct != 0) {
AR_LOGW(TAG, "rudder_command(%d): refused -- power OFF",
(int)pwm_pct);
}
do_write(PIN_DO1_PUMP_PORT, false);
do_write(PIN_DO2_PUMP_STBD, false);
g_last_command_pct = 0;
return false;
}
// Interlock #2: STANDBY mode never drives the rudder.
if (modes::is_standby()) {
if (pwm_pct != 0) {
AR_LOGW(TAG, "rudder_command(%d): refused -- STANDBY", (int)pwm_pct);
}
do_write(PIN_DO1_PUMP_PORT, false);
do_write(PIN_DO2_PUMP_STBD, false);
g_last_command_pct = 0;
return false;
}
// Interlock #3: limit switches block further travel in that direction.
if (pwm_pct < 0 && di_read(PIN_DI2_LIMIT_SWITCH_PORT)) {
AR_LOGW(TAG, "rudder_command(%d): blocked -- port limit", (int)pwm_pct);
do_write(PIN_DO1_PUMP_PORT, false);
return false;
}
if (pwm_pct > 0 && di_read(PIN_DI3_LIMIT_SWITCH_STBD)) {
AR_LOGW(TAG, "rudder_command(%d): blocked -- starboard limit",
(int)pwm_pct);
do_write(PIN_DO2_PUMP_STBD, false);
return false;
}
// ---- Direction output --------------------------------------------------
// Sprint 1 stub: just toggle the direction outputs. No PWM modulation;
// the intensity will be wired in Sprint 2 via LEDC.
if (pwm_pct > 0) {
do_write(PIN_DO1_PUMP_PORT, false);
do_write(PIN_DO2_PUMP_STBD, true);
} else if (pwm_pct < 0) {
do_write(PIN_DO2_PUMP_STBD, false);
do_write(PIN_DO1_PUMP_PORT, true);
} else {
do_write(PIN_DO1_PUMP_PORT, false);
do_write(PIN_DO2_PUMP_STBD, false);
}
g_last_command_pct = pwm_pct;
return true;
}
} // namespace arautopilot::hal
@@ -0,0 +1,52 @@
// =============================================================================
// rudder_actuator.h -- rudder actuator driver (Sprint 1 stub)
// =============================================================================
//
// Drives the rudder pump/motor via DO1 (port direction), DO2 (starboard
// direction), and DO3 (master power relay). The actual command output is
// produced by the inner PID loop (Sprint 2). In Sprint 1 this module:
//
// - Exposes a `rudder_command(int8_t pwm_pct)` API.
// - Refuses to act if the rudder is at its mechanical limit (DI2/DI3).
// - Refuses to act if master power is off (DO3 LOW).
// - Refuses to act if the system is in STANDBY (Sprint 1 default).
//
// All three "refuse" conditions are safety-critical: they remain in place
// for every subsequent sprint.
// =============================================================================
#pragma once
#include <cstdint>
namespace arautopilot::hal {
/// Configure DO1/DO2/DO3 as outputs and drive them all LOW.
void rudder_actuator_init();
/// Engage the master power relay (DO3 HIGH). Required before any
/// rudder_command() call has effect.
void rudder_actuator_power_on();
/// Drop the master power relay and stop both directions. Idempotent.
void rudder_actuator_power_off();
/// Send a signed command to the actuator:
/// pwm_pct = 0 -> both direction outputs LOW (rudder freewheels)
/// pwm_pct > 0 -> drive STARBOARD (DO2 HIGH)
/// pwm_pct < 0 -> drive PORT (DO1 HIGH)
///
/// Sprint 1 stub: only the direction outputs are toggled, no PWM modulation
/// (intensity ignored). Sprint 2 will replace the body with a real LEDC PWM.
///
/// Returns true if the command was actually applied; false if a safety
/// interlock blocked it (limit switch, power off, or standby mode).
bool rudder_command(int8_t pwm_pct);
/// True if the master power relay is currently engaged.
bool rudder_actuator_is_powered();
/// True if either limit switch is asserted.
bool rudder_actuator_at_limit();
} // namespace arautopilot::hal
@@ -0,0 +1,107 @@
// =============================================================================
// rudder_sensor.cpp -- AI1 rudder-angle sensor with median filter
// =============================================================================
#include "rudder_sensor.h"
#include <Arduino.h>
#include "../filters/median.h"
#include "../system/ar_log.h"
#include "../system/task_config.h"
#include "pinout.h"
namespace arautopilot::hal {
namespace {
constexpr const char* TAG = "AR/HAL";
// Calibration defaults: full-range ADC mapped to +-35 deg of rudder.
// Reasonable for an unconfigured bench but NEVER ship this to a customer
// vessel -- it must be replaced during sea-trial commissioning (Sprint 7).
constexpr int16_t DEFAULT_RAW_AT_MIN = 0;
constexpr int16_t DEFAULT_RAW_AT_MAX = 4095;
constexpr float DEFAULT_MIN_DEG = -35.0f;
constexpr float DEFAULT_MAX_DEG = +35.0f;
filters::MedianFilter<int16_t, 5> g_median{};
portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
RudderReading g_latest{0, 0.0f, 0, false};
struct Calibration {
int16_t raw_at_min;
int16_t raw_at_max;
float min_deg;
float max_deg;
};
Calibration g_calib{DEFAULT_RAW_AT_MIN, DEFAULT_RAW_AT_MAX, DEFAULT_MIN_DEG,
DEFAULT_MAX_DEG};
float raw_to_deg(int16_t raw, const Calibration& c) {
const float span_raw = static_cast<float>(c.raw_at_max - c.raw_at_min);
if (span_raw == 0.0f) {
return 0.0f;
}
const float t = (static_cast<float>(raw - c.raw_at_min)) / span_raw;
return c.min_deg + t * (c.max_deg - c.min_deg);
}
void SensorTask(void* /*pv*/) {
AR_LOGI(TAG, "rudder_sensor task started on core %d, pin %d (ADC1)",
xPortGetCoreID(), PIN_AI1_RUDDER_ANGLE);
TickType_t last_wake = xTaskGetTickCount();
for (;;) {
const int raw = analogRead(PIN_AI1_RUDDER_ANGLE);
const int16_t filtered = g_median.push(static_cast<int16_t>(raw));
Calibration cal;
portENTER_CRITICAL(&g_mux);
cal = g_calib;
portEXIT_CRITICAL(&g_mux);
const float angle = raw_to_deg(filtered, cal);
portENTER_CRITICAL(&g_mux);
g_latest.raw_adc = filtered;
g_latest.angle_deg = angle;
g_latest.timestamp_ms = millis();
g_latest.valid = g_median.is_full();
portEXIT_CRITICAL(&g_mux);
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_RUDDER_SENSOR));
}
}
} // namespace
void rudder_sensor_init() {
analogReadResolution(12); // 0..4095
analogSetAttenuation(ADC_11db); // ~0..3.3 V input range
g_median.reset();
AR_LOGI(TAG, "rudder_sensor_init: 12-bit ADC, 11dB attn, %d-sample median",
5);
}
void rudder_sensor_start_task() {
xTaskCreatePinnedToCore(SensorTask, "rudder_sensor",
AR_TASK_STACK_RUDDER_SENSOR, nullptr,
AR_TASK_PRIO_RUDDER_SENSOR, nullptr,
AR_TASK_CORE_REALTIME);
}
RudderReading rudder_sensor_latest() {
RudderReading copy;
portENTER_CRITICAL(&g_mux);
copy = g_latest;
portEXIT_CRITICAL(&g_mux);
return copy;
}
void rudder_sensor_set_calibration(int16_t raw_at_min_deg, int16_t raw_at_max_deg,
float min_deg, float max_deg) {
portENTER_CRITICAL(&g_mux);
g_calib = {raw_at_min_deg, raw_at_max_deg, min_deg, max_deg};
portEXIT_CRITICAL(&g_mux);
AR_LOGI(TAG,
"rudder_sensor calibration updated: raw [%d..%d] -> deg [%.2f..%.2f]",
raw_at_min_deg, raw_at_max_deg, min_deg, max_deg);
}
} // namespace arautopilot::hal
@@ -0,0 +1,47 @@
// =============================================================================
// rudder_sensor.h -- AI1 rudder-angle sensor with median filter
// =============================================================================
//
// Reads the raw ADC value of the rudder position sensor (pot, Hall, or
// 4-20 mA loop conditioned to 0-3.3 V) on PIN_AI1_RUDDER_ANGLE, applies a
// 5-sample median filter to suppress single-sample spikes, and converts the
// result to degrees via a linear calibration.
//
// Calibration in Sprint 1 is a hard-coded placeholder (-35 deg @ 0 raw,
// +35 deg @ 4095 raw). In Sprint 7 (commissioning), the calibration values
// will come from the persistent NVS store written during sea trial.
//
// The sensor task runs at 100 Hz on Core 1 (real-time core).
// =============================================================================
#pragma once
#include <cstdint>
namespace arautopilot::hal {
struct RudderReading {
int16_t raw_adc; ///< 0..4095, raw ADC after median filter
float angle_deg; ///< -35.0 .. +35.0 typical, after calibration
uint32_t timestamp_ms; ///< millis() at which the sample was taken
bool valid; ///< false until the filter has filled
};
/// Configure ADC1 channel for AI1, init the median filter.
void rudder_sensor_init();
/// Start the FreeRTOS sampling task (100 Hz, Core 1).
void rudder_sensor_start_task();
/// Get the latest filtered reading. Thread-safe (single critical section).
RudderReading rudder_sensor_latest();
/// Override the linear calibration. Called by the commissioning code
/// (Sprint 7); the placeholder is fine for Sprint 1 bench work.
///
/// raw_at_min_deg, raw_at_max_deg : ADC counts (0..4095)
/// min_deg, max_deg : physical angle in degrees
void rudder_sensor_set_calibration(int16_t raw_at_min_deg, int16_t raw_at_max_deg,
float min_deg, float max_deg);
} // namespace arautopilot::hal