alro65
080e47efc0
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.
349 lines
11 KiB
C++
349 lines
11 KiB
C++
// =============================================================================
|
|
// nmea2000_consumer.cpp -- NMEA 2000 backbone consumer
|
|
// =============================================================================
|
|
|
|
#include "nmea2000_consumer.h"
|
|
|
|
#include <Arduino.h>
|
|
|
|
// Override the default ESP32 CAN pins (16/4) to match our AR-NMEA-IO board.
|
|
#include "../hal/pinout.h"
|
|
#define ESP32_CAN_TX_PIN ((gpio_num_t)PIN_CAN_TX)
|
|
#define ESP32_CAN_RX_PIN ((gpio_num_t)PIN_CAN_RX)
|
|
|
|
#include <NMEA2000_CAN.h>
|
|
#include <N2kMessages.h>
|
|
|
|
#include "../system/ar_log.h"
|
|
#include "../system/task_config.h"
|
|
|
|
namespace arautopilot::protocols::nmea2000 {
|
|
|
|
namespace {
|
|
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,
|
|
.rate_of_turn_dps = 0.0f,
|
|
.heading_age_ms = 0,
|
|
.rot_age_ms = 0,
|
|
.heading_valid = false,
|
|
.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,
|
|
};
|
|
|
|
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.
|
|
while (d < 0.0f) d += 360.0f;
|
|
while (d >= 360.0f) d -= 360.0f;
|
|
return d;
|
|
}
|
|
|
|
float rad_to_deg_signed(float rad) {
|
|
return rad * (180.0f / (float)M_PI);
|
|
}
|
|
|
|
void HandleHeading(const tN2kMsg& msg) {
|
|
unsigned char sid;
|
|
double heading = 0.0, deviation = 0.0, variation = 0.0;
|
|
tN2kHeadingReference ref = N2khr_Unavailable;
|
|
if (!ParseN2kHeading(msg, sid, heading, deviation, variation, ref)) {
|
|
return;
|
|
}
|
|
if (heading > 1e8) {
|
|
// N2k "unavailable" markers come through as huge doubles.
|
|
return;
|
|
}
|
|
const float heading_deg = rad_to_deg_pos((float)heading);
|
|
const bool is_true = (ref == N2khr_true);
|
|
const uint32_t now = millis();
|
|
portENTER_CRITICAL(&g_mux);
|
|
g_snap.heading_deg = heading_deg;
|
|
g_snap.is_true = is_true;
|
|
g_snap.heading_age_ms = now;
|
|
g_snap.heading_valid = true;
|
|
portEXIT_CRITICAL(&g_mux);
|
|
AR_LOGV(TAG, "PGN 127250 heading=%.2f deg (%s)", heading_deg,
|
|
is_true ? "true" : "magnetic");
|
|
}
|
|
|
|
void HandleROT(const tN2kMsg& msg) {
|
|
unsigned char sid;
|
|
double rot = 0.0;
|
|
if (!ParseN2kRateOfTurn(msg, sid, rot)) {
|
|
return;
|
|
}
|
|
if (rot > 1e8 || rot < -1e8) {
|
|
return;
|
|
}
|
|
const float rot_dps = rad_to_deg_signed((float)rot);
|
|
const uint32_t now = millis();
|
|
portENTER_CRITICAL(&g_mux);
|
|
g_snap.rate_of_turn_dps = rot_dps;
|
|
g_snap.rot_age_ms = now;
|
|
g_snap.rot_valid = true;
|
|
portEXIT_CRITICAL(&g_mux);
|
|
AR_LOGV(TAG, "PGN 127251 ROT=%.3f deg/s", rot_dps);
|
|
}
|
|
|
|
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 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 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;
|
|
}
|
|
}
|
|
|
|
void RxTask(void* /*pv*/) {
|
|
AR_LOGI(TAG, "nmea2000_consumer task started on core %d", xPortGetCoreID());
|
|
for (;;) {
|
|
NMEA2000.ParseMessages();
|
|
const uint32_t now = millis();
|
|
portENTER_CRITICAL(&g_mux);
|
|
if (g_snap.heading_valid && (now - g_snap.heading_age_ms) > STALE_THRESHOLD_MS) {
|
|
g_snap.heading_valid = false;
|
|
}
|
|
if (g_snap.rot_valid && (now - g_snap.rot_age_ms) > STALE_THRESHOLD_MS) {
|
|
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;
|
|
}
|
|
// 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));
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
void nmea2000_consumer_init() {
|
|
NMEA2000.SetProductInformation(
|
|
"AR-AP-1", // Manufacturer's Model serial code
|
|
100, // Manufacturer's product code
|
|
"AR-Autopilot Controller v1", // Model ID
|
|
AR_FW_VERSION, // SW version
|
|
"AR-NMEA-IO v1.0" // Model version
|
|
);
|
|
NMEA2000.SetDeviceInformation(
|
|
1, // Unique number
|
|
150, // Device function: Autopilot
|
|
40, // Device class: Steering and Control surfaces
|
|
2046 // Manufacturer code: reserved/test (real one needs NMEA registration)
|
|
);
|
|
NMEA2000.SetMode(tNMEA2000::N2km_ListenAndNode, 25);
|
|
NMEA2000.EnableForward(false);
|
|
NMEA2000.SetMsgHandler(&MessageHandler);
|
|
|
|
if (NMEA2000.Open()) {
|
|
AR_LOGI(TAG,
|
|
"nmea2000_consumer_init: stack open, CAN TX=%d RX=%d, "
|
|
"subscribed to PGN 127250 + 127251",
|
|
PIN_CAN_TX, PIN_CAN_RX);
|
|
} else {
|
|
AR_LOGE(TAG,
|
|
"nmea2000_consumer_init: NMEA2000.Open() failed (CAN driver "
|
|
"init error). System will continue but no N2K traffic.");
|
|
}
|
|
}
|
|
|
|
void nmea2000_consumer_start_task() {
|
|
xTaskCreatePinnedToCore(RxTask, "n2k_rx", AR_TASK_STACK_N2K_RX, nullptr,
|
|
AR_TASK_PRIO_N2K_RX, nullptr, AR_TASK_CORE_COMMS);
|
|
}
|
|
|
|
HeadingSnapshot nmea2000_latest() {
|
|
HeadingSnapshot copy;
|
|
portENTER_CRITICAL(&g_mux);
|
|
copy = g_snap;
|
|
portEXIT_CRITICAL(&g_mux);
|
|
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;
|
|
}
|
|
|
|
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
|