alro65/AR-Autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
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>
This commit is contained in:
Alvaro Enrique Romero Donado
2026-05-20 00:16:24 -04:00
parent 0f00ad10da
commit e82dbc449c
8 changed files with 965 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
firmware/ar_autopilot_v1/src/main.cpp
+13 -1
View File
@@ -20,6 +20,7 @@
// =============================================================================
#include <Arduino.h>
#include <esp_system.h>
#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)");
}
firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp
+30 -17
View File
@@ -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
+127
View File
@@ -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
+31
View File
@@ -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
firmware/ar_autopilot_v1/src/safety/safety_monitor.cpp
+209 -21
View File
@@ -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 <Arduino.h>
#include <esp_system.h>
#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
firmware/ar_autopilot_v1/src/safety/safety_monitor.h
+39 -6
View File
@@ -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 <cstdint>
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