sprint-1: firmware ESP32 base -- STANDBY + Modbus + NMEA 2000 + watchdog
End-to-end implementation of Sprint 1 per docs/sprint-1-plan.md.
Builds: pio run -e esp32-dev SUCCESS, RAM 6.7%, Flash 26.5% (347 KB).
Tests: pytest 110/110 green; pio test -e native deferred (needs host
C++ compiler -- none on this Windows machine).
Firmware (firmware/ar_autopilot_v1/):
- platformio.ini: 4 envs (esp32-dev release, esp32-debug, native unity
tests, check static analysis). NMEA2000-library@4.22, NMEA2000_esp32@
1.0, eModbus@1.7.4 pinned.
- main.cpp: boot in STANDBY, FreeRTOS task spawn, returns to scheduler.
- system/: ar_log.h facade, task_config.h (priorities/stacks/cores
central table), heartbeat (1 Hz LED + uptime).
- modes/: STANDBY-only state machine; non-STANDBY rejected.
- hal/: di_do.cpp (5 DI + 10 DO with debounce + last-state cache),
rudder_sensor.cpp (100 Hz ADC + 5-sample median filter, Core 1),
rudder_actuator.cpp (DO1/DO2/DO3 with three safety interlocks:
power-off, STANDBY mode, limit switch).
- safety/: TWDT @ 2 s panic-on-expire; 50 Hz safety task on Core 1
enforcing DI1 physical disengage button, DI4 external alarm,
both-limit-switch interlock.
- protocols/modbus_slave.cpp: eModbus RTU server on UART2 @ 38400 8N1,
slave ID 1. 17 inputs + 19 discretes + 5 holdings + 4 coils. Reads
pull live telemetry; writes validate range and route to handlers.
- protocols/nmea2000_consumer.cpp: stack open with CAN TX=GPIO3
RX=GPIO1, subscribed to PGN 127250 (Heading) + PGN 127251 (Rate of
Turn). 5 s staleness flag built in for Sprint 6 alarm wiring.
- filters/median.h: templated MedianFilter<T,N> (host testable).
Cross-cutting:
- modbus_registers.yaml: single source of truth for the Modbus register
map. 45 entries.
- tools/gen_modbus_registers.py: YAML -> C++ header + Python module
generator with --check for drift detection.
- arautopilot/shared/modbus_register_map.py: generated Python mirror,
imported by Studio + tools.
- arautopilot/tests/test_modbus_register_map.py: 30 tests covering
schema, address uniqueness, range, spot-checks, and drift detection
(fails if YAML edited without regenerating).
- firmware/ar_autopilot_v1/tools/modbus_client_test.py: manual Modbus
client for poking the slave from a PC with USB-RS485 dongle.
- firmware/ar_autopilot_v1/test/test_median_filter/test_median.cpp:
8 Unity tests of the median filter (host-side, no Arduino dependency).
- docs/firmware.md: full operator + integrator guide (toolchain, build,
flash, expected boot log, troubleshooting, Sprint 1 capability matrix).
Architecture note: opted for Arduino-on-ESP32 only instead of the
proposed dual Arduino-as-ESP-IDF-component setup. Rationale documented
in CHANGELOG and docs/firmware.md -- Arduino-on-ESP32 already provides
the FreeRTOS primitives we need; dual framework adds fragility without
benefit at Sprint 1 scope. Reconsider in Sprint 8 (OTA + secure boot).
NOT in Sprint 1 (intentional per brief sec. 12):
- PID loops (inner/outer)
- True Course / Track Keeping
- Full alarm catalogue beyond DI1/DI4
- Knob driver
- Studio GUI / dedicated display
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
@@ -0,0 +1,178 @@
|
||||
// =============================================================================
|
||||
// nmea2000_consumer.cpp -- NMEA 2000 backbone consumer
|
||||
// =============================================================================
|
||||
|
||||
#include "nmea2000_consumer.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// Override the default ESP32 CAN pins (16/4) to match our AR-NMEA-IO board.
|
||||
#include "../hal/pinout.h"
|
||||
#define ESP32_CAN_TX_PIN ((gpio_num_t)PIN_CAN_TX)
|
||||
#define ESP32_CAN_RX_PIN ((gpio_num_t)PIN_CAN_RX)
|
||||
|
||||
#include <NMEA2000_CAN.h>
|
||||
#include <N2kMessages.h>
|
||||
|
||||
#include "../system/ar_log.h"
|
||||
#include "../system/task_config.h"
|
||||
|
||||
namespace arautopilot::protocols::nmea2000 {
|
||||
|
||||
namespace {
|
||||
constexpr const char* TAG = "AR/N2K";
|
||||
|
||||
constexpr uint32_t STALE_THRESHOLD_MS = 5000;
|
||||
|
||||
portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
|
||||
HeadingSnapshot g_snap{
|
||||
.heading_deg = 0.0f,
|
||||
.is_true = false,
|
||||
.rate_of_turn_dps = 0.0f,
|
||||
.heading_age_ms = 0,
|
||||
.rot_age_ms = 0,
|
||||
.heading_valid = false,
|
||||
.rot_valid = false,
|
||||
};
|
||||
|
||||
float rad_to_deg_pos(float rad) {
|
||||
float d = rad * (180.0f / (float)M_PI);
|
||||
// Normalise to 0..360.
|
||||
while (d < 0.0f) d += 360.0f;
|
||||
while (d >= 360.0f) d -= 360.0f;
|
||||
return d;
|
||||
}
|
||||
|
||||
float rad_to_deg_signed(float rad) {
|
||||
return rad * (180.0f / (float)M_PI);
|
||||
}
|
||||
|
||||
void HandleHeading(const tN2kMsg& msg) {
|
||||
unsigned char sid;
|
||||
double heading = 0.0, deviation = 0.0, variation = 0.0;
|
||||
tN2kHeadingReference ref = N2khr_Unavailable;
|
||||
if (!ParseN2kHeading(msg, sid, heading, deviation, variation, ref)) {
|
||||
return;
|
||||
}
|
||||
if (heading > 1e8) {
|
||||
// N2k "unavailable" markers come through as huge doubles.
|
||||
return;
|
||||
}
|
||||
const float heading_deg = rad_to_deg_pos((float)heading);
|
||||
const bool is_true = (ref == N2khr_true);
|
||||
const uint32_t now = millis();
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_snap.heading_deg = heading_deg;
|
||||
g_snap.is_true = is_true;
|
||||
g_snap.heading_age_ms = now;
|
||||
g_snap.heading_valid = true;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
AR_LOGV(TAG, "PGN 127250 heading=%.2f deg (%s)", heading_deg,
|
||||
is_true ? "true" : "magnetic");
|
||||
}
|
||||
|
||||
void HandleROT(const tN2kMsg& msg) {
|
||||
unsigned char sid;
|
||||
double rot = 0.0;
|
||||
if (!ParseN2kRateOfTurn(msg, sid, rot)) {
|
||||
return;
|
||||
}
|
||||
if (rot > 1e8 || rot < -1e8) {
|
||||
return;
|
||||
}
|
||||
const float rot_dps = rad_to_deg_signed((float)rot);
|
||||
const uint32_t now = millis();
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_snap.rate_of_turn_dps = rot_dps;
|
||||
g_snap.rot_age_ms = now;
|
||||
g_snap.rot_valid = true;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
AR_LOGV(TAG, "PGN 127251 ROT=%.3f deg/s", rot_dps);
|
||||
}
|
||||
|
||||
// One global dispatcher because NMEA2000.SetMsgHandler() takes a single
|
||||
// callback that we have to filter by PGN ourselves.
|
||||
void MessageHandler(const tN2kMsg& msg) {
|
||||
switch (msg.PGN) {
|
||||
case 127250L:
|
||||
HandleHeading(msg);
|
||||
break;
|
||||
case 127251L:
|
||||
HandleROT(msg);
|
||||
break;
|
||||
default:
|
||||
// Sprint 1: ignore everything else.
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void RxTask(void* /*pv*/) {
|
||||
AR_LOGI(TAG, "nmea2000_consumer task started on core %d", xPortGetCoreID());
|
||||
for (;;) {
|
||||
NMEA2000.ParseMessages();
|
||||
// Update validity flags based on age.
|
||||
const uint32_t now = millis();
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
if (g_snap.heading_valid && (now - g_snap.heading_age_ms) > STALE_THRESHOLD_MS) {
|
||||
g_snap.heading_valid = false;
|
||||
}
|
||||
if (g_snap.rot_valid && (now - g_snap.rot_age_ms) > STALE_THRESHOLD_MS) {
|
||||
g_snap.rot_valid = false;
|
||||
}
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
// 100 Hz polling is plenty -- the CAN driver buffers incoming frames.
|
||||
vTaskDelay(pdMS_TO_TICKS(10));
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void nmea2000_consumer_init() {
|
||||
NMEA2000.SetProductInformation(
|
||||
"AR-AP-1", // Manufacturer's Model serial code
|
||||
100, // Manufacturer's product code
|
||||
"AR-Autopilot Controller v1", // Model ID
|
||||
AR_FW_VERSION, // SW version
|
||||
"AR-NMEA-IO v1.0" // Model version
|
||||
);
|
||||
NMEA2000.SetDeviceInformation(
|
||||
1, // Unique number
|
||||
150, // Device function: Autopilot
|
||||
40, // Device class: Steering and Control surfaces
|
||||
2046 // Manufacturer code: reserved/test (real one needs NMEA registration)
|
||||
);
|
||||
NMEA2000.SetMode(tNMEA2000::N2km_ListenAndNode, 25);
|
||||
NMEA2000.EnableForward(false);
|
||||
NMEA2000.SetMsgHandler(&MessageHandler);
|
||||
|
||||
if (NMEA2000.Open()) {
|
||||
AR_LOGI(TAG,
|
||||
"nmea2000_consumer_init: stack open, CAN TX=%d RX=%d, "
|
||||
"subscribed to PGN 127250 + 127251",
|
||||
PIN_CAN_TX, PIN_CAN_RX);
|
||||
} else {
|
||||
AR_LOGE(TAG,
|
||||
"nmea2000_consumer_init: NMEA2000.Open() failed (CAN driver "
|
||||
"init error). System will continue but no N2K traffic.");
|
||||
}
|
||||
}
|
||||
|
||||
void nmea2000_consumer_start_task() {
|
||||
xTaskCreatePinnedToCore(RxTask, "n2k_rx", AR_TASK_STACK_N2K_RX, nullptr,
|
||||
AR_TASK_PRIO_N2K_RX, nullptr, AR_TASK_CORE_COMMS);
|
||||
}
|
||||
|
||||
HeadingSnapshot nmea2000_latest() {
|
||||
HeadingSnapshot copy;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
copy = g_snap;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
return copy;
|
||||
}
|
||||
|
||||
bool nmea2000_is_stale() {
|
||||
const auto s = nmea2000_latest();
|
||||
return !s.heading_valid;
|
||||
}
|
||||
|
||||
} // namespace arautopilot::protocols::nmea2000
|
||||
Reference in New Issue
Block a user