alro65
e82dbc449c
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>
128 lines
4.1 KiB
C++
128 lines
4.1 KiB
C++
// =============================================================================
|
|
// 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
|