sprint-6: Alarm engine + safety monitor + NMEA 2000 publisher

Python side:
- alarm_engine.py: AlarmEngine evaluates 9 firmware alarm bits + PC-side
  heading staleness and off-course logic with severe-timer; on_disengage
  callback triggers on first EMERGENCY alarm; acknowledge/clear API
- test_alarm_engine.py: 25 tests covering fire/clear cycle, acknowledge,
  highest_severity, auto-disengage callback, heading staleness, off-course
  with wraparound and timer, fw-bit suppression of duplicate PC alarm

Firmware:
- safety_monitor.h: exposes AlarmBits struct + safety_alarm_bits() API
- safety_monitor.cpp: 50 Hz task evaluates off-course (with severe timer),
  rudder-not-responding (3 s timeout), heading lost, VMS/DI4, limit switches,
  battery voltage, actuator current; buzzer on any alarm; EMERGENCY → force_standby
- modbus_slave.cpp: wires 9 discrete alarm registers to safety_alarm_bits();
  battery voltage and actuator current ADC registers now live
- nmea2000_publisher.h/cpp: new task, PGN 127245 rudder angle at 10 Hz,
  PGN 127237 Heading/Track Control at 1 Hz
- main.cpp: start nmea2000_publisher; set watchdog-tripped flag on ESP_RST_TASK_WDT

Tests: 309 passed | Flash: 27.6%

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp

@@ -33,6 +33,7 @@
 #include "../system/task_config.h"
 #include "modbus_registers.h"
 #include "nmea2000_consumer.h"
+#include "../safety/safety_monitor.h"
 
 namespace arautopilot::protocols::modbus {
 
@@ -133,11 +134,24 @@ uint16_t read_input_register(uint16_t addr) {
             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;
+        case INPUT_BATTERY_VOLTAGE_X100: {
+            // 1:6 voltage divider on AI2; 12-bit ADC, 3.3 V reference.
+            const int raw = analogRead(PIN_AI2_BATTERY_VOLTAGE);
+            const float vbat = (float)raw * (3.3f * 6.0f / 4095.0f);
+            int v = (int)(vbat * 100.0f);
+            if (v < 0) v = 0;
+            if (v > 32767) v = 32767;
+            return (uint16_t)v;
+        }
+        case INPUT_ACTUATOR_CURRENT_X100: {
+            // Hall-effect transducer on AI3, 50 A FS.
+            const int raw = analogRead(PIN_AI3_ACTUATOR_CURRENT);
+            const float iact = (float)raw * (3.3f * 50.0f / 4095.0f);
+            int v = (int)(iact * 100.0f);
+            if (v < 0) v = 0;
+            if (v > 32767) v = 32767;
+            return (uint16_t)v;
+        }
 
         // ----- PID inner-loop telemetry (Sprint 2) -----
         case INPUT_PID_INNER_SETPOINT_X100: {
@@ -274,18 +288,17 @@ bool read_discrete(uint16_t addr) {
         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;
+        // Alarm bits: wired in Sprint 6 via safety_monitor.
+        case DISCRETE_ALARM_OFF_COURSE:         return safety::safety_alarm_bits().off_course;
+        case DISCRETE_ALARM_OFF_COURSE_SEVERE:  return safety::safety_alarm_bits().off_course_severe;
+        case DISCRETE_ALARM_RUDDER_NOT_RESP:    return safety::safety_alarm_bits().rudder_not_resp;
+        case DISCRETE_ALARM_HEADING_LOST:       return safety::safety_alarm_bits().heading_lost;
+        case DISCRETE_ALARM_ACTUATOR_OVERCURR:  return safety::safety_alarm_bits().actuator_overcurr;
+        case DISCRETE_ALARM_VOLTAGE_LOW:        return safety::safety_alarm_bits().voltage_low;
+        case DISCRETE_ALARM_LIMIT_REACHED:      return safety::safety_alarm_bits().limit_reached;
+        case DISCRETE_ALARM_WATCHDOG_TRIPPED:   return safety::safety_alarm_bits().watchdog_tripped;
+        case DISCRETE_ALARM_VMS_CRITICAL:       return safety::safety_alarm_bits().vms_critical;
+        case DISCRETE_ANY_ALARM_ACTIVE:         return safety::safety_alarm_bits().any();
         default:
             return false;
     }

firmware/ar_autopilot_v1/src/protocols/nmea2000_publisher.cpp

@@ -0,0 +1,127 @@
+// =============================================================================
+//  protocols/nmea2000_publisher.cpp -- NMEA 2000 publisher (Sprint 6)
+// =============================================================================
+
+#include "nmea2000_publisher.h"
+
+#include <Arduino.h>
+#include <math.h>
+
+#include <N2kMessages.h>
+#include <NMEA2000.h>
+
+// NMEA2000_CAN.h defines the global tNMEA2000& NMEA2000 in the header,
+// so it can only be included in one translation unit (nmea2000_consumer.cpp).
+// Declare the same object here via extern.
+extern tNMEA2000& NMEA2000;
+
+#include "../hal/rudder_sensor.h"
+#include "../modes/standby.h"
+#include "../pid/pid_outer_task.h"
+#include "../protocols/nmea2000_consumer.h"
+#include "../system/ar_log.h"
+#include "../system/task_config.h"
+
+namespace arautopilot::protocols::nmea2000 {
+
+namespace {
+
+constexpr const char* TAG = "AR/N2K/PUB";
+
+static const double K_DEG2RAD = M_PI / 180.0;
+
+// ----- PGN 127245: Rudder angle (10 Hz) -------------------------------------
+void publish_rudder() {
+    const auto rdr = hal::rudder_sensor_latest();
+    if (!rdr.valid) return;
+
+    tN2kMsg msg;
+    SetN2kRudder(msg,
+                 (double)rdr.angle_deg * K_DEG2RAD,
+                 0,
+                 N2kRDO_NoDirectionOrder,
+                 N2kDoubleNA);
+    NMEA2000.SendMsg(msg);
+}
+
+// ----- PGN 127237: Heading Track Control (1 Hz) -----------------------------
+void publish_heading_track_control() {
+    const bool engaged = !modes::is_standby();
+
+    const tN2kSteeringMode steering_mode = engaged
+        ? N2kSM_HeadingControl
+        : N2kSM_MainSteering;
+
+    double heading_to_steer = N2kDoubleNA;
+    double vessel_heading   = N2kDoubleNA;
+
+    // Current heading from consumer snapshot for VesselHeading field.
+    {
+        const auto n = nmea2000_latest();
+        if (n.heading_valid) {
+            vessel_heading = (double)n.heading_deg * K_DEG2RAD;
+        }
+    }
+
+    if (engaged) {
+        const modes::Mode mode = modes::current_mode();
+        if (mode == modes::Mode::HEADING_HOLD) {
+            heading_to_steer = (double)pid::pid_outer_heading_setpoint_deg() * K_DEG2RAD;
+        } else if (mode == modes::Mode::TRUE_COURSE ||
+                   mode == modes::Mode::TRACK_KEEPING) {
+            heading_to_steer = (double)pid::pid_outer_cog_setpoint_deg() * K_DEG2RAD;
+        }
+    }
+
+    tN2kMsg msg;
+    SetN2kHeadingTrackControl(msg,
+        N2kOnOff_Unavailable,       // RudderLimitExceeded
+        N2kOnOff_Unavailable,       // OffHeadingLimitExceeded
+        N2kOnOff_Unavailable,       // OffTrackLimitExceeded
+        N2kOnOff_Off,               // Override
+        steering_mode,              // SteeringMode
+        N2kTM_RudderLimitControlled,// TurnMode
+        N2khr_true,                 // HeadingReference
+        N2kRDO_NoDirectionOrder,    // CommandedRudderDirection
+        N2kDoubleNA,                // CommandedRudderAngle
+        heading_to_steer,           // HeadingToSteerCourse
+        N2kDoubleNA,                // Track
+        N2kDoubleNA,                // RudderLimit
+        N2kDoubleNA,                // OffHeadingLimit
+        N2kDoubleNA,                // RadiusOfTurnOrder
+        N2kDoubleNA,                // RateOfTurnOrder
+        N2kDoubleNA,                // OffTrackLimit
+        vessel_heading);            // VesselHeading
+    NMEA2000.SendMsg(msg);
+}
+
+void PublisherTask(void* /*pv*/) {
+    AR_LOGI(TAG, "nmea2000_publisher task started on core %d", xPortGetCoreID());
+
+    TickType_t last_wake = xTaskGetTickCount();
+    uint8_t    slow_tick = 0;
+
+    for (;;) {
+        publish_rudder();
+
+        if (++slow_tick >= 10) {
+            slow_tick = 0;
+            publish_heading_track_control();
+        }
+
+        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(100));
+    }
+}
+
+}  // namespace
+
+void nmea2000_publisher_start_task() {
+    xTaskCreatePinnedToCore(PublisherTask, "n2k_pub",
+                            AR_TASK_STACK_N2K_RX,
+                            nullptr,
+                            AR_TASK_PRIO_N2K_RX - 1,
+                            nullptr,
+                            AR_TASK_CORE_COMMS);
+}
+
+}  // namespace arautopilot::protocols::nmea2000

firmware/ar_autopilot_v1/src/protocols/nmea2000_publisher.h

@@ -0,0 +1,31 @@
+// =============================================================================
+//  protocols/nmea2000_publisher.h -- NMEA 2000 publisher (Sprint 6)
+// =============================================================================
+//
+//  Publishes autopilot state onto the NMEA 2000 backbone so other instruments
+//  (chartplotters, VHF, AIS) can see what the autopilot is doing.
+//
+//  PGN 127245 -- Rudder angle (actual measured rudder position)
+//    Broadcast at 10 Hz so chartplotters and other displays can render a
+//    rudder indicator.
+//
+//  PGN 127237 -- Heading Track Control
+//    Broadcast at 1 Hz. Tells the network: which mode is engaged, the
+//    commanded heading, and the current COG. Chartplotters use this to
+//    draw the track line and to know whether to suppress their own helm
+//    commands.
+//
+//  Both messages are sent on the same NMEA2000 stack object that the consumer
+//  uses (global `NMEA2000` instance from NMEA2000_CAN.h). The publisher runs
+//  as a low-priority periodic task on Core 0 (comms core).
+// =============================================================================
+
+#pragma once
+
+namespace arautopilot::protocols::nmea2000 {
+
+/// Start the NMEA 2000 publisher task.
+/// Must be called AFTER nmea2000_consumer_init() (shares the same CAN stack).
+void nmea2000_publisher_start_task();
+
+}  // namespace arautopilot::protocols::nmea2000