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>
2026-05-20 00:16:24 -04:00
parent 0f00ad10da
commit e82dbc449c
8 changed files with 965 additions and 45 deletions
@@ -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