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

sprint-5: True Course + Track Keeping + XTE + PGN 129026/129284

Browse Source 
- Python: NmeaNavData (COG/SOG/XTE data models with staleness tracking)
- Python: TrueCoursePilot with TRUE_COURSE and TRACK_KEEPING modes
- Python: 26 new tests (test_nmea_data, test_true_course)
- Modbus: COG/SOG/XTE input registers + TC setpoint/XTE-gain holdings
- Firmware: nmea2000_consumer handles PGN 129026 + 129284
- Firmware: pid_outer_task wired for TC + TK modes with live SOG scheduling
- YAML regenerated; 284 tests pass, firmware compiles clean

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Alvaro Enrique Romero Donado
2026-05-19 20:12:57 -04:00
parent 0be60c5161
commit 0f00ad10da
12 changed files with 1051 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
firmware/ar_autopilot_v1/src/protocols/modbus_registers.h
+31 -4
View File
@@ -77,8 +77,8 @@ constexpr uint16_t COIL_CMD_ACK_ALL_ALARMS = 2;
constexpr uint16_t COIL_CMD_KNOB_ARM = 3;
// ----- Input registers (read-only words) -----
constexpr uint16_t INPUT_COUNT = 30;
constexpr uint16_t INPUT_MAX_ADDR = 56;
constexpr uint16_t INPUT_COUNT = 36;
constexpr uint16_t INPUT_MAX_ADDR = 65;
// Firmware major version
constexpr uint16_t INPUT_FW_VERSION_MAJOR = 0;
@@ -164,10 +164,28 @@ constexpr uint16_t INPUT_PID_OUTER_KI_X1000 = 55;
// Outer-loop active kd * 1000
// scale=0.001
constexpr uint16_t INPUT_PID_OUTER_KD_X1000 = 56;
// Course Over Ground (true) from PGN 129026, deg*100 (0..35999)
// unit=deg, scale=0.01
constexpr uint16_t INPUT_COG_DEG_X100 = 60;
// Speed Over Ground from PGN 129026, knots*10 (unsigned)
// unit=kn, scale=0.1
constexpr uint16_t INPUT_SOG_KN_X10 = 61;
// Milliseconds since last COG/SOG update (0..60000)
// unit=ms
constexpr uint16_t INPUT_COG_AGE_MS = 62;
// Cross Track Error from PGN 129284, decimetres (signed int16, +stbd/-port)
// unit=dm, scale=0.1
constexpr uint16_t INPUT_XTE_DM_SIGNED = 63;
// Milliseconds since last XTE update (0..60000)
// unit=ms
constexpr uint16_t INPUT_XTE_AGE_MS = 64;
// Distance to next waypoint, metres (unsigned int16, 0..65535)
// unit=m
constexpr uint16_t INPUT_DTW_M = 65;
// ----- Holding registers (read-write words) -----
constexpr uint16_t HOLDING_COUNT = 14;
constexpr uint16_t HOLDING_MAX_ADDR = 28;
constexpr uint16_t HOLDING_COUNT = 17;
constexpr uint16_t HOLDING_MAX_ADDR = 34;
// Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)
constexpr uint16_t HOLDING_MODE_REQUEST = 0;
@@ -210,5 +228,14 @@ constexpr uint16_t HOLDING_PID_OUTER_KI_REQ_X1000 = 27;
// Requested outer-loop base kd * 1000
// scale=0.001
constexpr uint16_t HOLDING_PID_OUTER_KD_REQ_X1000 = 28;
// Desired COG setpoint for TRUE_COURSE / TRACK_KEEPING, deg*100 (0..35999)
// unit=deg, scale=0.01
constexpr uint16_t HOLDING_TRUE_COURSE_SP_X100 = 32;
// XTE correction gain * 1000 (deg of heading correction per metre of XTE)
// scale=0.001
constexpr uint16_t HOLDING_XTE_GAIN_X1000 = 33;
// Maximum XTE heading correction, deg*100 (default 2000 = 20 deg)
// unit=deg, scale=0.01
constexpr uint16_t HOLDING_XTE_MAX_CORRECTION_X100 = 34;
} // namespace arautopilot::protocols::modbus
firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp
+71
View File
@@ -66,6 +66,10 @@ struct HoldingStorage {
uint16_t pid_outer_kp_req_x1000 = 0;
uint16_t pid_outer_ki_req_x1000 = 0;
uint16_t pid_outer_kd_req_x1000 = 0;
// Sprint 5: True Course + Track Keeping
uint16_t true_course_sp_x100 = 0;
uint16_t xte_gain_x1000 = 500; // 0.5 deg/m default
uint16_t xte_max_correction_x100 = 2000; // 20.0 deg default
};
HoldingStorage g_holding;
@@ -209,6 +213,51 @@ uint16_t read_input_register(uint16_t addr) {
return (uint16_t)scaled;
}
// ----- Sprint 5: COG / SOG / XTE telemetry -----
case INPUT_COG_DEG_X100: {
auto cs = nmea2000::nmea2000_cog_sog();
if (!cs.valid) return 0;
int v = (int)(cs.cog_deg * 100.0f);
if (v < 0) v = 0;
if (v > 35999) v = 35999;
return (uint16_t)v;
}
case INPUT_SOG_KN_X10: {
auto cs = nmea2000::nmea2000_cog_sog();
if (!cs.valid) return 0;
int v = (int)(cs.sog_kn * 10.0f);
if (v < 0) v = 0;
if (v > 65535) v = 65535;
return (uint16_t)v;
}
case INPUT_COG_AGE_MS: {
auto cs = nmea2000::nmea2000_cog_sog();
uint32_t age = cs.valid ? (millis() - cs.age_ms) : 60000;
if (age > 60000) age = 60000;
return (uint16_t)age;
}
case INPUT_XTE_DM_SIGNED: {
auto nd = nmea2000::nmea2000_nav_data();
if (!nd.valid) return 0;
int v = (int)(nd.xte_m * 10.0f); // metres → decimetres
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_XTE_AGE_MS: {
auto nd = nmea2000::nmea2000_nav_data();
uint32_t age = nd.valid ? (millis() - nd.age_ms) : 60000;
if (age > 60000) age = 60000;
return (uint16_t)age;
}
case INPUT_DTW_M: {
auto nd = nmea2000::nmea2000_nav_data();
if (!nd.valid) return 0;
uint32_t dtw = (uint32_t)nd.dtw_m;
if (dtw > 65535) dtw = 65535;
return (uint16_t)dtw;
}
default:
return 0;
}
@@ -258,6 +307,9 @@ uint16_t read_holding(uint16_t addr) {
case HOLDING_PID_OUTER_KP_REQ_X1000: return g_holding.pid_outer_kp_req_x1000;
case HOLDING_PID_OUTER_KI_REQ_X1000: return g_holding.pid_outer_ki_req_x1000;
case HOLDING_PID_OUTER_KD_REQ_X1000: return g_holding.pid_outer_kd_req_x1000;
case HOLDING_TRUE_COURSE_SP_X100: return g_holding.true_course_sp_x100;
case HOLDING_XTE_GAIN_X1000: return g_holding.xte_gain_x1000;
case HOLDING_XTE_MAX_CORRECTION_X100: return g_holding.xte_max_correction_x100;
default: return 0;
}
}
@@ -348,6 +400,25 @@ Modbus::Error write_holding(uint16_t addr, uint16_t value) {
pid::pid_inner_update_gains(kp, ki, kd);
return Modbus::Error::SUCCESS;
}
// ----- Sprint 5: True Course + XTE parameters -----
case HOLDING_TRUE_COURSE_SP_X100: {
if (value > 35999) return Modbus::Error::ILLEGAL_DATA_VALUE;
g_holding.true_course_sp_x100 = value;
pid::pid_outer_set_cog_setpoint_deg((float)value * 0.01f);
return Modbus::Error::SUCCESS;
}
case HOLDING_XTE_GAIN_X1000: {
g_holding.xte_gain_x1000 = value;
pid::pid_outer_set_xte_gain((float)value * 0.001f);
return Modbus::Error::SUCCESS;
}
case HOLDING_XTE_MAX_CORRECTION_X100: {
if (value > 9000) return Modbus::Error::ILLEGAL_DATA_VALUE; // 90 deg cap
g_holding.xte_max_correction_x100 = value;
pid::pid_outer_set_xte_max_correction((float)value * 0.01f);
return Modbus::Error::SUCCESS;
}
default:
return Modbus::Error::ILLEGAL_DATA_ADDRESS;
}
firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp
+99 -12
View File
@@ -25,6 +25,7 @@ constexpr const char* TAG = "AR/N2K";
constexpr uint32_t STALE_THRESHOLD_MS = 5000;
portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
HeadingSnapshot g_snap{
.heading_deg = 0.0f,
.is_true = false,
@@ -35,6 +36,21 @@ HeadingSnapshot g_snap{
.rot_valid = false,
};
CogSogSnapshot g_cog_sog{
.cog_deg = NAN,
.sog_kn = NAN,
.age_ms = 0,
.valid = false,
};
NavDataSnapshot g_nav_data{
.xte_m = NAN,
.dtw_m = NAN,
.wp_name = {},
.age_ms = 0,
.valid = false,
};
float rad_to_deg_pos(float rad) {
float d = rad * (180.0f / (float)M_PI);
// Normalise to 0..360.
@@ -90,19 +106,62 @@ void HandleROT(const tN2kMsg& msg) {
AR_LOGV(TAG, "PGN 127251 ROT=%.3f deg/s", rot_dps);
}
// One global dispatcher because NMEA2000.SetMsgHandler() takes a single
// callback that we have to filter by PGN ourselves.
void HandleCogSog(const tN2kMsg& msg) {
unsigned char sid;
tN2kHeadingReference ref;
double cog = 0.0, sog = 0.0;
if (!ParseN2kCOGSOGRapid(msg, sid, ref, cog, sog)) return;
if (cog > 1e8 || sog > 1e8) return;
const float cog_deg = rad_to_deg_pos((float)cog);
const float sog_kn = (float)(sog * 1.94384); // m/s → knots
const uint32_t now = millis();
portENTER_CRITICAL(&g_mux);
g_cog_sog.cog_deg = cog_deg;
g_cog_sog.sog_kn = sog_kn;
g_cog_sog.age_ms = now;
g_cog_sog.valid = true;
portEXIT_CRITICAL(&g_mux);
AR_LOGV(TAG, "PGN 129026 COG=%.2f deg SOG=%.2f kn", cog_deg, sog_kn);
}
void HandleNavData(const tN2kMsg& msg) {
unsigned char sid;
double dist = 0.0, xte = 0.0;
double origLat = 0.0, origLon = 0.0, destLat = 0.0, destLon = 0.0;
double closingVel = 0.0;
tN2kHeadingReference bearingRef = N2khr_Unavailable;
tN2kDistanceCalculationType calcType = N2kdct_GreatCircle;
bool perpCrossed = false, arrivalAlarm = false;
int16_t origWpNum = 0;
uint32_t destWpNum = 0, eta = 0;
if (!ParseN2kNavigationInfo(
msg, sid, dist, bearingRef,
perpCrossed, arrivalAlarm, calcType, xte,
origWpNum, origLat, origLon,
destWpNum, eta, destLat, destLon, closingVel)) {
return;
}
if (xte > 1e8 || xte < -1e8) return;
const float xte_m = (float)xte;
const float dtw_m = (dist < 1e8) ? (float)dist : NAN;
const uint32_t now = millis();
portENTER_CRITICAL(&g_mux);
g_nav_data.xte_m = xte_m;
g_nav_data.dtw_m = dtw_m;
g_nav_data.age_ms = now;
g_nav_data.valid = true;
portEXIT_CRITICAL(&g_mux);
AR_LOGV(TAG, "PGN 129284 XTE=%.2f m DTW=%.0f m", xte_m, dtw_m);
}
void MessageHandler(const tN2kMsg& msg) {
switch (msg.PGN) {
case 127250L:
HandleHeading(msg);
break;
case 127251L:
HandleROT(msg);
break;
default:
// Sprint 1: ignore everything else.
break;
case 127250L: HandleHeading(msg); break;
case 127251L: HandleROT(msg); break;
case 129026L: HandleCogSog(msg); break;
case 129284L: HandleNavData(msg); break;
default: break;
}
}
@@ -110,7 +169,6 @@ void RxTask(void* /*pv*/) {
AR_LOGI(TAG, "nmea2000_consumer task started on core %d", xPortGetCoreID());
for (;;) {
NMEA2000.ParseMessages();
// Update validity flags based on age.
const uint32_t now = millis();
portENTER_CRITICAL(&g_mux);
if (g_snap.heading_valid && (now - g_snap.heading_age_ms) > STALE_THRESHOLD_MS) {
@@ -120,6 +178,15 @@ void RxTask(void* /*pv*/) {
g_snap.rot_valid = false;
}
portEXIT_CRITICAL(&g_mux);
// Update COG/SOG and nav-data stale flags.
portENTER_CRITICAL(&g_mux);
if (g_cog_sog.valid && (now - g_cog_sog.age_ms) > STALE_THRESHOLD_MS) {
g_cog_sog.valid = false;
}
if (g_nav_data.valid && (now - g_nav_data.age_ms) > STALE_THRESHOLD_MS) {
g_nav_data.valid = false;
}
portEXIT_CRITICAL(&g_mux);
// 100 Hz polling is plenty -- the CAN driver buffers incoming frames.
vTaskDelay(pdMS_TO_TICKS(10));
}
@@ -170,9 +237,29 @@ HeadingSnapshot nmea2000_latest() {
return copy;
}
CogSogSnapshot nmea2000_cog_sog() {
CogSogSnapshot copy;
portENTER_CRITICAL(&g_mux);
copy = g_cog_sog;
portEXIT_CRITICAL(&g_mux);
return copy;
}
NavDataSnapshot nmea2000_nav_data() {
NavDataSnapshot copy;
portENTER_CRITICAL(&g_mux);
copy = g_nav_data;
portEXIT_CRITICAL(&g_mux);
return copy;
}
bool nmea2000_is_stale() {
const auto s = nmea2000_latest();
return !s.heading_valid;
}
bool nmea2000_cog_is_stale() {
return !nmea2000_cog_sog().valid;
}
} // namespace arautopilot::protocols::nmea2000
firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h
+53 -18
View File
@@ -2,34 +2,61 @@
// nmea2000_consumer.h -- NMEA 2000 backbone consumer
// =============================================================================
//
// Sprint 1 subscribes to PGN 127250 (Vessel Heading) and PGN 127251
// (Rate of Turn) from the boat's NMEA 2000 backbone. The latest values
// are stashed in a thread-safe snapshot that the Modbus slave (and later
// the PID outer loop) read from.
// Sprint 1: PGN 127250 (Heading) + 127251 (ROT).
// Sprint 5: PGN 129026 (COG/SOG) + 129284 (Navigation Data / XTE).
//
// Later sprints will also subscribe to PGN 129025/129029 (Position),
// 129026 (COG/SOG), 129284 (Navigation Data), 127257 (Attitude).
//
// This module owns the NMEA2000 instance and its dedicated FreeRTOS task
// pinned to Core 0.
// All snapshots are updated from the NMEA 2000 task (Core 0) and
// read from the PID outer-loop task (Core 1) via a portMUX spinlock.
// =============================================================================
#pragma once
#include <cstdint>
#include <cmath>
namespace arautopilot::protocols::nmea2000 {
// ---------------------------------------------------------------------------
// PGN 127250 / 127251 -- heading + ROT
// ---------------------------------------------------------------------------
struct HeadingSnapshot {
float heading_deg; ///< 0..360, magnetic or true depending on source
bool is_true; ///< true if reference is "true north", false if magnetic
float rate_of_turn_dps; ///< signed deg/s; positive = turning to starboard
uint32_t heading_age_ms; ///< millis() at the last 127250 update
uint32_t rot_age_ms; ///< millis() at the last 127251 update
bool heading_valid; ///< true if the heading is fresh (<5 s old)
bool rot_valid; ///< true if the ROT is fresh (<5 s old)
float heading_deg; ///< 0..360, magnetic or true per is_true flag
bool is_true; ///< true = true north reference
float rate_of_turn_dps; ///< signed deg/s; positive = turning to stbd
uint32_t heading_age_ms; ///< millis() at last 127250 update
uint32_t rot_age_ms; ///< millis() at last 127251 update
bool heading_valid; ///< fresh (<5 s)
bool rot_valid; ///< fresh (<5 s)
};
// ---------------------------------------------------------------------------
// PGN 129026 -- Course Over Ground (true) + Speed Over Ground
// ---------------------------------------------------------------------------
struct CogSogSnapshot {
float cog_deg; ///< 0..360, degrees true; NAN until first message
float sog_kn; ///< knots; NAN until first message
uint32_t age_ms; ///< millis() at last update
bool valid; ///< fresh (<5 s) and non-NaN
};
// ---------------------------------------------------------------------------
// PGN 129284 -- Navigation Data (XTE + waypoint)
// ---------------------------------------------------------------------------
struct NavDataSnapshot {
float xte_m; ///< cross-track error, metres; +stbd / -port; NAN if unknown
float dtw_m; ///< distance to waypoint, metres; NAN if unknown
char wp_name[16]; ///< null-terminated waypoint name (truncated at 15 chars)
uint32_t age_ms; ///< millis() at last update
bool valid; ///< fresh (<5 s) and non-NaN
};
// ---------------------------------------------------------------------------
// Public API
// ---------------------------------------------------------------------------
/// Initialise the NMEA2000 stack with our PGN handlers.
void nmea2000_consumer_init();
@@ -39,8 +66,16 @@ void nmea2000_consumer_start_task();
/// Thread-safe read of the latest heading + ROT snapshot.
HeadingSnapshot nmea2000_latest();
/// True if either heading_age_ms or rot_age_ms exceeds the stale-threshold
/// (default 5 s, brief section 7).
/// Thread-safe read of the latest COG/SOG snapshot (PGN 129026).
CogSogSnapshot nmea2000_cog_sog();
/// Thread-safe read of the latest navigation data snapshot (PGN 129284).
NavDataSnapshot nmea2000_nav_data();
/// True if heading age or ROT age exceeds 5 s.
bool nmea2000_is_stale();
/// True if COG/SOG age exceeds 5 s or data was never received.
bool nmea2000_cog_is_stale();
} // namespace arautopilot::protocols::nmea2000