feat(firmware): BNO085 node, concentrador y listener PGN 127237
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:
@@ -0,0 +1,52 @@
|
||||
; =============================================================================
|
||||
; AR Concentrador NMEA2000-USB v1 -- firmware build configuration
|
||||
; =============================================================================
|
||||
;
|
||||
; Target hardware: AR-CONCENTRADOR v1.0 (ESP32-DOWD)
|
||||
; Role: Bidirectional NMEA 2000 <-> NMEA 0183 USB gateway.
|
||||
;
|
||||
; UART1 TX (GPIO17) → 4x CH340N → USB-OUT1..4 (broadcast NMEA 0183 data)
|
||||
; UART2 RX (GPIO16) ← 4x CH340N ← USB-IN1..4 (receive $PARP commands)
|
||||
; CAN TX (GPIO21) → MCP2562T → NMEA 2000 backbone
|
||||
; CAN RX (GPIO22) ← MCP2562T ← NMEA 2000 backbone
|
||||
;
|
||||
; Protocol: docs/concentrador_protocol.md
|
||||
; =============================================================================
|
||||
|
||||
[platformio]
|
||||
src_dir = src
|
||||
default_envs = esp32-dev
|
||||
|
||||
[env]
|
||||
platform = espressif32@^6.7.0
|
||||
framework = arduino
|
||||
monitor_speed = 115200
|
||||
monitor_filters = esp32_exception_decoder, time
|
||||
build_flags =
|
||||
-std=gnu++17
|
||||
-DCORE_DEBUG_LEVEL=3
|
||||
-DAR_FW_VERSION=\"1.0.0\"
|
||||
-Wall
|
||||
-Wno-unused-parameter
|
||||
-Wno-missing-field-initializers
|
||||
build_unflags =
|
||||
-std=gnu++11
|
||||
lib_deps =
|
||||
ttlappalainen/NMEA2000-library@^4.22.0
|
||||
ttlappalainen/NMEA2000_esp32@^1.0.3
|
||||
|
||||
[env:esp32-dev]
|
||||
board = esp32dev
|
||||
build_type = release
|
||||
build_flags =
|
||||
${env.build_flags}
|
||||
-Os
|
||||
|
||||
[env:esp32-debug]
|
||||
board = esp32dev
|
||||
build_type = debug
|
||||
build_flags =
|
||||
${env.build_flags}
|
||||
-O0
|
||||
-ggdb
|
||||
-DCORE_DEBUG_LEVEL=5
|
||||
@@ -0,0 +1,188 @@
|
||||
// =============================================================================
|
||||
// AR Concentrador NMEA2000-USB v1 — main.cpp
|
||||
// =============================================================================
|
||||
//
|
||||
// Bidirectional NMEA 2000 <-> NMEA 0183 USB gateway with multi-station
|
||||
// command authority management.
|
||||
//
|
||||
// Hardware:
|
||||
// UART1 TX (GPIO17) → 4x CH340N → USB-OUT1..4 (broadcast NMEA 0183)
|
||||
// UART2 RX (GPIO16) ← 4x CH340N ← USB-IN1..4 (receive $PARP commands)
|
||||
// CAN TX (GPIO21) → MCP2562T → NMEA 2000 backbone
|
||||
// CAN RX (GPIO22) ← MCP2562T ← NMEA 2000 backbone
|
||||
//
|
||||
// Protocol: docs/concentrador_protocol.md
|
||||
// =============================================================================
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#define ESP32_CAN_TX_PIN GPIO_NUM_21
|
||||
#define ESP32_CAN_RX_PIN GPIO_NUM_22
|
||||
#include <NMEA2000_CAN.h>
|
||||
#include <N2kMessages.h>
|
||||
|
||||
#include "protocols/n2k_bridge.h"
|
||||
#include "protocols/nmea0183_parser.h"
|
||||
#include "station/station_mgr.h"
|
||||
|
||||
using namespace arconcentrador::protocols;
|
||||
using namespace arconcentrador::station;
|
||||
|
||||
// =============================================================================
|
||||
// Serial port configuration
|
||||
// =============================================================================
|
||||
#define UART_OUT Serial1 // UART1 TX → CH340N OUT ports
|
||||
#define UART_IN Serial2 // UART2 RX ← CH340N IN ports
|
||||
|
||||
static constexpr int PIN_UART1_TX = 17;
|
||||
static constexpr int PIN_UART1_RX = -1; // not used (TX only)
|
||||
static constexpr int PIN_UART2_TX = -1; // not used (RX only)
|
||||
static constexpr int PIN_UART2_RX = 16;
|
||||
static constexpr int UART_BAUD = 115200;
|
||||
|
||||
// =============================================================================
|
||||
// Broadcast helpers
|
||||
// =============================================================================
|
||||
|
||||
/// Write a sentence to all OUT ports (UART1 TX).
|
||||
static void broadcast(const char* sentence) {
|
||||
if (sentence && sentence[0]) {
|
||||
UART_OUT.print(sentence);
|
||||
}
|
||||
}
|
||||
|
||||
/// Build and broadcast the $PARP,STATUS sentence (2 Hz heartbeat).
|
||||
static void broadcast_status(uint8_t commander) {
|
||||
const ApStatus ap = n2k_ap_status();
|
||||
const float hdg = n2k_latest_heading_deg();
|
||||
|
||||
const char* mode_str = "STANDBY";
|
||||
if (ap.valid) {
|
||||
if (ap.mode == 1) mode_str = "HEADING_HOLD";
|
||||
else if (ap.mode == 2) mode_str = "TRACK";
|
||||
}
|
||||
|
||||
char body[128];
|
||||
snprintf(body, sizeof(body),
|
||||
"PARP,STATUS,%s,%.1f,%.1f,%.1f,%02d",
|
||||
mode_str,
|
||||
ap.valid ? ap.commanded_deg : 0.0f,
|
||||
hdg,
|
||||
ap.valid ? ap.rudder_deg : 0.0f,
|
||||
commander);
|
||||
|
||||
char sentence[160];
|
||||
uint8_t crc = 0;
|
||||
for (const char* p = body; *p; ++p) crc ^= (uint8_t)*p;
|
||||
snprintf(sentence, sizeof(sentence), "$%s*%02X\r\n", body, crc);
|
||||
broadcast(sentence);
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// NMEA 2000 message handler (pumped by NMEA2000.ParseMessages())
|
||||
// =============================================================================
|
||||
static const uint32_t PGNS_TO_FORWARD[] = {
|
||||
127250L, // Heading
|
||||
127251L, // ROT
|
||||
129026L, // COG/SOG
|
||||
128267L, // Depth
|
||||
130310L, // Water temp
|
||||
0
|
||||
};
|
||||
|
||||
static void on_n2k_message(const tN2kMsg& msg) {
|
||||
// Forward selected PGNs to all USB-OUT ports as NMEA 0183 sentences.
|
||||
for (int i = 0; PGNS_TO_FORWARD[i]; ++i) {
|
||||
if (msg.PGN == PGNS_TO_FORWARD[i]) {
|
||||
char buf[128];
|
||||
if (n2k_to_0183(msg, buf, sizeof(buf))) {
|
||||
broadcast(buf);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Command processor (reads from UART2 RX)
|
||||
// =============================================================================
|
||||
static void process_uart_in() {
|
||||
while (UART_IN.available()) {
|
||||
const char c = (char)UART_IN.read();
|
||||
ParpCommand cmd{};
|
||||
if (!parp_feed(c, cmd)) continue;
|
||||
|
||||
// --- Station management commands ---
|
||||
char station_broadcast[128];
|
||||
station_process(cmd.cmd, cmd.station_id,
|
||||
station_broadcast, sizeof(station_broadcast));
|
||||
if (station_broadcast[0]) {
|
||||
broadcast(station_broadcast);
|
||||
}
|
||||
|
||||
// --- Autopilot commands (only from current commander) ---
|
||||
const uint8_t cmdr = current_commander();
|
||||
const bool is_commander = (cmdr == cmd.station_id);
|
||||
const bool is_ap_cmd = (strcmp(cmd.cmd, "ENGAGE") == 0 ||
|
||||
strcmp(cmd.cmd, "DISENGAGE") == 0 ||
|
||||
strcmp(cmd.cmd, "SETHEADING") == 0 ||
|
||||
strcmp(cmd.cmd, "PORTONE") == 0 ||
|
||||
strcmp(cmd.cmd, "STBDONE") == 0 ||
|
||||
strcmp(cmd.cmd, "PORTTEN") == 0 ||
|
||||
strcmp(cmd.cmd, "STBDTEN") == 0);
|
||||
|
||||
if (is_ap_cmd && is_commander) {
|
||||
const float hdg = n2k_latest_heading_deg();
|
||||
parp_to_n2k(cmd, hdg);
|
||||
Serial.printf("[MAIN] cmd %s val=%.1f sta=%d → NMEA2000\n",
|
||||
cmd.cmd, cmd.value, cmd.station_id);
|
||||
} else if (is_ap_cmd && !is_commander) {
|
||||
Serial.printf("[MAIN] cmd %s rechazado: sta=%d no tiene el mando (mando=%d)\n",
|
||||
cmd.cmd, cmd.station_id, cmdr);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Arduino entry points
|
||||
// =============================================================================
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
Serial.println("[AR-CONCENTRADOR] booting...");
|
||||
|
||||
// UART1: TX only (broadcast to CH340N OUT ports)
|
||||
UART_OUT.begin(UART_BAUD, SERIAL_8N1, PIN_UART1_RX, PIN_UART1_TX);
|
||||
|
||||
// UART2: RX only (receive from CH340N IN ports)
|
||||
UART_IN.begin(UART_BAUD, SERIAL_8N1, PIN_UART2_RX, PIN_UART2_TX);
|
||||
|
||||
// Station manager
|
||||
station_init();
|
||||
|
||||
// NMEA 2000 bridge
|
||||
n2k_bridge_init();
|
||||
NMEA2000.SetMsgHandler(on_n2k_message);
|
||||
|
||||
Serial.println("[AR-CONCENTRADOR] ready.");
|
||||
Serial.println("[AR-CONCENTRADOR] UART1 TX=GPIO17 (OUT) | UART2 RX=GPIO16 (IN)");
|
||||
Serial.println("[AR-CONCENTRADOR] CAN TX=GPIO21 | CAN RX=GPIO22");
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// 1. Pump NMEA 2000 (calls on_n2k_message for each received PGN)
|
||||
NMEA2000.ParseMessages();
|
||||
|
||||
// 2. Process incoming $PARP commands from USB-IN ports
|
||||
process_uart_in();
|
||||
|
||||
// 3. Station management tick (REQCMD timeout)
|
||||
station_tick();
|
||||
|
||||
// 4. Broadcast autopilot status at 2 Hz
|
||||
static uint32_t last_status = 0;
|
||||
const uint32_t now = millis();
|
||||
if (now - last_status >= 500) {
|
||||
last_status = now;
|
||||
broadcast_status(current_commander());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,279 @@
|
||||
// =============================================================================
|
||||
// protocols/n2k_bridge.cpp -- NMEA 2000 <-> NMEA 0183 conversion
|
||||
// =============================================================================
|
||||
|
||||
#include "n2k_bridge.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <NMEA2000_CAN.h>
|
||||
#include <N2kMessages.h>
|
||||
#include <cstring>
|
||||
#include <cstdio>
|
||||
#include <cmath>
|
||||
|
||||
namespace arconcentrador::protocols {
|
||||
|
||||
namespace {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Shared state (updated by PGN handlers, read by main loop)
|
||||
// ---------------------------------------------------------------------------
|
||||
static float g_heading_deg = 0.0f;
|
||||
static bool g_heading_valid = false;
|
||||
static float g_rot_dps = 0.0f;
|
||||
static float g_sog_kn = NAN;
|
||||
static float g_cog_deg = NAN;
|
||||
static float g_depth_m = NAN;
|
||||
static float g_water_temp_c = NAN;
|
||||
|
||||
static ApStatus g_ap_status{};
|
||||
|
||||
static portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Helpers
|
||||
// ---------------------------------------------------------------------------
|
||||
static float rad2deg_pos(double rad) {
|
||||
float d = (float)(rad * 180.0 / M_PI);
|
||||
while (d < 0.0f) d += 360.0f;
|
||||
while (d >= 360.0f) d -= 360.0f;
|
||||
return d;
|
||||
}
|
||||
|
||||
static uint8_t nmea_crc(const char* body) {
|
||||
uint8_t crc = 0;
|
||||
for (; *body; ++body) crc ^= (uint8_t)*body;
|
||||
return crc;
|
||||
}
|
||||
|
||||
static int write_sentence(char* buf, size_t len, const char* body) {
|
||||
char tmp[256];
|
||||
snprintf(tmp, sizeof(tmp), "%s", body);
|
||||
const uint8_t crc = nmea_crc(tmp);
|
||||
return snprintf(buf, len, "$%s*%02X\r\n", body, crc);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// PGN handlers (called from NMEA2000.ParseMessages())
|
||||
// ---------------------------------------------------------------------------
|
||||
static void on_heading(const tN2kMsg& msg) {
|
||||
unsigned char sid;
|
||||
double hdg = 0.0, dev = 0.0, var = 0.0;
|
||||
tN2kHeadingReference ref;
|
||||
if (!ParseN2kHeading(msg, sid, hdg, dev, var, ref)) return;
|
||||
if (hdg > 1e8) return;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_heading_deg = rad2deg_pos(hdg);
|
||||
g_heading_valid = true;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
}
|
||||
|
||||
static void on_rot(const tN2kMsg& msg) {
|
||||
unsigned char sid;
|
||||
double rot = 0.0;
|
||||
if (!ParseN2kRateOfTurn(msg, sid, rot)) return;
|
||||
if (rot > 1e8 || rot < -1e8) return;
|
||||
// PGN 127251 is in rad/s; NMEA 0183 ROT sentence uses deg/min
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_rot_dps = (float)(rot * 180.0 / M_PI);
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
}
|
||||
|
||||
static void on_cog_sog(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;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_cog_deg = rad2deg_pos(cog);
|
||||
g_sog_kn = (float)(sog * 1.94384f);
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
}
|
||||
|
||||
static void on_depth(const tN2kMsg& msg) {
|
||||
unsigned char sid;
|
||||
double depth = 0.0, offset = 0.0, range = 0.0;
|
||||
if (!ParseN2kWaterDepth(msg, sid, depth, offset, range)) return;
|
||||
if (depth > 1e8) return;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_depth_m = (float)depth;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
}
|
||||
|
||||
static void on_water_temp(const tN2kMsg& msg) {
|
||||
unsigned char sid;
|
||||
tN2kTempSource src;
|
||||
double temp = 0.0, set_temp = 0.0;
|
||||
if (!ParseN2kTemperature(msg, sid, src, temp, set_temp)) return;
|
||||
if (src != N2kts_SeaTemperature) return;
|
||||
if (temp > 1e8) return;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_water_temp_c = (float)(temp - 273.15); // K → °C
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
}
|
||||
|
||||
static void on_htc(const tN2kMsg& msg) {
|
||||
// PGN 127237 from the autopilot: its current status.
|
||||
tN2kOnOff rl, ol, tl, ov;
|
||||
tN2kSteeringMode sm;
|
||||
tN2kTurnMode tm;
|
||||
tN2kHeadingReference hr;
|
||||
tN2kRudderDirectionOrder rdo;
|
||||
double cra = 0, hts = 0, trk = 0, rlim = 0, ohl = 0;
|
||||
double rot_ord = 0, xtel = 0, vhd = 0;
|
||||
if (!ParseN2kHeadingTrackControl(msg, rl, ol, tl, ov, sm, tm, hr,
|
||||
rdo, cra, hts, trk, rlim, ohl,
|
||||
rot_ord, rot_ord, xtel, vhd)) return;
|
||||
|
||||
ApStatus s{};
|
||||
s.heading_deg = (vhd < 1e8) ? rad2deg_pos(vhd) : g_heading_deg;
|
||||
s.commanded_deg = (hts < 1e8) ? rad2deg_pos(hts) : 0.0f;
|
||||
s.rudder_deg = (cra < 1e8) ? (float)(cra * 180.0 / M_PI) : 0.0f;
|
||||
s.mode = (sm == N2kSM_HeadingControl) ? 1 :
|
||||
(sm == N2kSM_TrackControl) ? 2 : 0;
|
||||
s.valid = true;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_ap_status = s;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
}
|
||||
|
||||
static void MessageHandler(const tN2kMsg& msg) {
|
||||
switch (msg.PGN) {
|
||||
case 127250L: on_heading(msg); break;
|
||||
case 127251L: on_rot(msg); break;
|
||||
case 127237L: on_htc(msg); break;
|
||||
case 128267L: on_depth(msg); break;
|
||||
case 129026L: on_cog_sog(msg); break;
|
||||
case 130310L: on_water_temp(msg); break;
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Public API
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void n2k_bridge_init() {
|
||||
NMEA2000.SetProductInformation("AR-CONC-1", 300,
|
||||
"AR-Concentrador v1", "1.0.0", "AR-CONCENTRADOR v1.0");
|
||||
NMEA2000.SetDeviceInformation(3, 170, 25, 2046);
|
||||
NMEA2000.SetMode(tNMEA2000::N2km_ListenAndNode, 27);
|
||||
NMEA2000.EnableForward(false);
|
||||
NMEA2000.SetMsgHandler(&MessageHandler);
|
||||
NMEA2000.Open();
|
||||
}
|
||||
|
||||
bool n2k_to_0183(const tN2kMsg& msg, char* out_buf, size_t out_len) {
|
||||
char body[200];
|
||||
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
const float hdg = g_heading_deg;
|
||||
const float rot = g_rot_dps;
|
||||
const float sog = g_sog_kn;
|
||||
const float cog = g_cog_deg;
|
||||
const float dep = g_depth_m;
|
||||
const float tmp = g_water_temp_c;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
|
||||
switch (msg.PGN) {
|
||||
case 127250L: // Heading → $IIHDT
|
||||
snprintf(body, sizeof(body), "IIHDT,%.1f,T", hdg);
|
||||
write_sentence(out_buf, out_len, body);
|
||||
return true;
|
||||
|
||||
case 127251L: // ROT → $IIROT (deg/min)
|
||||
snprintf(body, sizeof(body), "IIROT,%.2f,A", rot * 60.0f);
|
||||
write_sentence(out_buf, out_len, body);
|
||||
return true;
|
||||
|
||||
case 129026L: // COG/SOG → $IIVHW
|
||||
if (!isnan(sog) && !isnan(cog)) {
|
||||
snprintf(body, sizeof(body),
|
||||
"IIVHW,%.1f,T,,M,%.2f,N,,K", cog, sog);
|
||||
write_sentence(out_buf, out_len, body);
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case 128267L: // Depth → $IIDPT
|
||||
if (!isnan(dep)) {
|
||||
snprintf(body, sizeof(body), "IIDPT,%.1f,0.0", dep);
|
||||
write_sentence(out_buf, out_len, body);
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case 130310L: // Water temp → $IIMTW
|
||||
if (!isnan(tmp)) {
|
||||
snprintf(body, sizeof(body), "IIMTW,%.1f,C", tmp);
|
||||
write_sentence(out_buf, out_len, body);
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool parp_to_n2k(const ParpCommand& cmd, float current_heading_deg) {
|
||||
// Determine the target steering mode and commanded heading.
|
||||
tN2kSteeringMode mode = N2kSM_MainSteering; // STANDBY default
|
||||
double heading_to_steer = N2kDoubleNA;
|
||||
|
||||
if (strcmp(cmd.cmd, "ENGAGE") == 0) {
|
||||
mode = N2kSM_HeadingControl;
|
||||
// Engage on current heading if no specific heading commanded.
|
||||
heading_to_steer = (double)current_heading_deg * M_PI / 180.0;
|
||||
} else if (strcmp(cmd.cmd, "DISENGAGE") == 0) {
|
||||
mode = N2kSM_MainSteering;
|
||||
} else if (strcmp(cmd.cmd, "SETHEADING") == 0) {
|
||||
mode = N2kSM_HeadingControl;
|
||||
heading_to_steer = (double)cmd.value * M_PI / 180.0;
|
||||
} else if (strcmp(cmd.cmd, "PORTONE") == 0) {
|
||||
mode = N2kSM_HeadingControl;
|
||||
heading_to_steer = (double)(cmd.value - 1.0f) * M_PI / 180.0;
|
||||
} else if (strcmp(cmd.cmd, "STBDONE") == 0) {
|
||||
mode = N2kSM_HeadingControl;
|
||||
heading_to_steer = (double)(cmd.value + 1.0f) * M_PI / 180.0;
|
||||
} else if (strcmp(cmd.cmd, "PORTTEN") == 0) {
|
||||
mode = N2kSM_HeadingControl;
|
||||
heading_to_steer = (double)(cmd.value - 10.0f) * M_PI / 180.0;
|
||||
} else if (strcmp(cmd.cmd, "STBDTEN") == 0) {
|
||||
mode = N2kSM_HeadingControl;
|
||||
heading_to_steer = (double)(cmd.value + 10.0f) * M_PI / 180.0;
|
||||
} else {
|
||||
return false; // station management commands — not forwarded to NMEA2000
|
||||
}
|
||||
|
||||
tN2kMsg msg;
|
||||
SetN2kHeadingTrackControl(msg,
|
||||
N2kOnOff_Unavailable, N2kOnOff_Unavailable,
|
||||
N2kOnOff_Unavailable, N2kOnOff_Off,
|
||||
mode, N2kTM_RudderLimitControlled, N2khr_true,
|
||||
N2kRDO_NoDirectionOrder, N2kDoubleNA,
|
||||
heading_to_steer, N2kDoubleNA,
|
||||
N2kDoubleNA, N2kDoubleNA, N2kDoubleNA, N2kDoubleNA,
|
||||
N2kDoubleNA, N2kDoubleNA);
|
||||
return NMEA2000.SendMsg(msg);
|
||||
}
|
||||
|
||||
float n2k_latest_heading_deg() {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
const float h = g_heading_deg;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
return h;
|
||||
}
|
||||
|
||||
ApStatus n2k_ap_status() {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
const ApStatus s = g_ap_status;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
return s;
|
||||
}
|
||||
|
||||
} // namespace arconcentrador::protocols
|
||||
@@ -0,0 +1,45 @@
|
||||
// =============================================================================
|
||||
// protocols/n2k_bridge.h -- NMEA 2000 <-> NMEA 0183 conversion
|
||||
// =============================================================================
|
||||
//
|
||||
// Outbound (NMEA 2000 → NMEA 0183 for USB-OUT ports):
|
||||
// Receives PGNs from the backbone and formats standard/proprietary sentences.
|
||||
//
|
||||
// Inbound (NMEA 0183 → NMEA 2000 for $PARP commands from USB-IN ports):
|
||||
// Converts parsed ParpCommand structs into NMEA 2000 PGN 127237.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "nmea0183_parser.h"
|
||||
#include <N2kMessages.h>
|
||||
#include <Stream.h>
|
||||
|
||||
namespace arconcentrador::protocols {
|
||||
|
||||
/// Initialise the NMEA 2000 stack and register PGN handlers.
|
||||
void n2k_bridge_init();
|
||||
|
||||
/// Process one incoming NMEA 2000 message.
|
||||
/// Formats the corresponding NMEA 0183 sentence into out_buf.
|
||||
/// Returns true if a sentence was written.
|
||||
bool n2k_to_0183(const tN2kMsg& msg, char* out_buf, size_t out_len);
|
||||
|
||||
/// Convert a parsed $PARP command into a NMEA 2000 PGN 127237 and send it.
|
||||
/// Returns true if a PGN was sent.
|
||||
bool parp_to_n2k(const ParpCommand& cmd, float current_heading_deg);
|
||||
|
||||
/// Latest vessel heading received from backbone (degrees, for status broadcasts).
|
||||
float n2k_latest_heading_deg();
|
||||
|
||||
/// Latest autopilot status received from backbone (PGN 127237 from autopilot).
|
||||
struct ApStatus {
|
||||
float heading_deg; ///< actual vessel heading
|
||||
float commanded_deg; ///< heading setpoint
|
||||
float rudder_deg; ///< rudder angle
|
||||
uint8_t mode; ///< 0=STANDBY 1=HEADING_HOLD 2=TRACK
|
||||
bool valid;
|
||||
};
|
||||
ApStatus n2k_ap_status();
|
||||
|
||||
} // namespace arconcentrador::protocols
|
||||
@@ -0,0 +1,93 @@
|
||||
// =============================================================================
|
||||
// protocols/nmea0183_parser.cpp -- $PARP sentence parser
|
||||
// =============================================================================
|
||||
|
||||
#include "nmea0183_parser.h"
|
||||
|
||||
#include <cstring>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <Arduino.h>
|
||||
|
||||
namespace arconcentrador::protocols {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
uint8_t nmea_checksum(const char* sentence) {
|
||||
// Find content between '$' and '*'
|
||||
const char* start = strchr(sentence, '$');
|
||||
if (!start) return 0xFF;
|
||||
start++; // skip '$'
|
||||
const char* end = strchr(start, '*');
|
||||
if (!end) return 0xFF;
|
||||
uint8_t crc = 0;
|
||||
for (const char* p = start; p < end; ++p) crc ^= (uint8_t)*p;
|
||||
return crc;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
ParpCommand parp_parse(const char* sentence) {
|
||||
ParpCommand result{};
|
||||
result.valid = false;
|
||||
|
||||
// Must start with $PARP,
|
||||
if (strncmp(sentence, "$PARP,", 6) != 0) return result;
|
||||
|
||||
// Validate checksum
|
||||
const char* star = strchr(sentence, '*');
|
||||
if (!star || strlen(star) < 3) return result;
|
||||
const uint8_t recv_crc = (uint8_t)strtoul(star + 1, nullptr, 16);
|
||||
const uint8_t calc_crc = nmea_checksum(sentence);
|
||||
if (recv_crc != calc_crc) {
|
||||
Serial.printf("[PARSER] checksum error: recv=%02X calc=%02X\n",
|
||||
recv_crc, calc_crc);
|
||||
return result;
|
||||
}
|
||||
|
||||
// Copy body between "$PARP," and '*'
|
||||
char body[128];
|
||||
const char* body_start = sentence + 6;
|
||||
const size_t body_len = (size_t)(star - body_start);
|
||||
if (body_len == 0 || body_len >= sizeof(body)) return result;
|
||||
memcpy(body, body_start, body_len);
|
||||
body[body_len] = '\0';
|
||||
|
||||
// Tokenize: CMD,VALUE,STATION_ID
|
||||
char* saveptr = nullptr;
|
||||
const char* tok_cmd = strtok_r(body, ",", &saveptr);
|
||||
const char* tok_val = strtok_r(nullptr, ",", &saveptr);
|
||||
const char* tok_sta = strtok_r(nullptr, ",", &saveptr);
|
||||
|
||||
if (!tok_cmd || !tok_val || !tok_sta) return result;
|
||||
|
||||
strncpy(result.cmd, tok_cmd, sizeof(result.cmd) - 1);
|
||||
result.value = (float)atof(tok_val);
|
||||
result.station_id = (uint8_t)atoi(tok_sta);
|
||||
|
||||
if (result.station_id < 1 || result.station_id > 4) return result;
|
||||
|
||||
result.valid = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
static char s_line_buf[256];
|
||||
static size_t s_line_len = 0;
|
||||
|
||||
bool parp_feed(char c, ParpCommand& out) {
|
||||
if (c == '$') {
|
||||
// Start of new sentence — reset buffer.
|
||||
s_line_len = 0;
|
||||
}
|
||||
if (s_line_len < sizeof(s_line_buf) - 1) {
|
||||
s_line_buf[s_line_len++] = c;
|
||||
}
|
||||
if (c == '\n') {
|
||||
s_line_buf[s_line_len] = '\0';
|
||||
s_line_len = 0;
|
||||
out = parp_parse(s_line_buf);
|
||||
return out.valid;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace arconcentrador::protocols
|
||||
@@ -0,0 +1,35 @@
|
||||
// =============================================================================
|
||||
// protocols/nmea0183_parser.h -- $PARP sentence parser
|
||||
// =============================================================================
|
||||
//
|
||||
// Parses incoming $PARP sentences from UART2 RX (USB-IN ports).
|
||||
// Validates checksum before returning any data.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstdbool>
|
||||
|
||||
namespace arconcentrador::protocols {
|
||||
|
||||
struct ParpCommand {
|
||||
char cmd[16]; ///< e.g. "ENGAGE", "SETHEADING", "REQCMD"
|
||||
float value; ///< numeric parameter (heading degrees, etc.)
|
||||
uint8_t station_id; ///< 1-4
|
||||
bool valid; ///< false if checksum failed or format invalid
|
||||
};
|
||||
|
||||
/// Parse one null-terminated NMEA sentence (including the leading '$').
|
||||
/// Returns a ParpCommand with valid=true on success.
|
||||
ParpCommand parp_parse(const char* sentence);
|
||||
|
||||
/// Compute NMEA XOR checksum of content between '$' and '*'.
|
||||
uint8_t nmea_checksum(const char* sentence);
|
||||
|
||||
/// Append one character to the internal line buffer.
|
||||
/// When a complete sentence (\n) arrives, calls parp_parse and stores result.
|
||||
/// Returns true if a complete sentence was parsed this call.
|
||||
bool parp_feed(char c, ParpCommand& out);
|
||||
|
||||
} // namespace arconcentrador::protocols
|
||||
@@ -0,0 +1,122 @@
|
||||
// =============================================================================
|
||||
// station/station_mgr.cpp -- Control authority state machine
|
||||
// =============================================================================
|
||||
|
||||
#include "station_mgr.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <cstring>
|
||||
#include <cstdio>
|
||||
|
||||
namespace arconcentrador::station {
|
||||
|
||||
namespace {
|
||||
|
||||
uint8_t g_commander = STATION_NONE;
|
||||
uint8_t g_pending_request = STATION_NONE; // station waiting for REQCMD ack
|
||||
uint32_t g_request_time_ms = 0;
|
||||
static constexpr uint32_t REQCMD_TIMEOUT_MS = 10000;
|
||||
|
||||
// --- NMEA checksum ---
|
||||
static uint8_t nmea_crc(const char* s) {
|
||||
uint8_t crc = 0;
|
||||
for (; *s; ++s) crc ^= (uint8_t)*s;
|
||||
return crc;
|
||||
}
|
||||
|
||||
static void make_sentence(char* out, size_t len, const char* body) {
|
||||
char tmp[128];
|
||||
snprintf(tmp, sizeof(tmp), "PARP,%s", body);
|
||||
const uint8_t crc = nmea_crc(tmp);
|
||||
snprintf(out, len, "$PARP,%s*%02X\r\n", body, crc);
|
||||
}
|
||||
|
||||
static void transfer_to(uint8_t new_commander,
|
||||
char* out, size_t out_len) {
|
||||
const uint8_t prev = g_commander;
|
||||
g_commander = new_commander;
|
||||
g_pending_request = STATION_NONE;
|
||||
char body[64];
|
||||
snprintf(body, sizeof(body), "CMDTRANSFER,%02d,%02d", prev, new_commander);
|
||||
make_sentence(out, out_len, body);
|
||||
Serial.printf("[STATION] mando transferido %d → %d\n", prev, new_commander);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void station_init() {
|
||||
g_commander = STATION_NONE;
|
||||
g_pending_request = STATION_NONE;
|
||||
}
|
||||
|
||||
uint8_t current_commander() {
|
||||
return g_commander;
|
||||
}
|
||||
|
||||
void station_process(const char* cmd, uint8_t station_id,
|
||||
char* out_broadcast, size_t out_len) {
|
||||
out_broadcast[0] = '\0';
|
||||
|
||||
// --- TAKECMD: bridge override, always immediate ---
|
||||
if (strcmp(cmd, "TAKECMD") == 0 && station_id == STATION_BRIDGE) {
|
||||
transfer_to(STATION_BRIDGE, out_broadcast, out_len);
|
||||
return;
|
||||
}
|
||||
|
||||
// --- REQCMD: request command from another station ---
|
||||
if (strcmp(cmd, "REQCMD") == 0) {
|
||||
if (station_id == g_commander) return; // already the commander
|
||||
g_pending_request = station_id;
|
||||
g_request_time_ms = millis();
|
||||
char body[64];
|
||||
snprintf(body, sizeof(body), "CMDREQUEST,%02d", station_id);
|
||||
make_sentence(out_broadcast, out_len, body);
|
||||
Serial.printf("[STATION] estacion %d solicita el mando\n", station_id);
|
||||
return;
|
||||
}
|
||||
|
||||
// --- RELCMD: commander voluntarily releases ---
|
||||
if (strcmp(cmd, "RELCMD") == 0 && station_id == g_commander) {
|
||||
if (g_pending_request != STATION_NONE) {
|
||||
transfer_to(g_pending_request, out_broadcast, out_len);
|
||||
} else {
|
||||
transfer_to(STATION_NONE, out_broadcast, out_len);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// --- ACKCMD: commander confirms the pending request ---
|
||||
if (strcmp(cmd, "ACKCMD") == 0 && station_id == g_commander) {
|
||||
if (g_pending_request != STATION_NONE) {
|
||||
transfer_to(g_pending_request, out_broadcast, out_len);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// --- DENYCMD: commander denies the pending request ---
|
||||
if (strcmp(cmd, "DENYCMD") == 0 && station_id == g_commander) {
|
||||
g_pending_request = STATION_NONE;
|
||||
char body[64];
|
||||
snprintf(body, sizeof(body), "CMDDENIED,%02d", station_id);
|
||||
make_sentence(out_broadcast, out_len, body);
|
||||
Serial.printf("[STATION] solicitud de estacion %d denegada\n",
|
||||
g_pending_request);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void station_tick() {
|
||||
if (g_pending_request == STATION_NONE) return;
|
||||
if (millis() - g_request_time_ms >= REQCMD_TIMEOUT_MS) {
|
||||
// Auto-transfer after 10 s with no response from commander.
|
||||
Serial.printf("[STATION] timeout REQCMD → auto-transfer a estacion %d\n",
|
||||
g_pending_request);
|
||||
// We can't write out_broadcast here (no output buffer in tick),
|
||||
// so the main loop re-broadcasts the transfer status on the next
|
||||
// regular STATUS pulse.
|
||||
g_commander = g_pending_request;
|
||||
g_pending_request = STATION_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace arconcentrador::station
|
||||
@@ -0,0 +1,37 @@
|
||||
// =============================================================================
|
||||
// station/station_mgr.h -- Control authority state machine
|
||||
// =============================================================================
|
||||
//
|
||||
// Manages who has command authority over the autopilot.
|
||||
// Rules (from docs/concentrador_protocol.md):
|
||||
// - Station 01 (bridge) has override priority — TAKECMD is immediate.
|
||||
// - Other stations request command via REQCMD, wait for ACK or timeout (10s).
|
||||
// - Only the current commander can release (RELCMD) or deny requests (DENYCMD).
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace arconcentrador::station {
|
||||
|
||||
static constexpr uint8_t STATION_NONE = 0x00;
|
||||
static constexpr uint8_t STATION_BRIDGE = 0x01; // highest priority
|
||||
|
||||
/// Initialise with no commander (STATION_NONE).
|
||||
void station_init();
|
||||
|
||||
/// Return the current commander ID (0 = no commander).
|
||||
uint8_t current_commander();
|
||||
|
||||
/// Process an incoming $PARP sentence already parsed into fields.
|
||||
/// cmd is the command field (e.g. "REQCMD", "TAKECMD", "RELCMD", ...).
|
||||
/// station_id is the sender's station ID (1-4).
|
||||
/// out_broadcast is set to a full $PARP sentence to broadcast (or empty string).
|
||||
void station_process(const char* cmd, uint8_t station_id,
|
||||
char* out_broadcast, size_t out_len);
|
||||
|
||||
/// Call once per loop — handles the 10-second REQCMD timeout.
|
||||
void station_tick();
|
||||
|
||||
} // namespace arconcentrador::station
|
||||
Reference in New Issue
Block a user