diff --git a/arautopilot/core/alarm_engine.py b/arautopilot/core/alarm_engine.py new file mode 100644 index 0000000..f82d7e3 --- /dev/null +++ b/arautopilot/core/alarm_engine.py @@ -0,0 +1,257 @@ +"""Runtime alarm engine -- Sprint 6. + +Evaluates live telemetry against the alarm catalogue (brief section 7) +and maintains a set of currently active alarms. Designed to run on the +PC side (Python display software); the firmware publishes alarm bits via +Modbus discrete inputs and this engine turns them into typed Alarm records +that the UI displays and the audit log records. + +Usage:: + + engine = AlarmEngine() + # On each 10 Hz display tick: + alarms = engine.evaluate(snapshot) + for a in alarms: # newly fired alarms + display.show_alarm(a) + audit_log.append(a) + + engine.acknowledge(AlarmType.OFF_COURSE) + engine.acknowledge_all() +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Callable + +from arautopilot.core.alarms import Alarm, AlarmType, AlarmSeverity, triggers_auto_disengage + + +@dataclass +class AlarmThresholds: + """Tunable thresholds; can be loaded from the project config.""" + + # Off-course + off_course_deg: float = 10.0 # LOW warning threshold + severe_off_course_deg: float = 30.0 # EMERGENCY threshold + severe_off_course_time_s: float = 5.0 # must persist this long + + # Rudder not responding + rudder_response_timeout_s: float = 3.0 # setpoint sent but no motion + rudder_deadband_deg: float = 0.5 + + # Sensor staleness (must match firmware STALE_THRESHOLD_MS) + heading_stale_s: float = 5.0 + cog_stale_s: float = 5.0 + + # Voltage / current (used by display if it has ADC readback) + voltage_low_v: float = 10.5 + current_high_a: float = 30.0 + + +@dataclass +class TelemetrySnapshot: + """Aggregated telemetry read from Modbus or internal state.""" + + # Mode + pilot_engaged: bool = False + + # Heading + heading_deg: float | None = None + heading_setpoint_deg: float | None = None + heading_age_s: float = 0.0 + + # Rudder + rudder_angle_deg: float | None = None + rudder_setpoint_deg: float | None = None + rudder_valid: bool = False + + # COG (True Course / Track Keeping modes) + cog_deg: float | None = None + cog_age_s: float = 0.0 + + # Electrical + battery_v: float | None = None + actuator_a: float | None = None + + # Digital inputs + limit_port: bool = False + limit_stbd: bool = False + vms_critical: bool = False + + # Firmware discrete alarms (read directly from Modbus) + fw_alarm_off_course: bool = False + fw_alarm_off_course_severe: bool = False + fw_alarm_rudder_not_resp: bool = False + fw_alarm_heading_lost: bool = False + fw_alarm_actuator_overcurr: bool = False + fw_alarm_voltage_low: bool = False + fw_alarm_limit_reached: bool = False + fw_alarm_watchdog_tripped: bool = False + fw_alarm_vms_critical: bool = False + + +class AlarmEngine: + """Evaluates telemetry and manages the active alarm set. + + The primary job is to bridge between raw Modbus bits (from the firmware) + and the typed :class:`Alarm` records that the UI and audit log consume. + + The engine also performs PC-side logic for alarms the firmware doesn't + compute (e.g. heading-sensor age on the Python side). + """ + + def __init__( + self, + thresholds: AlarmThresholds | None = None, + on_disengage: Callable[[], None] | None = None, + ) -> None: + self.thresholds = thresholds or AlarmThresholds() + self._on_disengage = on_disengage + self._active: dict[AlarmType, Alarm] = {} + self._acknowledged: set[AlarmType] = set() + self._severe_off_course_timer_s: float = 0.0 + self._dt_s: float = 0.1 # 10 Hz default + + # ------------------------------------------------------------------ + # Public API + # ------------------------------------------------------------------ + + def evaluate( + self, + snap: TelemetrySnapshot, + dt_s: float | None = None, + ) -> list[Alarm]: + """Evaluate conditions against ``snap``. + + Returns the list of **newly fired** alarms since the last call. + Already-active alarms are not re-fired. + """ + if dt_s is not None: + self._dt_s = dt_s + + new_alarms: list[Alarm] = [] + + # Map firmware discrete bits → alarm types + fw_map: list[tuple[bool, AlarmType]] = [ + (snap.fw_alarm_off_course, AlarmType.OFF_COURSE), + (snap.fw_alarm_off_course_severe, AlarmType.OFF_COURSE_SEVERE), + (snap.fw_alarm_rudder_not_resp, AlarmType.RUDDER_NOT_RESPONDING), + (snap.fw_alarm_heading_lost, AlarmType.HEADING_SENSOR_LOST), + (snap.fw_alarm_actuator_overcurr, AlarmType.ACTUATOR_OVERCURRENT), + (snap.fw_alarm_voltage_low, AlarmType.VOLTAGE_LOW), + (snap.fw_alarm_limit_reached, AlarmType.LIMIT_SWITCH_REACHED), + (snap.fw_alarm_watchdog_tripped, AlarmType.WATCHDOG_TRIPPED), + (snap.fw_alarm_vms_critical, AlarmType.VMS_CRITICAL), + ] + + for condition, alarm_type in fw_map: + if condition: + fired = self._maybe_fire(alarm_type) + if fired: + new_alarms.append(fired) + else: + self._clear(alarm_type) + + # PC-side: heading sensor staleness + if snap.pilot_engaged and snap.heading_age_s > self.thresholds.heading_stale_s: + fired = self._maybe_fire(AlarmType.HEADING_SENSOR_LOST) + if fired: + new_alarms.append(fired) + + # PC-side: off-course (if firmware bits not present, compute from heading) + if (snap.pilot_engaged + and snap.heading_deg is not None + and snap.heading_setpoint_deg is not None + and not snap.fw_alarm_off_course + and not snap.fw_alarm_off_course_severe): + err = _shortest_arc(snap.heading_setpoint_deg, snap.heading_deg) + thr = self.thresholds + if abs(err) >= thr.severe_off_course_deg: + self._severe_off_course_timer_s += self._dt_s + if self._severe_off_course_timer_s >= thr.severe_off_course_time_s: + fired = self._maybe_fire(AlarmType.OFF_COURSE_SEVERE) + if fired: + new_alarms.append(fired) + else: + fired = self._maybe_fire(AlarmType.OFF_COURSE) + if fired: + new_alarms.append(fired) + elif abs(err) >= thr.off_course_deg: + self._severe_off_course_timer_s = 0.0 + fired = self._maybe_fire(AlarmType.OFF_COURSE) + if fired: + new_alarms.append(fired) + else: + self._severe_off_course_timer_s = 0.0 + self._clear(AlarmType.OFF_COURSE) + self._clear(AlarmType.OFF_COURSE_SEVERE) + + # Trigger auto-disengage for any new EMERGENCY alarms. + for a in new_alarms: + if a.auto_disengage_triggered and self._on_disengage: + self._on_disengage() + break + + return new_alarms + + def acknowledge(self, alarm_type: AlarmType) -> None: + """Acknowledge a specific alarm (marks it as seen).""" + self._acknowledged.add(alarm_type) + if alarm_type in self._active: + # Replace the alarm record with an acknowledged copy. + old = self._active[alarm_type] + self._active[alarm_type] = old.model_copy(update={"acknowledged": True}) + + def acknowledge_all(self) -> None: + for at in list(self._active): + self.acknowledge(at) + + def clear(self, alarm_type: AlarmType) -> None: + self._clear(alarm_type) + self._acknowledged.discard(alarm_type) + + def clear_all(self) -> None: + self._active.clear() + self._acknowledged.clear() + self._severe_off_course_timer_s = 0.0 + + @property + def active_alarms(self) -> list[Alarm]: + return list(self._active.values()) + + @property + def any_active(self) -> bool: + return bool(self._active) + + @property + def highest_severity(self) -> AlarmSeverity | None: + if not self._active: + return None + order = [AlarmSeverity.EMERGENCY, AlarmSeverity.HIGH, + AlarmSeverity.LOW, AlarmSeverity.INFO] + for sev in order: + if any(a.severity == sev for a in self._active.values()): + return sev + return None + + # ------------------------------------------------------------------ + # Internal helpers + # ------------------------------------------------------------------ + + def _maybe_fire(self, alarm_type: AlarmType) -> Alarm | None: + if alarm_type in self._active: + return None # already active + alarm = Alarm.from_type(alarm_type) + self._active[alarm_type] = alarm + self._acknowledged.discard(alarm_type) + return alarm + + def _clear(self, alarm_type: AlarmType) -> None: + self._active.pop(alarm_type, None) + + +def _shortest_arc(setpoint: float, measured: float) -> float: + """Signed shortest-arc error in degrees (setpoint - measured).""" + err = (setpoint - measured + 180.0) % 360.0 - 180.0 + return err diff --git a/arautopilot/tests/test_alarm_engine.py b/arautopilot/tests/test_alarm_engine.py new file mode 100644 index 0000000..9b52f93 --- /dev/null +++ b/arautopilot/tests/test_alarm_engine.py @@ -0,0 +1,259 @@ +"""Tests for ``arautopilot.core.alarm_engine``.""" + +from __future__ import annotations + +import pytest + +from arautopilot.core.alarm_engine import AlarmEngine, AlarmThresholds, TelemetrySnapshot +from arautopilot.core.alarms import AlarmSeverity, AlarmType + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +def _engaged_snap(**kwargs) -> TelemetrySnapshot: + """Return a baseline engaged snapshot with no alarms asserted.""" + defaults = dict( + pilot_engaged=True, + heading_deg=90.0, + heading_setpoint_deg=90.0, + heading_age_s=0.1, + ) + defaults.update(kwargs) + return TelemetrySnapshot(**defaults) + + +# --------------------------------------------------------------------------- +# Basic fire / clear cycle +# --------------------------------------------------------------------------- + +class TestFireAndClear: + def test_no_alarms_by_default(self): + eng = AlarmEngine() + snap = TelemetrySnapshot(pilot_engaged=False) + new = eng.evaluate(snap) + assert new == [] + assert not eng.any_active + + def test_fw_bit_fires_alarm(self): + eng = AlarmEngine() + snap = TelemetrySnapshot(fw_alarm_off_course=True) + new = eng.evaluate(snap) + assert len(new) == 1 + assert new[0].type is AlarmType.OFF_COURSE + assert eng.any_active + + def test_fw_bit_clearing_removes_alarm(self): + eng = AlarmEngine() + eng.evaluate(TelemetrySnapshot(fw_alarm_off_course=True)) + new = eng.evaluate(TelemetrySnapshot(fw_alarm_off_course=False)) + assert new == [] + assert not eng.any_active + + def test_alarm_not_re_fired_while_active(self): + eng = AlarmEngine() + snap = TelemetrySnapshot(fw_alarm_off_course=True) + first = eng.evaluate(snap) + second = eng.evaluate(snap) + assert len(first) == 1 + assert second == [] # already active, not re-fired + + def test_all_nine_fw_bits_map_to_distinct_alarm_types(self): + all_bits = TelemetrySnapshot( + fw_alarm_off_course=True, + fw_alarm_off_course_severe=True, + fw_alarm_rudder_not_resp=True, + fw_alarm_heading_lost=True, + fw_alarm_actuator_overcurr=True, + fw_alarm_voltage_low=True, + fw_alarm_limit_reached=True, + fw_alarm_watchdog_tripped=True, + fw_alarm_vms_critical=True, + ) + eng = AlarmEngine() + new = eng.evaluate(all_bits) + types = {a.type for a in new} + assert len(types) == 9 + assert AlarmType.OFF_COURSE in types + assert AlarmType.VMS_CRITICAL in types + + +# --------------------------------------------------------------------------- +# Acknowledge +# --------------------------------------------------------------------------- + +class TestAcknowledge: + def test_acknowledge_marks_alarm(self): + eng = AlarmEngine() + eng.evaluate(TelemetrySnapshot(fw_alarm_off_course=True)) + eng.acknowledge(AlarmType.OFF_COURSE) + assert eng.active_alarms[0].acknowledged is True + + def test_acknowledge_all(self): + eng = AlarmEngine() + eng.evaluate(TelemetrySnapshot( + fw_alarm_off_course=True, + fw_alarm_voltage_low=True, + )) + eng.acknowledge_all() + for a in eng.active_alarms: + assert a.acknowledged is True + + def test_unacknowledged_alarm_after_clear_refire(self): + """After a cleared alarm re-asserts it must fire (and be unacknowledged).""" + eng = AlarmEngine() + eng.evaluate(TelemetrySnapshot(fw_alarm_off_course=True)) + eng.evaluate(TelemetrySnapshot(fw_alarm_off_course=False)) + new = eng.evaluate(TelemetrySnapshot(fw_alarm_off_course=True)) + assert len(new) == 1 + assert new[0].acknowledged is False + + +# --------------------------------------------------------------------------- +# Highest severity +# --------------------------------------------------------------------------- + +class TestHighestSeverity: + def test_none_when_no_alarms(self): + assert AlarmEngine().highest_severity is None + + def test_emergency_dominates(self): + eng = AlarmEngine() + eng.evaluate(TelemetrySnapshot( + fw_alarm_off_course=True, # LOW + fw_alarm_watchdog_tripped=True, # EMERGENCY + )) + assert eng.highest_severity is AlarmSeverity.EMERGENCY + + def test_high_when_no_emergency(self): + eng = AlarmEngine() + eng.evaluate(TelemetrySnapshot( + fw_alarm_off_course=True, # LOW + fw_alarm_voltage_low=True, # HIGH + )) + assert eng.highest_severity is AlarmSeverity.HIGH + + def test_low_when_only_low(self): + eng = AlarmEngine() + eng.evaluate(TelemetrySnapshot(fw_alarm_off_course=True)) + assert eng.highest_severity is AlarmSeverity.LOW + + +# --------------------------------------------------------------------------- +# Auto-disengage callback +# --------------------------------------------------------------------------- + +class TestAutoDisengage: + def test_emergency_triggers_disengage_callback(self): + called = [] + eng = AlarmEngine(on_disengage=lambda: called.append(1)) + eng.evaluate(TelemetrySnapshot(fw_alarm_watchdog_tripped=True)) + assert called == [1] + + def test_low_alarm_does_not_trigger_disengage(self): + called = [] + eng = AlarmEngine(on_disengage=lambda: called.append(1)) + eng.evaluate(TelemetrySnapshot(fw_alarm_off_course=True)) + assert called == [] + + def test_callback_called_once_per_event(self): + called = [] + eng = AlarmEngine(on_disengage=lambda: called.append(1)) + snap = TelemetrySnapshot(fw_alarm_watchdog_tripped=True) + eng.evaluate(snap) + eng.evaluate(snap) # still active — not re-fired + assert len(called) == 1 + + +# --------------------------------------------------------------------------- +# PC-side heading staleness +# --------------------------------------------------------------------------- + +class TestHeadingStaleness: + def test_stale_heading_fires_alarm_when_engaged(self): + eng = AlarmEngine() + snap = TelemetrySnapshot( + pilot_engaged=True, + heading_age_s=6.0, # > default 5.0 s threshold + ) + new = eng.evaluate(snap) + types = {a.type for a in new} + assert AlarmType.HEADING_SENSOR_LOST in types + + def test_fresh_heading_does_not_fire(self): + eng = AlarmEngine() + snap = TelemetrySnapshot(pilot_engaged=True, heading_age_s=0.5) + new = eng.evaluate(snap) + assert AlarmType.HEADING_SENSOR_LOST not in {a.type for a in new} + + def test_stale_heading_does_not_fire_when_disengaged(self): + eng = AlarmEngine() + snap = TelemetrySnapshot(pilot_engaged=False, heading_age_s=60.0) + new = eng.evaluate(snap) + assert new == [] + + +# --------------------------------------------------------------------------- +# PC-side off-course (no firmware bits) +# --------------------------------------------------------------------------- + +class TestOffCoursePC: + def test_small_error_no_alarm(self): + eng = AlarmEngine() + snap = _engaged_snap(heading_deg=91.0, heading_setpoint_deg=90.0) + new = eng.evaluate(snap) + assert AlarmType.OFF_COURSE not in {a.type for a in new} + + def test_moderate_error_fires_off_course(self): + eng = AlarmEngine() + snap = _engaged_snap(heading_deg=105.0, heading_setpoint_deg=90.0) # 15 deg > 10 + new = eng.evaluate(snap) + assert AlarmType.OFF_COURSE in {a.type for a in new} + + def test_severe_error_not_immediate(self): + """Large error fires OFF_COURSE immediately but not SEVERE until timer expires.""" + thr = AlarmThresholds(severe_off_course_deg=30.0, severe_off_course_time_s=5.0) + eng = AlarmEngine(thresholds=thr) + snap = _engaged_snap(heading_deg=125.0, heading_setpoint_deg=90.0) # 35 deg + new = eng.evaluate(snap, dt_s=0.1) + types = {a.type for a in new} + # Severe timer hasn't elapsed yet + assert AlarmType.OFF_COURSE_SEVERE not in types + + def test_severe_error_fires_after_timer(self): + thr = AlarmThresholds(severe_off_course_deg=30.0, severe_off_course_time_s=1.0) + eng = AlarmEngine(thresholds=thr) + snap = _engaged_snap(heading_deg=125.0, heading_setpoint_deg=90.0) + result_types: set[AlarmType] = set() + for _ in range(15): # 15 × 0.1 s = 1.5 s > 1.0 s threshold + new = eng.evaluate(snap, dt_s=0.1) + result_types |= {a.type for a in new} + assert AlarmType.OFF_COURSE_SEVERE in result_types + + def test_fw_bit_suppresses_pc_off_course(self): + """When firmware asserts the alarm bit, PC-side must not duplicate it.""" + eng = AlarmEngine() + snap = _engaged_snap( + heading_deg=125.0, + heading_setpoint_deg=90.0, + fw_alarm_off_course=True, # firmware owns it + ) + eng.evaluate(snap) + # Should have exactly one OFF_COURSE (from fw bit), not two + active_types = [a.type for a in eng.active_alarms] + assert active_types.count(AlarmType.OFF_COURSE) == 1 + + def test_wraparound_heading(self): + """Shortest-arc logic: 5 deg east of 358 deg setpoint is +5, not -353.""" + eng = AlarmEngine() + snap = _engaged_snap(heading_deg=3.0, heading_setpoint_deg=358.0) # +5 deg arc + new = eng.evaluate(snap) + assert AlarmType.OFF_COURSE not in {a.type for a in new} + + def test_clear_all_resets_state(self): + eng = AlarmEngine() + eng.evaluate(TelemetrySnapshot(fw_alarm_off_course=True)) + eng.clear_all() + assert not eng.any_active + assert eng.highest_severity is None diff --git a/firmware/ar_autopilot_v1/src/main.cpp b/firmware/ar_autopilot_v1/src/main.cpp index b52bcef..412948e 100644 --- a/firmware/ar_autopilot_v1/src/main.cpp +++ b/firmware/ar_autopilot_v1/src/main.cpp @@ -20,6 +20,7 @@ // ============================================================================= #include +#include #include "hal/di_do.h" #include "hal/pinout.h" @@ -30,6 +31,7 @@ #include "pid/pid_outer_task.h" #include "protocols/modbus_slave.h" #include "protocols/nmea2000_consumer.h" +#include "protocols/nmea2000_publisher.h" #include "safety/safety_monitor.h" #include "safety/watchdog.h" #include "system/ar_log.h" @@ -74,6 +76,12 @@ void setup() { // watchdog is armed before slower subsystems come up. arautopilot::safety::watchdog_init(); + // Sprint 6: if the last reset was a task-watchdog trip, set the sticky + // alarm bit so the display knows the MCU was force-reset by the WDT. + if (esp_reset_reason() == ESP_RST_TASK_WDT) { + arautopilot::safety::safety_set_watchdog_tripped(); + } + AR_LOGI(TAG, "spawning Sprint 1 tasks ..."); arautopilot::safety::safety_monitor_start_task(); @@ -95,10 +103,14 @@ void setup() { arautopilot::protocols::modbus::modbus_slave_init(); arautopilot::protocols::modbus::modbus_slave_start(); - // NMEA 2000 consumer (PGN 127250 + 127251 in Sprint 1). + // NMEA 2000 consumer (PGN 127250 + 127251 in Sprint 1; 129026/129284 in Sprint 5). arautopilot::protocols::nmea2000::nmea2000_consumer_init(); arautopilot::protocols::nmea2000::nmea2000_consumer_start_task(); + // NMEA 2000 publisher (Sprint 6): PGN 127245 rudder + PGN 127237 HTC. + // Must be started AFTER nmea2000_consumer_init() (shared CAN stack). + arautopilot::protocols::nmea2000::nmea2000_publisher_start_task(); + AR_LOGI(TAG, "setup() complete; control loop is FreeRTOS-driven."); AR_LOGI(TAG, "current mode: STANDBY (helm is manual)"); } diff --git a/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp b/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp index 361b6e1..0ca7304 100644 --- a/firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp +++ b/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; } diff --git a/firmware/ar_autopilot_v1/src/protocols/nmea2000_publisher.cpp b/firmware/ar_autopilot_v1/src/protocols/nmea2000_publisher.cpp new file mode 100644 index 0000000..035a9ae --- /dev/null +++ b/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 +#include + +#include +#include + +// 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 diff --git a/firmware/ar_autopilot_v1/src/protocols/nmea2000_publisher.h b/firmware/ar_autopilot_v1/src/protocols/nmea2000_publisher.h new file mode 100644 index 0000000..21b49e1 --- /dev/null +++ b/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 diff --git a/firmware/ar_autopilot_v1/src/safety/safety_monitor.cpp b/firmware/ar_autopilot_v1/src/safety/safety_monitor.cpp index 71d9fd7..4c6a05d 100644 --- a/firmware/ar_autopilot_v1/src/safety/safety_monitor.cpp +++ b/firmware/ar_autopilot_v1/src/safety/safety_monitor.cpp @@ -1,15 +1,20 @@ // ============================================================================= -// safety/safety_monitor.cpp -- 50 Hz safety task +// safety/safety_monitor.cpp -- 50 Hz safety task (Sprint 6) // ============================================================================= #include "safety_monitor.h" #include +#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 "../pid/pid_inner_task.h" +#include "../pid/pid_outer_task.h" +#include "../protocols/nmea2000_consumer.h" #include "../system/ar_log.h" #include "../system/task_config.h" #include "watchdog.h" @@ -17,49 +22,213 @@ namespace arautopilot::safety { namespace { + constexpr const char* TAG = "AR/SAFE"; +// ----- Thresholds (mirrors pinout.h constants and brief section 7) ----------- +// Off-course +constexpr float OFF_COURSE_DEG = AR_DEFAULT_OFF_COURSE_DEG; +constexpr float SEVERE_OFF_COURSE_DEG = AR_SEVERE_OFF_COURSE_DEG; +constexpr uint32_t SEVERE_HOLD_MS = AR_SEVERE_OFF_COURSE_HOLD_MS; + +// Rudder not responding: setpoint must have moved by this much before we check +constexpr float RUDDER_DEADBAND_DEG = 1.0f; +// If setpoint moves outside deadband and rudder doesn't follow within this, alarm +constexpr uint32_t RUDDER_TIMEOUT_MS = 3000; + +// Battery / current ADC (linear: 0..4095 raw → 0..3.3 V; then scaled by +// voltage divider / shunt amplifier on the PCB). +// Calibration: V_bat = raw * 3.3 / 4095 * 6.0 (1:6 voltage divider assumed) +constexpr float BATTERY_SCALE = 3.3f * 6.0f / 4095.0f; +// Current: I_act = raw * 3.3 / 4095 * 50.0 (example transducer) +constexpr float CURRENT_SCALE = 3.3f * 50.0f / 4095.0f; +// Alarm thresholds +constexpr float VOLTAGE_LOW_V = 10.5f; +constexpr float CURRENT_HIGH_A = 30.0f; + +// ----- Shared alarm state ---------------------------------------------------- +portMUX_TYPE g_alarm_mux = portMUX_INITIALIZER_UNLOCKED; +AlarmBits g_alarms = {}; + +// ----- Rudder-not-responding state ------------------------------------------- +float g_rnr_setpoint_snapshot = 0.0f; // setpoint at timer start +uint32_t g_rnr_timer_start_ms = 0; +bool g_rnr_timer_running = false; + +// ----- Severe off-course timer ----------------------------------------------- +uint32_t g_severe_oc_start_ms = 0; +bool g_severe_oc_timer_running = false; + +// ----- Helpers --------------------------------------------------------------- + +inline float shortest_arc_deg(float sp, float meas) { + float e = sp - meas; + while (e > 180.0f) e -= 360.0f; + while (e < -180.0f) e += 360.0f; + return e; +} + +void update_alarms(const uint32_t now) { + using namespace arautopilot; + + AlarmBits bits = {}; + + const bool engaged = !modes::is_standby(); + + // ----- Heading lost ------------------------------------------------------ + bits.heading_lost = engaged && protocols::nmea2000::nmea2000_is_stale(); + + // ----- VMS critical (DI4) ------------------------------------------------ + bits.vms_critical = hal::di_read(PIN_DI4_EXTERNAL_ALARM); + + // ----- Limit switch reached (DI2 or DI3) --------------------------------- + bits.limit_reached = hal::di_read(PIN_DI2_LIMIT_SWITCH_PORT) + || hal::di_read(PIN_DI3_LIMIT_SWITCH_STBD); + + // ----- Off-course (outer-loop error) ------------------------------------- + // Requires pilot to be actively steering a heading. + if (engaged) { + // pid_outer_last_error_deg() is signed shortest-arc (setpoint - meas). + const float err_abs = fabsf(pid::pid_outer_last_error_deg()); + + if (err_abs >= SEVERE_OFF_COURSE_DEG) { + bits.off_course = true; // severe also implies off_course + if (!g_severe_oc_timer_running) { + g_severe_oc_start_ms = now; + g_severe_oc_timer_running = true; + } + if ((now - g_severe_oc_start_ms) >= SEVERE_HOLD_MS) { + bits.off_course_severe = true; + } + } else { + g_severe_oc_timer_running = false; + if (err_abs >= OFF_COURSE_DEG) { + bits.off_course = true; + } + } + } else { + g_severe_oc_timer_running = false; + } + + // ----- Rudder not responding --------------------------------------------- + // We watch whether the inner-loop rudder setpoint has moved (i.e. outer + // loop or manual command is asking for motion) and whether the rudder + // sensor follows within RUDDER_TIMEOUT_MS. + if (engaged) { + const float sp = pid::pid_inner_setpoint_deg(); + const auto rdr = hal::rudder_sensor_latest(); + + if (!g_rnr_timer_running) { + // Start timer if setpoint is outside deadband from actual angle. + if (rdr.valid && + fabsf(sp - rdr.angle_deg) > RUDDER_DEADBAND_DEG) { + g_rnr_setpoint_snapshot = sp; + g_rnr_timer_start_ms = now; + g_rnr_timer_running = true; + } + } else { + // Timer running: check if rudder has caught up. + if (!rdr.valid || + fabsf(rdr.angle_deg - g_rnr_setpoint_snapshot) < RUDDER_DEADBAND_DEG) { + // Rudder moved (or sensor gone invalid -- don't alarm on that). + g_rnr_timer_running = false; + } else if ((now - g_rnr_timer_start_ms) >= RUDDER_TIMEOUT_MS) { + bits.rudder_not_resp = true; + } + } + } else { + g_rnr_timer_running = false; + } + + // ----- Battery voltage --------------------------------------------------- + { + const int raw = analogRead(PIN_AI2_BATTERY_VOLTAGE); + const float vbat = (float)raw * BATTERY_SCALE; + bits.voltage_low = (vbat < VOLTAGE_LOW_V); + } + + // ----- Actuator current -------------------------------------------------- + { + const int raw = analogRead(PIN_AI3_ACTUATOR_CURRENT); + const float iact = (float)raw * CURRENT_SCALE; + bits.actuator_overcurr = (iact > CURRENT_HIGH_A); + } + + // ----- Watchdog tripped (sticky, set on boot) ---------------------------- + // Preserved across the update so we don't clear it here. + portENTER_CRITICAL(&g_alarm_mux); + bits.watchdog_tripped = g_alarms.watchdog_tripped; + g_alarms = bits; + portEXIT_CRITICAL(&g_alarm_mux); +} + +// ----- Actions triggered by alarms ------------------------------------------ + +void react_to_alarms(const AlarmBits& bits) { + // EMERGENCY conditions that require immediate forced-STANDBY. + if (bits.off_course_severe || bits.rudder_not_resp || + bits.heading_lost || bits.watchdog_tripped || + bits.vms_critical) { + + if (!modes::is_standby()) { + AR_LOGE(TAG, + "emergency alarm asserted -- forcing STANDBY " + "(oc_severe=%d rnr=%d hdg_lost=%d wdog=%d vms=%d)", + (int)bits.off_course_severe, (int)bits.rudder_not_resp, + (int)bits.heading_lost, (int)bits.watchdog_tripped, + (int)bits.vms_critical); + modes::force_standby("safety alarm"); + hal::rudder_actuator_power_off(); + } + } + + // Actuator overcurrent / voltage low → also disengage (HIGH severity). + if (bits.actuator_overcurr || bits.voltage_low) { + if (!modes::is_standby()) { + AR_LOGE(TAG, + "power alarm -- forcing STANDBY (overcurr=%d vlow=%d)", + (int)bits.actuator_overcurr, (int)bits.voltage_low); + modes::force_standby("power alarm"); + hal::rudder_actuator_power_off(); + } + } + + // Audible buzzer: any active alarm. + hal::do_write(PIN_DO4_BUZZER, bits.any()); +} + +// ----- FreeRTOS task --------------------------------------------------------- + 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(); + const uint32_t now = millis(); - // ---- 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. + // ---- DI1: Engage/Disengage physical button -------------------------- 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. + // ---- Both limit switches: cut power (wiring fault) ------------------ 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?)"); + "both limit switches asserted simultaneously -- " + "cutting actuator power (wiring fault?)"); hal::rudder_actuator_power_off(); } + // ---- Sprint 6: evaluate full alarm catalogue ------------------------ + update_alarms(now); + react_to_alarms(safety_alarm_bits()); + watchdog_feed(); vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_SAFETY)); } @@ -67,6 +236,10 @@ void SafetyTask(void* /*pv*/) { } // namespace +// ----------------------------------------------------------------------------- +// Public API +// ----------------------------------------------------------------------------- + void safety_monitor_start_task() { xTaskCreatePinnedToCore(SafetyTask, "safety_monitor", AR_TASK_STACK_SAFETY, nullptr, @@ -74,4 +247,19 @@ void safety_monitor_start_task() { AR_TASK_CORE_REALTIME); } +AlarmBits safety_alarm_bits() { + AlarmBits copy; + portENTER_CRITICAL(&g_alarm_mux); + copy = g_alarms; + portEXIT_CRITICAL(&g_alarm_mux); + return copy; +} + +void safety_set_watchdog_tripped() { + portENTER_CRITICAL(&g_alarm_mux); + g_alarms.watchdog_tripped = true; + portEXIT_CRITICAL(&g_alarm_mux); + AR_LOGW(TAG, "watchdog tripped flag set (reset reason: TASK_WDT)"); +} + } // 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 index a57f16c..81a2b7c 100644 --- a/firmware/ar_autopilot_v1/src/safety/safety_monitor.h +++ b/firmware/ar_autopilot_v1/src/safety/safety_monitor.h @@ -6,21 +6,54 @@ // - 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 limit switches: cuts actuator power if both assert. // - 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.). +// Sprint 6: full alarm catalogue -- off-course, rudder not responding, +// heading lost, actuator overcurrent, voltage low, limit switch reached, +// watchdog tripped, VMS critical. +// +// Alarm bits are written by the 50 Hz safety task and read on demand by the +// Modbus slave (FC 0x02 discrete inputs). All reads/writes are protected by +// a portMUX critical section. // ============================================================================= #pragma once +#include + namespace arautopilot::safety { -/// Spawn the safety monitor task. Must be called AFTER di_init() and -/// rudder_actuator_init(). +/// Spawn the safety monitor task. Must be called AFTER di_init(), +/// rudder_sensor_start_task(), and pid_outer_task_start(). void safety_monitor_start_task(); +/// Live alarm bits -- true when the corresponding condition is active. +/// Updated at 50 Hz by the safety task. Read by the Modbus slave handler. +struct AlarmBits { + bool off_course = false; + bool off_course_severe = false; + bool rudder_not_resp = false; + bool heading_lost = false; + bool actuator_overcurr = false; + bool voltage_low = false; + bool limit_reached = false; + bool watchdog_tripped = false; + bool vms_critical = false; + + bool any() const { + return off_course | off_course_severe | rudder_not_resp + | heading_lost | actuator_overcurr | voltage_low + | limit_reached | watchdog_tripped | vms_critical; + } +}; + +/// Return a thread-safe snapshot of the current alarm bits. +AlarmBits safety_alarm_bits(); + +/// Force-set the watchdog-tripped bit. Called once at boot if +/// esp_reset_reason() == ESP_RST_TASK_WDT. +void safety_set_watchdog_tripped(); + } // namespace arautopilot::safety