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/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;
}