diff --git a/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp b/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp index 28793e6..fe2d278 100644 --- a/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp +++ b/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp @@ -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 diff --git a/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h b/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h index 086230a..795aff4 100644 --- a/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h +++ b/firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h @@ -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 diff --git a/firmware/ar_bno085_node_v1/platformio.ini b/firmware/ar_bno085_node_v1/platformio.ini new file mode 100644 index 0000000..3abfb93 --- /dev/null +++ b/firmware/ar_bno085_node_v1/platformio.ini @@ -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 diff --git a/firmware/ar_bno085_node_v1/src/main.cpp b/firmware/ar_bno085_node_v1/src/main.cpp new file mode 100644 index 0000000..f6d510d --- /dev/null +++ b/firmware/ar_bno085_node_v1/src/main.cpp @@ -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 +#include + +// --- NMEA 2000 --- +#define ESP32_CAN_TX_PIN GPIO_NUM_23 +#define ESP32_CAN_RX_PIN GPIO_NUM_4 +#include +#include + +// --- BNO085 --- +#include + +// ============================================================================= +// 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(); +} diff --git a/firmware/ar_concentrador_v1/platformio.ini b/firmware/ar_concentrador_v1/platformio.ini new file mode 100644 index 0000000..2b96352 --- /dev/null +++ b/firmware/ar_concentrador_v1/platformio.ini @@ -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 diff --git a/firmware/ar_concentrador_v1/src/main.cpp b/firmware/ar_concentrador_v1/src/main.cpp new file mode 100644 index 0000000..70888f3 --- /dev/null +++ b/firmware/ar_concentrador_v1/src/main.cpp @@ -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 + +#define ESP32_CAN_TX_PIN GPIO_NUM_21 +#define ESP32_CAN_RX_PIN GPIO_NUM_22 +#include +#include + +#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()); + } +} diff --git a/firmware/ar_concentrador_v1/src/protocols/n2k_bridge.cpp b/firmware/ar_concentrador_v1/src/protocols/n2k_bridge.cpp new file mode 100644 index 0000000..d83945d --- /dev/null +++ b/firmware/ar_concentrador_v1/src/protocols/n2k_bridge.cpp @@ -0,0 +1,279 @@ +// ============================================================================= +// protocols/n2k_bridge.cpp -- NMEA 2000 <-> NMEA 0183 conversion +// ============================================================================= + +#include "n2k_bridge.h" + +#include +#include +#include +#include +#include +#include + +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 diff --git a/firmware/ar_concentrador_v1/src/protocols/n2k_bridge.h b/firmware/ar_concentrador_v1/src/protocols/n2k_bridge.h new file mode 100644 index 0000000..101bc18 --- /dev/null +++ b/firmware/ar_concentrador_v1/src/protocols/n2k_bridge.h @@ -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 +#include + +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 diff --git a/firmware/ar_concentrador_v1/src/protocols/nmea0183_parser.cpp b/firmware/ar_concentrador_v1/src/protocols/nmea0183_parser.cpp new file mode 100644 index 0000000..8472a60 --- /dev/null +++ b/firmware/ar_concentrador_v1/src/protocols/nmea0183_parser.cpp @@ -0,0 +1,93 @@ +// ============================================================================= +// protocols/nmea0183_parser.cpp -- $PARP sentence parser +// ============================================================================= + +#include "nmea0183_parser.h" + +#include +#include +#include +#include + +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 diff --git a/firmware/ar_concentrador_v1/src/protocols/nmea0183_parser.h b/firmware/ar_concentrador_v1/src/protocols/nmea0183_parser.h new file mode 100644 index 0000000..d48766d --- /dev/null +++ b/firmware/ar_concentrador_v1/src/protocols/nmea0183_parser.h @@ -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 +#include + +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 diff --git a/firmware/ar_concentrador_v1/src/station/station_mgr.cpp b/firmware/ar_concentrador_v1/src/station/station_mgr.cpp new file mode 100644 index 0000000..c7bbff7 --- /dev/null +++ b/firmware/ar_concentrador_v1/src/station/station_mgr.cpp @@ -0,0 +1,122 @@ +// ============================================================================= +// station/station_mgr.cpp -- Control authority state machine +// ============================================================================= + +#include "station_mgr.h" + +#include +#include +#include + +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 diff --git a/firmware/ar_concentrador_v1/src/station/station_mgr.h b/firmware/ar_concentrador_v1/src/station/station_mgr.h new file mode 100644 index 0000000..2ac2bba --- /dev/null +++ b/firmware/ar_concentrador_v1/src/station/station_mgr.h @@ -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 + +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