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:
@@ -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,8 +166,63 @@ 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 127237L: HandleHeadingControl(msg); break;
|
||||
case 127250L: HandleHeading(msg); break;
|
||||
case 127251L: HandleROT(msg); break;
|
||||
case 129026L: HandleCogSog(msg); 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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -0,0 +1,45 @@
|
||||
; =============================================================================
|
||||
; AR BNO085 Compact Node v1 -- firmware build configuration
|
||||
; =============================================================================
|
||||
;
|
||||
; Target hardware: AR-BNO085-NODE v1.0 (ESP32-DOWD, compact board)
|
||||
; Role: NMEA 2000 IMU node -- publishes heading (PGN 127250)
|
||||
; and rate-of-turn (PGN 127251) from BNO085 sensor.
|
||||
;
|
||||
; Pinout:
|
||||
; GPIO21 -- I2C SDA (BNO085)
|
||||
; GPIO22 -- I2C SCL (BNO085)
|
||||
; GPIO34 -- BNO085 INT (data-ready, active low, input-only)
|
||||
; GPIO13 -- BNO085 NRST (output, active low, drive LOW to reset)
|
||||
; GPIO23 -- CAN TX (MCP2562T)
|
||||
; GPIO4 -- CAN RX (MCP2562T)
|
||||
; =============================================================================
|
||||
|
||||
[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
|
||||
build_unflags =
|
||||
-std=gnu++11
|
||||
lib_deps =
|
||||
ttlappalainen/NMEA2000-library@^4.22.0
|
||||
ttlappalainen/NMEA2000_esp32@^1.0.3
|
||||
sparkfun/SparkFun BNO08x Cortex Based IMU@^1.0.3
|
||||
|
||||
[env:esp32-dev]
|
||||
board = esp32dev
|
||||
build_type = release
|
||||
build_flags =
|
||||
${env.build_flags}
|
||||
-Os
|
||||
@@ -0,0 +1,182 @@
|
||||
// =============================================================================
|
||||
// AR BNO085 Compact Node v1 — main.cpp
|
||||
// =============================================================================
|
||||
//
|
||||
// Reads heading (ARVR Stabilized Rotation Vector) and yaw rate (Gyroscope)
|
||||
// from the BNO085 via I2C and publishes them on the NMEA 2000 backbone:
|
||||
// PGN 127250 — Vessel Heading (100 Hz)
|
||||
// PGN 127251 — Rate of Turn (100 Hz)
|
||||
//
|
||||
// The main autopilot board (ar_autopilot_v1) receives these PGNs from the
|
||||
// backbone via its nmea2000_consumer and feeds them into the PID outer loop.
|
||||
// =============================================================================
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
|
||||
// --- NMEA 2000 ---
|
||||
#define ESP32_CAN_TX_PIN GPIO_NUM_23
|
||||
#define ESP32_CAN_RX_PIN GPIO_NUM_4
|
||||
#include <NMEA2000_CAN.h>
|
||||
#include <N2kMessages.h>
|
||||
|
||||
// --- BNO085 ---
|
||||
#include <SparkFun_BNO08x_Arduino_Library.h>
|
||||
|
||||
// =============================================================================
|
||||
// Pin definitions
|
||||
// =============================================================================
|
||||
static constexpr int PIN_I2C_SDA = 21;
|
||||
static constexpr int PIN_I2C_SCL = 22;
|
||||
static constexpr int PIN_BNO_INT = 34; // data-ready, active low
|
||||
static constexpr int PIN_BNO_NRST = 13; // reset, active low
|
||||
|
||||
// =============================================================================
|
||||
// BNO085 instance
|
||||
// =============================================================================
|
||||
static BNO08x imu;
|
||||
|
||||
// Latest measurements (updated from BNO085 reports)
|
||||
static volatile float g_heading_rad = 0.0f; // true heading, radians
|
||||
static volatile float g_rot_rad_s = 0.0f; // yaw rate, rad/s (+ve = stbd)
|
||||
static volatile bool g_heading_valid = false;
|
||||
static volatile bool g_rot_valid = false;
|
||||
|
||||
// =============================================================================
|
||||
// NMEA 2000 setup
|
||||
// =============================================================================
|
||||
static void nmea2000_init() {
|
||||
NMEA2000.SetProductInformation(
|
||||
"AR-BNO-1",
|
||||
200,
|
||||
"AR-BNO085-NODE v1",
|
||||
"1.0.0",
|
||||
"AR-BNO085-NODE v1.0"
|
||||
);
|
||||
NMEA2000.SetDeviceInformation(
|
||||
2, // Unique number (different from autopilot node = 1)
|
||||
140, // Device function: Attitude sensor
|
||||
60, // Device class: Navigation
|
||||
2046 // Manufacturer code (test)
|
||||
);
|
||||
NMEA2000.SetMode(tNMEA2000::N2km_NodeOnly, 26);
|
||||
NMEA2000.EnableForward(false);
|
||||
NMEA2000.Open();
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// BNO085 initialisation
|
||||
// =============================================================================
|
||||
static bool bno085_init() {
|
||||
// Hard reset — hold NRST low for 10 ms, then release.
|
||||
pinMode(PIN_BNO_NRST, OUTPUT);
|
||||
digitalWrite(PIN_BNO_NRST, LOW);
|
||||
delay(15);
|
||||
digitalWrite(PIN_BNO_NRST, HIGH);
|
||||
delay(100); // wait for BNO085 startup (~50 ms typical)
|
||||
|
||||
Wire.begin(PIN_I2C_SDA, PIN_I2C_SCL);
|
||||
Wire.setClock(400000); // 400 kHz Fast Mode
|
||||
|
||||
if (!imu.begin(0x4A, Wire, PIN_BNO_INT)) {
|
||||
Serial.println("[BNO085] begin() failed — check wiring / I2C address");
|
||||
return false;
|
||||
}
|
||||
|
||||
// ARVR Stabilized Rotation Vector → heading (tilt-compensated), 100 Hz
|
||||
if (!imu.enableARVRStabilizedRotationVector(10000)) { // 10 ms = 100 Hz
|
||||
Serial.println("[BNO085] enableARVRStabilizedRotationVector() failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Calibrated Gyroscope → yaw rate, 100 Hz
|
||||
if (!imu.enableGyro(10000)) {
|
||||
Serial.println("[BNO085] enableGyro() failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
Serial.println("[BNO085] init OK — heading + yaw rate @ 100 Hz");
|
||||
return true;
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Read BNO085 reports (call frequently)
|
||||
// =============================================================================
|
||||
static void bno085_read() {
|
||||
if (!imu.dataAvailable()) return;
|
||||
|
||||
// Rotation Vector → heading (yaw around Z)
|
||||
if (imu.getSensorEventID() == SENSOR_REPORTID_ARVR_STABILIZED_ROTATION_VECTOR) {
|
||||
// SparkFun library: getYaw() returns yaw in radians, -pi..+pi
|
||||
const float yaw = imu.getYaw();
|
||||
// Convert to 0..2pi (nautical convention, 0 = North, + = clockwise)
|
||||
float hdg = -yaw; // BNO085: +yaw = CCW (math convention); nautical = CW
|
||||
if (hdg < 0.0f) hdg += 2.0f * (float)M_PI;
|
||||
if (hdg >= 2.0f * (float)M_PI) hdg -= 2.0f * (float)M_PI;
|
||||
g_heading_rad = hdg;
|
||||
g_heading_valid = true;
|
||||
}
|
||||
|
||||
// Gyroscope Z → yaw rate (+ = clockwise = starboard turn)
|
||||
if (imu.getSensorEventID() == SENSOR_REPORTID_GYROSCOPE_CALIBRATED) {
|
||||
// getGyroZ(): rad/s, +Z = looking down = CW from above = starboard
|
||||
g_rot_rad_s = imu.getGyroZ();
|
||||
g_rot_valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Publish NMEA 2000 PGNs
|
||||
// =============================================================================
|
||||
static void publish_pgns() {
|
||||
if (g_heading_valid) {
|
||||
tN2kMsg msg;
|
||||
SetN2kHeading(msg,
|
||||
0, // SID
|
||||
(double)g_heading_rad,
|
||||
N2kDoubleNA, // Deviation
|
||||
N2kDoubleNA, // Variation
|
||||
N2khr_true); // Reference: true north (BNO085 is absolute)
|
||||
NMEA2000.SendMsg(msg);
|
||||
}
|
||||
|
||||
if (g_rot_valid) {
|
||||
tN2kMsg msg;
|
||||
// PGN 127251: rate in rad/s
|
||||
SetN2kRateOfTurn(msg, 0, (double)g_rot_rad_s);
|
||||
NMEA2000.SendMsg(msg);
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Arduino entry points
|
||||
// =============================================================================
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
Serial.println("[AR-BNO085-NODE] booting...");
|
||||
|
||||
nmea2000_init();
|
||||
|
||||
if (!bno085_init()) {
|
||||
Serial.println("[AR-BNO085-NODE] FATAL: IMU init failed. Halting.");
|
||||
for (;;) delay(1000);
|
||||
}
|
||||
|
||||
Serial.println("[AR-BNO085-NODE] ready.");
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// Read sensor at ~100 Hz (non-blocking, interrupt-driven via INT pin)
|
||||
bno085_read();
|
||||
|
||||
// Publish to NMEA 2000 backbone at 10 Hz
|
||||
static uint32_t last_pub = 0;
|
||||
const uint32_t now = millis();
|
||||
if (now - last_pub >= 100) {
|
||||
last_pub = now;
|
||||
publish_pgns();
|
||||
}
|
||||
|
||||
// Keep the NMEA2000 stack alive
|
||||
NMEA2000.ParseMessages();
|
||||
}
|
||||
@@ -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