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,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
|
||||
Reference in New Issue
Block a user