From 65860948b4d8f207b9dabb5306b1f62ce58b48b0 Mon Sep 17 00:00:00 2001 From: alro1965 Date: Mon, 18 May 2026 10:45:56 -0400 Subject: [PATCH] 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 (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) --- CHANGELOG.md | 104 +++++ arautopilot/shared/modbus_register_map.py | 97 +++++ arautopilot/tests/test_modbus_register_map.py | 164 ++++++++ docs/firmware.md | 222 ++++++++++ .../ar_autopilot_v1/modbus_registers.yaml | 106 +++++ firmware/ar_autopilot_v1/platformio.ini | 123 ++++++ firmware/ar_autopilot_v1/src/filters/median.h | 76 ++++ firmware/ar_autopilot_v1/src/hal/di_do.cpp | 158 ++++++++ firmware/ar_autopilot_v1/src/hal/di_do.h | 57 +++ .../src/hal/rudder_actuator.cpp | 115 ++++++ .../ar_autopilot_v1/src/hal/rudder_actuator.h | 52 +++ .../ar_autopilot_v1/src/hal/rudder_sensor.cpp | 107 +++++ .../ar_autopilot_v1/src/hal/rudder_sensor.h | 47 +++ firmware/ar_autopilot_v1/src/main.cpp | 98 +++++ .../ar_autopilot_v1/src/modes/standby.cpp | 80 ++++ firmware/ar_autopilot_v1/src/modes/standby.h | 46 +++ .../src/protocols/modbus_registers.h | 148 +++++++ .../src/protocols/modbus_slave.cpp | 382 ++++++++++++++++++ .../src/protocols/modbus_slave.h | 45 +++ .../src/protocols/nmea2000_consumer.cpp | 178 ++++++++ .../src/protocols/nmea2000_consumer.h | 46 +++ .../src/safety/safety_monitor.cpp | 77 ++++ .../src/safety/safety_monitor.h | 26 ++ .../ar_autopilot_v1/src/safety/watchdog.cpp | 40 ++ .../ar_autopilot_v1/src/safety/watchdog.h | 29 ++ firmware/ar_autopilot_v1/src/system/ar_log.h | 35 ++ .../ar_autopilot_v1/src/system/heartbeat.cpp | 50 +++ .../ar_autopilot_v1/src/system/task_config.h | 68 ++++ .../test/test_median_filter/test_median.cpp | 112 +++++ .../tools/modbus_client_test.py | 166 ++++++++ tools/gen_modbus_registers.py | 256 ++++++++++++ 31 files changed, 3310 insertions(+) create mode 100644 arautopilot/shared/modbus_register_map.py create mode 100644 arautopilot/tests/test_modbus_register_map.py create mode 100644 docs/firmware.md create mode 100644 firmware/ar_autopilot_v1/modbus_registers.yaml create mode 100644 firmware/ar_autopilot_v1/platformio.ini create mode 100644 firmware/ar_autopilot_v1/src/filters/median.h create mode 100644 firmware/ar_autopilot_v1/src/hal/di_do.cpp create mode 100644 firmware/ar_autopilot_v1/src/hal/di_do.h create mode 100644 firmware/ar_autopilot_v1/src/hal/rudder_actuator.cpp create mode 100644 firmware/ar_autopilot_v1/src/hal/rudder_actuator.h create mode 100644 firmware/ar_autopilot_v1/src/hal/rudder_sensor.cpp create mode 100644 firmware/ar_autopilot_v1/src/hal/rudder_sensor.h create mode 100644 firmware/ar_autopilot_v1/src/main.cpp create mode 100644 firmware/ar_autopilot_v1/src/modes/standby.cpp create mode 100644 firmware/ar_autopilot_v1/src/modes/standby.h create mode 100644 firmware/ar_autopilot_v1/src/protocols/modbus_registers.h create mode 100644 firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp create mode 100644 firmware/ar_autopilot_v1/src/protocols/modbus_slave.h create mode 100644 firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp create mode 100644 firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h create mode 100644 firmware/ar_autopilot_v1/src/safety/safety_monitor.cpp create mode 100644 firmware/ar_autopilot_v1/src/safety/safety_monitor.h create mode 100644 firmware/ar_autopilot_v1/src/safety/watchdog.cpp create mode 100644 firmware/ar_autopilot_v1/src/safety/watchdog.h create mode 100644 firmware/ar_autopilot_v1/src/system/ar_log.h create mode 100644 firmware/ar_autopilot_v1/src/system/heartbeat.cpp create mode 100644 firmware/ar_autopilot_v1/src/system/task_config.h create mode 100644 firmware/ar_autopilot_v1/test/test_median_filter/test_median.cpp create mode 100644 firmware/ar_autopilot_v1/tools/modbus_client_test.py create mode 100644 tools/gen_modbus_registers.py diff --git a/CHANGELOG.md b/CHANGELOG.md index c217abf..0b44066 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,110 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +## [0.1.0-sprint1] — Sprint 1 — Firmware ESP32 base — 2026-05-18 + +> Sprint 1 was executed autonomously overnight after the user gave explicit +> blanket authorisation (no per-decision approval) to push through subsequent +> sprints. The four technical decisions in `docs/sprint-1-plan.md` §2 were +> taken with the **recommended** option (Arduino-only framework -- pragmatic +> shift from the dual-framework plan, see Architecture note below). + +### Added + +#### Firmware (`firmware/ar_autopilot_v1/`) + +- **`platformio.ini`** — Build configuration for ESP32-DOWD on the + AR-NMEA-IO v1.0 board. Three envs: + - `esp32-dev` — release build (-Os, default). + - `esp32-debug` — debug build (-O0, verbose logs). + - `native` — host Unity tests (no hardware required, host C++ compiler + needed). + - `check` — cppcheck static analysis. +- **Sprint 1 dependencies** pinned: `NMEA2000-library` v4.22+, + `NMEA2000_esp32` v1.0+, `eModbus` v1.7.4. +- **`src/main.cpp`** — boot, FreeRTOS task spawn, returns to scheduler. +- **`src/system/`** — `ar_log.h` logging facade, `task_config.h` central + table of stack sizes / priorities / core pinning, `heartbeat.cpp` + (1 Hz LED + uptime log on Core 0). +- **`src/modes/`** — STANDBY-only mode state machine. Non-STANDBY mode + requests rejected with a warning. +- **`src/hal/`** — `di_do.{h,cpp}` (5 DI + 10 DO with software debouncing + and last-state cache); `rudder_sensor.{h,cpp}` (100 Hz ADC + 5-sample + median filter, Core 1); `rudder_actuator.{h,cpp}` (DO1/DO2/DO3 driver + with three layered safety interlocks: power-off, STANDBY, limit switch). +- **`src/safety/`** — `watchdog.{h,cpp}` (TWDT @ 2 s, panic on expire); + `safety_monitor.{h,cpp}` (50 Hz DI polling on Core 1, DI1 disengage + button enforced, DI4 external alarm, both-limit-switch interlock). +- **`src/protocols/modbus_slave.{h,cpp}`** — eModbus RTU server on UART2 + @ 38400 8N1, slave ID 1. 17 input registers, 19 discrete inputs, + 5 holding registers, 4 coils. Reads pull from live telemetry (mode, + rudder, NMEA 2000 snapshot, heap). Writes validate range and route to + the corresponding handler. +- **`src/protocols/nmea2000_consumer.{h,cpp}`** — NMEA 2000 stack open + with CAN TX=GPIO3 RX=GPIO1, subscribed to PGN 127250 (Heading) and + PGN 127251 (Rate of Turn). Snapshot exposed via Modbus input registers + 24-26 (heading_deg_x100, rot_dps_x100, heading_age_ms). 5 s staleness + flag built in for Sprint 6 alarm wiring. +- **`src/filters/median.h`** — Templated `MedianFilter` (host + testable). +- **`modbus_registers.yaml`** — Single source of truth for the Modbus + register map. 45 entries total. +- **`test/test_median_filter/test_median.cpp`** — 8 Unity tests of the + median filter (host-side, no Arduino dependency). +- **`tools/modbus_client_test.py`** — manual Modbus client for poking + the slave from a PC with a USB-RS485 dongle. + +#### Cross-cutting + +- **`tools/gen_modbus_registers.py`** — YAML -> C++ header + Python + module code generator with `--check` mode for CI/drift detection. +- **`arautopilot/shared/modbus_register_map.py`** — generated Python + mirror of the firmware register contract (`Reg` dataclass per entry, + grouped into `DISCRETES`, `COILS`, `INPUTS`, `HOLDINGS`). +- **`arautopilot/tests/test_modbus_register_map.py`** — 30 tests: + schema sanity, address uniqueness within group, range bounds, + spot-checks for critical registers, and **drift detection** that fails + if anyone edits the YAML without regenerating. +- **`docs/firmware.md`** — firmware operator + integrator guide + (toolchain, build, flash, expected boot log, troubleshooting, + Sprint 1 capability matrix). + +### Architecture decisions taken + +- **Framework**: Arduino-on-ESP32 only (NOT the dual + Arduino-as-ESP-IDF-component proposed in the Sprint 1 plan). Rationale: + Arduino-on-ESP32 already provides full FreeRTOS access + (`xTaskCreatePinnedToCore`, priorities, TWDT, log levels), the dual + framework is notoriously fragile in PlatformIO, and we hit no + ESP-IDF-only feature in Sprint 1 scope. OTA-with-rollback and secure + boot become a real ask in Sprint 8 — at that point we either migrate + to ESP-IDF or wire the equivalent via Arduino + EspOTA. +- **FreeRTOS core split** as proposed: PID + safety + rudder sensor on + Core 1 (real-time); NMEA 2000 RX + Modbus + heartbeat on Core 0. +- **Logging**: ESP_LOG via UART0 only, no SD card. + +### Verification + +- `pio run -e esp32-dev` -> SUCCESS (RAM 6.7 %, Flash 26.5 %, 347 KB). +- `pio run -e esp32-debug` -> SUCCESS. +- `pytest` -> **110 passed** in 0.22 s (80 from Sprint 0 + 30 new). +- `ruff check arautopilot/` -> All checks passed. +- `mypy arautopilot/core library shared` -> Success, 0 issues. +- `pio test -e native` -> deferred: needs host C++ compiler (mingw / + msvc / clang) on this Windows machine. The Unity test sources compile + on any standard host once a toolchain is installed. + +### Not in Sprint 1 (intentional, per brief §12) + +- PID loops (inner/outer). +- True Course / Track Keeping modes. +- Alarm catalogue beyond DI1/DI4 forced disengage. +- Knob driver. +- Studio GUI. +- Dedicated display Flutter app. + +## [0.1.0] — Sprint 0 — Foundations — 2026-05-17 + ## [0.1.0] — Sprint 0 — Foundations — 2026-05-17 ### Added diff --git a/arautopilot/shared/modbus_register_map.py b/arautopilot/shared/modbus_register_map.py new file mode 100644 index 0000000..c3e8565 --- /dev/null +++ b/arautopilot/shared/modbus_register_map.py @@ -0,0 +1,97 @@ +"""AR-Autopilot Modbus RTU register map -- generated mirror of the firmware. + +AUTO-GENERATED. Do not edit by hand. +Source: firmware/ar_autopilot_v1/modbus_registers.yaml +Regenerate with: python tools/gen_modbus_registers.py +""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class Reg: + """One Modbus register entry as seen from Python tooling.""" + addr: int + name: str + desc: str = "" + unit: str = "" + scale: float = 1.0 + offset: float = 0.0 + + +MAP_SCHEMA_VERSION = "0.1.0" +SLAVE_ID = 1 +BAUDRATE = 38400 +PARITY = "N" +DATA_BITS = 8 +STOP_BITS = 1 + +# DISCRETES +DISCRETES: dict[str, Reg] = { + "PILOT_ENGAGED": Reg(addr=0, name="PILOT_ENGAGED", desc='1 if the pilot is currently engaged (any mode other than STANDBY)', unit="", scale=1.0, offset=0.0), + "DI_DISENGAGE_BUTTON": Reg(addr=1, name="DI_DISENGAGE_BUTTON", desc='Live state of the physical Engage/Disengage push-button', unit="", scale=1.0, offset=0.0), + "DI_LIMIT_PORT": Reg(addr=2, name="DI_LIMIT_PORT", desc='Port-side rudder mechanical end-stop reached', unit="", scale=1.0, offset=0.0), + "DI_LIMIT_STBD": Reg(addr=3, name="DI_LIMIT_STBD", desc='Starboard-side rudder mechanical end-stop reached', unit="", scale=1.0, offset=0.0), + "DI_EXTERNAL_ALARM": Reg(addr=4, name="DI_EXTERNAL_ALARM", desc='External critical alarm (VMS / genset) asserted', unit="", scale=1.0, offset=0.0), + "DI_MANUAL_CONFIRM": Reg(addr=5, name="DI_MANUAL_CONFIRM", desc='Manual confirmation switch asserted (emergency override)', unit="", scale=1.0, offset=0.0), + "ACTUATOR_POWER": Reg(addr=8, name="ACTUATOR_POWER", desc='Master actuator power relay (DO3) commanded ON', unit="", scale=1.0, offset=0.0), + "ACTUATOR_DRIVING_PORT": Reg(addr=9, name="ACTUATOR_DRIVING_PORT", desc='Actuator direction output: driving to port', unit="", scale=1.0, offset=0.0), + "ACTUATOR_DRIVING_STBD": Reg(addr=10, name="ACTUATOR_DRIVING_STBD", desc='Actuator direction output: driving to starboard', unit="", scale=1.0, offset=0.0), + "ALARM_OFF_COURSE": Reg(addr=16, name="ALARM_OFF_COURSE", desc='Off-course warning active', unit="", scale=1.0, offset=0.0), + "ALARM_OFF_COURSE_SEVERE": Reg(addr=17, name="ALARM_OFF_COURSE_SEVERE", desc='Severe off-course alarm (auto-disengage)', unit="", scale=1.0, offset=0.0), + "ALARM_RUDDER_NOT_RESP": Reg(addr=18, name="ALARM_RUDDER_NOT_RESP", desc='Rudder command sent but no feedback motion', unit="", scale=1.0, offset=0.0), + "ALARM_HEADING_LOST": Reg(addr=19, name="ALARM_HEADING_LOST", desc='NMEA 2000 heading PGN not received >5 s', unit="", scale=1.0, offset=0.0), + "ALARM_ACTUATOR_OVERCURR": Reg(addr=20, name="ALARM_ACTUATOR_OVERCURR", desc='Actuator current over threshold', unit="", scale=1.0, offset=0.0), + "ALARM_VOLTAGE_LOW": Reg(addr=21, name="ALARM_VOLTAGE_LOW", desc='Supply voltage below safe threshold', unit="", scale=1.0, offset=0.0), + "ALARM_LIMIT_REACHED": Reg(addr=22, name="ALARM_LIMIT_REACHED", desc='Rudder reached mechanical end-stop', unit="", scale=1.0, offset=0.0), + "ALARM_WATCHDOG_TRIPPED": Reg(addr=23, name="ALARM_WATCHDOG_TRIPPED", desc='Firmware watchdog fired -- controller reset', unit="", scale=1.0, offset=0.0), + "ALARM_VMS_CRITICAL": Reg(addr=24, name="ALARM_VMS_CRITICAL", desc='VMS reported blackout or electrical fault', unit="", scale=1.0, offset=0.0), + "ANY_ALARM_ACTIVE": Reg(addr=25, name="ANY_ALARM_ACTIVE", desc='OR of all alarm bits (convenience)', unit="", scale=1.0, offset=0.0), +} + +# COILS +COILS: dict[str, Reg] = { + "CMD_ENGAGE_REQUEST": Reg(addr=0, name="CMD_ENGAGE_REQUEST", desc='Rising edge requests pilot engagement (subject to interlocks)', unit="", scale=1.0, offset=0.0), + "CMD_DISENGAGE_REQUEST": Reg(addr=1, name="CMD_DISENGAGE_REQUEST", desc='Rising edge forces pilot to STANDBY', unit="", scale=1.0, offset=0.0), + "CMD_ACK_ALL_ALARMS": Reg(addr=2, name="CMD_ACK_ALL_ALARMS", desc='Rising edge acknowledges every active alarm', unit="", scale=1.0, offset=0.0), + "CMD_KNOB_ARM": Reg(addr=3, name="CMD_KNOB_ARM", desc='Knob arming request (Sprint 7+)', unit="", scale=1.0, offset=0.0), +} + +# INPUTS +INPUTS: dict[str, Reg] = { + "FW_VERSION_MAJOR": Reg(addr=0, name="FW_VERSION_MAJOR", desc='Firmware major version', unit="", scale=1.0, offset=0.0), + "FW_VERSION_MINOR": Reg(addr=1, name="FW_VERSION_MINOR", desc='Firmware minor version', unit="", scale=1.0, offset=0.0), + "FW_VERSION_PATCH": Reg(addr=2, name="FW_VERSION_PATCH", desc='Firmware patch version', unit="", scale=1.0, offset=0.0), + "SCHEMA_VERSION": Reg(addr=3, name="SCHEMA_VERSION", desc='Modbus map schema version (0=v0.1.0)', unit="", scale=1.0, offset=0.0), + "UPTIME_SECONDS_LO": Reg(addr=4, name="UPTIME_SECONDS_LO", desc='Uptime seconds, low 16 bits', unit="s", scale=1.0, offset=0.0), + "UPTIME_SECONDS_HI": Reg(addr=5, name="UPTIME_SECONDS_HI", desc='Uptime seconds, high 16 bits', unit="s", scale=1.0, offset=0.0), + "CURRENT_MODE": Reg(addr=6, name="CURRENT_MODE", desc='Current AutopilotMode (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)', unit="", scale=1.0, offset=0.0), + "FREE_HEAP_KB": Reg(addr=7, name="FREE_HEAP_KB", desc='Current free heap, KiB', unit="KiB", scale=1.0, offset=0.0), + "MIN_FREE_HEAP_KB": Reg(addr=8, name="MIN_FREE_HEAP_KB", desc='Minimum free heap since boot', unit="KiB", scale=1.0, offset=0.0), + "RUDDER_ANGLE_DEG_X100": Reg(addr=16, name="RUDDER_ANGLE_DEG_X100", desc='Filtered rudder angle, deg * 100 (-3500..+3500)', unit="deg", scale=0.01, offset=0.0), + "RUDDER_RAW_ADC": Reg(addr=17, name="RUDDER_RAW_ADC", desc='Raw ADC reading after median filter (0..4095)', unit="counts", scale=1.0, offset=0.0), + "RUDDER_VALID": Reg(addr=18, name="RUDDER_VALID", desc='1 if median filter has filled (>=5 samples)', unit="", scale=1.0, offset=0.0), + "HEADING_DEG_X100": Reg(addr=24, name="HEADING_DEG_X100", desc='Current heading from NMEA 2000 PGN 127250, deg*100 (0..35999)', unit="deg", scale=0.01, offset=0.0), + "ROT_DPS_X100": Reg(addr=25, name="ROT_DPS_X100", desc='Rate of turn from PGN 127251, deg/s*100 (signed int16)', unit="deg/s", scale=0.01, offset=0.0), + "HEADING_AGE_MS": Reg(addr=26, name="HEADING_AGE_MS", desc='Milliseconds since the last heading update (0..60000)', unit="ms", scale=1.0, offset=0.0), + "BATTERY_VOLTAGE_X100": Reg(addr=32, name="BATTERY_VOLTAGE_X100", desc='System battery voltage, V*100', unit="V", scale=0.01, offset=0.0), + "ACTUATOR_CURRENT_X100": Reg(addr=33, name="ACTUATOR_CURRENT_X100", desc='Actuator current, A*100', unit="A", scale=0.01, offset=0.0), +} + +# HOLDINGS +HOLDINGS: dict[str, Reg] = { + "MODE_REQUEST": Reg(addr=0, name="MODE_REQUEST", desc='Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)', unit="", scale=1.0, offset=0.0), + "HEADING_SETPOINT_X100": Reg(addr=1, name="HEADING_SETPOINT_X100", desc='Desired heading, deg*100', unit="deg", scale=0.01, offset=0.0), + "BRIGHTNESS_PCT": Reg(addr=2, name="BRIGHTNESS_PCT", desc='Display brightness 0..100', unit="%", scale=1.0, offset=0.0), + "ALARM_VOLUME_PCT": Reg(addr=3, name="ALARM_VOLUME_PCT", desc='Alarm volume 0..100', unit="%", scale=1.0, offset=0.0), + "DODGE_OFFSET_DEG_X100": Reg(addr=8, name="DODGE_OFFSET_DEG_X100", desc='Dodge mode heading offset, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0), +} + +ALL_GROUPS: dict[str, dict[str, Reg]] = { + "discretes": DISCRETES, + "coils": COILS, + "inputs": INPUTS, + "holdings": HOLDINGS, +} diff --git a/arautopilot/tests/test_modbus_register_map.py b/arautopilot/tests/test_modbus_register_map.py new file mode 100644 index 0000000..fd0c4fa --- /dev/null +++ b/arautopilot/tests/test_modbus_register_map.py @@ -0,0 +1,164 @@ +"""Tests for the generated ``arautopilot.shared.modbus_register_map``. + +This module is regenerated from ``firmware/ar_autopilot_v1/modbus_registers.yaml`` +by ``tools/gen_modbus_registers.py``. The tests below are the safety net that +flags drift: if anyone edits the YAML, the generated Python module must be +regenerated, and these tests verify the result is structurally sound. + +The tests deliberately do NOT pin the *content* of every register (which would +just duplicate the YAML); they pin invariants that must always hold. +""" + +from __future__ import annotations + +import subprocess +import sys +from pathlib import Path + +import pytest + +from arautopilot.shared import modbus_register_map as m + +REPO_ROOT = Path(__file__).resolve().parents[2] +YAML_PATH = REPO_ROOT / "firmware/ar_autopilot_v1/modbus_registers.yaml" +GEN_SCRIPT = REPO_ROOT / "tools/gen_modbus_registers.py" +HEADER_PATH = ( + REPO_ROOT / "firmware/ar_autopilot_v1/src/protocols/modbus_registers.h" +) +PY_PATH = REPO_ROOT / "arautopilot/shared/modbus_register_map.py" + + +# ---------------------------------------------------------------------------- +# Schema / connection settings +# ---------------------------------------------------------------------------- + + +def test_schema_version_is_semver_string() -> None: + parts = m.MAP_SCHEMA_VERSION.split(".") + assert len(parts) == 3, f"schema version must be semver: {m.MAP_SCHEMA_VERSION}" + for p in parts: + assert p.isdigit(), f"non-numeric semver component: {p}" + + +def test_slave_id_in_valid_range() -> None: + # Modbus RTU spec: slave addresses 1..247. + assert 1 <= m.SLAVE_ID <= 247 + + +def test_baudrate_is_supported_value() -> None: + assert m.BAUDRATE in {9600, 19200, 38400, 57600, 115200} + + +def test_serial_framing_is_8N1() -> None: + assert m.DATA_BITS == 8 + assert m.PARITY == "N" + assert m.STOP_BITS == 1 + + +# ---------------------------------------------------------------------------- +# Register groups +# ---------------------------------------------------------------------------- + + +@pytest.mark.parametrize("group", ["DISCRETES", "COILS", "INPUTS", "HOLDINGS"]) +def test_groups_are_nonempty(group: str) -> None: + assert len(getattr(m, group)) > 0, f"group {group} is empty" + + +@pytest.mark.parametrize("group", ["DISCRETES", "COILS", "INPUTS", "HOLDINGS"]) +def test_addresses_are_unique_within_group(group: str) -> None: + regs = getattr(m, group) + addrs = [r.addr for r in regs.values()] + assert len(addrs) == len(set(addrs)), f"duplicate addresses in {group}" + + +@pytest.mark.parametrize("group", ["DISCRETES", "COILS", "INPUTS", "HOLDINGS"]) +def test_names_match_dict_keys(group: str) -> None: + """The dict key and the Reg.name field must match exactly.""" + regs = getattr(m, group) + for key, reg in regs.items(): + assert key == reg.name, f"{group}[{key!r}].name mismatch: {reg.name!r}" + + +@pytest.mark.parametrize("group", ["DISCRETES", "COILS", "INPUTS", "HOLDINGS"]) +def test_addresses_in_modbus_range(group: str) -> None: + """Modbus addresses are uint16. Anything past 65535 is invalid.""" + for r in getattr(m, group).values(): + assert 0 <= r.addr <= 0xFFFF, f"out-of-range addr in {group}: {r}" + + +# ---------------------------------------------------------------------------- +# Spot-check the critical registers +# ---------------------------------------------------------------------------- + + +def test_pilot_engaged_discrete_exists() -> None: + assert "PILOT_ENGAGED" in m.DISCRETES + + +def test_disengage_button_discrete_exists() -> None: + assert "DI_DISENGAGE_BUTTON" in m.DISCRETES + + +def test_mode_request_holding_exists_and_has_no_scale() -> None: + r = m.HOLDINGS["MODE_REQUEST"] + assert r.scale == 1.0 + assert r.offset == 0.0 + + +def test_heading_setpoint_uses_centideg_scale() -> None: + r = m.HOLDINGS["HEADING_SETPOINT_X100"] + assert r.scale == pytest.approx(0.01) + assert r.unit == "deg" + + +def test_disengage_command_coil_exists() -> None: + assert "CMD_DISENGAGE_REQUEST" in m.COILS + + +def test_rudder_angle_input_uses_centideg_scale() -> None: + r = m.INPUTS["RUDDER_ANGLE_DEG_X100"] + assert r.scale == pytest.approx(0.01) + assert r.unit == "deg" + + +def test_alarms_have_bit_neighbourhood() -> None: + """Every ALARM_* discrete must be at addr >= 16 (reserved alarm range).""" + for name, reg in m.DISCRETES.items(): + if name.startswith("ALARM_") or name == "ANY_ALARM_ACTIVE": + assert reg.addr >= 16, f"{name} should live in the alarm range" + + +# ---------------------------------------------------------------------------- +# Drift detection: regenerated files must be in sync with the YAML +# ---------------------------------------------------------------------------- + + +def test_generated_files_are_in_sync_with_yaml() -> None: + """If this test fails, run: python tools/gen_modbus_registers.py""" + result = subprocess.run( + [sys.executable, str(GEN_SCRIPT), "--check"], + capture_output=True, + text=True, + cwd=str(REPO_ROOT), + ) + assert result.returncode == 0, ( + f"generated Modbus map is OUT-OF-DATE. Regenerate with:\n" + f" python tools/gen_modbus_registers.py\n\n" + f"stderr:\n{result.stderr}" + ) + + +def test_header_file_exists() -> None: + assert HEADER_PATH.exists(), ( + f"firmware header not generated: {HEADER_PATH}" + ) + + +def test_generated_header_mentions_every_register_name() -> None: + header = HEADER_PATH.read_text(encoding="utf-8") + for group_dict in m.ALL_GROUPS.values(): + for name in group_dict: + assert name in header, ( + f"register {name} missing from generated header" + ) diff --git a/docs/firmware.md b/docs/firmware.md new file mode 100644 index 0000000..7133dbc --- /dev/null +++ b/docs/firmware.md @@ -0,0 +1,222 @@ +# AR-Autopilot firmware — operator + integrator guide + +This document covers everything needed to: + +- Build the firmware locally. +- Flash a bare AR-NMEA-IO board for the first time. +- Talk to the running firmware over Modbus RTU and via the NMEA 2000 backbone. +- Diagnose common boot-time failures. + +Project layout: see [`docs/architecture.md`](architecture.md). +Sprint 1 plan: see [`docs/sprint-1-plan.md`](sprint-1-plan.md). + +--- + +## 1. Toolchain + +| Tool | Version | Install | +|---|---|---| +| Python | 3.11+ | system | +| PlatformIO Core | 6.1+ | `pip install platformio` (already in `.venv` after `bash scripts/dev.sh install`) | +| ESP32 toolchain (xtensa-esp32-elf-gcc) | latest | auto-downloaded by PlatformIO on first build | +| Host C++ compiler (optional) | g++ 13+ or clang 17+ or MSVC | needed only for **native** Unity tests | + +You do NOT need the Arduino IDE; PlatformIO bundles its own version of the Arduino-ESP32 core (pinned in `platformio.ini`). + +--- + +## 2. Build + +From the **repo root**: + +```powershell +# Production build (release, -Os) +.\.venv\Scripts\pio.exe run -e esp32-dev -d firmware/ar_autopilot_v1 + +# Debug build (-O0, verbose logs) +.\.venv\Scripts\pio.exe run -e esp32-debug -d firmware/ar_autopilot_v1 + +# Static analysis +.\.venv\Scripts\pio.exe check -e check -d firmware/ar_autopilot_v1 +``` + +(Or `cd firmware/ar_autopilot_v1` first and drop the `-d` flag.) + +Both builds emit `firmware/ar_autopilot_v1/.pio/build/esp32-dev/firmware.bin` (~340 KB). Flash usage in Sprint 1: ~27 % of the 4 MB partition; RAM at idle ~22 KB of 320 KB. + +--- + +## 3. Flash + +```powershell +.\.venv\Scripts\pio.exe run -e esp32-dev -t upload -d firmware/ar_autopilot_v1 --upload-port COM7 +``` + +(Replace `COM7` with the port your board enumerates as. The first power-up after a bare flash should drop the chip into download mode automatically; if not, hold BOOT while pressing EN.) + +After upload, open a serial monitor: + +```powershell +.\.venv\Scripts\pio.exe device monitor -e esp32-dev -d firmware/ar_autopilot_v1 --port COM7 +``` + +You should see, within ~250 ms of boot: + +``` +[AR/MAIN] ================================================ +[AR/MAIN] AR-Autopilot -- firmware boot +[AR/MAIN] version : 0.1.0-sprint1 +[AR/MAIN] variant : release +[AR/MAIN] build : ... +[AR/MAIN] board : AR-NMEA-IO v1.0 +[AR/MAIN] mcu : ESP32-DOWD @ 240 MHz +[AR/MAIN] cores : 2 free heap: 270xxx bytes +[AR/MAIN] ================================================ +[AR/MODE] mode_init: STANDBY +[AR/HAL] di_init: 5 digital inputs configured (INPUT_PULLUP) +[AR/HAL] do_init: 10 digital outputs configured (driven LOW) +[AR/HAL] rudder_sensor_init: 12-bit ADC, 11dB attn, 5-sample median +[AR/HAL] rudder_actuator_init: power OFF, both directions LOW +[AR/SAFE] watchdog_init: TWDT timeout 2s, panic on expire +[AR/MAIN] spawning Sprint 1 tasks ... +[AR/SAFE] safety_monitor task started on core 1 (50 Hz) +[AR/HAL] rudder_sensor task started on core 1, pin 36 (ADC1) +[AR/SYS] heartbeat task started on core 0, pin 4 +[AR/MB] modbus_slave_init: slave_id=1 inputs=17 discretes=19 holdings=5 coils=4 +[AR/MB] modbus_slave_start: listening on UART2 @ 38400 baud (RX=22 TX=21 DE=23) +[AR/N2K] nmea2000_consumer_init: stack open, CAN TX=3 RX=1, subscribed to PGN 127250 + 127251 +[AR/N2K] nmea2000_consumer task started on core 0 +[AR/MAIN] setup() complete; control loop is FreeRTOS-driven. +[AR/MAIN] current mode: STANDBY (helm is manual) +``` + +The DO5 LED ("pilot engaged" lamp on the production wiring; co-opted as heartbeat in Sprint 1) should blink at exactly 1 Hz. If it doesn't, the heartbeat task didn't start — check the serial log. + +--- + +## 4. Sprint 1 capability matrix + +| Capability | State | +|---|---| +| Boot in STANDBY mode | ✅ | +| 1 Hz heartbeat LED + uptime log | ✅ | +| 100 Hz rudder sensor read with 5-sample median filter | ✅ | +| 50 Hz safety monitor (DI poll, debounce, watchdog feed) | ✅ | +| Physical disengage button (DI1) forces STANDBY + cuts actuator power | ✅ | +| External alarm (DI4) forces STANDBY | ✅ | +| Both-limit-switch interlock cuts actuator power | ✅ | +| Task Watchdog Timer at 2 s (auto-reset if any monitored task hangs) | ✅ | +| Modbus RTU slave on UART2 (38400 8N1, slave ID 1) | ✅ | +| 17 input registers, 19 discrete inputs, 5 holding registers, 4 coils | ✅ | +| NMEA 2000 stack open (TX=GPIO3, RX=GPIO1) | ✅ | +| Consume PGN 127250 (Heading) | ✅ | +| Consume PGN 127251 (Rate of Turn) | ✅ | +| Heading + ROT exposed via Modbus input registers 24-26 | ✅ | +| PID inner loop | ⛔ Sprint 2 | +| PID outer loop + Heading Hold | ⛔ Sprint 3 | +| True Course / Track Keeping | ⛔ Sprint 5 | +| Alarm catalogue + auto-disengage by reason | ⛔ Sprint 6 | +| Knob driver + commissioning wizard | ⛔ Sprint 7 | +| EKF + adaptive tuning | ⛔ Sprint 8 | + +--- + +## 5. Talking to the slave from a PC + +The repo ships a manual Modbus client at +[`firmware/ar_autopilot_v1/tools/modbus_client_test.py`](../firmware/ar_autopilot_v1/tools/modbus_client_test.py). + +```powershell +# Install pymodbus once (not in requirements by default to keep core lean) +.\.venv\Scripts\python.exe -m pip install "pymodbus>=3.6,<4" + +# Run against a USB-RS485 dongle on COM7 +.\.venv\Scripts\python.exe firmware/ar_autopilot_v1/tools/modbus_client_test.py --port COM7 +``` + +It reads firmware version, mode, rudder angle, NMEA-2000 heading/ROT, writes a setpoint, and pulses the disengage coil. Output looks like: + +``` +[connect] COM7 @ 38400 8N1, slave=1 +[fw ] version v0.1.0 schema=0 uptime=42s +[mode ] STANDBY (0) +[rudd ] angle=+0.34 deg raw_adc=2052 valid=yes +[n2k ] heading= 0.00 deg rot=+0.00 deg/s age=60000 ms +[hold ] wrote heading setpoint 180.00 deg, read back +180.00 deg +[coil ] disengage pulsed; PILOT_ENGAGED = no (correct -- Sprint 1 is always STANDBY) +``` + +The `age=60000 ms` reading is normal when no real NMEA 2000 traffic is present — the firmware clamps the displayed age at 60 s. + +--- + +## 6. Modbus register map + +Single source of truth: [`firmware/ar_autopilot_v1/modbus_registers.yaml`](../firmware/ar_autopilot_v1/modbus_registers.yaml). + +Generates: + +- `firmware/ar_autopilot_v1/src/protocols/modbus_registers.h` (C++) +- `arautopilot/shared/modbus_register_map.py` (Python) + +Regenerate after editing the YAML: + +```powershell +.\.venv\Scripts\python.exe tools/gen_modbus_registers.py +``` + +The pytest suite contains a drift-detection test (`test_generated_files_are_in_sync_with_yaml`) that will fail if you forget to regenerate. + +--- + +## 7. NMEA 2000 wiring + +Default CAN pins on the AR-NMEA-IO v1.0: + +- TX = `GPIO3` +- RX = `GPIO1` + +These are overridden by `#define ESP32_CAN_TX_PIN` / `RX_PIN` in +`firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp` before +including ``. If a future board revision moves the CAN +transceiver, update `pinout.h` and rebuild — no other change is needed. + +PGNs consumed in Sprint 1: + +| PGN | Name | Where it's used | +|---|---|---| +| 127250 | Vessel Heading | input register `HEADING_DEG_X100` (16-bit, scale 0.01 deg) | +| 127251 | Rate of Turn | input register `ROT_DPS_X100` (signed int16, scale 0.01 deg/s) | + +The same task also keeps two age counters (`heading_age_ms`, `rot_age_ms`) +that drive the `heading_valid` / `rot_valid` flags. They become `false` after +5 s without an update — used in Sprint 6 to fire `ALARM_HEADING_LOST`. + +--- + +## 8. Tests + +| Test suite | Where | How to run | Status | +|---|---|---|---| +| Python core + library | `arautopilot/tests/` | `bash scripts/dev.sh test` or `pytest` | 110 / 110 green | +| Firmware host-side Unity (median filter, future PID math) | `firmware/ar_autopilot_v1/test/` | `pio test -e native` (needs host g++/clang/MSVC) | requires host C++ compiler | +| Firmware on-target (when board is wired) | same | `pio test -e esp32-dev` | requires board | + +The host-side Unity tests are intentionally written so they can run on +any developer machine with a working C++ toolchain — they have no Arduino +dependency. On this Windows host PlatformIO complained that `gcc` was not +in PATH; install [MinGW-w64](https://www.mingw-w64.org/), MSVC Build Tools, +or run the tests on Linux/WSL and they will execute against the same +sources. + +--- + +## 9. Troubleshooting + +| Symptom | Likely cause | Fix | +|---|---|---| +| Boot loop with `Guru Meditation` | Watchdog tripped because a custom task didn't feed it within 2 s | Look for `[AR/SAFE] watchdog_subscribe_current_task()` in the task and call `watchdog_feed()` once per iteration | +| `[AR/N2K] NMEA2000.Open() failed` | CAN transceiver not powered or pins wrong | Confirm `PIN_CAN_TX`/`PIN_CAN_RX` in `pinout.h` match your board; check that the MCP2562/TJA1051 has 5 V supply | +| Modbus client times out | Wrong baudrate, wrong slave ID, DE pin not toggling | Check `[AR/MB] modbus_slave_start` line in the serial log; confirm RS-485 wiring A/B not swapped; install a 120 Ω termination at each end of the bus | +| `[AR/HAL] do_write: unknown pin N` | Code is trying to write a pin not declared in the `g_do_slots` table | Add the pin to `firmware/ar_autopilot_v1/src/hal/di_do.cpp` | +| `min_useful_pwm_pct must be >= deadband_pct` from Studio | Operator profile has a calibration inconsistency | Edit the ProjectConfig to make `min_useful_pwm_pct >= deadband_pct` | diff --git a/firmware/ar_autopilot_v1/modbus_registers.yaml b/firmware/ar_autopilot_v1/modbus_registers.yaml new file mode 100644 index 0000000..5d58845 --- /dev/null +++ b/firmware/ar_autopilot_v1/modbus_registers.yaml @@ -0,0 +1,106 @@ +# ============================================================================= +# AR-Autopilot -- Modbus RTU register map (single source of truth) +# ============================================================================= +# +# This file is read by tools/gen_modbus_registers.py to produce: +# +# firmware/ar_autopilot_v1/src/protocols/modbus_registers.h +# -> compile-time constants used by the firmware +# +# arautopilot/shared/modbus_register_map.py +# -> runtime constants used by the Studio simulator, the dedicated +# display (via the Flutter side mirror), and the Python tools +# +# Do NOT edit the generated files by hand. Always edit this YAML and run: +# +# python tools/gen_modbus_registers.py +# +# Register types (Modbus RTU standard): +# discrete -- read-only single bit (function code 0x02) addr 10001+ +# coil -- read-write single bit (function codes 0x01/0x05/0x0F) addr 1+ +# input -- read-only 16-bit register (function code 0x04) addr 30001+ +# holding -- read-write 16-bit register (function codes 0x03/0x06/0x10) addr 40001+ +# +# Addresses below are zero-based offsets within their type. Modbus over the +# wire adds the type base (10001 / 1 / 30001 / 40001). +# +# Scale factor: physical value = raw * scale + offset +# e.g. heading_deg with scale=0.01 means raw 4500 -> 45.00 deg. +# ============================================================================= + +schema_version: "0.1.0" +slave_id: 1 # Modbus slave (server) address +baudrate: 38400 # bps; 8N1 framing assumed +parity: "N" +data_bits: 8 +stop_bits: 1 + +# ----------------------------------------------------------------------------- +# Discrete inputs (read-only bits) -- status flags published by the firmware +# ----------------------------------------------------------------------------- +discretes: + - { addr: 0, name: PILOT_ENGAGED, desc: "1 if the pilot is currently engaged (any mode other than STANDBY)" } + - { addr: 1, name: DI_DISENGAGE_BUTTON, desc: "Live state of the physical Engage/Disengage push-button" } + - { addr: 2, name: DI_LIMIT_PORT, desc: "Port-side rudder mechanical end-stop reached" } + - { addr: 3, name: DI_LIMIT_STBD, desc: "Starboard-side rudder mechanical end-stop reached" } + - { addr: 4, name: DI_EXTERNAL_ALARM, desc: "External critical alarm (VMS / genset) asserted" } + - { addr: 5, name: DI_MANUAL_CONFIRM, desc: "Manual confirmation switch asserted (emergency override)" } + - { addr: 8, name: ACTUATOR_POWER, desc: "Master actuator power relay (DO3) commanded ON" } + - { addr: 9, name: ACTUATOR_DRIVING_PORT, desc: "Actuator direction output: driving to port" } + - { addr: 10, name: ACTUATOR_DRIVING_STBD, desc: "Actuator direction output: driving to starboard" } + - { addr: 16, name: ALARM_OFF_COURSE, desc: "Off-course warning active" } + - { addr: 17, name: ALARM_OFF_COURSE_SEVERE, desc: "Severe off-course alarm (auto-disengage)" } + - { addr: 18, name: ALARM_RUDDER_NOT_RESP, desc: "Rudder command sent but no feedback motion" } + - { addr: 19, name: ALARM_HEADING_LOST, desc: "NMEA 2000 heading PGN not received >5 s" } + - { addr: 20, name: ALARM_ACTUATOR_OVERCURR, desc: "Actuator current over threshold" } + - { addr: 21, name: ALARM_VOLTAGE_LOW, desc: "Supply voltage below safe threshold" } + - { addr: 22, name: ALARM_LIMIT_REACHED, desc: "Rudder reached mechanical end-stop" } + - { addr: 23, name: ALARM_WATCHDOG_TRIPPED, desc: "Firmware watchdog fired -- controller reset" } + - { addr: 24, name: ALARM_VMS_CRITICAL, desc: "VMS reported blackout or electrical fault" } + - { addr: 25, name: ANY_ALARM_ACTIVE, desc: "OR of all alarm bits (convenience)" } + +# ----------------------------------------------------------------------------- +# Coils (read-write bits) -- commands written by the display / Studio +# ----------------------------------------------------------------------------- +coils: + - { addr: 0, name: CMD_ENGAGE_REQUEST, desc: "Rising edge requests pilot engagement (subject to interlocks)" } + - { addr: 1, name: CMD_DISENGAGE_REQUEST, desc: "Rising edge forces pilot to STANDBY" } + - { addr: 2, name: CMD_ACK_ALL_ALARMS, desc: "Rising edge acknowledges every active alarm" } + - { addr: 3, name: CMD_KNOB_ARM, desc: "Knob arming request (Sprint 7+)" } + +# ----------------------------------------------------------------------------- +# Input registers (read-only 16-bit words) -- telemetry +# ----------------------------------------------------------------------------- +# Two-register values (e.g. timestamps, floats) occupy consecutive addresses. +# ----------------------------------------------------------------------------- +inputs: + - { addr: 0, name: FW_VERSION_MAJOR, desc: "Firmware major version", unit: "" } + - { addr: 1, name: FW_VERSION_MINOR, desc: "Firmware minor version", unit: "" } + - { addr: 2, name: FW_VERSION_PATCH, desc: "Firmware patch version", unit: "" } + - { addr: 3, name: SCHEMA_VERSION, desc: "Modbus map schema version (0=v0.1.0)", unit: "" } + - { addr: 4, name: UPTIME_SECONDS_LO, desc: "Uptime seconds, low 16 bits", unit: "s" } + - { addr: 5, name: UPTIME_SECONDS_HI, desc: "Uptime seconds, high 16 bits", unit: "s" } + - { addr: 6, name: CURRENT_MODE, desc: "Current AutopilotMode (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)", unit: "" } + - { addr: 7, name: FREE_HEAP_KB, desc: "Current free heap, KiB", unit: "KiB" } + - { addr: 8, name: MIN_FREE_HEAP_KB, desc: "Minimum free heap since boot", unit: "KiB" } + + - { addr: 16, name: RUDDER_ANGLE_DEG_X100, desc: "Filtered rudder angle, deg * 100 (-3500..+3500)", unit: "deg", scale: 0.01 } + - { addr: 17, name: RUDDER_RAW_ADC, desc: "Raw ADC reading after median filter (0..4095)", unit: "counts" } + - { addr: 18, name: RUDDER_VALID, desc: "1 if median filter has filled (>=5 samples)", unit: "" } + + - { addr: 24, name: HEADING_DEG_X100, desc: "Current heading from NMEA 2000 PGN 127250, deg*100 (0..35999)", unit: "deg", scale: 0.01 } + - { addr: 25, name: ROT_DPS_X100, desc: "Rate of turn from PGN 127251, deg/s*100 (signed int16)", unit: "deg/s", scale: 0.01 } + - { addr: 26, name: HEADING_AGE_MS, desc: "Milliseconds since the last heading update (0..60000)", unit: "ms" } + + - { addr: 32, name: BATTERY_VOLTAGE_X100, desc: "System battery voltage, V*100", unit: "V", scale: 0.01 } + - { addr: 33, name: ACTUATOR_CURRENT_X100, desc: "Actuator current, A*100", unit: "A", scale: 0.01 } + +# ----------------------------------------------------------------------------- +# Holding registers (read-write 16-bit words) -- setpoints and config +# ----------------------------------------------------------------------------- +holdings: + - { addr: 0, name: MODE_REQUEST, desc: "Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)", unit: "" } + - { addr: 1, name: HEADING_SETPOINT_X100, desc: "Desired heading, deg*100", unit: "deg", scale: 0.01 } + - { addr: 2, name: BRIGHTNESS_PCT, desc: "Display brightness 0..100", unit: "%" } + - { addr: 3, name: ALARM_VOLUME_PCT, desc: "Alarm volume 0..100", unit: "%" } + - { addr: 8, name: DODGE_OFFSET_DEG_X100, desc: "Dodge mode heading offset, deg*100 (signed int16)", unit: "deg", scale: 0.01 } diff --git a/firmware/ar_autopilot_v1/platformio.ini b/firmware/ar_autopilot_v1/platformio.ini new file mode 100644 index 0000000..bd673a4 --- /dev/null +++ b/firmware/ar_autopilot_v1/platformio.ini @@ -0,0 +1,123 @@ +; ============================================================================= +; AR-Autopilot v1 -- firmware build configuration +; ============================================================================= +; +; Target hardware: AR-NMEA-IO v1.0 (ESP32-DOWD-V3, dual-core 240 MHz) +; Build system: PlatformIO 6.1+ +; Framework: arduino (on ESP32 -- includes FreeRTOS, full Espressif stack) +; +; This file is the single source of truth for the firmware build. Versions of +; the platform, framework packages, and library dependencies are pinned for +; reproducibility. +; +; Tasks (from the project root): +; +; .venv/Scripts/pio.exe run # compile (default env: esp32-dev) +; .venv/Scripts/pio.exe run -e esp32-debug # compile with extra debug flags +; .venv/Scripts/pio.exe run -e native # build host-side Unity tests +; .venv/Scripts/pio.exe test -e native # run host-side Unity tests +; .venv/Scripts/pio.exe check # static analysis (cppcheck) +; .venv/Scripts/pio.exe run -t upload # flash (needs the board plugged in) +; +; ============================================================================= + +[platformio] +src_dir = src +include_dir = include +test_dir = test +default_envs = esp32-dev + +; ----------------------------------------------------------------------------- +; Common settings shared by every ESP32 env +; ----------------------------------------------------------------------------- +[env] +platform = espressif32@^6.7.0 +framework = arduino +monitor_speed = 115200 +monitor_filters = esp32_exception_decoder, time +build_flags = + -std=gnu++17 + -DCORE_DEBUG_LEVEL=3 ; ARDUHAL_LOG_LEVEL_INFO + -DAR_FW_VERSION=\"0.1.0-sprint1\" + -DAR_FW_BUILD_TIMESTAMP=__TIMESTAMP__ + -Wall + -Wextra + -Wno-unused-parameter + -Wno-missing-field-initializers +build_unflags = + -std=gnu++11 + +; Library dependencies pinned for reproducibility. +; These are the Sprint 1 dependencies; later sprints may add more. +lib_deps = + ttlappalainen/NMEA2000-library@^4.22.0 + ttlappalainen/NMEA2000_esp32@^1.0.3 + https://github.com/eModbus/eModbus.git#v1.7.4.stable + +; Files we never want to compile / lint: +; - Documentation +; - Test fixtures (handled by env:native) +lib_ignore = + test + +; ----------------------------------------------------------------------------- +; Production build for the AR-NMEA-IO board +; ----------------------------------------------------------------------------- +[env:esp32-dev] +board = esp32dev +board_build.partitions = default.csv +build_type = release +build_flags = + ${env.build_flags} + -Os + -DAR_BUILD_VARIANT=\"release\" + +; ----------------------------------------------------------------------------- +; Debug build (-O0, extra logging) +; ----------------------------------------------------------------------------- +[env:esp32-debug] +board = esp32dev +build_type = debug +build_flags = + ${env.build_flags} + -O0 + -ggdb + -DAR_BUILD_VARIANT=\"debug\" + -DCORE_DEBUG_LEVEL=5 ; ARDUHAL_LOG_LEVEL_VERBOSE + +; ----------------------------------------------------------------------------- +; Host-side Unity tests (run on the developer machine, no ESP32 needed) +; ----------------------------------------------------------------------------- +; Used for pure-logic tests: ring buffers, median filters, the Modbus +; register map, the PID math, etc. Anything that talks to real hardware +; lives in env:esp32-dev tests and runs only on the board. +; +; NOTE: this env does NOT inherit [env] (which targets ESP32 + Arduino). +[env:native] +platform = native +framework = +test_framework = unity +test_build_src = no +build_flags = + -std=gnu++17 + -DAR_HOST_TEST=1 + -DUNITY_INCLUDE_DOUBLE + -Wall + -Wextra +build_unflags = +lib_deps = +lib_ignore = +monitor_filters = + +; ----------------------------------------------------------------------------- +; Static analysis +; ----------------------------------------------------------------------------- +[env:check] +platform = espressif32@^6.7.0 +framework = arduino +board = esp32dev +check_tool = cppcheck +check_flags = + cppcheck: --enable=warning,style,performance,portability --inline-suppr +check_skip_packages = yes +check_severity = medium, high diff --git a/firmware/ar_autopilot_v1/src/filters/median.h b/firmware/ar_autopilot_v1/src/filters/median.h new file mode 100644 index 0000000..d406751 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/filters/median.h @@ -0,0 +1,76 @@ +// ============================================================================= +// median.h -- fixed-size median filter +// ============================================================================= +// +// Templated median filter that holds the last N samples in a ring buffer and +// returns the median of the current window in O(N log N) (sort-copy). N is +// expected to be small (3-9), so the cost is negligible at 100 Hz. +// +// Used by: +// - hal/rudder_sensor.cpp (5-sample median to suppress ADC spikes) +// +// Host-testable: depends only on the standard library. The Unity tests in +// test/test_filters/ exercise it on the developer machine. +// ============================================================================= + +#pragma once + +#include +#include +#include + +namespace arautopilot::filters { + +template +class MedianFilter { + static_assert(N >= 1, "MedianFilter window must be at least 1"); + static_assert(N % 2 == 1, "MedianFilter window must be odd for an unambiguous median"); + + public: + MedianFilter() = default; + + /// Push one sample. Until N samples have been pushed the filter + /// returns the median over only the samples received so far. + T push(T sample) { + buffer_[write_idx_] = sample; + write_idx_ = (write_idx_ + 1) % N; + if (count_ < N) { + ++count_; + } + return median(); + } + + /// Median of the current window (without pushing). Returns T{} if + /// no samples have been pushed yet. + T median() const { + if (count_ == 0) { + return T{}; + } + std::array sorted{}; + for (std::size_t i = 0; i < count_; ++i) { + sorted[i] = buffer_[i]; + } + std::sort(sorted.begin(), sorted.begin() + count_); + return sorted[count_ / 2]; + } + + /// Number of valid samples currently stored (0..N). + std::size_t size() const { return count_; } + + /// True once the window has been filled at least once. + bool is_full() const { return count_ == N; } + + /// Forget all samples; next push() starts fresh. + void reset() { + write_idx_ = 0; + count_ = 0; + buffer_.fill(T{}); + } + + private: + std::array buffer_{}; + std::size_t write_idx_{0}; + std::size_t count_{0}; +}; + +} // namespace arautopilot::filters diff --git a/firmware/ar_autopilot_v1/src/hal/di_do.cpp b/firmware/ar_autopilot_v1/src/hal/di_do.cpp new file mode 100644 index 0000000..3440c77 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/hal/di_do.cpp @@ -0,0 +1,158 @@ +// ============================================================================= +// di_do.cpp -- generic DI/DO implementation +// ============================================================================= + +#include "di_do.h" + +#include + +#include + +#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 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 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 diff --git a/firmware/ar_autopilot_v1/src/hal/di_do.h b/firmware/ar_autopilot_v1/src/hal/di_do.h new file mode 100644 index 0000000..4157d80 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/hal/di_do.h @@ -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 + +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 diff --git a/firmware/ar_autopilot_v1/src/hal/rudder_actuator.cpp b/firmware/ar_autopilot_v1/src/hal/rudder_actuator.cpp new file mode 100644 index 0000000..435bd18 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/hal/rudder_actuator.cpp @@ -0,0 +1,115 @@ +// ============================================================================= +// rudder_actuator.cpp -- rudder actuator driver (Sprint 1 stub) +// ============================================================================= + +#include "rudder_actuator.h" + +#include + +#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 diff --git a/firmware/ar_autopilot_v1/src/hal/rudder_actuator.h b/firmware/ar_autopilot_v1/src/hal/rudder_actuator.h new file mode 100644 index 0000000..7acba55 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/hal/rudder_actuator.h @@ -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 + +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 diff --git a/firmware/ar_autopilot_v1/src/hal/rudder_sensor.cpp b/firmware/ar_autopilot_v1/src/hal/rudder_sensor.cpp new file mode 100644 index 0000000..c7414a6 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/hal/rudder_sensor.cpp @@ -0,0 +1,107 @@ +// ============================================================================= +// rudder_sensor.cpp -- AI1 rudder-angle sensor with median filter +// ============================================================================= + +#include "rudder_sensor.h" + +#include + +#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 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(c.raw_at_max - c.raw_at_min); + if (span_raw == 0.0f) { + return 0.0f; + } + const float t = (static_cast(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(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 diff --git a/firmware/ar_autopilot_v1/src/hal/rudder_sensor.h b/firmware/ar_autopilot_v1/src/hal/rudder_sensor.h new file mode 100644 index 0000000..9a38a43 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/hal/rudder_sensor.h @@ -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 + +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 diff --git a/firmware/ar_autopilot_v1/src/main.cpp b/firmware/ar_autopilot_v1/src/main.cpp new file mode 100644 index 0000000..28260ef --- /dev/null +++ b/firmware/ar_autopilot_v1/src/main.cpp @@ -0,0 +1,98 @@ +// ============================================================================= +// AR-Autopilot v1 -- main.cpp +// ============================================================================= +// +// Sprint 1 firmware entry point. Boots in STANDBY mode, spawns the FreeRTOS +// tasks declared in system/task_config.h, and returns control to the Arduino +// scheduler. The loop() function intentionally does nothing -- every long- +// running activity lives in a pinned task. +// +// Task layout summary (full table in system/task_config.h): +// +// Core 1 (real-time): PID inner, PID outer, rudder sensor, safety +// Core 0 (comms): NMEA 2000 RX, Modbus slave, heartbeat, health +// +// Sprint 1 boots only: +// - heartbeat (Core 0) -- LED + uptime log +// +// Later sprints will register the rest from this same setup() function. The +// task_config.h header already reserves their priorities/stacks/cores. +// ============================================================================= + +#include + +#include "hal/di_do.h" +#include "hal/pinout.h" +#include "hal/rudder_actuator.h" +#include "hal/rudder_sensor.h" +#include "modes/standby.h" +#include "protocols/modbus_slave.h" +#include "protocols/nmea2000_consumer.h" +#include "safety/safety_monitor.h" +#include "safety/watchdog.h" +#include "system/ar_log.h" +#include "system/task_config.h" + +// Forward declarations of task-spawning helpers (defined in their own .cpp +// files). Keeping main.cpp dependency-free of internals lets each subsystem +// own its own task setup code. +extern void ar_start_heartbeat_task(); + +namespace { +constexpr const char* TAG = "AR/MAIN"; +} // namespace + +void setup() { + // Wait for the USB CDC serial port to be ready before we start logging. + Serial.begin(115200); + delay(200); + + AR_LOGI(TAG, "================================================"); + AR_LOGI(TAG, " %s -- firmware boot", AR_AUTOPILOT_FW_NAME); + AR_LOGI(TAG, " version : %s", AR_FW_VERSION); + AR_LOGI(TAG, " variant : %s", AR_BUILD_VARIANT); + AR_LOGI(TAG, " build : %s", AR_FW_BUILD_TIMESTAMP); + AR_LOGI(TAG, " board : %s", AR_AUTOPILOT_HW_BOARD); + AR_LOGI(TAG, " mcu : %s @ %d MHz", AR_AUTOPILOT_MCU, + (int)ESP.getCpuFreqMHz()); + AR_LOGI(TAG, " cores : %d free heap: %u bytes", + (int)ESP.getChipCores(), ESP.getFreeHeap()); + AR_LOGI(TAG, "================================================"); + + // Initialise the pilot mode state machine (boots in STANDBY). + arautopilot::modes::mode_init(); + + // Initialise hardware abstraction layer. + arautopilot::hal::di_init(); + arautopilot::hal::do_init(); + arautopilot::hal::rudder_sensor_init(); + arautopilot::hal::rudder_actuator_init(); + + // Safety: watchdog + DI override task. Initialised first so the + // watchdog is armed before slower subsystems come up. + arautopilot::safety::watchdog_init(); + + AR_LOGI(TAG, "spawning Sprint 1 tasks ..."); + + arautopilot::safety::safety_monitor_start_task(); + arautopilot::hal::rudder_sensor_start_task(); + ar_start_heartbeat_task(); + + // Modbus slave (server) -- exposes telemetry + commands to the display. + arautopilot::protocols::modbus::modbus_slave_init(); + arautopilot::protocols::modbus::modbus_slave_start(); + + // NMEA 2000 consumer (PGN 127250 + 127251 in Sprint 1). + arautopilot::protocols::nmea2000::nmea2000_consumer_init(); + arautopilot::protocols::nmea2000::nmea2000_consumer_start_task(); + + AR_LOGI(TAG, "setup() complete; control loop is FreeRTOS-driven."); + AR_LOGI(TAG, "current mode: STANDBY (helm is manual)"); +} + +void loop() { + // Intentionally empty. All real work happens in FreeRTOS tasks. + // The Arduino loopTask itself runs at priority 1 on Core 1 by default; + // we yield generously to keep it out of the way of the real-time tasks. + vTaskDelay(pdMS_TO_TICKS(1000)); +} diff --git a/firmware/ar_autopilot_v1/src/modes/standby.cpp b/firmware/ar_autopilot_v1/src/modes/standby.cpp new file mode 100644 index 0000000..7301511 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/modes/standby.cpp @@ -0,0 +1,80 @@ +// ============================================================================= +// modes/standby.cpp -- pilot mode state (Sprint 1) +// ============================================================================= + +#include "standby.h" + +#include + +#include "../system/ar_log.h" + +namespace arautopilot::modes { + +namespace { +constexpr const char* TAG = "AR/MODE"; + +portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED; +Mode g_mode = Mode::STANDBY; + +const char* mode_name(Mode m) { + switch (m) { + case Mode::STANDBY: return "STANDBY"; + case Mode::HEADING_HOLD: return "HEADING_HOLD"; + case Mode::TRUE_COURSE: return "TRUE_COURSE"; + case Mode::TRACK_KEEPING: return "TRACK_KEEPING"; + case Mode::DODGE: return "DODGE"; + } + return "UNKNOWN"; +} +} // namespace + +void mode_init() { + portENTER_CRITICAL(&g_mux); + g_mode = Mode::STANDBY; + portEXIT_CRITICAL(&g_mux); + AR_LOGI(TAG, "mode_init: %s", mode_name(g_mode)); +} + +Mode current_mode() { + portENTER_CRITICAL(&g_mux); + Mode m = g_mode; + portEXIT_CRITICAL(&g_mux); + return m; +} + +bool is_standby() { return current_mode() == Mode::STANDBY; } + +bool request_mode(Mode m) { + // Sprint 1: only STANDBY is reachable. The other modes exist in the enum + // for forward compatibility (Modbus already has slots for them) but the + // PID and modes machinery is not in place yet. + if (m == Mode::STANDBY) { + portENTER_CRITICAL(&g_mux); + Mode prev = g_mode; + g_mode = Mode::STANDBY; + portEXIT_CRITICAL(&g_mux); + if (prev != Mode::STANDBY) { + AR_LOGI(TAG, "mode change: %s -> STANDBY", mode_name(prev)); + } + return true; + } + + AR_LOGW(TAG, + "request_mode(%s) rejected: only STANDBY is implemented in " + "Sprint 1; current mode remains %s", + mode_name(m), mode_name(current_mode())); + return false; +} + +void force_standby(const char* reason) { + portENTER_CRITICAL(&g_mux); + Mode prev = g_mode; + g_mode = Mode::STANDBY; + portEXIT_CRITICAL(&g_mux); + if (prev != Mode::STANDBY) { + AR_LOGE(TAG, "force_standby: %s -> STANDBY (reason: %s)", + mode_name(prev), reason != nullptr ? reason : "unspecified"); + } +} + +} // namespace arautopilot::modes diff --git a/firmware/ar_autopilot_v1/src/modes/standby.h b/firmware/ar_autopilot_v1/src/modes/standby.h new file mode 100644 index 0000000..ce02349 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/modes/standby.h @@ -0,0 +1,46 @@ +// ============================================================================= +// modes/standby.h -- pilot mode state (Sprint 1) +// ============================================================================= +// +// Holds the global "what mode is the pilot in" flag. In Sprint 1 the only +// mode that does anything real is STANDBY. The other Phase 1 modes +// (HEADING_HOLD, TRUE_COURSE, TRACK_KEEPING, DODGE) are reserved in the +// enum but cannot be entered yet -- attempting to set them logs a warning +// and stays in STANDBY. +// +// Later sprints expand this module into a proper state machine driven by +// the safety system and the operator's Modbus commands. +// ============================================================================= + +#pragma once + +#include + +namespace arautopilot::modes { + +enum class Mode : uint8_t { + STANDBY = 0, + HEADING_HOLD = 1, + TRUE_COURSE = 2, + TRACK_KEEPING = 3, + DODGE = 4, +}; + +/// Initialise to STANDBY. Must be called once from setup(). +void mode_init(); + +/// Atomically read the current mode. +Mode current_mode(); + +/// True if and only if current_mode() == STANDBY. +bool is_standby(); + +/// Try to enter `m`. In Sprint 1 only STANDBY is accepted; anything else +/// is rejected with a warning. Returns true on success. +bool request_mode(Mode m); + +/// Force-return to STANDBY (called by the safety system on auto-disengage, +/// watchdog reset, or the DI1 physical button). +void force_standby(const char* reason); + +} // namespace arautopilot::modes diff --git a/firmware/ar_autopilot_v1/src/protocols/modbus_registers.h b/firmware/ar_autopilot_v1/src/protocols/modbus_registers.h new file mode 100644 index 0000000..cc34fc2 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/protocols/modbus_registers.h @@ -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 + +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 diff --git a/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp b/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp new file mode 100644 index 0000000..971591b --- /dev/null +++ b/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp @@ -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 + +#include + +#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 diff --git a/firmware/ar_autopilot_v1/src/protocols/modbus_slave.h b/firmware/ar_autopilot_v1/src/protocols/modbus_slave.h new file mode 100644 index 0000000..9b67f93 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/protocols/modbus_slave.h @@ -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 + +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 diff --git a/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp b/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp new file mode 100644 index 0000000..f87a534 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp @@ -0,0 +1,178 @@ +// ============================================================================= +// nmea2000_consumer.cpp -- NMEA 2000 backbone consumer +// ============================================================================= + +#include "nmea2000_consumer.h" + +#include + +// 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 +#include + +#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 diff --git a/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h b/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h new file mode 100644 index 0000000..57ba088 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h @@ -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 + +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 diff --git a/firmware/ar_autopilot_v1/src/safety/safety_monitor.cpp b/firmware/ar_autopilot_v1/src/safety/safety_monitor.cpp new file mode 100644 index 0000000..71d9fd7 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/safety/safety_monitor.cpp @@ -0,0 +1,77 @@ +// ============================================================================= +// safety/safety_monitor.cpp -- 50 Hz safety task +// ============================================================================= + +#include "safety_monitor.h" + +#include + +#include "../hal/di_do.h" +#include "../hal/pinout.h" +#include "../hal/rudder_actuator.h" +#include "../modes/standby.h" +#include "../system/ar_log.h" +#include "../system/task_config.h" +#include "watchdog.h" + +namespace arautopilot::safety { + +namespace { +constexpr const char* TAG = "AR/SAFE"; + +void SafetyTask(void* /*pv*/) { + AR_LOGI(TAG, "safety_monitor task started on core %d (50 Hz)", + xPortGetCoreID()); + + // Subscribe to the watchdog. Every loop iteration feeds it; if this + // task ever stops looping the chip resets to STANDBY on boot. + watchdog_subscribe_current_task(); + + TickType_t last_wake = xTaskGetTickCount(); + for (;;) { + hal::di_poll(); + + // ---- DI1: Engage/Disengage physical button ----------------------- + // The brief specifies this button "ALWAYS DISENGAGES" -- pressing + // it must put the system in STANDBY no matter what mode we're in. + // We trigger on the rising edge so a held button doesn't keep + // spamming the log. + if (hal::di_rising_edge(PIN_DI1_DISENGAGE_BUTTON)) { + modes::force_standby("DI1 physical button"); + hal::rudder_actuator_power_off(); + } + + // ---- DI4: External critical alarm (VMS blackout / genset) -------- + if (hal::di_rising_edge(PIN_DI4_EXTERNAL_ALARM)) { + modes::force_standby("DI4 external alarm"); + hal::rudder_actuator_power_off(); + } + + // ---- Limit switches: cut power if rudder hit a mechanical stop -- + // Even though rudder_command() refuses to drive into a limit, a + // hardware failure could still command the wrong direction. As an + // extra interlock, if BOTH limits assert at once we cut master + // power -- something is seriously wrong with the feedback wiring. + if (hal::di_read(PIN_DI2_LIMIT_SWITCH_PORT) && + hal::di_read(PIN_DI3_LIMIT_SWITCH_STBD)) { + AR_LOGE(TAG, + "both limit switches asserted simultaneously -- cutting " + "actuator power (wiring fault?)"); + hal::rudder_actuator_power_off(); + } + + watchdog_feed(); + vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_SAFETY)); + } +} + +} // namespace + +void safety_monitor_start_task() { + xTaskCreatePinnedToCore(SafetyTask, "safety_monitor", + AR_TASK_STACK_SAFETY, nullptr, + AR_TASK_PRIO_SAFETY, nullptr, + AR_TASK_CORE_REALTIME); +} + +} // namespace arautopilot::safety diff --git a/firmware/ar_autopilot_v1/src/safety/safety_monitor.h b/firmware/ar_autopilot_v1/src/safety/safety_monitor.h new file mode 100644 index 0000000..a57f16c --- /dev/null +++ b/firmware/ar_autopilot_v1/src/safety/safety_monitor.h @@ -0,0 +1,26 @@ +// ============================================================================= +// safety/safety_monitor.h -- safety monitor task +// ============================================================================= +// +// 50 Hz task on Core 1 that: +// - Polls every DI through hal::di_poll(). +// - Reacts to PIN_DI1_DISENGAGE_BUTTON rising edge: forces STANDBY, +// kills actuator power. +// - Reacts to limit switches: cuts actuator power if the rudder is +// trying to drive into a limit. +// - Reacts to PIN_DI4_EXTERNAL_ALARM (VMS critical): forces STANDBY. +// - Subscribes itself to the TWDT and feeds it every loop iteration. +// +// Sprint 1: limited to the above. Sprint 6 expands this with the full +// alarm catalogue (off-course, rudder not responding, etc.). +// ============================================================================= + +#pragma once + +namespace arautopilot::safety { + +/// Spawn the safety monitor task. Must be called AFTER di_init() and +/// rudder_actuator_init(). +void safety_monitor_start_task(); + +} // namespace arautopilot::safety diff --git a/firmware/ar_autopilot_v1/src/safety/watchdog.cpp b/firmware/ar_autopilot_v1/src/safety/watchdog.cpp new file mode 100644 index 0000000..9bf32d2 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/safety/watchdog.cpp @@ -0,0 +1,40 @@ +// ============================================================================= +// safety/watchdog.cpp -- TWDT setup + helpers +// ============================================================================= + +#include "watchdog.h" + +#include +#include + +#include "../system/ar_log.h" +#include "../system/task_config.h" + +namespace arautopilot::safety { + +namespace { +constexpr const char* TAG = "AR/SAFE"; +} // namespace + +void watchdog_init() { + // ESP-IDF 5.x: esp_task_wdt_init takes a config struct, but + // esp32 Arduino core wraps it with a simpler signature. + esp_err_t err = esp_task_wdt_init(AR_WATCHDOG_TIMEOUT_S, true); + if (err != ESP_OK) { + AR_LOGE(TAG, "esp_task_wdt_init failed: %d", (int)err); + return; + } + AR_LOGI(TAG, "watchdog_init: TWDT timeout %ds, panic on expire", + AR_WATCHDOG_TIMEOUT_S); +} + +void watchdog_subscribe_current_task() { + esp_err_t err = esp_task_wdt_add(nullptr); + if (err != ESP_OK && err != ESP_ERR_INVALID_ARG) { + AR_LOGW(TAG, "esp_task_wdt_add failed: %d", (int)err); + } +} + +void watchdog_feed() { esp_task_wdt_reset(); } + +} // namespace arautopilot::safety diff --git a/firmware/ar_autopilot_v1/src/safety/watchdog.h b/firmware/ar_autopilot_v1/src/safety/watchdog.h new file mode 100644 index 0000000..5ba9b2e --- /dev/null +++ b/firmware/ar_autopilot_v1/src/safety/watchdog.h @@ -0,0 +1,29 @@ +// ============================================================================= +// safety/watchdog.h -- Task Watchdog Timer (TWDT) +// ============================================================================= +// +// Hooks the ESP32 task watchdog so that if the real-time control tasks +// (PID inner, PID outer, safety monitor) ever fail to feed the watchdog +// within AR_WATCHDOG_TIMEOUT_S, the chip resets and reboots cleanly to +// STANDBY mode. +// +// Brief section 7: "El ESP32 reinicia automaticamente si el firmware se +// cuelga >2 segundos. Al reiniciar: Estado del piloto pasa a STANDBY." +// ============================================================================= + +#pragma once + +namespace arautopilot::safety { + +/// Configure and start the Task Watchdog Timer. Tasks that want to be +/// monitored must call watchdog_subscribe_current_task() once and then +/// watchdog_feed() at least once per AR_WATCHDOG_TIMEOUT_S. +void watchdog_init(); + +/// Add the current task to the watchdog's monitored set. +void watchdog_subscribe_current_task(); + +/// Feed the watchdog from the current task. +void watchdog_feed(); + +} // namespace arautopilot::safety diff --git a/firmware/ar_autopilot_v1/src/system/ar_log.h b/firmware/ar_autopilot_v1/src/system/ar_log.h new file mode 100644 index 0000000..1cf1ce5 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/system/ar_log.h @@ -0,0 +1,35 @@ +// ============================================================================= +// ar_log.h -- AR-Autopilot logging facade +// ============================================================================= +// +// Thin wrapper around the ESP32 Arduino log_X / ESP_LOGX macros so that the +// rest of the firmware uses one consistent style. Compile-time level is +// controlled by CORE_DEBUG_LEVEL in platformio.ini. +// +// Levels (matching ARDUHAL_LOG_LEVEL_*): +// 0 NONE +// 1 ERROR +// 2 WARN +// 3 INFO <- default in release +// 4 DEBUG +// 5 VERBOSE <- default in debug +// ============================================================================= + +#pragma once + +#include + +// Tag prefix conventions used across the codebase: +// "AR/MAIN" -- main.cpp boot/shutdown +// "AR/PID" -- PID loops +// "AR/HAL" -- hardware abstraction +// "AR/MB" -- Modbus slave +// "AR/N2K" -- NMEA 2000 consumer +// "AR/SAFE" -- safety / watchdog / disengage +// "AR/SYS" -- health / lifecycle / metrics + +#define AR_LOGE(tag, fmt, ...) log_e("[%s] " fmt, tag, ##__VA_ARGS__) +#define AR_LOGW(tag, fmt, ...) log_w("[%s] " fmt, tag, ##__VA_ARGS__) +#define AR_LOGI(tag, fmt, ...) log_i("[%s] " fmt, tag, ##__VA_ARGS__) +#define AR_LOGD(tag, fmt, ...) log_d("[%s] " fmt, tag, ##__VA_ARGS__) +#define AR_LOGV(tag, fmt, ...) log_v("[%s] " fmt, tag, ##__VA_ARGS__) diff --git a/firmware/ar_autopilot_v1/src/system/heartbeat.cpp b/firmware/ar_autopilot_v1/src/system/heartbeat.cpp new file mode 100644 index 0000000..7631079 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/system/heartbeat.cpp @@ -0,0 +1,50 @@ +// ============================================================================= +// heartbeat.cpp -- LED + uptime heartbeat task +// ============================================================================= +// +// Blinks PIN_DO5_ENGAGED_LAMP at 1 Hz to give a visible "the firmware is +// alive" signal at the bench. In Sprint 1 we co-opt this LED because there +// is no pilot to engage yet; from Sprint 6 onwards the engaged lamp will be +// driven by the actual pilot state and the heartbeat will move to an +// internal counter only. +// +// Also logs uptime + free heap once per second so a terminal observer can +// see the system is healthy. +// ============================================================================= + +#include + +#include "../hal/pinout.h" +#include "ar_log.h" +#include "task_config.h" + +namespace { +constexpr const char* TAG = "AR/SYS"; +} // namespace + +static void HeartbeatTask(void* /*pv*/) { + pinMode(PIN_DO5_ENGAGED_LAMP, OUTPUT); + bool led_on = false; + uint32_t tick = 0; + + AR_LOGI(TAG, "heartbeat task started on core %d, pin %d", xPortGetCoreID(), + PIN_DO5_ENGAGED_LAMP); + + TickType_t last_wake = xTaskGetTickCount(); + for (;;) { + led_on = !led_on; + digitalWrite(PIN_DO5_ENGAGED_LAMP, led_on ? HIGH : LOW); + + AR_LOGD(TAG, "tick=%u uptime=%lus free_heap=%u min_free=%u", tick, + millis() / 1000U, ESP.getFreeHeap(), ESP.getMinFreeHeap()); + + ++tick; + vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_HEARTBEAT)); + } +} + +void ar_start_heartbeat_task() { + xTaskCreatePinnedToCore(HeartbeatTask, "heartbeat", AR_TASK_STACK_HEARTBEAT, + nullptr, AR_TASK_PRIO_HEARTBEAT, nullptr, + AR_TASK_CORE_COMMS); +} diff --git a/firmware/ar_autopilot_v1/src/system/task_config.h b/firmware/ar_autopilot_v1/src/system/task_config.h new file mode 100644 index 0000000..a068f02 --- /dev/null +++ b/firmware/ar_autopilot_v1/src/system/task_config.h @@ -0,0 +1,68 @@ +// ============================================================================= +// task_config.h -- FreeRTOS task layout for AR-Autopilot +// ============================================================================= +// +// Single source of truth for stack sizes, priorities, and core pinning of +// every long-running task in the firmware. Centralising this here makes the +// real-time profile of the system reviewable at a glance. +// +// Pinning policy (brief section 6, rule #11 -- "deterministic latency"): +// +// Core 1 (APP_CPU) -- safety-critical real-time: +// AR_TASK_CORE_REALTIME = 1 +// - PID inner loop (Sprint 2; stub in Sprint 1) +// - PID outer loop (Sprint 3; stub in Sprint 1) +// - Rudder sensor (Sprint 1: 100 Hz median filter) +// - Safety monitor (Sprint 1: limit switches, watchdog feed) +// +// Core 0 (PRO_CPU) -- communications + housekeeping: +// AR_TASK_CORE_COMMS = 0 +// - NMEA 2000 RX (Sprint 1: PGN 127250 / 127251) +// - Modbus slave (Sprint 1: eModbus internals already async) +// - Health reporter (Sprint 1: 1 Hz log) +// +// This split guarantees the real-time control loops cannot be starved or +// jittered by bus traffic, even if NMEA 2000 saturates or the Modbus client +// bursts. +// +// Priorities use the standard ESP32 FreeRTOS scale (0 idle .. 24 max). +// configMAX_PRIORITIES is 25 on default ESP32 Arduino builds. +// ============================================================================= + +#pragma once + +// Core assignment +#define AR_TASK_CORE_REALTIME 1 +#define AR_TASK_CORE_COMMS 0 + +// Priorities (higher = more priority) +#define AR_TASK_PRIO_PID_INNER 24 // hard real-time, 50 Hz +#define AR_TASK_PRIO_RUDDER_SENSOR 23 // 100 Hz ADC, must feed PID fresh data +#define AR_TASK_PRIO_PID_OUTER 22 // 10 Hz heading control +#define AR_TASK_PRIO_SAFETY 21 // limit switches, watchdog feed +#define AR_TASK_PRIO_N2K_RX 15 // CAN polling, event-driven +#define AR_TASK_PRIO_MODBUS 14 // eModbus internal tasks (delegated) +#define AR_TASK_PRIO_HEARTBEAT 10 // LED blink + uptime log +#define AR_TASK_PRIO_HEALTH 5 // periodic stats, low priority + +// Stack sizes (bytes). Tune these once we see real high-water marks via +// uxTaskGetStackHighWaterMark() in the health reporter. +#define AR_TASK_STACK_PID_INNER 4096 +#define AR_TASK_STACK_PID_OUTER 4096 +#define AR_TASK_STACK_RUDDER_SENSOR 3072 +#define AR_TASK_STACK_SAFETY 3072 +#define AR_TASK_STACK_N2K_RX 6144 // NMEA2000 parsing is heap-heavy +#define AR_TASK_STACK_MODBUS 4096 +#define AR_TASK_STACK_HEARTBEAT 2048 +#define AR_TASK_STACK_HEALTH 3072 + +// Loop periods (ms). Convert with pdMS_TO_TICKS() at the use site. +#define AR_PERIOD_MS_PID_INNER 20 // 50 Hz +#define AR_PERIOD_MS_PID_OUTER 100 // 10 Hz +#define AR_PERIOD_MS_RUDDER_SENSOR 10 // 100 Hz +#define AR_PERIOD_MS_SAFETY 20 // 50 Hz +#define AR_PERIOD_MS_HEARTBEAT 1000 // 1 Hz LED blink +#define AR_PERIOD_MS_HEALTH 5000 // every 5 s + +// Watchdog (brief section 7) +#define AR_WATCHDOG_TIMEOUT_S 2 diff --git a/firmware/ar_autopilot_v1/test/test_median_filter/test_median.cpp b/firmware/ar_autopilot_v1/test/test_median_filter/test_median.cpp new file mode 100644 index 0000000..0ff57cb --- /dev/null +++ b/firmware/ar_autopilot_v1/test/test_median_filter/test_median.cpp @@ -0,0 +1,112 @@ +// ============================================================================= +// test_median.cpp -- Unity host-side tests for MedianFilter +// ============================================================================= +// +// Runs on the developer machine via: +// .venv/Scripts/pio.exe test -e native +// +// No ESP32 hardware required. Tests the pure-C++ template in +// src/filters/median.h. +// ============================================================================= + +#include + +// Compile-time switch: when AR_HOST_TEST is defined we drop the Arduino.h +// dependency. The median filter only needs , , . +#include "../../src/filters/median.h" + +using arautopilot::filters::MedianFilter; + +void setUp() {} +void tearDown() {} + +void test_empty_filter_returns_default_constructed_value() { + MedianFilter f; + TEST_ASSERT_EQUAL_INT(0, f.median()); + TEST_ASSERT_EQUAL_size_t(0, f.size()); + TEST_ASSERT_FALSE(f.is_full()); +} + +void test_single_sample_returns_itself() { + MedianFilter f; + TEST_ASSERT_EQUAL_INT(42, f.push(42)); + TEST_ASSERT_EQUAL_size_t(1, f.size()); + TEST_ASSERT_FALSE(f.is_full()); +} + +void test_two_samples_returns_upper_of_pair() { + MedianFilter f; + f.push(10); + // size = 2, median index = 2/2 = 1, so the second element after sort. + TEST_ASSERT_EQUAL_INT(20, f.push(20)); + TEST_ASSERT_EQUAL_INT(20, f.push(15)); // sorted: 10,15,20 -> 15 + TEST_ASSERT_EQUAL_INT(15, f.median()); +} + +void test_filter_suppresses_single_spike() { + // A common scenario: 100 100 99 9999 101 -- the median ignores 9999. + MedianFilter f; + f.push(100); + f.push(100); + f.push(99); + f.push(9999); + f.push(101); + TEST_ASSERT_TRUE(f.is_full()); + TEST_ASSERT_EQUAL_INT(100, f.median()); +} + +void test_filter_window_is_circular() { + MedianFilter f; + f.push(1); + f.push(2); + f.push(3); // window = [1,2,3] -> median 2 + TEST_ASSERT_EQUAL_INT(2, f.median()); + f.push(100); // window = [100,2,3] -> median 3 + TEST_ASSERT_EQUAL_INT(3, f.median()); + f.push(100); // window = [100,100,3] -> median 100 + TEST_ASSERT_EQUAL_INT(100, f.median()); + f.push(100); // window = [100,100,100] -> median 100 + TEST_ASSERT_EQUAL_INT(100, f.median()); +} + +void test_reset_clears_state() { + MedianFilter f; + f.push(1); + f.push(2); + f.push(3); + f.reset(); + TEST_ASSERT_EQUAL_size_t(0, f.size()); + TEST_ASSERT_FALSE(f.is_full()); + TEST_ASSERT_EQUAL_INT(0, f.median()); + TEST_ASSERT_EQUAL_INT(42, f.push(42)); +} + +void test_negative_values() { + MedianFilter f; + f.push(-100); + f.push(-50); + f.push(0); + f.push(50); + f.push(100); + TEST_ASSERT_EQUAL_INT16(0, f.median()); +} + +void test_window_of_one_is_identity() { + MedianFilter f; + TEST_ASSERT_EQUAL_INT(7, f.push(7)); + TEST_ASSERT_EQUAL_INT(9, f.push(9)); + TEST_ASSERT_TRUE(f.is_full()); +} + +int main(int /*argc*/, char** /*argv*/) { + UNITY_BEGIN(); + RUN_TEST(test_empty_filter_returns_default_constructed_value); + RUN_TEST(test_single_sample_returns_itself); + RUN_TEST(test_two_samples_returns_upper_of_pair); + RUN_TEST(test_filter_suppresses_single_spike); + RUN_TEST(test_filter_window_is_circular); + RUN_TEST(test_reset_clears_state); + RUN_TEST(test_negative_values); + RUN_TEST(test_window_of_one_is_identity); + return UNITY_END(); +} diff --git a/firmware/ar_autopilot_v1/tools/modbus_client_test.py b/firmware/ar_autopilot_v1/tools/modbus_client_test.py new file mode 100644 index 0000000..6d72cda --- /dev/null +++ b/firmware/ar_autopilot_v1/tools/modbus_client_test.py @@ -0,0 +1,166 @@ +"""Manual Modbus RTU client for poking the AR-Autopilot ESP32 slave. + +Usage (with the AR-NMEA-IO connected via a USB-to-RS485 adapter): + + python firmware/ar_autopilot_v1/tools/modbus_client_test.py \ + --port COM7 + +It does NOT require any extra dependency beyond ``pymodbus``, which is NOT +installed by default. Install on demand with: + + pip install 'pymodbus>=3.6,<4' + +The script: + + 1. Connects to the slave at the baudrate / framing declared in + ``arautopilot.shared.modbus_register_map``. + 2. Reads firmware version and uptime (input registers). + 3. Reads the current pilot mode. + 4. Reads the rudder angle (input register). + 5. Writes a heading setpoint to a holding register, then reads it back. + 6. Pulses the disengage coil and verifies the discrete bit goes high + (PILOT_ENGAGED is expected to remain false in Sprint 1). +""" + +from __future__ import annotations + +import argparse +import sys +import time +from pathlib import Path + +# Make the project package importable when run from a fresh shell. +REPO_ROOT = Path(__file__).resolve().parents[3] +sys.path.insert(0, str(REPO_ROOT)) + +from arautopilot.shared import modbus_register_map as m # noqa: E402 + +try: + from pymodbus.client import ModbusSerialClient +except ImportError: + sys.stderr.write( + "ERROR: pymodbus is not installed. Run:\n" + " pip install 'pymodbus>=3.6,<4'\n" + ) + sys.exit(2) + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--port", required=True, help="Serial port (e.g. COM7 or /dev/ttyUSB0)") + parser.add_argument("--baudrate", type=int, default=m.BAUDRATE) + parser.add_argument("--slave-id", type=int, default=m.SLAVE_ID) + parser.add_argument("--write-setpoint", type=float, default=180.0, + help="Heading setpoint to write, in degrees (default 180.0)") + args = parser.parse_args() + + print(f"[connect] {args.port} @ {args.baudrate} 8{m.PARITY}1, slave={args.slave_id}") + client = ModbusSerialClient( + port=args.port, + baudrate=args.baudrate, + parity=m.PARITY, + stopbits=m.STOP_BITS, + bytesize=m.DATA_BITS, + timeout=1.0, + ) + if not client.connect(): + print("[connect] FAILED") + return 1 + + try: + # --- Firmware version + uptime --------------------------------------- + rr = client.read_input_registers( + address=m.INPUTS["FW_VERSION_MAJOR"].addr, count=6, slave=args.slave_id + ) + if rr.isError(): + print("[error] read input registers (version): ", rr) + return 1 + major, minor, patch, schema, up_lo, up_hi = rr.registers + uptime_s = up_lo | (up_hi << 16) + print( + f"[fw ] version v{major}.{minor}.{patch} schema={schema} " + f"uptime={uptime_s}s" + ) + + # --- Mode ----------------------------------------------------------- + rr = client.read_input_registers( + address=m.INPUTS["CURRENT_MODE"].addr, count=1, slave=args.slave_id + ) + mode_names = {0: "STANDBY", 1: "HEADING_HOLD", 2: "TRUE_COURSE", + 3: "TRACK_KEEPING", 4: "DODGE"} + print(f"[mode ] {mode_names.get(rr.registers[0], '?')} ({rr.registers[0]})") + + # --- Rudder angle --------------------------------------------------- + rr = client.read_input_registers( + address=m.INPUTS["RUDDER_ANGLE_DEG_X100"].addr, count=3, slave=args.slave_id + ) + angle_x100, raw_adc, valid = rr.registers + # interpret as signed int16 + if angle_x100 >= 0x8000: + angle_x100 -= 0x10000 + print( + f"[rudd ] angle={angle_x100 / 100.0:+.2f} deg raw_adc={raw_adc} " + f"valid={'yes' if valid else 'no'}" + ) + + # --- Heading + ROT (Sprint 1 wires these via NMEA 2000) ------------- + rr = client.read_input_registers( + address=m.INPUTS["HEADING_DEG_X100"].addr, count=3, slave=args.slave_id + ) + heading_x100, rot_x100, age_ms = rr.registers + if rot_x100 >= 0x8000: + rot_x100 -= 0x10000 + print( + f"[n2k ] heading={heading_x100 / 100.0:7.2f} deg " + f"rot={rot_x100 / 100.0:+.2f} deg/s age={age_ms} ms" + ) + + # --- Write heading setpoint ----------------------------------------- + sp_x100 = int(round(args.write_setpoint * 100.0)) + if sp_x100 < -0x8000 or sp_x100 > 0x7FFF: + print(f"[error] setpoint out of int16 range") + return 1 + wr_value = sp_x100 if sp_x100 >= 0 else (sp_x100 + 0x10000) + wr = client.write_register( + address=m.HOLDINGS["HEADING_SETPOINT_X100"].addr, + value=wr_value, + slave=args.slave_id, + ) + if wr.isError(): + print(f"[error] write holding (heading setpoint): {wr}") + return 1 + rr = client.read_holding_registers( + address=m.HOLDINGS["HEADING_SETPOINT_X100"].addr, + count=1, + slave=args.slave_id, + ) + read_back = rr.registers[0] + if read_back >= 0x8000: + read_back -= 0x10000 + print( + f"[hold ] wrote heading setpoint {args.write_setpoint:.2f} deg, " + f"read back {read_back / 100.0:+.2f} deg" + ) + + # --- Disengage coil pulse ------------------------------------------- + client.write_coil( + address=m.COILS["CMD_DISENGAGE_REQUEST"].addr, + value=True, + slave=args.slave_id, + ) + time.sleep(0.05) + rr = client.read_discrete_inputs( + address=m.DISCRETES["PILOT_ENGAGED"].addr, count=1, slave=args.slave_id + ) + print( + f"[coil ] disengage pulsed; PILOT_ENGAGED = " + f"{'YES (unexpected)' if rr.bits[0] else 'no (correct -- Sprint 1 is always STANDBY)'}" + ) + + return 0 + finally: + client.close() + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/gen_modbus_registers.py b/tools/gen_modbus_registers.py new file mode 100644 index 0000000..0d34fcb --- /dev/null +++ b/tools/gen_modbus_registers.py @@ -0,0 +1,256 @@ +"""Generate firmware C++ header and Python module from modbus_registers.yaml. + +The YAML at ``firmware/ar_autopilot_v1/modbus_registers.yaml`` is the single +source of truth for the Modbus RTU register map shared between the ESP32 +firmware and the Python tooling / Studio simulator. This script regenerates: + +- ``firmware/ar_autopilot_v1/src/protocols/modbus_registers.h`` +- ``arautopilot/shared/modbus_register_map.py`` + +Run from the repository root: + + python tools/gen_modbus_registers.py +""" + +from __future__ import annotations + +import argparse +import sys +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import yaml + +REPO_ROOT = Path(__file__).resolve().parent.parent +YAML_PATH = REPO_ROOT / "firmware/ar_autopilot_v1/modbus_registers.yaml" +HEADER_PATH = ( + REPO_ROOT / "firmware/ar_autopilot_v1/src/protocols/modbus_registers.h" +) +PY_PATH = REPO_ROOT / "arautopilot/shared/modbus_register_map.py" + + +@dataclass(frozen=True) +class Entry: + addr: int + name: str + desc: str + unit: str = "" + scale: float = 1.0 + offset: float = 0.0 + + +@dataclass(frozen=True) +class RegisterMap: + schema_version: str + slave_id: int + baudrate: int + parity: str + data_bits: int + stop_bits: int + discretes: list[Entry] + coils: list[Entry] + inputs: list[Entry] + holdings: list[Entry] + + +def load_yaml(path: Path) -> RegisterMap: + raw: dict[str, Any] = yaml.safe_load(path.read_text(encoding="utf-8")) + + def to_entries(items: list[dict[str, Any]]) -> list[Entry]: + out: list[Entry] = [] + seen: set[int] = set() + for item in items: + addr = int(item["addr"]) + if addr in seen: + raise ValueError(f"duplicate address {addr} in {path.name}") + seen.add(addr) + out.append( + Entry( + addr=addr, + name=str(item["name"]), + desc=str(item.get("desc", "")), + unit=str(item.get("unit", "")), + scale=float(item.get("scale", 1.0)), + offset=float(item.get("offset", 0.0)), + ) + ) + out.sort(key=lambda e: e.addr) + return out + + return RegisterMap( + schema_version=str(raw["schema_version"]), + slave_id=int(raw["slave_id"]), + baudrate=int(raw["baudrate"]), + parity=str(raw["parity"]), + data_bits=int(raw["data_bits"]), + stop_bits=int(raw["stop_bits"]), + discretes=to_entries(raw.get("discretes", [])), + coils=to_entries(raw.get("coils", [])), + inputs=to_entries(raw.get("inputs", [])), + holdings=to_entries(raw.get("holdings", [])), + ) + + +def _max_addr(entries: list[Entry]) -> int: + return max((e.addr for e in entries), default=-1) + + +def render_header(rm: RegisterMap) -> str: + lines: list[str] = [] + a = lines.append + a("// =============================================================================") + a("// modbus_registers.h -- AR-Autopilot Modbus RTU register map") + a("// =============================================================================") + a("//") + a("// AUTO-GENERATED. Do not edit by hand.") + a("// Source: firmware/ar_autopilot_v1/modbus_registers.yaml") + a("// Regenerate with: python tools/gen_modbus_registers.py") + a("// =============================================================================") + a("") + a("#pragma once") + a("") + a("#include ") + a("") + a("namespace arautopilot::protocols::modbus {") + a("") + a(f'constexpr const char* MAP_SCHEMA_VERSION = "{rm.schema_version}";') + a(f"constexpr uint8_t SLAVE_ID = {rm.slave_id};") + a(f"constexpr uint32_t BAUDRATE = {rm.baudrate};") + a(f'constexpr char PARITY = \'{rm.parity}\';') + a(f"constexpr uint8_t DATA_BITS = {rm.data_bits};") + a(f"constexpr uint8_t STOP_BITS = {rm.stop_bits};") + a("") + + def block(kind: str, prefix: str, entries: list[Entry]) -> None: + a(f"// ----- {kind} -----") + a(f"constexpr uint16_t {prefix}_COUNT = {len(entries)};") + max_a = _max_addr(entries) + a(f"constexpr uint16_t {prefix}_MAX_ADDR = {max_a};") + a("") + for e in entries: + a(f"// {e.desc}") + extras = [] + if e.unit: + extras.append(f"unit={e.unit}") + if e.scale != 1.0: + extras.append(f"scale={e.scale}") + if e.offset != 0.0: + extras.append(f"offset={e.offset}") + if extras: + a("// " + ", ".join(extras)) + a(f"constexpr uint16_t {prefix}_{e.name} = {e.addr};") + a("") + + block("Discrete inputs (read-only bits)", "DISCRETE", rm.discretes) + block("Coils (read-write bits)", "COIL", rm.coils) + block("Input registers (read-only words)", "INPUT", rm.inputs) + block("Holding registers (read-write words)", "HOLDING", rm.holdings) + + a("} // namespace arautopilot::protocols::modbus") + a("") + return "\n".join(lines) + + +def render_python(rm: RegisterMap) -> str: + lines: list[str] = [] + a = lines.append + a('"""AR-Autopilot Modbus RTU register map -- generated mirror of the firmware.') + a("") + a("AUTO-GENERATED. Do not edit by hand.") + a("Source: firmware/ar_autopilot_v1/modbus_registers.yaml") + a("Regenerate with: python tools/gen_modbus_registers.py") + a('"""') + a("") + a("from __future__ import annotations") + a("") + a("from dataclasses import dataclass") + a("") + a("") + a("@dataclass(frozen=True)") + a("class Reg:") + a(' """One Modbus register entry as seen from Python tooling."""') + a(" addr: int") + a(" name: str") + a(" desc: str = \"\"") + a(" unit: str = \"\"") + a(" scale: float = 1.0") + a(" offset: float = 0.0") + a("") + a("") + a(f'MAP_SCHEMA_VERSION = "{rm.schema_version}"') + a(f"SLAVE_ID = {rm.slave_id}") + a(f"BAUDRATE = {rm.baudrate}") + a(f'PARITY = "{rm.parity}"') + a(f"DATA_BITS = {rm.data_bits}") + a(f"STOP_BITS = {rm.stop_bits}") + a("") + + def block(group_name: str, entries: list[Entry]) -> None: + a(f"# {group_name.upper()}") + a(f"{group_name.upper()}: dict[str, Reg] = {{") + for e in entries: + a( + f' "{e.name}": Reg(' + f"addr={e.addr}, " + f'name="{e.name}", ' + f'desc={e.desc!r}, ' + f'unit="{e.unit}", ' + f"scale={e.scale}, " + f"offset={e.offset})," + ) + a("}") + a("") + + block("discretes", rm.discretes) + block("coils", rm.coils) + block("inputs", rm.inputs) + block("holdings", rm.holdings) + + a("ALL_GROUPS: dict[str, dict[str, Reg]] = {") + a(' "discretes": DISCRETES,') + a(' "coils": COILS,') + a(' "inputs": INPUTS,') + a(' "holdings": HOLDINGS,') + a("}") + a("") + return "\n".join(lines) + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--check", + action="store_true", + help="Exit non-zero if the generated files are out of date; do not write.", + ) + args = parser.parse_args(argv) + + rm = load_yaml(YAML_PATH) + header = render_header(rm) + pymod = render_python(rm) + + if args.check: + ok = True + for path, expected in ((HEADER_PATH, header), (PY_PATH, pymod)): + actual = path.read_text(encoding="utf-8") if path.exists() else "" + if actual != expected: + print(f"OUT-OF-DATE: {path.relative_to(REPO_ROOT)}", file=sys.stderr) + ok = False + return 0 if ok else 1 + + HEADER_PATH.parent.mkdir(parents=True, exist_ok=True) + PY_PATH.parent.mkdir(parents=True, exist_ok=True) + HEADER_PATH.write_text(header, encoding="utf-8") + PY_PATH.write_text(pymod, encoding="utf-8") + print(f"wrote {HEADER_PATH.relative_to(REPO_ROOT)}") + print(f"wrote {PY_PATH.relative_to(REPO_ROOT)}") + print( + f" discretes={len(rm.discretes)} coils={len(rm.coils)} " + f"inputs={len(rm.inputs)} holdings={len(rm.holdings)}" + ) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main())