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

feat(firmware): BNO085 node, concentrador y listener PGN 127237

Browse Source 
AR_electronics — AR-Autopilot Project

Tarea #3 — firmware/ar_bno085_node_v1/
  Nodo compacto ESP32+CAN lee heading y yaw rate del BNO085 via I2C
  y los publica en el backbone NMEA 2000 como PGN 127250+127251 a 10 Hz.

Tarea #4 — firmware/ar_concentrador_v1/
  Gateway NMEA2000<->NMEA0183 con 4 puertos OUT (UART1 TX broadcast)
  y 4 puertos IN (UART2 RX comandos). Parsea sentencias $PARP, gestiona
  autoridad de mando entre estaciones con timeout 10s y override del puente.
  Reenvía comandos autopilot como PGN 127237 al backbone.

Tarea #5 — nmea2000_consumer (ar_autopilot_v1)
  Listener PGN 127237 entrante desde concentrador:
  HeadingControlSnapshot, HandleHeadingControl() con filtro source address,
  nmea2000_htc() publico, ventana stale 3s para comandos externos.
This commit is contained in:
Alvaro Enrique Romero Donado
2026-05-23 11:00:22 -04:00
parent 4cc5b19f0c
commit 080e47efc0
12 changed files with 1191 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp
+87 -4
View File
@@ -51,6 +51,17 @@ NavDataSnapshot g_nav_data{
.valid = false,
};
HeadingControlSnapshot g_htc{
.commanded_heading_deg = NAN,
.mode = HtcMode::UNKNOWN,
.source_addr = 0xFF,
.age_ms = 0,
.valid = false,
};
// Our own NMEA2000 source address — filter out self-echoes.
constexpr uint8_t OWN_SOURCE_ADDR = 25;
float rad_to_deg_pos(float rad) {
float d = rad * (180.0f / (float)M_PI);
// Normalise to 0..360.
@@ -155,12 +166,67 @@ void HandleNavData(const tN2kMsg& msg) {
AR_LOGV(TAG, "PGN 129284 XTE=%.2f m DTW=%.0f m", xte_m, dtw_m);
}
void HandleHeadingControl(const tN2kMsg& msg) {
// Ignore our own broadcasts — concentrador uses a different source address.
if (msg.Source == OWN_SOURCE_ADDR) return;
tN2kOnOff rud_lim, off_hdg, off_trk, override_st;
tN2kSteeringMode steering_mode;
tN2kTurnMode turn_mode;
tN2kHeadingReference hdg_ref;
tN2kRudderDirectionOrder cmd_rud_dir;
double cmd_rud_angle = N2kDoubleNA;
double hdg_to_steer = N2kDoubleNA;
double track = N2kDoubleNA;
double rud_limit = N2kDoubleNA;
double off_hdg_limit = N2kDoubleNA;
double radius = N2kDoubleNA;
double rot_order = N2kDoubleNA;
double xte_lim = N2kDoubleNA;
double vessel_hdg = N2kDoubleNA;
if (!ParseN2kHeadingTrackControl(msg,
rud_lim, off_hdg, off_trk, override_st,
steering_mode, turn_mode, hdg_ref,
cmd_rud_dir, cmd_rud_angle,
hdg_to_steer, track,
rud_limit, off_hdg_limit, radius, rot_order,
xte_lim, vessel_hdg)) {
return;
}
// Map NMEA2000 steering mode to our internal enum.
HtcMode our_mode;
switch (steering_mode) {
case N2kSM_HeadingControl: our_mode = HtcMode::HEADING_HOLD; break;
case N2kSM_MainSteering: our_mode = HtcMode::STANDBY; break;
default: return; // ignore unsupported modes
}
const float hdg_deg = (hdg_to_steer < 1e8)
? rad_to_deg_pos((float)hdg_to_steer)
: NAN;
const uint32_t now = millis();
portENTER_CRITICAL(&g_mux);
g_htc.commanded_heading_deg = hdg_deg;
g_htc.mode = our_mode;
g_htc.source_addr = msg.Source;
g_htc.age_ms = now;
g_htc.valid = true;
portEXIT_CRITICAL(&g_mux);
AR_LOGI(TAG, "PGN 127237 src=%d mode=%d hdg=%.2f deg",
msg.Source, (int)our_mode, hdg_deg);
}
void MessageHandler(const tN2kMsg& msg) {
switch (msg.PGN) {
case 127250L: HandleHeading(msg); break;
case 127251L: HandleROT(msg); break;
case 129026L: HandleCogSog(msg); break;
case 129284L: HandleNavData(msg); break;
case 127237L: HandleHeadingControl(msg); break;
case 127250L: HandleHeading(msg); break;
case 127251L: HandleROT(msg); break;
case 129026L: HandleCogSog(msg); break;
case 129284L: HandleNavData(msg); break;
default: break;
}
}
@@ -186,6 +252,11 @@ void RxTask(void* /*pv*/) {
if (g_nav_data.valid && (now - g_nav_data.age_ms) > STALE_THRESHOLD_MS) {
g_nav_data.valid = false;
}
// HTC commands have a shorter stale window (3 s) — if the remote stops
// sending, we don't want to keep acting on an old command.
if (g_htc.valid && (now - g_htc.age_ms) > 3000U) {
g_htc.valid = false;
}
portEXIT_CRITICAL(&g_mux);
// 100 Hz polling is plenty -- the CAN driver buffers incoming frames.
vTaskDelay(pdMS_TO_TICKS(10));
@@ -262,4 +333,16 @@ bool nmea2000_cog_is_stale() {
return !nmea2000_cog_sog().valid;
}
HeadingControlSnapshot nmea2000_htc() {
HeadingControlSnapshot copy;
portENTER_CRITICAL(&g_mux);
copy = g_htc;
portEXIT_CRITICAL(&g_mux);
return copy;
}
bool nmea2000_htc_is_stale() {
return !nmea2000_htc().valid;
}
} // namespace arautopilot::protocols::nmea2000
firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h
+26
View File
@@ -53,6 +53,25 @@ struct NavDataSnapshot {
bool valid; ///< fresh (<5 s) and non-NaN
};
// ---------------------------------------------------------------------------
// PGN 127237 -- Heading Track Control (incoming command from concentrador)
// ---------------------------------------------------------------------------
enum class HtcMode : uint8_t {
STANDBY = 0, ///< Autopilot off / manual steering
HEADING_HOLD = 1, ///< Hold commanded heading
TRACK = 2, ///< Track to waypoint (NMEA route)
UNKNOWN = 0xFF,
};
struct HeadingControlSnapshot {
float commanded_heading_deg; ///< 0..360 (NAN if not set)
HtcMode mode; ///< desired autopilot mode
uint8_t source_addr; ///< NMEA2000 source address of sender
uint32_t age_ms; ///< millis() at last 127237 update
bool valid; ///< fresh (<3 s) and parsed correctly
};
// ---------------------------------------------------------------------------
// Public API
// ---------------------------------------------------------------------------
@@ -78,4 +97,11 @@ bool nmea2000_is_stale();
/// True if COG/SOG age exceeds 5 s or data was never received.
bool nmea2000_cog_is_stale();
/// Thread-safe read of the latest Heading Track Control command (PGN 127237).
/// Returns the last command received from an external concentrador.
HeadingControlSnapshot nmea2000_htc();
/// True if no PGN 127237 has been received in the last 3 s.
bool nmea2000_htc_is_stale();
} // namespace arautopilot::protocols::nmea2000