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,106 @@
|
||||
# =============================================================================
|
||||
# AR-Autopilot -- Modbus RTU register map (single source of truth)
|
||||
# =============================================================================
|
||||
#
|
||||
# This file is read by tools/gen_modbus_registers.py to produce:
|
||||
#
|
||||
# firmware/ar_autopilot_v1/src/protocols/modbus_registers.h
|
||||
# -> compile-time constants used by the firmware
|
||||
#
|
||||
# arautopilot/shared/modbus_register_map.py
|
||||
# -> runtime constants used by the Studio simulator, the dedicated
|
||||
# display (via the Flutter side mirror), and the Python tools
|
||||
#
|
||||
# Do NOT edit the generated files by hand. Always edit this YAML and run:
|
||||
#
|
||||
# python tools/gen_modbus_registers.py
|
||||
#
|
||||
# Register types (Modbus RTU standard):
|
||||
# discrete -- read-only single bit (function code 0x02) addr 10001+
|
||||
# coil -- read-write single bit (function codes 0x01/0x05/0x0F) addr 1+
|
||||
# input -- read-only 16-bit register (function code 0x04) addr 30001+
|
||||
# holding -- read-write 16-bit register (function codes 0x03/0x06/0x10) addr 40001+
|
||||
#
|
||||
# Addresses below are zero-based offsets within their type. Modbus over the
|
||||
# wire adds the type base (10001 / 1 / 30001 / 40001).
|
||||
#
|
||||
# Scale factor: physical value = raw * scale + offset
|
||||
# e.g. heading_deg with scale=0.01 means raw 4500 -> 45.00 deg.
|
||||
# =============================================================================
|
||||
|
||||
schema_version: "0.1.0"
|
||||
slave_id: 1 # Modbus slave (server) address
|
||||
baudrate: 38400 # bps; 8N1 framing assumed
|
||||
parity: "N"
|
||||
data_bits: 8
|
||||
stop_bits: 1
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Discrete inputs (read-only bits) -- status flags published by the firmware
|
||||
# -----------------------------------------------------------------------------
|
||||
discretes:
|
||||
- { addr: 0, name: PILOT_ENGAGED, desc: "1 if the pilot is currently engaged (any mode other than STANDBY)" }
|
||||
- { addr: 1, name: DI_DISENGAGE_BUTTON, desc: "Live state of the physical Engage/Disengage push-button" }
|
||||
- { addr: 2, name: DI_LIMIT_PORT, desc: "Port-side rudder mechanical end-stop reached" }
|
||||
- { addr: 3, name: DI_LIMIT_STBD, desc: "Starboard-side rudder mechanical end-stop reached" }
|
||||
- { addr: 4, name: DI_EXTERNAL_ALARM, desc: "External critical alarm (VMS / genset) asserted" }
|
||||
- { addr: 5, name: DI_MANUAL_CONFIRM, desc: "Manual confirmation switch asserted (emergency override)" }
|
||||
- { addr: 8, name: ACTUATOR_POWER, desc: "Master actuator power relay (DO3) commanded ON" }
|
||||
- { addr: 9, name: ACTUATOR_DRIVING_PORT, desc: "Actuator direction output: driving to port" }
|
||||
- { addr: 10, name: ACTUATOR_DRIVING_STBD, desc: "Actuator direction output: driving to starboard" }
|
||||
- { addr: 16, name: ALARM_OFF_COURSE, desc: "Off-course warning active" }
|
||||
- { addr: 17, name: ALARM_OFF_COURSE_SEVERE, desc: "Severe off-course alarm (auto-disengage)" }
|
||||
- { addr: 18, name: ALARM_RUDDER_NOT_RESP, desc: "Rudder command sent but no feedback motion" }
|
||||
- { addr: 19, name: ALARM_HEADING_LOST, desc: "NMEA 2000 heading PGN not received >5 s" }
|
||||
- { addr: 20, name: ALARM_ACTUATOR_OVERCURR, desc: "Actuator current over threshold" }
|
||||
- { addr: 21, name: ALARM_VOLTAGE_LOW, desc: "Supply voltage below safe threshold" }
|
||||
- { addr: 22, name: ALARM_LIMIT_REACHED, desc: "Rudder reached mechanical end-stop" }
|
||||
- { addr: 23, name: ALARM_WATCHDOG_TRIPPED, desc: "Firmware watchdog fired -- controller reset" }
|
||||
- { addr: 24, name: ALARM_VMS_CRITICAL, desc: "VMS reported blackout or electrical fault" }
|
||||
- { addr: 25, name: ANY_ALARM_ACTIVE, desc: "OR of all alarm bits (convenience)" }
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Coils (read-write bits) -- commands written by the display / Studio
|
||||
# -----------------------------------------------------------------------------
|
||||
coils:
|
||||
- { addr: 0, name: CMD_ENGAGE_REQUEST, desc: "Rising edge requests pilot engagement (subject to interlocks)" }
|
||||
- { addr: 1, name: CMD_DISENGAGE_REQUEST, desc: "Rising edge forces pilot to STANDBY" }
|
||||
- { addr: 2, name: CMD_ACK_ALL_ALARMS, desc: "Rising edge acknowledges every active alarm" }
|
||||
- { addr: 3, name: CMD_KNOB_ARM, desc: "Knob arming request (Sprint 7+)" }
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Input registers (read-only 16-bit words) -- telemetry
|
||||
# -----------------------------------------------------------------------------
|
||||
# Two-register values (e.g. timestamps, floats) occupy consecutive addresses.
|
||||
# -----------------------------------------------------------------------------
|
||||
inputs:
|
||||
- { addr: 0, name: FW_VERSION_MAJOR, desc: "Firmware major version", unit: "" }
|
||||
- { addr: 1, name: FW_VERSION_MINOR, desc: "Firmware minor version", unit: "" }
|
||||
- { addr: 2, name: FW_VERSION_PATCH, desc: "Firmware patch version", unit: "" }
|
||||
- { addr: 3, name: SCHEMA_VERSION, desc: "Modbus map schema version (0=v0.1.0)", unit: "" }
|
||||
- { addr: 4, name: UPTIME_SECONDS_LO, desc: "Uptime seconds, low 16 bits", unit: "s" }
|
||||
- { addr: 5, name: UPTIME_SECONDS_HI, desc: "Uptime seconds, high 16 bits", unit: "s" }
|
||||
- { addr: 6, name: CURRENT_MODE, desc: "Current AutopilotMode (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)", unit: "" }
|
||||
- { addr: 7, name: FREE_HEAP_KB, desc: "Current free heap, KiB", unit: "KiB" }
|
||||
- { addr: 8, name: MIN_FREE_HEAP_KB, desc: "Minimum free heap since boot", unit: "KiB" }
|
||||
|
||||
- { addr: 16, name: RUDDER_ANGLE_DEG_X100, desc: "Filtered rudder angle, deg * 100 (-3500..+3500)", unit: "deg", scale: 0.01 }
|
||||
- { addr: 17, name: RUDDER_RAW_ADC, desc: "Raw ADC reading after median filter (0..4095)", unit: "counts" }
|
||||
- { addr: 18, name: RUDDER_VALID, desc: "1 if median filter has filled (>=5 samples)", unit: "" }
|
||||
|
||||
- { addr: 24, name: HEADING_DEG_X100, desc: "Current heading from NMEA 2000 PGN 127250, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
|
||||
- { addr: 25, name: ROT_DPS_X100, desc: "Rate of turn from PGN 127251, deg/s*100 (signed int16)", unit: "deg/s", scale: 0.01 }
|
||||
- { addr: 26, name: HEADING_AGE_MS, desc: "Milliseconds since the last heading update (0..60000)", unit: "ms" }
|
||||
|
||||
- { addr: 32, name: BATTERY_VOLTAGE_X100, desc: "System battery voltage, V*100", unit: "V", scale: 0.01 }
|
||||
- { addr: 33, name: ACTUATOR_CURRENT_X100, desc: "Actuator current, A*100", unit: "A", scale: 0.01 }
|
||||
|
||||
# -----------------------------------------------------------------------------
|
||||
# Holding registers (read-write 16-bit words) -- setpoints and config
|
||||
# -----------------------------------------------------------------------------
|
||||
holdings:
|
||||
- { addr: 0, name: MODE_REQUEST, desc: "Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)", unit: "" }
|
||||
- { addr: 1, name: HEADING_SETPOINT_X100, desc: "Desired heading, deg*100", unit: "deg", scale: 0.01 }
|
||||
- { addr: 2, name: BRIGHTNESS_PCT, desc: "Display brightness 0..100", unit: "%" }
|
||||
- { addr: 3, name: ALARM_VOLUME_PCT, desc: "Alarm volume 0..100", unit: "%" }
|
||||
- { addr: 8, name: DODGE_OFFSET_DEG_X100, desc: "Dodge mode heading offset, deg*100 (signed int16)", unit: "deg", scale: 0.01 }
|
||||
@@ -0,0 +1,123 @@
|
||||
; =============================================================================
|
||||
; AR-Autopilot v1 -- firmware build configuration
|
||||
; =============================================================================
|
||||
;
|
||||
; Target hardware: AR-NMEA-IO v1.0 (ESP32-DOWD-V3, dual-core 240 MHz)
|
||||
; Build system: PlatformIO 6.1+
|
||||
; Framework: arduino (on ESP32 -- includes FreeRTOS, full Espressif stack)
|
||||
;
|
||||
; This file is the single source of truth for the firmware build. Versions of
|
||||
; the platform, framework packages, and library dependencies are pinned for
|
||||
; reproducibility.
|
||||
;
|
||||
; Tasks (from the project root):
|
||||
;
|
||||
; .venv/Scripts/pio.exe run # compile (default env: esp32-dev)
|
||||
; .venv/Scripts/pio.exe run -e esp32-debug # compile with extra debug flags
|
||||
; .venv/Scripts/pio.exe run -e native # build host-side Unity tests
|
||||
; .venv/Scripts/pio.exe test -e native # run host-side Unity tests
|
||||
; .venv/Scripts/pio.exe check # static analysis (cppcheck)
|
||||
; .venv/Scripts/pio.exe run -t upload # flash (needs the board plugged in)
|
||||
;
|
||||
; =============================================================================
|
||||
|
||||
[platformio]
|
||||
src_dir = src
|
||||
include_dir = include
|
||||
test_dir = test
|
||||
default_envs = esp32-dev
|
||||
|
||||
; -----------------------------------------------------------------------------
|
||||
; Common settings shared by every ESP32 env
|
||||
; -----------------------------------------------------------------------------
|
||||
[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 ; ARDUHAL_LOG_LEVEL_INFO
|
||||
-DAR_FW_VERSION=\"0.1.0-sprint1\"
|
||||
-DAR_FW_BUILD_TIMESTAMP=__TIMESTAMP__
|
||||
-Wall
|
||||
-Wextra
|
||||
-Wno-unused-parameter
|
||||
-Wno-missing-field-initializers
|
||||
build_unflags =
|
||||
-std=gnu++11
|
||||
|
||||
; Library dependencies pinned for reproducibility.
|
||||
; These are the Sprint 1 dependencies; later sprints may add more.
|
||||
lib_deps =
|
||||
ttlappalainen/NMEA2000-library@^4.22.0
|
||||
ttlappalainen/NMEA2000_esp32@^1.0.3
|
||||
https://github.com/eModbus/eModbus.git#v1.7.4.stable
|
||||
|
||||
; Files we never want to compile / lint:
|
||||
; - Documentation
|
||||
; - Test fixtures (handled by env:native)
|
||||
lib_ignore =
|
||||
test
|
||||
|
||||
; -----------------------------------------------------------------------------
|
||||
; Production build for the AR-NMEA-IO board
|
||||
; -----------------------------------------------------------------------------
|
||||
[env:esp32-dev]
|
||||
board = esp32dev
|
||||
board_build.partitions = default.csv
|
||||
build_type = release
|
||||
build_flags =
|
||||
${env.build_flags}
|
||||
-Os
|
||||
-DAR_BUILD_VARIANT=\"release\"
|
||||
|
||||
; -----------------------------------------------------------------------------
|
||||
; Debug build (-O0, extra logging)
|
||||
; -----------------------------------------------------------------------------
|
||||
[env:esp32-debug]
|
||||
board = esp32dev
|
||||
build_type = debug
|
||||
build_flags =
|
||||
${env.build_flags}
|
||||
-O0
|
||||
-ggdb
|
||||
-DAR_BUILD_VARIANT=\"debug\"
|
||||
-DCORE_DEBUG_LEVEL=5 ; ARDUHAL_LOG_LEVEL_VERBOSE
|
||||
|
||||
; -----------------------------------------------------------------------------
|
||||
; Host-side Unity tests (run on the developer machine, no ESP32 needed)
|
||||
; -----------------------------------------------------------------------------
|
||||
; Used for pure-logic tests: ring buffers, median filters, the Modbus
|
||||
; register map, the PID math, etc. Anything that talks to real hardware
|
||||
; lives in env:esp32-dev tests and runs only on the board.
|
||||
;
|
||||
; NOTE: this env does NOT inherit [env] (which targets ESP32 + Arduino).
|
||||
[env:native]
|
||||
platform = native
|
||||
framework =
|
||||
test_framework = unity
|
||||
test_build_src = no
|
||||
build_flags =
|
||||
-std=gnu++17
|
||||
-DAR_HOST_TEST=1
|
||||
-DUNITY_INCLUDE_DOUBLE
|
||||
-Wall
|
||||
-Wextra
|
||||
build_unflags =
|
||||
lib_deps =
|
||||
lib_ignore =
|
||||
monitor_filters =
|
||||
|
||||
; -----------------------------------------------------------------------------
|
||||
; Static analysis
|
||||
; -----------------------------------------------------------------------------
|
||||
[env:check]
|
||||
platform = espressif32@^6.7.0
|
||||
framework = arduino
|
||||
board = esp32dev
|
||||
check_tool = cppcheck
|
||||
check_flags =
|
||||
cppcheck: --enable=warning,style,performance,portability --inline-suppr
|
||||
check_skip_packages = yes
|
||||
check_severity = medium, high
|
||||
@@ -0,0 +1,76 @@
|
||||
// =============================================================================
|
||||
// median.h -- fixed-size median filter
|
||||
// =============================================================================
|
||||
//
|
||||
// Templated median filter that holds the last N samples in a ring buffer and
|
||||
// returns the median of the current window in O(N log N) (sort-copy). N is
|
||||
// expected to be small (3-9), so the cost is negligible at 100 Hz.
|
||||
//
|
||||
// Used by:
|
||||
// - hal/rudder_sensor.cpp (5-sample median to suppress ADC spikes)
|
||||
//
|
||||
// Host-testable: depends only on the standard library. The Unity tests in
|
||||
// test/test_filters/ exercise it on the developer machine.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
|
||||
namespace arautopilot::filters {
|
||||
|
||||
template <typename T, std::size_t N>
|
||||
class MedianFilter {
|
||||
static_assert(N >= 1, "MedianFilter window must be at least 1");
|
||||
static_assert(N % 2 == 1, "MedianFilter window must be odd for an unambiguous median");
|
||||
|
||||
public:
|
||||
MedianFilter() = default;
|
||||
|
||||
/// Push one sample. Until N samples have been pushed the filter
|
||||
/// returns the median over only the samples received so far.
|
||||
T push(T sample) {
|
||||
buffer_[write_idx_] = sample;
|
||||
write_idx_ = (write_idx_ + 1) % N;
|
||||
if (count_ < N) {
|
||||
++count_;
|
||||
}
|
||||
return median();
|
||||
}
|
||||
|
||||
/// Median of the current window (without pushing). Returns T{} if
|
||||
/// no samples have been pushed yet.
|
||||
T median() const {
|
||||
if (count_ == 0) {
|
||||
return T{};
|
||||
}
|
||||
std::array<T, N> sorted{};
|
||||
for (std::size_t i = 0; i < count_; ++i) {
|
||||
sorted[i] = buffer_[i];
|
||||
}
|
||||
std::sort(sorted.begin(), sorted.begin() + count_);
|
||||
return sorted[count_ / 2];
|
||||
}
|
||||
|
||||
/// Number of valid samples currently stored (0..N).
|
||||
std::size_t size() const { return count_; }
|
||||
|
||||
/// True once the window has been filled at least once.
|
||||
bool is_full() const { return count_ == N; }
|
||||
|
||||
/// Forget all samples; next push() starts fresh.
|
||||
void reset() {
|
||||
write_idx_ = 0;
|
||||
count_ = 0;
|
||||
buffer_.fill(T{});
|
||||
}
|
||||
|
||||
private:
|
||||
std::array<T, N> buffer_{};
|
||||
std::size_t write_idx_{0};
|
||||
std::size_t count_{0};
|
||||
};
|
||||
|
||||
} // namespace arautopilot::filters
|
||||
@@ -0,0 +1,158 @@
|
||||
// =============================================================================
|
||||
// di_do.cpp -- generic DI/DO implementation
|
||||
// =============================================================================
|
||||
|
||||
#include "di_do.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include <array>
|
||||
|
||||
#include "../system/ar_log.h"
|
||||
#include "pinout.h"
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
namespace {
|
||||
constexpr const char* TAG = "AR/HAL";
|
||||
|
||||
// Tunable: how many consecutive identical samples constitute a confirmed
|
||||
// state for a DI. At a 50 Hz poll rate, 2 -> ~40 ms debounce.
|
||||
constexpr uint8_t DI_DEBOUNCE_SAMPLES = 2;
|
||||
|
||||
struct DiSlot {
|
||||
uint8_t pin;
|
||||
bool state; // last confirmed (debounced) state
|
||||
bool prev_state; // state on the *previous* poll, for edge detection
|
||||
uint8_t stable_count; // consecutive identical raw reads
|
||||
bool raw_last; // last raw read
|
||||
};
|
||||
|
||||
constexpr std::size_t DI_COUNT = 5;
|
||||
std::array<DiSlot, DI_COUNT> g_di_slots{
|
||||
DiSlot{PIN_DI1_DISENGAGE_BUTTON, false, false, 0, false},
|
||||
DiSlot{PIN_DI2_LIMIT_SWITCH_PORT, false, false, 0, false},
|
||||
DiSlot{PIN_DI3_LIMIT_SWITCH_STBD, false, false, 0, false},
|
||||
DiSlot{PIN_DI4_EXTERNAL_ALARM, false, false, 0, false},
|
||||
DiSlot{PIN_DI5_MANUAL_CONFIRM, false, false, 0, false},
|
||||
};
|
||||
|
||||
struct DoSlot {
|
||||
uint8_t pin;
|
||||
bool state;
|
||||
};
|
||||
|
||||
constexpr std::size_t DO_COUNT = 10;
|
||||
std::array<DoSlot, DO_COUNT> g_do_slots{
|
||||
DoSlot{PIN_DO1_PUMP_PORT, false}, DoSlot{PIN_DO2_PUMP_STBD, false},
|
||||
DoSlot{PIN_DO3_ACTUATOR_POWER, false}, DoSlot{PIN_DO4_BUZZER, false},
|
||||
DoSlot{PIN_DO5_ENGAGED_LAMP, false}, DoSlot{PIN_DO6_RESERVED, false},
|
||||
DoSlot{PIN_DO7_RESERVED, false}, DoSlot{PIN_DO8_RESERVED, false},
|
||||
DoSlot{PIN_DO9_RESERVED, false}, DoSlot{PIN_DO10_RESERVED, false},
|
||||
};
|
||||
|
||||
DiSlot* find_di(uint8_t pin) {
|
||||
for (auto& slot : g_di_slots) {
|
||||
if (slot.pin == pin) {
|
||||
return &slot;
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
DoSlot* find_do(uint8_t pin) {
|
||||
for (auto& slot : g_do_slots) {
|
||||
if (slot.pin == pin) {
|
||||
return &slot;
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
void di_init() {
|
||||
for (auto& slot : g_di_slots) {
|
||||
pinMode(slot.pin, INPUT_PULLUP);
|
||||
// INPUT_PULLUP means a closed-to-ground switch reads LOW=pressed.
|
||||
// Our logical "true" = "pressed/active", so we invert in di_poll().
|
||||
slot.state = false;
|
||||
slot.prev_state = false;
|
||||
slot.stable_count = 0;
|
||||
slot.raw_last = false;
|
||||
}
|
||||
AR_LOGI(TAG, "di_init: %u digital inputs configured (INPUT_PULLUP)",
|
||||
(unsigned)g_di_slots.size());
|
||||
}
|
||||
|
||||
void di_poll() {
|
||||
for (auto& slot : g_di_slots) {
|
||||
// Active-low (INPUT_PULLUP + switch-to-GND).
|
||||
bool raw = (digitalRead(slot.pin) == LOW);
|
||||
slot.prev_state = slot.state;
|
||||
if (raw == slot.raw_last) {
|
||||
if (slot.stable_count < DI_DEBOUNCE_SAMPLES) {
|
||||
++slot.stable_count;
|
||||
}
|
||||
if (slot.stable_count >= DI_DEBOUNCE_SAMPLES) {
|
||||
slot.state = raw;
|
||||
}
|
||||
} else {
|
||||
slot.stable_count = 0;
|
||||
}
|
||||
slot.raw_last = raw;
|
||||
}
|
||||
}
|
||||
|
||||
bool di_read(uint8_t pin) {
|
||||
auto* slot = find_di(pin);
|
||||
return slot != nullptr ? slot->state : false;
|
||||
}
|
||||
|
||||
bool di_rising_edge(uint8_t pin) {
|
||||
auto* slot = find_di(pin);
|
||||
return slot != nullptr && slot->state && !slot->prev_state;
|
||||
}
|
||||
|
||||
bool di_falling_edge(uint8_t pin) {
|
||||
auto* slot = find_di(pin);
|
||||
return slot != nullptr && !slot->state && slot->prev_state;
|
||||
}
|
||||
|
||||
void do_init() {
|
||||
for (auto& slot : g_do_slots) {
|
||||
pinMode(slot.pin, OUTPUT);
|
||||
digitalWrite(slot.pin, LOW);
|
||||
slot.state = false;
|
||||
}
|
||||
AR_LOGI(TAG, "do_init: %u digital outputs configured (driven LOW)",
|
||||
(unsigned)g_do_slots.size());
|
||||
}
|
||||
|
||||
bool do_write(uint8_t pin, bool high) {
|
||||
auto* slot = find_do(pin);
|
||||
if (slot == nullptr) {
|
||||
AR_LOGW(TAG, "do_write: unknown pin %d", (int)pin);
|
||||
return false;
|
||||
}
|
||||
if (slot->state == high) {
|
||||
return false;
|
||||
}
|
||||
digitalWrite(slot->pin, high ? HIGH : LOW);
|
||||
slot->state = high;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool do_state(uint8_t pin) {
|
||||
auto* slot = find_do(pin);
|
||||
return slot != nullptr ? slot->state : false;
|
||||
}
|
||||
|
||||
void do_all_off() {
|
||||
for (auto& slot : g_do_slots) {
|
||||
digitalWrite(slot.pin, LOW);
|
||||
slot.state = false;
|
||||
}
|
||||
AR_LOGW(TAG, "do_all_off: all digital outputs driven LOW");
|
||||
}
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -0,0 +1,57 @@
|
||||
// =============================================================================
|
||||
// di_do.h -- generic digital-input / digital-output helpers
|
||||
// =============================================================================
|
||||
//
|
||||
// Thin layer above Arduino digitalRead / digitalWrite that:
|
||||
// - Tracks the *logical* state of every input/output so other tasks can
|
||||
// query without re-reading the pin.
|
||||
// - Debounces DIs in software (configurable, default 20 ms).
|
||||
// - Caches the last commanded DO state so we never write the same value
|
||||
// twice (cheap optimisation for slow open-drain outputs).
|
||||
//
|
||||
// Pins handled by this module are the ones listed in hal/pinout.h.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
// ----- Digital inputs -------------------------------------------------------
|
||||
|
||||
/// Initialise all DI pins declared in pinout.h with INPUT_PULLUP.
|
||||
/// Must be called once during setup() before any di_read() call.
|
||||
void di_init();
|
||||
|
||||
/// Sample every DI once, run the debounce filter, and store the result.
|
||||
/// Intended to be called from the safety task at a fixed rate (50 Hz).
|
||||
void di_poll();
|
||||
|
||||
/// Return the last debounced logical state of a DI pin.
|
||||
/// Pin must be one of the PIN_DIx_* constants from pinout.h.
|
||||
bool di_read(uint8_t pin);
|
||||
|
||||
/// Return true if the pin changed state on the most recent di_poll() call
|
||||
/// (edge-detect). Useful for the disengage button.
|
||||
bool di_rising_edge(uint8_t pin);
|
||||
bool di_falling_edge(uint8_t pin);
|
||||
|
||||
// ----- Digital outputs ------------------------------------------------------
|
||||
|
||||
/// Initialise all DO pins declared in pinout.h as OUTPUT and drive them LOW.
|
||||
/// Must be called once during setup() before any do_write() call.
|
||||
void do_init();
|
||||
|
||||
/// Set a DO pin to the given logical state. No-op if the pin was already in
|
||||
/// that state. Returns true if the write actually happened.
|
||||
/// Pin must be one of the PIN_DOx_* constants from pinout.h.
|
||||
bool do_write(uint8_t pin, bool high);
|
||||
|
||||
/// Return the last commanded state of a DO pin.
|
||||
bool do_state(uint8_t pin);
|
||||
|
||||
/// Drive every DO LOW. Called by the safety system on emergency disengage.
|
||||
void do_all_off();
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -0,0 +1,115 @@
|
||||
// =============================================================================
|
||||
// rudder_actuator.cpp -- rudder actuator driver (Sprint 1 stub)
|
||||
// =============================================================================
|
||||
|
||||
#include "rudder_actuator.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "../modes/standby.h"
|
||||
#include "../system/ar_log.h"
|
||||
#include "di_do.h"
|
||||
#include "pinout.h"
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
namespace {
|
||||
constexpr const char* TAG = "AR/HAL";
|
||||
|
||||
bool g_power_on = false;
|
||||
int8_t g_last_command_pct = 0;
|
||||
} // namespace
|
||||
|
||||
void rudder_actuator_init() {
|
||||
// do_init() in di_do.cpp already configured DO1..DO10 as outputs LOW.
|
||||
// Just make sure our local state matches and assert the safe defaults.
|
||||
do_write(PIN_DO1_PUMP_PORT, false);
|
||||
do_write(PIN_DO2_PUMP_STBD, false);
|
||||
do_write(PIN_DO3_ACTUATOR_POWER, false);
|
||||
g_power_on = false;
|
||||
g_last_command_pct = 0;
|
||||
AR_LOGI(TAG, "rudder_actuator_init: power OFF, both directions LOW");
|
||||
}
|
||||
|
||||
void rudder_actuator_power_on() {
|
||||
do_write(PIN_DO1_PUMP_PORT, false); // ensure direction outputs LOW before
|
||||
do_write(PIN_DO2_PUMP_STBD, false); // we energise the relay
|
||||
do_write(PIN_DO3_ACTUATOR_POWER, true);
|
||||
g_power_on = true;
|
||||
AR_LOGI(TAG, "rudder_actuator: master power ON");
|
||||
}
|
||||
|
||||
void rudder_actuator_power_off() {
|
||||
do_write(PIN_DO1_PUMP_PORT, false);
|
||||
do_write(PIN_DO2_PUMP_STBD, false);
|
||||
do_write(PIN_DO3_ACTUATOR_POWER, false);
|
||||
g_power_on = false;
|
||||
g_last_command_pct = 0;
|
||||
AR_LOGW(TAG, "rudder_actuator: master power OFF (interlock or operator)");
|
||||
}
|
||||
|
||||
bool rudder_actuator_is_powered() { return g_power_on; }
|
||||
|
||||
bool rudder_actuator_at_limit() {
|
||||
return di_read(PIN_DI2_LIMIT_SWITCH_PORT) ||
|
||||
di_read(PIN_DI3_LIMIT_SWITCH_STBD);
|
||||
}
|
||||
|
||||
bool rudder_command(int8_t pwm_pct) {
|
||||
// ---- Safety interlocks -------------------------------------------------
|
||||
// Interlock #1: master power must be on. Without DO3, the H-bridge or
|
||||
// pump relays are isolated and the direction outputs do nothing -- but
|
||||
// we refuse anyway so the firmware state is consistent.
|
||||
if (!g_power_on) {
|
||||
if (pwm_pct != 0) {
|
||||
AR_LOGW(TAG, "rudder_command(%d): refused -- power OFF",
|
||||
(int)pwm_pct);
|
||||
}
|
||||
do_write(PIN_DO1_PUMP_PORT, false);
|
||||
do_write(PIN_DO2_PUMP_STBD, false);
|
||||
g_last_command_pct = 0;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Interlock #2: STANDBY mode never drives the rudder.
|
||||
if (modes::is_standby()) {
|
||||
if (pwm_pct != 0) {
|
||||
AR_LOGW(TAG, "rudder_command(%d): refused -- STANDBY", (int)pwm_pct);
|
||||
}
|
||||
do_write(PIN_DO1_PUMP_PORT, false);
|
||||
do_write(PIN_DO2_PUMP_STBD, false);
|
||||
g_last_command_pct = 0;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Interlock #3: limit switches block further travel in that direction.
|
||||
if (pwm_pct < 0 && di_read(PIN_DI2_LIMIT_SWITCH_PORT)) {
|
||||
AR_LOGW(TAG, "rudder_command(%d): blocked -- port limit", (int)pwm_pct);
|
||||
do_write(PIN_DO1_PUMP_PORT, false);
|
||||
return false;
|
||||
}
|
||||
if (pwm_pct > 0 && di_read(PIN_DI3_LIMIT_SWITCH_STBD)) {
|
||||
AR_LOGW(TAG, "rudder_command(%d): blocked -- starboard limit",
|
||||
(int)pwm_pct);
|
||||
do_write(PIN_DO2_PUMP_STBD, false);
|
||||
return false;
|
||||
}
|
||||
|
||||
// ---- Direction output --------------------------------------------------
|
||||
// Sprint 1 stub: just toggle the direction outputs. No PWM modulation;
|
||||
// the intensity will be wired in Sprint 2 via LEDC.
|
||||
if (pwm_pct > 0) {
|
||||
do_write(PIN_DO1_PUMP_PORT, false);
|
||||
do_write(PIN_DO2_PUMP_STBD, true);
|
||||
} else if (pwm_pct < 0) {
|
||||
do_write(PIN_DO2_PUMP_STBD, false);
|
||||
do_write(PIN_DO1_PUMP_PORT, true);
|
||||
} else {
|
||||
do_write(PIN_DO1_PUMP_PORT, false);
|
||||
do_write(PIN_DO2_PUMP_STBD, false);
|
||||
}
|
||||
g_last_command_pct = pwm_pct;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -0,0 +1,52 @@
|
||||
// =============================================================================
|
||||
// rudder_actuator.h -- rudder actuator driver (Sprint 1 stub)
|
||||
// =============================================================================
|
||||
//
|
||||
// Drives the rudder pump/motor via DO1 (port direction), DO2 (starboard
|
||||
// direction), and DO3 (master power relay). The actual command output is
|
||||
// produced by the inner PID loop (Sprint 2). In Sprint 1 this module:
|
||||
//
|
||||
// - Exposes a `rudder_command(int8_t pwm_pct)` API.
|
||||
// - Refuses to act if the rudder is at its mechanical limit (DI2/DI3).
|
||||
// - Refuses to act if master power is off (DO3 LOW).
|
||||
// - Refuses to act if the system is in STANDBY (Sprint 1 default).
|
||||
//
|
||||
// All three "refuse" conditions are safety-critical: they remain in place
|
||||
// for every subsequent sprint.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
/// Configure DO1/DO2/DO3 as outputs and drive them all LOW.
|
||||
void rudder_actuator_init();
|
||||
|
||||
/// Engage the master power relay (DO3 HIGH). Required before any
|
||||
/// rudder_command() call has effect.
|
||||
void rudder_actuator_power_on();
|
||||
|
||||
/// Drop the master power relay and stop both directions. Idempotent.
|
||||
void rudder_actuator_power_off();
|
||||
|
||||
/// Send a signed command to the actuator:
|
||||
/// pwm_pct = 0 -> both direction outputs LOW (rudder freewheels)
|
||||
/// pwm_pct > 0 -> drive STARBOARD (DO2 HIGH)
|
||||
/// pwm_pct < 0 -> drive PORT (DO1 HIGH)
|
||||
///
|
||||
/// Sprint 1 stub: only the direction outputs are toggled, no PWM modulation
|
||||
/// (intensity ignored). Sprint 2 will replace the body with a real LEDC PWM.
|
||||
///
|
||||
/// Returns true if the command was actually applied; false if a safety
|
||||
/// interlock blocked it (limit switch, power off, or standby mode).
|
||||
bool rudder_command(int8_t pwm_pct);
|
||||
|
||||
/// True if the master power relay is currently engaged.
|
||||
bool rudder_actuator_is_powered();
|
||||
|
||||
/// True if either limit switch is asserted.
|
||||
bool rudder_actuator_at_limit();
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -0,0 +1,107 @@
|
||||
// =============================================================================
|
||||
// rudder_sensor.cpp -- AI1 rudder-angle sensor with median filter
|
||||
// =============================================================================
|
||||
|
||||
#include "rudder_sensor.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "../filters/median.h"
|
||||
#include "../system/ar_log.h"
|
||||
#include "../system/task_config.h"
|
||||
#include "pinout.h"
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
namespace {
|
||||
constexpr const char* TAG = "AR/HAL";
|
||||
|
||||
// Calibration defaults: full-range ADC mapped to +-35 deg of rudder.
|
||||
// Reasonable for an unconfigured bench but NEVER ship this to a customer
|
||||
// vessel -- it must be replaced during sea-trial commissioning (Sprint 7).
|
||||
constexpr int16_t DEFAULT_RAW_AT_MIN = 0;
|
||||
constexpr int16_t DEFAULT_RAW_AT_MAX = 4095;
|
||||
constexpr float DEFAULT_MIN_DEG = -35.0f;
|
||||
constexpr float DEFAULT_MAX_DEG = +35.0f;
|
||||
|
||||
filters::MedianFilter<int16_t, 5> g_median{};
|
||||
portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
|
||||
RudderReading g_latest{0, 0.0f, 0, false};
|
||||
|
||||
struct Calibration {
|
||||
int16_t raw_at_min;
|
||||
int16_t raw_at_max;
|
||||
float min_deg;
|
||||
float max_deg;
|
||||
};
|
||||
Calibration g_calib{DEFAULT_RAW_AT_MIN, DEFAULT_RAW_AT_MAX, DEFAULT_MIN_DEG,
|
||||
DEFAULT_MAX_DEG};
|
||||
|
||||
float raw_to_deg(int16_t raw, const Calibration& c) {
|
||||
const float span_raw = static_cast<float>(c.raw_at_max - c.raw_at_min);
|
||||
if (span_raw == 0.0f) {
|
||||
return 0.0f;
|
||||
}
|
||||
const float t = (static_cast<float>(raw - c.raw_at_min)) / span_raw;
|
||||
return c.min_deg + t * (c.max_deg - c.min_deg);
|
||||
}
|
||||
|
||||
void SensorTask(void* /*pv*/) {
|
||||
AR_LOGI(TAG, "rudder_sensor task started on core %d, pin %d (ADC1)",
|
||||
xPortGetCoreID(), PIN_AI1_RUDDER_ANGLE);
|
||||
TickType_t last_wake = xTaskGetTickCount();
|
||||
for (;;) {
|
||||
const int raw = analogRead(PIN_AI1_RUDDER_ANGLE);
|
||||
const int16_t filtered = g_median.push(static_cast<int16_t>(raw));
|
||||
Calibration cal;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
cal = g_calib;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
const float angle = raw_to_deg(filtered, cal);
|
||||
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_latest.raw_adc = filtered;
|
||||
g_latest.angle_deg = angle;
|
||||
g_latest.timestamp_ms = millis();
|
||||
g_latest.valid = g_median.is_full();
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
|
||||
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_RUDDER_SENSOR));
|
||||
}
|
||||
}
|
||||
} // namespace
|
||||
|
||||
void rudder_sensor_init() {
|
||||
analogReadResolution(12); // 0..4095
|
||||
analogSetAttenuation(ADC_11db); // ~0..3.3 V input range
|
||||
g_median.reset();
|
||||
AR_LOGI(TAG, "rudder_sensor_init: 12-bit ADC, 11dB attn, %d-sample median",
|
||||
5);
|
||||
}
|
||||
|
||||
void rudder_sensor_start_task() {
|
||||
xTaskCreatePinnedToCore(SensorTask, "rudder_sensor",
|
||||
AR_TASK_STACK_RUDDER_SENSOR, nullptr,
|
||||
AR_TASK_PRIO_RUDDER_SENSOR, nullptr,
|
||||
AR_TASK_CORE_REALTIME);
|
||||
}
|
||||
|
||||
RudderReading rudder_sensor_latest() {
|
||||
RudderReading copy;
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
copy = g_latest;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
return copy;
|
||||
}
|
||||
|
||||
void rudder_sensor_set_calibration(int16_t raw_at_min_deg, int16_t raw_at_max_deg,
|
||||
float min_deg, float max_deg) {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_calib = {raw_at_min_deg, raw_at_max_deg, min_deg, max_deg};
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
AR_LOGI(TAG,
|
||||
"rudder_sensor calibration updated: raw [%d..%d] -> deg [%.2f..%.2f]",
|
||||
raw_at_min_deg, raw_at_max_deg, min_deg, max_deg);
|
||||
}
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -0,0 +1,47 @@
|
||||
// =============================================================================
|
||||
// rudder_sensor.h -- AI1 rudder-angle sensor with median filter
|
||||
// =============================================================================
|
||||
//
|
||||
// Reads the raw ADC value of the rudder position sensor (pot, Hall, or
|
||||
// 4-20 mA loop conditioned to 0-3.3 V) on PIN_AI1_RUDDER_ANGLE, applies a
|
||||
// 5-sample median filter to suppress single-sample spikes, and converts the
|
||||
// result to degrees via a linear calibration.
|
||||
//
|
||||
// Calibration in Sprint 1 is a hard-coded placeholder (-35 deg @ 0 raw,
|
||||
// +35 deg @ 4095 raw). In Sprint 7 (commissioning), the calibration values
|
||||
// will come from the persistent NVS store written during sea trial.
|
||||
//
|
||||
// The sensor task runs at 100 Hz on Core 1 (real-time core).
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
struct RudderReading {
|
||||
int16_t raw_adc; ///< 0..4095, raw ADC after median filter
|
||||
float angle_deg; ///< -35.0 .. +35.0 typical, after calibration
|
||||
uint32_t timestamp_ms; ///< millis() at which the sample was taken
|
||||
bool valid; ///< false until the filter has filled
|
||||
};
|
||||
|
||||
/// Configure ADC1 channel for AI1, init the median filter.
|
||||
void rudder_sensor_init();
|
||||
|
||||
/// Start the FreeRTOS sampling task (100 Hz, Core 1).
|
||||
void rudder_sensor_start_task();
|
||||
|
||||
/// Get the latest filtered reading. Thread-safe (single critical section).
|
||||
RudderReading rudder_sensor_latest();
|
||||
|
||||
/// Override the linear calibration. Called by the commissioning code
|
||||
/// (Sprint 7); the placeholder is fine for Sprint 1 bench work.
|
||||
///
|
||||
/// raw_at_min_deg, raw_at_max_deg : ADC counts (0..4095)
|
||||
/// min_deg, max_deg : physical angle in degrees
|
||||
void rudder_sensor_set_calibration(int16_t raw_at_min_deg, int16_t raw_at_max_deg,
|
||||
float min_deg, float max_deg);
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -0,0 +1,98 @@
|
||||
// =============================================================================
|
||||
// AR-Autopilot v1 -- main.cpp
|
||||
// =============================================================================
|
||||
//
|
||||
// Sprint 1 firmware entry point. Boots in STANDBY mode, spawns the FreeRTOS
|
||||
// tasks declared in system/task_config.h, and returns control to the Arduino
|
||||
// scheduler. The loop() function intentionally does nothing -- every long-
|
||||
// running activity lives in a pinned task.
|
||||
//
|
||||
// Task layout summary (full table in system/task_config.h):
|
||||
//
|
||||
// Core 1 (real-time): PID inner, PID outer, rudder sensor, safety
|
||||
// Core 0 (comms): NMEA 2000 RX, Modbus slave, heartbeat, health
|
||||
//
|
||||
// Sprint 1 boots only:
|
||||
// - heartbeat (Core 0) -- LED + uptime log
|
||||
//
|
||||
// Later sprints will register the rest from this same setup() function. The
|
||||
// task_config.h header already reserves their priorities/stacks/cores.
|
||||
// =============================================================================
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "hal/di_do.h"
|
||||
#include "hal/pinout.h"
|
||||
#include "hal/rudder_actuator.h"
|
||||
#include "hal/rudder_sensor.h"
|
||||
#include "modes/standby.h"
|
||||
#include "protocols/modbus_slave.h"
|
||||
#include "protocols/nmea2000_consumer.h"
|
||||
#include "safety/safety_monitor.h"
|
||||
#include "safety/watchdog.h"
|
||||
#include "system/ar_log.h"
|
||||
#include "system/task_config.h"
|
||||
|
||||
// Forward declarations of task-spawning helpers (defined in their own .cpp
|
||||
// files). Keeping main.cpp dependency-free of internals lets each subsystem
|
||||
// own its own task setup code.
|
||||
extern void ar_start_heartbeat_task();
|
||||
|
||||
namespace {
|
||||
constexpr const char* TAG = "AR/MAIN";
|
||||
} // namespace
|
||||
|
||||
void setup() {
|
||||
// Wait for the USB CDC serial port to be ready before we start logging.
|
||||
Serial.begin(115200);
|
||||
delay(200);
|
||||
|
||||
AR_LOGI(TAG, "================================================");
|
||||
AR_LOGI(TAG, " %s -- firmware boot", AR_AUTOPILOT_FW_NAME);
|
||||
AR_LOGI(TAG, " version : %s", AR_FW_VERSION);
|
||||
AR_LOGI(TAG, " variant : %s", AR_BUILD_VARIANT);
|
||||
AR_LOGI(TAG, " build : %s", AR_FW_BUILD_TIMESTAMP);
|
||||
AR_LOGI(TAG, " board : %s", AR_AUTOPILOT_HW_BOARD);
|
||||
AR_LOGI(TAG, " mcu : %s @ %d MHz", AR_AUTOPILOT_MCU,
|
||||
(int)ESP.getCpuFreqMHz());
|
||||
AR_LOGI(TAG, " cores : %d free heap: %u bytes",
|
||||
(int)ESP.getChipCores(), ESP.getFreeHeap());
|
||||
AR_LOGI(TAG, "================================================");
|
||||
|
||||
// Initialise the pilot mode state machine (boots in STANDBY).
|
||||
arautopilot::modes::mode_init();
|
||||
|
||||
// Initialise hardware abstraction layer.
|
||||
arautopilot::hal::di_init();
|
||||
arautopilot::hal::do_init();
|
||||
arautopilot::hal::rudder_sensor_init();
|
||||
arautopilot::hal::rudder_actuator_init();
|
||||
|
||||
// Safety: watchdog + DI override task. Initialised first so the
|
||||
// watchdog is armed before slower subsystems come up.
|
||||
arautopilot::safety::watchdog_init();
|
||||
|
||||
AR_LOGI(TAG, "spawning Sprint 1 tasks ...");
|
||||
|
||||
arautopilot::safety::safety_monitor_start_task();
|
||||
arautopilot::hal::rudder_sensor_start_task();
|
||||
ar_start_heartbeat_task();
|
||||
|
||||
// Modbus slave (server) -- exposes telemetry + commands to the display.
|
||||
arautopilot::protocols::modbus::modbus_slave_init();
|
||||
arautopilot::protocols::modbus::modbus_slave_start();
|
||||
|
||||
// NMEA 2000 consumer (PGN 127250 + 127251 in Sprint 1).
|
||||
arautopilot::protocols::nmea2000::nmea2000_consumer_init();
|
||||
arautopilot::protocols::nmea2000::nmea2000_consumer_start_task();
|
||||
|
||||
AR_LOGI(TAG, "setup() complete; control loop is FreeRTOS-driven.");
|
||||
AR_LOGI(TAG, "current mode: STANDBY (helm is manual)");
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// Intentionally empty. All real work happens in FreeRTOS tasks.
|
||||
// The Arduino loopTask itself runs at priority 1 on Core 1 by default;
|
||||
// we yield generously to keep it out of the way of the real-time tasks.
|
||||
vTaskDelay(pdMS_TO_TICKS(1000));
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
// =============================================================================
|
||||
// modes/standby.cpp -- pilot mode state (Sprint 1)
|
||||
// =============================================================================
|
||||
|
||||
#include "standby.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "../system/ar_log.h"
|
||||
|
||||
namespace arautopilot::modes {
|
||||
|
||||
namespace {
|
||||
constexpr const char* TAG = "AR/MODE";
|
||||
|
||||
portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
|
||||
Mode g_mode = Mode::STANDBY;
|
||||
|
||||
const char* mode_name(Mode m) {
|
||||
switch (m) {
|
||||
case Mode::STANDBY: return "STANDBY";
|
||||
case Mode::HEADING_HOLD: return "HEADING_HOLD";
|
||||
case Mode::TRUE_COURSE: return "TRUE_COURSE";
|
||||
case Mode::TRACK_KEEPING: return "TRACK_KEEPING";
|
||||
case Mode::DODGE: return "DODGE";
|
||||
}
|
||||
return "UNKNOWN";
|
||||
}
|
||||
} // namespace
|
||||
|
||||
void mode_init() {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_mode = Mode::STANDBY;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
AR_LOGI(TAG, "mode_init: %s", mode_name(g_mode));
|
||||
}
|
||||
|
||||
Mode current_mode() {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
Mode m = g_mode;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
return m;
|
||||
}
|
||||
|
||||
bool is_standby() { return current_mode() == Mode::STANDBY; }
|
||||
|
||||
bool request_mode(Mode m) {
|
||||
// Sprint 1: only STANDBY is reachable. The other modes exist in the enum
|
||||
// for forward compatibility (Modbus already has slots for them) but the
|
||||
// PID and modes machinery is not in place yet.
|
||||
if (m == Mode::STANDBY) {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
Mode prev = g_mode;
|
||||
g_mode = Mode::STANDBY;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
if (prev != Mode::STANDBY) {
|
||||
AR_LOGI(TAG, "mode change: %s -> STANDBY", mode_name(prev));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
AR_LOGW(TAG,
|
||||
"request_mode(%s) rejected: only STANDBY is implemented in "
|
||||
"Sprint 1; current mode remains %s",
|
||||
mode_name(m), mode_name(current_mode()));
|
||||
return false;
|
||||
}
|
||||
|
||||
void force_standby(const char* reason) {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
Mode prev = g_mode;
|
||||
g_mode = Mode::STANDBY;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
if (prev != Mode::STANDBY) {
|
||||
AR_LOGE(TAG, "force_standby: %s -> STANDBY (reason: %s)",
|
||||
mode_name(prev), reason != nullptr ? reason : "unspecified");
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace arautopilot::modes
|
||||
@@ -0,0 +1,46 @@
|
||||
// =============================================================================
|
||||
// modes/standby.h -- pilot mode state (Sprint 1)
|
||||
// =============================================================================
|
||||
//
|
||||
// Holds the global "what mode is the pilot in" flag. In Sprint 1 the only
|
||||
// mode that does anything real is STANDBY. The other Phase 1 modes
|
||||
// (HEADING_HOLD, TRUE_COURSE, TRACK_KEEPING, DODGE) are reserved in the
|
||||
// enum but cannot be entered yet -- attempting to set them logs a warning
|
||||
// and stays in STANDBY.
|
||||
//
|
||||
// Later sprints expand this module into a proper state machine driven by
|
||||
// the safety system and the operator's Modbus commands.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace arautopilot::modes {
|
||||
|
||||
enum class Mode : uint8_t {
|
||||
STANDBY = 0,
|
||||
HEADING_HOLD = 1,
|
||||
TRUE_COURSE = 2,
|
||||
TRACK_KEEPING = 3,
|
||||
DODGE = 4,
|
||||
};
|
||||
|
||||
/// Initialise to STANDBY. Must be called once from setup().
|
||||
void mode_init();
|
||||
|
||||
/// Atomically read the current mode.
|
||||
Mode current_mode();
|
||||
|
||||
/// True if and only if current_mode() == STANDBY.
|
||||
bool is_standby();
|
||||
|
||||
/// Try to enter `m`. In Sprint 1 only STANDBY is accepted; anything else
|
||||
/// is rejected with a warning. Returns true on success.
|
||||
bool request_mode(Mode m);
|
||||
|
||||
/// Force-return to STANDBY (called by the safety system on auto-disengage,
|
||||
/// watchdog reset, or the DI1 physical button).
|
||||
void force_standby(const char* reason);
|
||||
|
||||
} // namespace arautopilot::modes
|
||||
@@ -0,0 +1,148 @@
|
||||
// =============================================================================
|
||||
// modbus_registers.h -- AR-Autopilot Modbus RTU register map
|
||||
// =============================================================================
|
||||
//
|
||||
// AUTO-GENERATED. Do not edit by hand.
|
||||
// Source: firmware/ar_autopilot_v1/modbus_registers.yaml
|
||||
// Regenerate with: python tools/gen_modbus_registers.py
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace arautopilot::protocols::modbus {
|
||||
|
||||
constexpr const char* MAP_SCHEMA_VERSION = "0.1.0";
|
||||
constexpr uint8_t SLAVE_ID = 1;
|
||||
constexpr uint32_t BAUDRATE = 38400;
|
||||
constexpr char PARITY = 'N';
|
||||
constexpr uint8_t DATA_BITS = 8;
|
||||
constexpr uint8_t STOP_BITS = 1;
|
||||
|
||||
// ----- Discrete inputs (read-only bits) -----
|
||||
constexpr uint16_t DISCRETE_COUNT = 19;
|
||||
constexpr uint16_t DISCRETE_MAX_ADDR = 25;
|
||||
|
||||
// 1 if the pilot is currently engaged (any mode other than STANDBY)
|
||||
constexpr uint16_t DISCRETE_PILOT_ENGAGED = 0;
|
||||
// Live state of the physical Engage/Disengage push-button
|
||||
constexpr uint16_t DISCRETE_DI_DISENGAGE_BUTTON = 1;
|
||||
// Port-side rudder mechanical end-stop reached
|
||||
constexpr uint16_t DISCRETE_DI_LIMIT_PORT = 2;
|
||||
// Starboard-side rudder mechanical end-stop reached
|
||||
constexpr uint16_t DISCRETE_DI_LIMIT_STBD = 3;
|
||||
// External critical alarm (VMS / genset) asserted
|
||||
constexpr uint16_t DISCRETE_DI_EXTERNAL_ALARM = 4;
|
||||
// Manual confirmation switch asserted (emergency override)
|
||||
constexpr uint16_t DISCRETE_DI_MANUAL_CONFIRM = 5;
|
||||
// Master actuator power relay (DO3) commanded ON
|
||||
constexpr uint16_t DISCRETE_ACTUATOR_POWER = 8;
|
||||
// Actuator direction output: driving to port
|
||||
constexpr uint16_t DISCRETE_ACTUATOR_DRIVING_PORT = 9;
|
||||
// Actuator direction output: driving to starboard
|
||||
constexpr uint16_t DISCRETE_ACTUATOR_DRIVING_STBD = 10;
|
||||
// Off-course warning active
|
||||
constexpr uint16_t DISCRETE_ALARM_OFF_COURSE = 16;
|
||||
// Severe off-course alarm (auto-disengage)
|
||||
constexpr uint16_t DISCRETE_ALARM_OFF_COURSE_SEVERE = 17;
|
||||
// Rudder command sent but no feedback motion
|
||||
constexpr uint16_t DISCRETE_ALARM_RUDDER_NOT_RESP = 18;
|
||||
// NMEA 2000 heading PGN not received >5 s
|
||||
constexpr uint16_t DISCRETE_ALARM_HEADING_LOST = 19;
|
||||
// Actuator current over threshold
|
||||
constexpr uint16_t DISCRETE_ALARM_ACTUATOR_OVERCURR = 20;
|
||||
// Supply voltage below safe threshold
|
||||
constexpr uint16_t DISCRETE_ALARM_VOLTAGE_LOW = 21;
|
||||
// Rudder reached mechanical end-stop
|
||||
constexpr uint16_t DISCRETE_ALARM_LIMIT_REACHED = 22;
|
||||
// Firmware watchdog fired -- controller reset
|
||||
constexpr uint16_t DISCRETE_ALARM_WATCHDOG_TRIPPED = 23;
|
||||
// VMS reported blackout or electrical fault
|
||||
constexpr uint16_t DISCRETE_ALARM_VMS_CRITICAL = 24;
|
||||
// OR of all alarm bits (convenience)
|
||||
constexpr uint16_t DISCRETE_ANY_ALARM_ACTIVE = 25;
|
||||
|
||||
// ----- Coils (read-write bits) -----
|
||||
constexpr uint16_t COIL_COUNT = 4;
|
||||
constexpr uint16_t COIL_MAX_ADDR = 3;
|
||||
|
||||
// Rising edge requests pilot engagement (subject to interlocks)
|
||||
constexpr uint16_t COIL_CMD_ENGAGE_REQUEST = 0;
|
||||
// Rising edge forces pilot to STANDBY
|
||||
constexpr uint16_t COIL_CMD_DISENGAGE_REQUEST = 1;
|
||||
// Rising edge acknowledges every active alarm
|
||||
constexpr uint16_t COIL_CMD_ACK_ALL_ALARMS = 2;
|
||||
// Knob arming request (Sprint 7+)
|
||||
constexpr uint16_t COIL_CMD_KNOB_ARM = 3;
|
||||
|
||||
// ----- Input registers (read-only words) -----
|
||||
constexpr uint16_t INPUT_COUNT = 17;
|
||||
constexpr uint16_t INPUT_MAX_ADDR = 33;
|
||||
|
||||
// Firmware major version
|
||||
constexpr uint16_t INPUT_FW_VERSION_MAJOR = 0;
|
||||
// Firmware minor version
|
||||
constexpr uint16_t INPUT_FW_VERSION_MINOR = 1;
|
||||
// Firmware patch version
|
||||
constexpr uint16_t INPUT_FW_VERSION_PATCH = 2;
|
||||
// Modbus map schema version (0=v0.1.0)
|
||||
constexpr uint16_t INPUT_SCHEMA_VERSION = 3;
|
||||
// Uptime seconds, low 16 bits
|
||||
// unit=s
|
||||
constexpr uint16_t INPUT_UPTIME_SECONDS_LO = 4;
|
||||
// Uptime seconds, high 16 bits
|
||||
// unit=s
|
||||
constexpr uint16_t INPUT_UPTIME_SECONDS_HI = 5;
|
||||
// Current AutopilotMode (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)
|
||||
constexpr uint16_t INPUT_CURRENT_MODE = 6;
|
||||
// Current free heap, KiB
|
||||
// unit=KiB
|
||||
constexpr uint16_t INPUT_FREE_HEAP_KB = 7;
|
||||
// Minimum free heap since boot
|
||||
// unit=KiB
|
||||
constexpr uint16_t INPUT_MIN_FREE_HEAP_KB = 8;
|
||||
// Filtered rudder angle, deg * 100 (-3500..+3500)
|
||||
// unit=deg, scale=0.01
|
||||
constexpr uint16_t INPUT_RUDDER_ANGLE_DEG_X100 = 16;
|
||||
// Raw ADC reading after median filter (0..4095)
|
||||
// unit=counts
|
||||
constexpr uint16_t INPUT_RUDDER_RAW_ADC = 17;
|
||||
// 1 if median filter has filled (>=5 samples)
|
||||
constexpr uint16_t INPUT_RUDDER_VALID = 18;
|
||||
// Current heading from NMEA 2000 PGN 127250, deg*100 (0..35999)
|
||||
// unit=deg, scale=0.01
|
||||
constexpr uint16_t INPUT_HEADING_DEG_X100 = 24;
|
||||
// Rate of turn from PGN 127251, deg/s*100 (signed int16)
|
||||
// unit=deg/s, scale=0.01
|
||||
constexpr uint16_t INPUT_ROT_DPS_X100 = 25;
|
||||
// Milliseconds since the last heading update (0..60000)
|
||||
// unit=ms
|
||||
constexpr uint16_t INPUT_HEADING_AGE_MS = 26;
|
||||
// System battery voltage, V*100
|
||||
// unit=V, scale=0.01
|
||||
constexpr uint16_t INPUT_BATTERY_VOLTAGE_X100 = 32;
|
||||
// Actuator current, A*100
|
||||
// unit=A, scale=0.01
|
||||
constexpr uint16_t INPUT_ACTUATOR_CURRENT_X100 = 33;
|
||||
|
||||
// ----- Holding registers (read-write words) -----
|
||||
constexpr uint16_t HOLDING_COUNT = 5;
|
||||
constexpr uint16_t HOLDING_MAX_ADDR = 8;
|
||||
|
||||
// Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)
|
||||
constexpr uint16_t HOLDING_MODE_REQUEST = 0;
|
||||
// Desired heading, deg*100
|
||||
// unit=deg, scale=0.01
|
||||
constexpr uint16_t HOLDING_HEADING_SETPOINT_X100 = 1;
|
||||
// Display brightness 0..100
|
||||
// unit=%
|
||||
constexpr uint16_t HOLDING_BRIGHTNESS_PCT = 2;
|
||||
// Alarm volume 0..100
|
||||
// unit=%
|
||||
constexpr uint16_t HOLDING_ALARM_VOLUME_PCT = 3;
|
||||
// Dodge mode heading offset, deg*100 (signed int16)
|
||||
// unit=deg, scale=0.01
|
||||
constexpr uint16_t HOLDING_DODGE_OFFSET_DEG_X100 = 8;
|
||||
|
||||
} // namespace arautopilot::protocols::modbus
|
||||
@@ -0,0 +1,382 @@
|
||||
// =============================================================================
|
||||
// modbus_slave.cpp -- eModbus-based RTU server (slave) implementation
|
||||
// =============================================================================
|
||||
//
|
||||
// Sprint 1 scope:
|
||||
// - Input registers (FC 0x04): firmware version, uptime, mode, heap,
|
||||
// rudder angle/raw/valid, heading + ROT (stub for Sprint 1), battery
|
||||
// and actuator current placeholders.
|
||||
// - Discrete inputs (FC 0x02): engaged flag, all DIs, alarm bits.
|
||||
// - Holding registers (FC 0x03/0x06/0x10): mode request, heading
|
||||
// setpoint, brightness, alarm volume, dodge offset.
|
||||
// - Coils (FC 0x01/0x05/0x0F): engage/disengage requests, ack-all,
|
||||
// knob arm (reserved).
|
||||
//
|
||||
// In Sprint 1 the writes that try to change to a non-STANDBY mode are
|
||||
// silently coerced (the modes::request_mode() helper rejects them with a
|
||||
// warning).
|
||||
// =============================================================================
|
||||
|
||||
#include "modbus_slave.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include <ModbusServerRTU.h>
|
||||
|
||||
#include "../hal/di_do.h"
|
||||
#include "../hal/pinout.h"
|
||||
#include "../hal/rudder_sensor.h"
|
||||
#include "../modes/standby.h"
|
||||
#include "../system/ar_log.h"
|
||||
#include "../system/task_config.h"
|
||||
#include "modbus_registers.h"
|
||||
#include "nmea2000_consumer.h"
|
||||
|
||||
namespace arautopilot::protocols::modbus {
|
||||
|
||||
namespace {
|
||||
constexpr const char* TAG = "AR/MB";
|
||||
|
||||
// eModbus server with a 1000 ms timeout for client transactions.
|
||||
ModbusServerRTU g_server(1000, PIN_RS485_DE);
|
||||
|
||||
bool g_running = false;
|
||||
volatile uint32_t g_req_count = 0;
|
||||
volatile uint32_t g_err_count = 0;
|
||||
|
||||
// ----- Storage for the writable side (coils + holding registers) -----------
|
||||
// Discrete inputs and input registers are computed on the fly from the live
|
||||
// telemetry helpers, so they don't need backing storage. Coils and holding
|
||||
// registers do (eModbus expects deterministic reads of the values it last
|
||||
// stored).
|
||||
struct HoldingStorage {
|
||||
uint16_t mode_request = 0;
|
||||
int16_t heading_setpoint_x100 = 0;
|
||||
uint16_t brightness_pct = 80;
|
||||
uint16_t alarm_volume_pct = 60;
|
||||
int16_t dodge_offset_deg_x100 = 0;
|
||||
};
|
||||
HoldingStorage g_holding;
|
||||
|
||||
struct CoilStorage {
|
||||
bool engage_request = false;
|
||||
bool disengage_request = false;
|
||||
bool ack_all = false;
|
||||
bool knob_arm = false;
|
||||
};
|
||||
CoilStorage g_coils;
|
||||
|
||||
// ----- Helpers ---------------------------------------------------------------
|
||||
|
||||
uint16_t read_input_register(uint16_t addr) {
|
||||
switch (addr) {
|
||||
case INPUT_FW_VERSION_MAJOR: return 0;
|
||||
case INPUT_FW_VERSION_MINOR: return 1;
|
||||
case INPUT_FW_VERSION_PATCH: return 0;
|
||||
case INPUT_SCHEMA_VERSION: return 0;
|
||||
case INPUT_UPTIME_SECONDS_LO: return (uint16_t)((millis() / 1000U) & 0xFFFFU);
|
||||
case INPUT_UPTIME_SECONDS_HI: return (uint16_t)(((millis() / 1000U) >> 16) & 0xFFFFU);
|
||||
case INPUT_CURRENT_MODE: return (uint16_t)modes::current_mode();
|
||||
case INPUT_FREE_HEAP_KB: return (uint16_t)(ESP.getFreeHeap() / 1024U);
|
||||
case INPUT_MIN_FREE_HEAP_KB: return (uint16_t)(ESP.getMinFreeHeap() / 1024U);
|
||||
|
||||
case INPUT_RUDDER_ANGLE_DEG_X100: {
|
||||
auto r = hal::rudder_sensor_latest();
|
||||
int v = (int)(r.angle_deg * 100.0f);
|
||||
if (v < -32768) v = -32768;
|
||||
if (v > 32767) v = 32767;
|
||||
return (uint16_t)(int16_t)v;
|
||||
}
|
||||
case INPUT_RUDDER_RAW_ADC: {
|
||||
auto r = hal::rudder_sensor_latest();
|
||||
return (uint16_t)r.raw_adc;
|
||||
}
|
||||
case INPUT_RUDDER_VALID: {
|
||||
auto r = hal::rudder_sensor_latest();
|
||||
return r.valid ? 1 : 0;
|
||||
}
|
||||
|
||||
case INPUT_HEADING_DEG_X100: {
|
||||
auto n = nmea2000::nmea2000_latest();
|
||||
int v = (int)(n.heading_deg * 100.0f);
|
||||
if (v < 0) v = 0;
|
||||
if (v > 35999) v = 35999;
|
||||
return (uint16_t)v;
|
||||
}
|
||||
case INPUT_ROT_DPS_X100: {
|
||||
auto n = nmea2000::nmea2000_latest();
|
||||
int v = (int)(n.rate_of_turn_dps * 100.0f);
|
||||
if (v < -32768) v = -32768;
|
||||
if (v > 32767) v = 32767;
|
||||
return (uint16_t)(int16_t)v;
|
||||
}
|
||||
case INPUT_HEADING_AGE_MS: {
|
||||
auto n = nmea2000::nmea2000_latest();
|
||||
uint32_t age = (millis() - n.heading_age_ms);
|
||||
if (!n.heading_valid) age = 60000;
|
||||
if (age > 60000) age = 60000;
|
||||
return (uint16_t)age;
|
||||
}
|
||||
|
||||
// Sprint 1: battery voltage and actuator current wired in Sprint 6
|
||||
// (Safety + alarms). For now, return 0.
|
||||
case INPUT_BATTERY_VOLTAGE_X100:
|
||||
case INPUT_ACTUATOR_CURRENT_X100:
|
||||
return 0;
|
||||
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
bool read_discrete(uint16_t addr) {
|
||||
switch (addr) {
|
||||
case DISCRETE_PILOT_ENGAGED: return !modes::is_standby();
|
||||
case DISCRETE_DI_DISENGAGE_BUTTON: return hal::di_read(PIN_DI1_DISENGAGE_BUTTON);
|
||||
case DISCRETE_DI_LIMIT_PORT: return hal::di_read(PIN_DI2_LIMIT_SWITCH_PORT);
|
||||
case DISCRETE_DI_LIMIT_STBD: return hal::di_read(PIN_DI3_LIMIT_SWITCH_STBD);
|
||||
case DISCRETE_DI_EXTERNAL_ALARM: return hal::di_read(PIN_DI4_EXTERNAL_ALARM);
|
||||
case DISCRETE_DI_MANUAL_CONFIRM: return hal::di_read(PIN_DI5_MANUAL_CONFIRM);
|
||||
case DISCRETE_ACTUATOR_POWER: return hal::do_state(PIN_DO3_ACTUATOR_POWER);
|
||||
case DISCRETE_ACTUATOR_DRIVING_PORT:return hal::do_state(PIN_DO1_PUMP_PORT);
|
||||
case DISCRETE_ACTUATOR_DRIVING_STBD:return hal::do_state(PIN_DO2_PUMP_STBD);
|
||||
// Alarm bits: stubs in Sprint 1, wired in Sprint 6.
|
||||
case DISCRETE_ALARM_OFF_COURSE:
|
||||
case DISCRETE_ALARM_OFF_COURSE_SEVERE:
|
||||
case DISCRETE_ALARM_RUDDER_NOT_RESP:
|
||||
case DISCRETE_ALARM_HEADING_LOST:
|
||||
case DISCRETE_ALARM_ACTUATOR_OVERCURR:
|
||||
case DISCRETE_ALARM_VOLTAGE_LOW:
|
||||
case DISCRETE_ALARM_LIMIT_REACHED:
|
||||
case DISCRETE_ALARM_WATCHDOG_TRIPPED:
|
||||
case DISCRETE_ALARM_VMS_CRITICAL:
|
||||
case DISCRETE_ANY_ALARM_ACTIVE:
|
||||
return false;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t read_holding(uint16_t addr) {
|
||||
switch (addr) {
|
||||
case HOLDING_MODE_REQUEST: return g_holding.mode_request;
|
||||
case HOLDING_HEADING_SETPOINT_X100: return (uint16_t)g_holding.heading_setpoint_x100;
|
||||
case HOLDING_BRIGHTNESS_PCT: return g_holding.brightness_pct;
|
||||
case HOLDING_ALARM_VOLUME_PCT: return g_holding.alarm_volume_pct;
|
||||
case HOLDING_DODGE_OFFSET_DEG_X100: return (uint16_t)g_holding.dodge_offset_deg_x100;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Returns Modbus exception code (0 = ok). Side effect: stores value.
|
||||
Modbus::Error write_holding(uint16_t addr, uint16_t value) {
|
||||
switch (addr) {
|
||||
case HOLDING_MODE_REQUEST:
|
||||
if (value > 4) return Modbus::Error::ILLEGAL_DATA_VALUE;
|
||||
g_holding.mode_request = value;
|
||||
modes::request_mode((modes::Mode)value);
|
||||
return Modbus::Error::SUCCESS;
|
||||
case HOLDING_HEADING_SETPOINT_X100:
|
||||
g_holding.heading_setpoint_x100 = (int16_t)value;
|
||||
return Modbus::Error::SUCCESS;
|
||||
case HOLDING_BRIGHTNESS_PCT:
|
||||
if (value > 100) return Modbus::Error::ILLEGAL_DATA_VALUE;
|
||||
g_holding.brightness_pct = value;
|
||||
return Modbus::Error::SUCCESS;
|
||||
case HOLDING_ALARM_VOLUME_PCT:
|
||||
if (value > 100) return Modbus::Error::ILLEGAL_DATA_VALUE;
|
||||
g_holding.alarm_volume_pct = value;
|
||||
return Modbus::Error::SUCCESS;
|
||||
case HOLDING_DODGE_OFFSET_DEG_X100:
|
||||
g_holding.dodge_offset_deg_x100 = (int16_t)value;
|
||||
return Modbus::Error::SUCCESS;
|
||||
default:
|
||||
return Modbus::Error::ILLEGAL_DATA_ADDRESS;
|
||||
}
|
||||
}
|
||||
|
||||
Modbus::Error write_coil(uint16_t addr, bool value) {
|
||||
switch (addr) {
|
||||
case COIL_CMD_ENGAGE_REQUEST:
|
||||
g_coils.engage_request = value;
|
||||
// Sprint 1: requesting engagement does nothing yet -- modes
|
||||
// engine is implemented in Sprint 3+. Just log and store.
|
||||
if (value) {
|
||||
AR_LOGI(TAG, "engage request received (no-op in Sprint 1)");
|
||||
}
|
||||
return Modbus::Error::SUCCESS;
|
||||
case COIL_CMD_DISENGAGE_REQUEST:
|
||||
g_coils.disengage_request = value;
|
||||
if (value) {
|
||||
modes::force_standby("modbus-disengage");
|
||||
}
|
||||
return Modbus::Error::SUCCESS;
|
||||
case COIL_CMD_ACK_ALL_ALARMS:
|
||||
g_coils.ack_all = value;
|
||||
if (value) {
|
||||
AR_LOGI(TAG, "ack-all-alarms received (no alarms in Sprint 1)");
|
||||
}
|
||||
return Modbus::Error::SUCCESS;
|
||||
case COIL_CMD_KNOB_ARM:
|
||||
g_coils.knob_arm = value;
|
||||
return Modbus::Error::SUCCESS;
|
||||
default:
|
||||
return Modbus::Error::ILLEGAL_DATA_ADDRESS;
|
||||
}
|
||||
}
|
||||
|
||||
// ----- eModbus worker callbacks ---------------------------------------------
|
||||
|
||||
ModbusMessage on_read_input(ModbusMessage req) {
|
||||
uint16_t start, count;
|
||||
req.get(2, start);
|
||||
req.get(4, count);
|
||||
if (count == 0 ||
|
||||
(uint32_t)start + count > (uint32_t)INPUT_MAX_ADDR + 1U ||
|
||||
count > 125) {
|
||||
++g_err_count;
|
||||
ModbusMessage resp;
|
||||
resp.setError(req.getServerID(), req.getFunctionCode(),
|
||||
Modbus::Error::ILLEGAL_DATA_VALUE);
|
||||
return resp;
|
||||
}
|
||||
ModbusMessage resp;
|
||||
resp.add(req.getServerID(), req.getFunctionCode(),
|
||||
(uint8_t)(count * 2));
|
||||
for (uint16_t i = 0; i < count; ++i) {
|
||||
resp.add(read_input_register(start + i));
|
||||
}
|
||||
++g_req_count;
|
||||
return resp;
|
||||
}
|
||||
|
||||
ModbusMessage on_read_discrete(ModbusMessage req) {
|
||||
uint16_t start, count;
|
||||
req.get(2, start);
|
||||
req.get(4, count);
|
||||
if (count == 0 ||
|
||||
(uint32_t)start + count > (uint32_t)DISCRETE_MAX_ADDR + 1U ||
|
||||
count > 2000) {
|
||||
++g_err_count;
|
||||
ModbusMessage resp;
|
||||
resp.setError(req.getServerID(), req.getFunctionCode(),
|
||||
Modbus::Error::ILLEGAL_DATA_VALUE);
|
||||
return resp;
|
||||
}
|
||||
ModbusMessage resp;
|
||||
uint8_t byte_count = (uint8_t)((count + 7) / 8);
|
||||
resp.add(req.getServerID(), req.getFunctionCode(), byte_count);
|
||||
for (uint8_t b = 0; b < byte_count; ++b) {
|
||||
uint8_t v = 0;
|
||||
for (uint8_t bit = 0; bit < 8; ++bit) {
|
||||
uint16_t idx = b * 8U + bit;
|
||||
if (idx < count && read_discrete(start + idx)) {
|
||||
v |= (uint8_t)(1U << bit);
|
||||
}
|
||||
}
|
||||
resp.add(v);
|
||||
}
|
||||
++g_req_count;
|
||||
return resp;
|
||||
}
|
||||
|
||||
ModbusMessage on_read_holding(ModbusMessage req) {
|
||||
uint16_t start, count;
|
||||
req.get(2, start);
|
||||
req.get(4, count);
|
||||
if (count == 0 ||
|
||||
(uint32_t)start + count > (uint32_t)HOLDING_MAX_ADDR + 1U ||
|
||||
count > 125) {
|
||||
++g_err_count;
|
||||
ModbusMessage resp;
|
||||
resp.setError(req.getServerID(), req.getFunctionCode(),
|
||||
Modbus::Error::ILLEGAL_DATA_VALUE);
|
||||
return resp;
|
||||
}
|
||||
ModbusMessage resp;
|
||||
resp.add(req.getServerID(), req.getFunctionCode(),
|
||||
(uint8_t)(count * 2));
|
||||
for (uint16_t i = 0; i < count; ++i) {
|
||||
resp.add(read_holding(start + i));
|
||||
}
|
||||
++g_req_count;
|
||||
return resp;
|
||||
}
|
||||
|
||||
ModbusMessage on_write_holding(ModbusMessage req) {
|
||||
uint16_t addr, value;
|
||||
req.get(2, addr);
|
||||
req.get(4, value);
|
||||
auto err = write_holding(addr, value);
|
||||
if (err != Modbus::Error::SUCCESS) {
|
||||
++g_err_count;
|
||||
ModbusMessage resp;
|
||||
resp.setError(req.getServerID(), req.getFunctionCode(), err);
|
||||
return resp;
|
||||
}
|
||||
// Echo per Modbus 0x06 convention.
|
||||
ModbusMessage resp;
|
||||
resp.add(req.getServerID(), req.getFunctionCode(), addr, value);
|
||||
++g_req_count;
|
||||
return resp;
|
||||
}
|
||||
|
||||
ModbusMessage on_write_coil(ModbusMessage req) {
|
||||
uint16_t addr, value;
|
||||
req.get(2, addr);
|
||||
req.get(4, value);
|
||||
// 0xFF00 = ON, 0x0000 = OFF per Modbus spec.
|
||||
if (value != 0x0000 && value != 0xFF00) {
|
||||
++g_err_count;
|
||||
ModbusMessage resp;
|
||||
resp.setError(req.getServerID(), req.getFunctionCode(),
|
||||
Modbus::Error::ILLEGAL_DATA_VALUE);
|
||||
return resp;
|
||||
}
|
||||
auto err = write_coil(addr, value == 0xFF00);
|
||||
if (err != Modbus::Error::SUCCESS) {
|
||||
++g_err_count;
|
||||
ModbusMessage resp;
|
||||
resp.setError(req.getServerID(), req.getFunctionCode(), err);
|
||||
return resp;
|
||||
}
|
||||
ModbusMessage resp;
|
||||
resp.add(req.getServerID(), req.getFunctionCode(), addr, value);
|
||||
++g_req_count;
|
||||
return resp;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void modbus_slave_init() {
|
||||
// Register worker callbacks BEFORE starting the server (eModbus
|
||||
// requirement).
|
||||
g_server.registerWorker(SLAVE_ID, READ_INPUT_REGISTER, &on_read_input);
|
||||
g_server.registerWorker(SLAVE_ID, READ_DISCR_INPUT, &on_read_discrete);
|
||||
g_server.registerWorker(SLAVE_ID, READ_HOLD_REGISTER, &on_read_holding);
|
||||
g_server.registerWorker(SLAVE_ID, WRITE_HOLD_REGISTER, &on_write_holding);
|
||||
g_server.registerWorker(SLAVE_ID, WRITE_COIL, &on_write_coil);
|
||||
|
||||
AR_LOGI(TAG,
|
||||
"modbus_slave_init: slave_id=%u inputs=%u discretes=%u "
|
||||
"holdings=%u coils=%u",
|
||||
(unsigned)SLAVE_ID, (unsigned)INPUT_COUNT, (unsigned)DISCRETE_COUNT,
|
||||
(unsigned)HOLDING_COUNT, (unsigned)COIL_COUNT);
|
||||
}
|
||||
|
||||
void modbus_slave_start() {
|
||||
// UART2 on the AR-NMEA-IO is wired to RS-485.
|
||||
Serial2.begin(BAUDRATE, SERIAL_8N1, PIN_RS485_RX, PIN_RS485_TX);
|
||||
g_server.begin(Serial2, AR_TASK_CORE_COMMS);
|
||||
g_running = true;
|
||||
AR_LOGI(TAG, "modbus_slave_start: listening on UART2 @ %u baud (RX=%d TX=%d DE=%d)",
|
||||
(unsigned)BAUDRATE, PIN_RS485_RX, PIN_RS485_TX, PIN_RS485_DE);
|
||||
}
|
||||
|
||||
bool modbus_slave_is_running() { return g_running; }
|
||||
|
||||
uint32_t modbus_slave_request_count() { return g_req_count; }
|
||||
|
||||
uint32_t modbus_slave_error_count() { return g_err_count; }
|
||||
|
||||
} // namespace arautopilot::protocols::modbus
|
||||
@@ -0,0 +1,45 @@
|
||||
// =============================================================================
|
||||
// modbus_slave.h -- Modbus RTU server (slave) interface
|
||||
// =============================================================================
|
||||
//
|
||||
// Sprint 1: exposes the read-only telemetry registers (input registers and
|
||||
// discrete inputs) and accepts writes to the holding registers and coils
|
||||
// defined in modbus_registers.h (auto-generated).
|
||||
//
|
||||
// Reads come straight from the live telemetry helpers
|
||||
// (modes::current_mode(), hal::rudder_sensor_latest(), etc.).
|
||||
//
|
||||
// Writes are validated and routed to the corresponding command handler
|
||||
// (mode change, alarm ack, etc.). Invalid values are rejected with
|
||||
// ILLEGAL_DATA_VALUE per Modbus convention.
|
||||
//
|
||||
// All Modbus internals (RX task, response framing, RS-485 DE/RE control)
|
||||
// are delegated to the eModbus library, which spawns its own tasks on
|
||||
// Core 0.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace arautopilot::protocols::modbus {
|
||||
|
||||
/// Initialise the RS-485 UART, configure the Modbus server, and register
|
||||
/// every read/write callback. Must be called from setup() after di_init()
|
||||
/// and rudder_sensor_init().
|
||||
void modbus_slave_init();
|
||||
|
||||
/// Start the eModbus server. After this returns, the slave is listening on
|
||||
/// the configured UART/baudrate and replies asynchronously.
|
||||
void modbus_slave_start();
|
||||
|
||||
/// True after modbus_slave_start() has successfully initialised the server.
|
||||
bool modbus_slave_is_running();
|
||||
|
||||
/// Number of valid requests processed since boot (cheap performance counter).
|
||||
uint32_t modbus_slave_request_count();
|
||||
|
||||
/// Number of malformed requests / illegal-data-value rejections since boot.
|
||||
uint32_t modbus_slave_error_count();
|
||||
|
||||
} // namespace arautopilot::protocols::modbus
|
||||
@@ -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
|
||||
@@ -0,0 +1,46 @@
|
||||
// =============================================================================
|
||||
// nmea2000_consumer.h -- NMEA 2000 backbone consumer
|
||||
// =============================================================================
|
||||
//
|
||||
// Sprint 1 subscribes to PGN 127250 (Vessel Heading) and PGN 127251
|
||||
// (Rate of Turn) from the boat's NMEA 2000 backbone. The latest values
|
||||
// are stashed in a thread-safe snapshot that the Modbus slave (and later
|
||||
// the PID outer loop) read from.
|
||||
//
|
||||
// Later sprints will also subscribe to PGN 129025/129029 (Position),
|
||||
// 129026 (COG/SOG), 129284 (Navigation Data), 127257 (Attitude).
|
||||
//
|
||||
// This module owns the NMEA2000 instance and its dedicated FreeRTOS task
|
||||
// pinned to Core 0.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace arautopilot::protocols::nmea2000 {
|
||||
|
||||
struct HeadingSnapshot {
|
||||
float heading_deg; ///< 0..360, magnetic or true depending on source
|
||||
bool is_true; ///< true if reference is "true north", false if magnetic
|
||||
float rate_of_turn_dps; ///< signed deg/s; positive = turning to starboard
|
||||
uint32_t heading_age_ms; ///< millis() at the last 127250 update
|
||||
uint32_t rot_age_ms; ///< millis() at the last 127251 update
|
||||
bool heading_valid; ///< true if the heading is fresh (<5 s old)
|
||||
bool rot_valid; ///< true if the ROT is fresh (<5 s old)
|
||||
};
|
||||
|
||||
/// Initialise the NMEA2000 stack with our PGN handlers.
|
||||
void nmea2000_consumer_init();
|
||||
|
||||
/// Spawn the FreeRTOS task that pumps NMEA2000.ParseMessages() forever.
|
||||
void nmea2000_consumer_start_task();
|
||||
|
||||
/// Thread-safe read of the latest heading + ROT snapshot.
|
||||
HeadingSnapshot nmea2000_latest();
|
||||
|
||||
/// True if either heading_age_ms or rot_age_ms exceeds the stale-threshold
|
||||
/// (default 5 s, brief section 7).
|
||||
bool nmea2000_is_stale();
|
||||
|
||||
} // namespace arautopilot::protocols::nmea2000
|
||||
@@ -0,0 +1,77 @@
|
||||
// =============================================================================
|
||||
// safety/safety_monitor.cpp -- 50 Hz safety task
|
||||
// =============================================================================
|
||||
|
||||
#include "safety_monitor.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "../hal/di_do.h"
|
||||
#include "../hal/pinout.h"
|
||||
#include "../hal/rudder_actuator.h"
|
||||
#include "../modes/standby.h"
|
||||
#include "../system/ar_log.h"
|
||||
#include "../system/task_config.h"
|
||||
#include "watchdog.h"
|
||||
|
||||
namespace arautopilot::safety {
|
||||
|
||||
namespace {
|
||||
constexpr const char* TAG = "AR/SAFE";
|
||||
|
||||
void SafetyTask(void* /*pv*/) {
|
||||
AR_LOGI(TAG, "safety_monitor task started on core %d (50 Hz)",
|
||||
xPortGetCoreID());
|
||||
|
||||
// Subscribe to the watchdog. Every loop iteration feeds it; if this
|
||||
// task ever stops looping the chip resets to STANDBY on boot.
|
||||
watchdog_subscribe_current_task();
|
||||
|
||||
TickType_t last_wake = xTaskGetTickCount();
|
||||
for (;;) {
|
||||
hal::di_poll();
|
||||
|
||||
// ---- DI1: Engage/Disengage physical button -----------------------
|
||||
// The brief specifies this button "ALWAYS DISENGAGES" -- pressing
|
||||
// it must put the system in STANDBY no matter what mode we're in.
|
||||
// We trigger on the rising edge so a held button doesn't keep
|
||||
// spamming the log.
|
||||
if (hal::di_rising_edge(PIN_DI1_DISENGAGE_BUTTON)) {
|
||||
modes::force_standby("DI1 physical button");
|
||||
hal::rudder_actuator_power_off();
|
||||
}
|
||||
|
||||
// ---- DI4: External critical alarm (VMS blackout / genset) --------
|
||||
if (hal::di_rising_edge(PIN_DI4_EXTERNAL_ALARM)) {
|
||||
modes::force_standby("DI4 external alarm");
|
||||
hal::rudder_actuator_power_off();
|
||||
}
|
||||
|
||||
// ---- Limit switches: cut power if rudder hit a mechanical stop --
|
||||
// Even though rudder_command() refuses to drive into a limit, a
|
||||
// hardware failure could still command the wrong direction. As an
|
||||
// extra interlock, if BOTH limits assert at once we cut master
|
||||
// power -- something is seriously wrong with the feedback wiring.
|
||||
if (hal::di_read(PIN_DI2_LIMIT_SWITCH_PORT) &&
|
||||
hal::di_read(PIN_DI3_LIMIT_SWITCH_STBD)) {
|
||||
AR_LOGE(TAG,
|
||||
"both limit switches asserted simultaneously -- cutting "
|
||||
"actuator power (wiring fault?)");
|
||||
hal::rudder_actuator_power_off();
|
||||
}
|
||||
|
||||
watchdog_feed();
|
||||
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_SAFETY));
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void safety_monitor_start_task() {
|
||||
xTaskCreatePinnedToCore(SafetyTask, "safety_monitor",
|
||||
AR_TASK_STACK_SAFETY, nullptr,
|
||||
AR_TASK_PRIO_SAFETY, nullptr,
|
||||
AR_TASK_CORE_REALTIME);
|
||||
}
|
||||
|
||||
} // namespace arautopilot::safety
|
||||
@@ -0,0 +1,26 @@
|
||||
// =============================================================================
|
||||
// safety/safety_monitor.h -- safety monitor task
|
||||
// =============================================================================
|
||||
//
|
||||
// 50 Hz task on Core 1 that:
|
||||
// - Polls every DI through hal::di_poll().
|
||||
// - Reacts to PIN_DI1_DISENGAGE_BUTTON rising edge: forces STANDBY,
|
||||
// kills actuator power.
|
||||
// - Reacts to limit switches: cuts actuator power if the rudder is
|
||||
// trying to drive into a limit.
|
||||
// - Reacts to PIN_DI4_EXTERNAL_ALARM (VMS critical): forces STANDBY.
|
||||
// - Subscribes itself to the TWDT and feeds it every loop iteration.
|
||||
//
|
||||
// Sprint 1: limited to the above. Sprint 6 expands this with the full
|
||||
// alarm catalogue (off-course, rudder not responding, etc.).
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace arautopilot::safety {
|
||||
|
||||
/// Spawn the safety monitor task. Must be called AFTER di_init() and
|
||||
/// rudder_actuator_init().
|
||||
void safety_monitor_start_task();
|
||||
|
||||
} // namespace arautopilot::safety
|
||||
@@ -0,0 +1,40 @@
|
||||
// =============================================================================
|
||||
// safety/watchdog.cpp -- TWDT setup + helpers
|
||||
// =============================================================================
|
||||
|
||||
#include "watchdog.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <esp_task_wdt.h>
|
||||
|
||||
#include "../system/ar_log.h"
|
||||
#include "../system/task_config.h"
|
||||
|
||||
namespace arautopilot::safety {
|
||||
|
||||
namespace {
|
||||
constexpr const char* TAG = "AR/SAFE";
|
||||
} // namespace
|
||||
|
||||
void watchdog_init() {
|
||||
// ESP-IDF 5.x: esp_task_wdt_init takes a config struct, but
|
||||
// esp32 Arduino core wraps it with a simpler signature.
|
||||
esp_err_t err = esp_task_wdt_init(AR_WATCHDOG_TIMEOUT_S, true);
|
||||
if (err != ESP_OK) {
|
||||
AR_LOGE(TAG, "esp_task_wdt_init failed: %d", (int)err);
|
||||
return;
|
||||
}
|
||||
AR_LOGI(TAG, "watchdog_init: TWDT timeout %ds, panic on expire",
|
||||
AR_WATCHDOG_TIMEOUT_S);
|
||||
}
|
||||
|
||||
void watchdog_subscribe_current_task() {
|
||||
esp_err_t err = esp_task_wdt_add(nullptr);
|
||||
if (err != ESP_OK && err != ESP_ERR_INVALID_ARG) {
|
||||
AR_LOGW(TAG, "esp_task_wdt_add failed: %d", (int)err);
|
||||
}
|
||||
}
|
||||
|
||||
void watchdog_feed() { esp_task_wdt_reset(); }
|
||||
|
||||
} // namespace arautopilot::safety
|
||||
@@ -0,0 +1,29 @@
|
||||
// =============================================================================
|
||||
// safety/watchdog.h -- Task Watchdog Timer (TWDT)
|
||||
// =============================================================================
|
||||
//
|
||||
// Hooks the ESP32 task watchdog so that if the real-time control tasks
|
||||
// (PID inner, PID outer, safety monitor) ever fail to feed the watchdog
|
||||
// within AR_WATCHDOG_TIMEOUT_S, the chip resets and reboots cleanly to
|
||||
// STANDBY mode.
|
||||
//
|
||||
// Brief section 7: "El ESP32 reinicia automaticamente si el firmware se
|
||||
// cuelga >2 segundos. Al reiniciar: Estado del piloto pasa a STANDBY."
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace arautopilot::safety {
|
||||
|
||||
/// Configure and start the Task Watchdog Timer. Tasks that want to be
|
||||
/// monitored must call watchdog_subscribe_current_task() once and then
|
||||
/// watchdog_feed() at least once per AR_WATCHDOG_TIMEOUT_S.
|
||||
void watchdog_init();
|
||||
|
||||
/// Add the current task to the watchdog's monitored set.
|
||||
void watchdog_subscribe_current_task();
|
||||
|
||||
/// Feed the watchdog from the current task.
|
||||
void watchdog_feed();
|
||||
|
||||
} // namespace arautopilot::safety
|
||||
@@ -0,0 +1,35 @@
|
||||
// =============================================================================
|
||||
// ar_log.h -- AR-Autopilot logging facade
|
||||
// =============================================================================
|
||||
//
|
||||
// Thin wrapper around the ESP32 Arduino log_X / ESP_LOGX macros so that the
|
||||
// rest of the firmware uses one consistent style. Compile-time level is
|
||||
// controlled by CORE_DEBUG_LEVEL in platformio.ini.
|
||||
//
|
||||
// Levels (matching ARDUHAL_LOG_LEVEL_*):
|
||||
// 0 NONE
|
||||
// 1 ERROR
|
||||
// 2 WARN
|
||||
// 3 INFO <- default in release
|
||||
// 4 DEBUG
|
||||
// 5 VERBOSE <- default in debug
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// Tag prefix conventions used across the codebase:
|
||||
// "AR/MAIN" -- main.cpp boot/shutdown
|
||||
// "AR/PID" -- PID loops
|
||||
// "AR/HAL" -- hardware abstraction
|
||||
// "AR/MB" -- Modbus slave
|
||||
// "AR/N2K" -- NMEA 2000 consumer
|
||||
// "AR/SAFE" -- safety / watchdog / disengage
|
||||
// "AR/SYS" -- health / lifecycle / metrics
|
||||
|
||||
#define AR_LOGE(tag, fmt, ...) log_e("[%s] " fmt, tag, ##__VA_ARGS__)
|
||||
#define AR_LOGW(tag, fmt, ...) log_w("[%s] " fmt, tag, ##__VA_ARGS__)
|
||||
#define AR_LOGI(tag, fmt, ...) log_i("[%s] " fmt, tag, ##__VA_ARGS__)
|
||||
#define AR_LOGD(tag, fmt, ...) log_d("[%s] " fmt, tag, ##__VA_ARGS__)
|
||||
#define AR_LOGV(tag, fmt, ...) log_v("[%s] " fmt, tag, ##__VA_ARGS__)
|
||||
@@ -0,0 +1,50 @@
|
||||
// =============================================================================
|
||||
// heartbeat.cpp -- LED + uptime heartbeat task
|
||||
// =============================================================================
|
||||
//
|
||||
// Blinks PIN_DO5_ENGAGED_LAMP at 1 Hz to give a visible "the firmware is
|
||||
// alive" signal at the bench. In Sprint 1 we co-opt this LED because there
|
||||
// is no pilot to engage yet; from Sprint 6 onwards the engaged lamp will be
|
||||
// driven by the actual pilot state and the heartbeat will move to an
|
||||
// internal counter only.
|
||||
//
|
||||
// Also logs uptime + free heap once per second so a terminal observer can
|
||||
// see the system is healthy.
|
||||
// =============================================================================
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "../hal/pinout.h"
|
||||
#include "ar_log.h"
|
||||
#include "task_config.h"
|
||||
|
||||
namespace {
|
||||
constexpr const char* TAG = "AR/SYS";
|
||||
} // namespace
|
||||
|
||||
static void HeartbeatTask(void* /*pv*/) {
|
||||
pinMode(PIN_DO5_ENGAGED_LAMP, OUTPUT);
|
||||
bool led_on = false;
|
||||
uint32_t tick = 0;
|
||||
|
||||
AR_LOGI(TAG, "heartbeat task started on core %d, pin %d", xPortGetCoreID(),
|
||||
PIN_DO5_ENGAGED_LAMP);
|
||||
|
||||
TickType_t last_wake = xTaskGetTickCount();
|
||||
for (;;) {
|
||||
led_on = !led_on;
|
||||
digitalWrite(PIN_DO5_ENGAGED_LAMP, led_on ? HIGH : LOW);
|
||||
|
||||
AR_LOGD(TAG, "tick=%u uptime=%lus free_heap=%u min_free=%u", tick,
|
||||
millis() / 1000U, ESP.getFreeHeap(), ESP.getMinFreeHeap());
|
||||
|
||||
++tick;
|
||||
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_HEARTBEAT));
|
||||
}
|
||||
}
|
||||
|
||||
void ar_start_heartbeat_task() {
|
||||
xTaskCreatePinnedToCore(HeartbeatTask, "heartbeat", AR_TASK_STACK_HEARTBEAT,
|
||||
nullptr, AR_TASK_PRIO_HEARTBEAT, nullptr,
|
||||
AR_TASK_CORE_COMMS);
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
// =============================================================================
|
||||
// task_config.h -- FreeRTOS task layout for AR-Autopilot
|
||||
// =============================================================================
|
||||
//
|
||||
// Single source of truth for stack sizes, priorities, and core pinning of
|
||||
// every long-running task in the firmware. Centralising this here makes the
|
||||
// real-time profile of the system reviewable at a glance.
|
||||
//
|
||||
// Pinning policy (brief section 6, rule #11 -- "deterministic latency"):
|
||||
//
|
||||
// Core 1 (APP_CPU) -- safety-critical real-time:
|
||||
// AR_TASK_CORE_REALTIME = 1
|
||||
// - PID inner loop (Sprint 2; stub in Sprint 1)
|
||||
// - PID outer loop (Sprint 3; stub in Sprint 1)
|
||||
// - Rudder sensor (Sprint 1: 100 Hz median filter)
|
||||
// - Safety monitor (Sprint 1: limit switches, watchdog feed)
|
||||
//
|
||||
// Core 0 (PRO_CPU) -- communications + housekeeping:
|
||||
// AR_TASK_CORE_COMMS = 0
|
||||
// - NMEA 2000 RX (Sprint 1: PGN 127250 / 127251)
|
||||
// - Modbus slave (Sprint 1: eModbus internals already async)
|
||||
// - Health reporter (Sprint 1: 1 Hz log)
|
||||
//
|
||||
// This split guarantees the real-time control loops cannot be starved or
|
||||
// jittered by bus traffic, even if NMEA 2000 saturates or the Modbus client
|
||||
// bursts.
|
||||
//
|
||||
// Priorities use the standard ESP32 FreeRTOS scale (0 idle .. 24 max).
|
||||
// configMAX_PRIORITIES is 25 on default ESP32 Arduino builds.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
// Core assignment
|
||||
#define AR_TASK_CORE_REALTIME 1
|
||||
#define AR_TASK_CORE_COMMS 0
|
||||
|
||||
// Priorities (higher = more priority)
|
||||
#define AR_TASK_PRIO_PID_INNER 24 // hard real-time, 50 Hz
|
||||
#define AR_TASK_PRIO_RUDDER_SENSOR 23 // 100 Hz ADC, must feed PID fresh data
|
||||
#define AR_TASK_PRIO_PID_OUTER 22 // 10 Hz heading control
|
||||
#define AR_TASK_PRIO_SAFETY 21 // limit switches, watchdog feed
|
||||
#define AR_TASK_PRIO_N2K_RX 15 // CAN polling, event-driven
|
||||
#define AR_TASK_PRIO_MODBUS 14 // eModbus internal tasks (delegated)
|
||||
#define AR_TASK_PRIO_HEARTBEAT 10 // LED blink + uptime log
|
||||
#define AR_TASK_PRIO_HEALTH 5 // periodic stats, low priority
|
||||
|
||||
// Stack sizes (bytes). Tune these once we see real high-water marks via
|
||||
// uxTaskGetStackHighWaterMark() in the health reporter.
|
||||
#define AR_TASK_STACK_PID_INNER 4096
|
||||
#define AR_TASK_STACK_PID_OUTER 4096
|
||||
#define AR_TASK_STACK_RUDDER_SENSOR 3072
|
||||
#define AR_TASK_STACK_SAFETY 3072
|
||||
#define AR_TASK_STACK_N2K_RX 6144 // NMEA2000 parsing is heap-heavy
|
||||
#define AR_TASK_STACK_MODBUS 4096
|
||||
#define AR_TASK_STACK_HEARTBEAT 2048
|
||||
#define AR_TASK_STACK_HEALTH 3072
|
||||
|
||||
// Loop periods (ms). Convert with pdMS_TO_TICKS() at the use site.
|
||||
#define AR_PERIOD_MS_PID_INNER 20 // 50 Hz
|
||||
#define AR_PERIOD_MS_PID_OUTER 100 // 10 Hz
|
||||
#define AR_PERIOD_MS_RUDDER_SENSOR 10 // 100 Hz
|
||||
#define AR_PERIOD_MS_SAFETY 20 // 50 Hz
|
||||
#define AR_PERIOD_MS_HEARTBEAT 1000 // 1 Hz LED blink
|
||||
#define AR_PERIOD_MS_HEALTH 5000 // every 5 s
|
||||
|
||||
// Watchdog (brief section 7)
|
||||
#define AR_WATCHDOG_TIMEOUT_S 2
|
||||
@@ -0,0 +1,112 @@
|
||||
// =============================================================================
|
||||
// test_median.cpp -- Unity host-side tests for MedianFilter<T, N>
|
||||
// =============================================================================
|
||||
//
|
||||
// Runs on the developer machine via:
|
||||
// .venv/Scripts/pio.exe test -e native
|
||||
//
|
||||
// No ESP32 hardware required. Tests the pure-C++ template in
|
||||
// src/filters/median.h.
|
||||
// =============================================================================
|
||||
|
||||
#include <unity.h>
|
||||
|
||||
// Compile-time switch: when AR_HOST_TEST is defined we drop the Arduino.h
|
||||
// dependency. The median filter only needs <algorithm>, <array>, <cstddef>.
|
||||
#include "../../src/filters/median.h"
|
||||
|
||||
using arautopilot::filters::MedianFilter;
|
||||
|
||||
void setUp() {}
|
||||
void tearDown() {}
|
||||
|
||||
void test_empty_filter_returns_default_constructed_value() {
|
||||
MedianFilter<int, 5> f;
|
||||
TEST_ASSERT_EQUAL_INT(0, f.median());
|
||||
TEST_ASSERT_EQUAL_size_t(0, f.size());
|
||||
TEST_ASSERT_FALSE(f.is_full());
|
||||
}
|
||||
|
||||
void test_single_sample_returns_itself() {
|
||||
MedianFilter<int, 5> f;
|
||||
TEST_ASSERT_EQUAL_INT(42, f.push(42));
|
||||
TEST_ASSERT_EQUAL_size_t(1, f.size());
|
||||
TEST_ASSERT_FALSE(f.is_full());
|
||||
}
|
||||
|
||||
void test_two_samples_returns_upper_of_pair() {
|
||||
MedianFilter<int, 5> f;
|
||||
f.push(10);
|
||||
// size = 2, median index = 2/2 = 1, so the second element after sort.
|
||||
TEST_ASSERT_EQUAL_INT(20, f.push(20));
|
||||
TEST_ASSERT_EQUAL_INT(20, f.push(15)); // sorted: 10,15,20 -> 15
|
||||
TEST_ASSERT_EQUAL_INT(15, f.median());
|
||||
}
|
||||
|
||||
void test_filter_suppresses_single_spike() {
|
||||
// A common scenario: 100 100 99 9999 101 -- the median ignores 9999.
|
||||
MedianFilter<int, 5> f;
|
||||
f.push(100);
|
||||
f.push(100);
|
||||
f.push(99);
|
||||
f.push(9999);
|
||||
f.push(101);
|
||||
TEST_ASSERT_TRUE(f.is_full());
|
||||
TEST_ASSERT_EQUAL_INT(100, f.median());
|
||||
}
|
||||
|
||||
void test_filter_window_is_circular() {
|
||||
MedianFilter<int, 3> f;
|
||||
f.push(1);
|
||||
f.push(2);
|
||||
f.push(3); // window = [1,2,3] -> median 2
|
||||
TEST_ASSERT_EQUAL_INT(2, f.median());
|
||||
f.push(100); // window = [100,2,3] -> median 3
|
||||
TEST_ASSERT_EQUAL_INT(3, f.median());
|
||||
f.push(100); // window = [100,100,3] -> median 100
|
||||
TEST_ASSERT_EQUAL_INT(100, f.median());
|
||||
f.push(100); // window = [100,100,100] -> median 100
|
||||
TEST_ASSERT_EQUAL_INT(100, f.median());
|
||||
}
|
||||
|
||||
void test_reset_clears_state() {
|
||||
MedianFilter<int, 5> f;
|
||||
f.push(1);
|
||||
f.push(2);
|
||||
f.push(3);
|
||||
f.reset();
|
||||
TEST_ASSERT_EQUAL_size_t(0, f.size());
|
||||
TEST_ASSERT_FALSE(f.is_full());
|
||||
TEST_ASSERT_EQUAL_INT(0, f.median());
|
||||
TEST_ASSERT_EQUAL_INT(42, f.push(42));
|
||||
}
|
||||
|
||||
void test_negative_values() {
|
||||
MedianFilter<int16_t, 5> f;
|
||||
f.push(-100);
|
||||
f.push(-50);
|
||||
f.push(0);
|
||||
f.push(50);
|
||||
f.push(100);
|
||||
TEST_ASSERT_EQUAL_INT16(0, f.median());
|
||||
}
|
||||
|
||||
void test_window_of_one_is_identity() {
|
||||
MedianFilter<int, 1> f;
|
||||
TEST_ASSERT_EQUAL_INT(7, f.push(7));
|
||||
TEST_ASSERT_EQUAL_INT(9, f.push(9));
|
||||
TEST_ASSERT_TRUE(f.is_full());
|
||||
}
|
||||
|
||||
int main(int /*argc*/, char** /*argv*/) {
|
||||
UNITY_BEGIN();
|
||||
RUN_TEST(test_empty_filter_returns_default_constructed_value);
|
||||
RUN_TEST(test_single_sample_returns_itself);
|
||||
RUN_TEST(test_two_samples_returns_upper_of_pair);
|
||||
RUN_TEST(test_filter_suppresses_single_spike);
|
||||
RUN_TEST(test_filter_window_is_circular);
|
||||
RUN_TEST(test_reset_clears_state);
|
||||
RUN_TEST(test_negative_values);
|
||||
RUN_TEST(test_window_of_one_is_identity);
|
||||
return UNITY_END();
|
||||
}
|
||||
@@ -0,0 +1,166 @@
|
||||
"""Manual Modbus RTU client for poking the AR-Autopilot ESP32 slave.
|
||||
|
||||
Usage (with the AR-NMEA-IO connected via a USB-to-RS485 adapter):
|
||||
|
||||
python firmware/ar_autopilot_v1/tools/modbus_client_test.py \
|
||||
--port COM7
|
||||
|
||||
It does NOT require any extra dependency beyond ``pymodbus``, which is NOT
|
||||
installed by default. Install on demand with:
|
||||
|
||||
pip install 'pymodbus>=3.6,<4'
|
||||
|
||||
The script:
|
||||
|
||||
1. Connects to the slave at the baudrate / framing declared in
|
||||
``arautopilot.shared.modbus_register_map``.
|
||||
2. Reads firmware version and uptime (input registers).
|
||||
3. Reads the current pilot mode.
|
||||
4. Reads the rudder angle (input register).
|
||||
5. Writes a heading setpoint to a holding register, then reads it back.
|
||||
6. Pulses the disengage coil and verifies the discrete bit goes high
|
||||
(PILOT_ENGAGED is expected to remain false in Sprint 1).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
# Make the project package importable when run from a fresh shell.
|
||||
REPO_ROOT = Path(__file__).resolve().parents[3]
|
||||
sys.path.insert(0, str(REPO_ROOT))
|
||||
|
||||
from arautopilot.shared import modbus_register_map as m # noqa: E402
|
||||
|
||||
try:
|
||||
from pymodbus.client import ModbusSerialClient
|
||||
except ImportError:
|
||||
sys.stderr.write(
|
||||
"ERROR: pymodbus is not installed. Run:\n"
|
||||
" pip install 'pymodbus>=3.6,<4'\n"
|
||||
)
|
||||
sys.exit(2)
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--port", required=True, help="Serial port (e.g. COM7 or /dev/ttyUSB0)")
|
||||
parser.add_argument("--baudrate", type=int, default=m.BAUDRATE)
|
||||
parser.add_argument("--slave-id", type=int, default=m.SLAVE_ID)
|
||||
parser.add_argument("--write-setpoint", type=float, default=180.0,
|
||||
help="Heading setpoint to write, in degrees (default 180.0)")
|
||||
args = parser.parse_args()
|
||||
|
||||
print(f"[connect] {args.port} @ {args.baudrate} 8{m.PARITY}1, slave={args.slave_id}")
|
||||
client = ModbusSerialClient(
|
||||
port=args.port,
|
||||
baudrate=args.baudrate,
|
||||
parity=m.PARITY,
|
||||
stopbits=m.STOP_BITS,
|
||||
bytesize=m.DATA_BITS,
|
||||
timeout=1.0,
|
||||
)
|
||||
if not client.connect():
|
||||
print("[connect] FAILED")
|
||||
return 1
|
||||
|
||||
try:
|
||||
# --- Firmware version + uptime ---------------------------------------
|
||||
rr = client.read_input_registers(
|
||||
address=m.INPUTS["FW_VERSION_MAJOR"].addr, count=6, slave=args.slave_id
|
||||
)
|
||||
if rr.isError():
|
||||
print("[error] read input registers (version): ", rr)
|
||||
return 1
|
||||
major, minor, patch, schema, up_lo, up_hi = rr.registers
|
||||
uptime_s = up_lo | (up_hi << 16)
|
||||
print(
|
||||
f"[fw ] version v{major}.{minor}.{patch} schema={schema} "
|
||||
f"uptime={uptime_s}s"
|
||||
)
|
||||
|
||||
# --- Mode -----------------------------------------------------------
|
||||
rr = client.read_input_registers(
|
||||
address=m.INPUTS["CURRENT_MODE"].addr, count=1, slave=args.slave_id
|
||||
)
|
||||
mode_names = {0: "STANDBY", 1: "HEADING_HOLD", 2: "TRUE_COURSE",
|
||||
3: "TRACK_KEEPING", 4: "DODGE"}
|
||||
print(f"[mode ] {mode_names.get(rr.registers[0], '?')} ({rr.registers[0]})")
|
||||
|
||||
# --- Rudder angle ---------------------------------------------------
|
||||
rr = client.read_input_registers(
|
||||
address=m.INPUTS["RUDDER_ANGLE_DEG_X100"].addr, count=3, slave=args.slave_id
|
||||
)
|
||||
angle_x100, raw_adc, valid = rr.registers
|
||||
# interpret as signed int16
|
||||
if angle_x100 >= 0x8000:
|
||||
angle_x100 -= 0x10000
|
||||
print(
|
||||
f"[rudd ] angle={angle_x100 / 100.0:+.2f} deg raw_adc={raw_adc} "
|
||||
f"valid={'yes' if valid else 'no'}"
|
||||
)
|
||||
|
||||
# --- Heading + ROT (Sprint 1 wires these via NMEA 2000) -------------
|
||||
rr = client.read_input_registers(
|
||||
address=m.INPUTS["HEADING_DEG_X100"].addr, count=3, slave=args.slave_id
|
||||
)
|
||||
heading_x100, rot_x100, age_ms = rr.registers
|
||||
if rot_x100 >= 0x8000:
|
||||
rot_x100 -= 0x10000
|
||||
print(
|
||||
f"[n2k ] heading={heading_x100 / 100.0:7.2f} deg "
|
||||
f"rot={rot_x100 / 100.0:+.2f} deg/s age={age_ms} ms"
|
||||
)
|
||||
|
||||
# --- Write heading setpoint -----------------------------------------
|
||||
sp_x100 = int(round(args.write_setpoint * 100.0))
|
||||
if sp_x100 < -0x8000 or sp_x100 > 0x7FFF:
|
||||
print(f"[error] setpoint out of int16 range")
|
||||
return 1
|
||||
wr_value = sp_x100 if sp_x100 >= 0 else (sp_x100 + 0x10000)
|
||||
wr = client.write_register(
|
||||
address=m.HOLDINGS["HEADING_SETPOINT_X100"].addr,
|
||||
value=wr_value,
|
||||
slave=args.slave_id,
|
||||
)
|
||||
if wr.isError():
|
||||
print(f"[error] write holding (heading setpoint): {wr}")
|
||||
return 1
|
||||
rr = client.read_holding_registers(
|
||||
address=m.HOLDINGS["HEADING_SETPOINT_X100"].addr,
|
||||
count=1,
|
||||
slave=args.slave_id,
|
||||
)
|
||||
read_back = rr.registers[0]
|
||||
if read_back >= 0x8000:
|
||||
read_back -= 0x10000
|
||||
print(
|
||||
f"[hold ] wrote heading setpoint {args.write_setpoint:.2f} deg, "
|
||||
f"read back {read_back / 100.0:+.2f} deg"
|
||||
)
|
||||
|
||||
# --- Disengage coil pulse -------------------------------------------
|
||||
client.write_coil(
|
||||
address=m.COILS["CMD_DISENGAGE_REQUEST"].addr,
|
||||
value=True,
|
||||
slave=args.slave_id,
|
||||
)
|
||||
time.sleep(0.05)
|
||||
rr = client.read_discrete_inputs(
|
||||
address=m.DISCRETES["PILOT_ENGAGED"].addr, count=1, slave=args.slave_id
|
||||
)
|
||||
print(
|
||||
f"[coil ] disengage pulsed; PILOT_ENGAGED = "
|
||||
f"{'YES (unexpected)' if rr.bits[0] else 'no (correct -- Sprint 1 is always STANDBY)'}"
|
||||
)
|
||||
|
||||
return 0
|
||||
finally:
|
||||
client.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Reference in New Issue
Block a user