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