alro65/AR-Autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

sprint-3: PID outer + Heading Hold + ROT feed-forward + gain scheduling

Browse Source 
End-to-end implementation per docs/sprint-3-plan.md.

Closes the cascade: outer loop (heading control, 10 Hz on Core 1) drives
the inner loop (rudder position control, 50 Hz from Sprint 2). First real
mode other than STANDBY is now activable: HEADING_HOLD.

Builds: pio run -e esp32-dev SUCCESS, RAM 6.8%, Flash 27.1% (355 KB).
Tests: pytest 258/258 green (231 Sprint 2.5 + 27 Sprint 3 new).

Python (arautopilot/studio/simulator/):

- vessel_heading.py: first-order yaw model. ROT responds to
  rudder*speed; damping returns ROT to zero when rudder is centred.
  Defaults tuned so 5 deg rudder @ 10 kn -> ~3 dps steady-state ROT.
  Includes heading_error_deg() shortest-arc helper.
- pid_outer.py: pure-Python outer heading PID. Anti-windup via back-
  calculation, gain scheduling by SOG, deadband, derivative LPF,
  output saturation, ROT feed-forward (brief sec. 6 -- the term that
  distinguishes a premium autopilot from a basic one), rate limit on
  produced rudder setpoint, shortest-arc heading wrap-around.

Firmware (firmware/ar_autopilot_v1/src/pid/):

- pid_outer.h: header-only C++17 port. Same algorithm, same variables,
  same numerics. Fixed-capacity gain schedule (up to 8 points).
- pid_outer_task.{h,cpp}: 10 Hz FreeRTOS task on Core 1. Subscribes to
  TWDT. Reads heading + ROT from the NMEA 2000 snapshot. Uses
  operator-configurable SOG (default 15 kn until PGN 129026 wiring in
  Sprint 5). Pushes rudder setpoint into the inner loop only when
  current_mode == HEADING_HOLD.

Modes (firmware/ar_autopilot_v1/src/modes/standby.cpp):

- HEADING_HOLD activable via request_mode(). Pre-conditions:
    * NMEA 2000 heading sensor valid (fresh PGN 127250)
    * Rudder sensor valid (median filter filled)
  On success, captures current heading as initial setpoint so the
  operator doesn't get a sudden swing toward an old setpoint.

Modbus (regenerated from YAML):

- 7 new INPUTs (50-56): outer heading setpoint, produced rudder
  setpoint, error, current SOG, live kp/ki/kd.
- 5 new HOLDINGs (24-28): writable heading setpoint, SOG override,
  outer base gains. Writing any of kp/ki/kd disables the built-in
  3-point gain schedule (operator override).

Tests:

- test_vessel_heading_simulator.py: 6 dynamics tests + 9 parameterised
  heading_error_deg edge cases (wrap-around).
- test_pid_outer_python.py: 12 tests covering gain interpolation,
  per-tick PID behaviour (deadband, sign, ROT feed-forward,
  saturation, rate limit, allowed=false), and three end-to-end cascade
  tests (positive step, negative step, wrap-around 360->10).

Cascade verification: outer + inner + rudder dynamics + vessel-heading
simulator settles a 30 deg step within +-2 deg in 60 s.

NOT in Sprint 3 (intentional):
  - True Course / Track Keeping / Dodge -- Sprint 5
  - Off-course alarms + auto-disengage on sensor loss -- Sprint 6
  - COG / SOG / Position via N2K PGN 129025/9/6 -- Sprint 5

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Alvaro Enrique Romero Donado
2026-05-18 18:20:23 -04:00
parent 13a2867ef6
commit 42ee63b776
15 changed files with 1462 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
firmware/ar_autopilot_v1/modbus_registers.yaml
+16
View File
@@ -103,6 +103,15 @@ inputs:
- { addr: 44, name: PID_INNER_KI_X1000, desc: "Inner-loop ki * 1000 (unsigned)", scale: 0.001 }
- { addr: 45, name: PID_INNER_KD_X1000, desc: "Inner-loop kd * 1000 (unsigned)", scale: 0.001 }
# ----- PID outer loop telemetry (Sprint 3) -----
- { addr: 50, name: PID_OUTER_HEADING_SP_X100, desc: "Outer-loop heading setpoint, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
- { addr: 51, name: PID_OUTER_RUDDER_SP_X100, desc: "Rudder setpoint produced by outer loop, deg*100 (signed int16)", unit: "deg", scale: 0.01 }
- { addr: 52, name: PID_OUTER_ERROR_X100, desc: "Outer-loop heading error, deg*100 (signed int16)", unit: "deg", scale: 0.01 }
- { addr: 53, name: PID_OUTER_SPEED_KN_X10, desc: "SOG currently used for gain scheduling, knots*10", unit: "kn", scale: 0.1 }
- { addr: 54, name: PID_OUTER_KP_X1000, desc: "Outer-loop active kp * 1000", scale: 0.001 }
- { addr: 55, name: PID_OUTER_KI_X1000, desc: "Outer-loop active ki * 1000", scale: 0.001 }
- { addr: 56, name: PID_OUTER_KD_X1000, desc: "Outer-loop active kd * 1000", scale: 0.001 }
# -----------------------------------------------------------------------------
# Holding registers (read-write 16-bit words) -- setpoints and config
# -----------------------------------------------------------------------------
@@ -118,3 +127,10 @@ holdings:
- { addr: 17, name: PID_INNER_KP_REQ_X1000, desc: "Requested inner-loop kp * 1000 (unsigned)", scale: 0.001 }
- { addr: 18, name: PID_INNER_KI_REQ_X1000, desc: "Requested inner-loop ki * 1000 (unsigned)", scale: 0.001 }
- { addr: 19, name: PID_INNER_KD_REQ_X1000, desc: "Requested inner-loop kd * 1000 (unsigned)", scale: 0.001 }
# ----- PID outer loop tunable holdings (Sprint 3) -----
- { addr: 24, name: PID_OUTER_HEADING_SP_REQ_X100, desc: "Requested outer-loop heading setpoint, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
- { addr: 25, name: PID_OUTER_SPEED_KN_REQ_X10, desc: "Requested SOG for gain scheduling, knots*10", unit: "kn", scale: 0.1 }
- { addr: 26, name: PID_OUTER_KP_REQ_X1000, desc: "Requested outer-loop base kp * 1000", scale: 0.001 }
- { addr: 27, name: PID_OUTER_KI_REQ_X1000, desc: "Requested outer-loop base ki * 1000", scale: 0.001 }
- { addr: 28, name: PID_OUTER_KD_REQ_X1000, desc: "Requested outer-loop base kd * 1000", scale: 0.001 }
firmware/ar_autopilot_v1/src/main.cpp
+6
View File
@@ -27,6 +27,7 @@
#include "hal/rudder_sensor.h"
#include "modes/standby.h"
#include "pid/pid_inner_task.h"
#include "pid/pid_outer_task.h"
#include "protocols/modbus_slave.h"
#include "protocols/nmea2000_consumer.h"
#include "safety/safety_monitor.h"
@@ -83,6 +84,11 @@ void setup() {
arautopilot::pid::pid_inner_task_init();
arautopilot::pid::pid_inner_task_start();
// PID outer loop (Sprint 3) -- Heading Hold. Only writes to the inner
// loop's setpoint when current mode == HEADING_HOLD.
arautopilot::pid::pid_outer_task_init();
arautopilot::pid::pid_outer_task_start();
ar_start_heartbeat_task();
// Modbus slave (server) -- exposes telemetry + commands to the display.
firmware/ar_autopilot_v1/src/modes/standby.cpp
+34 -5
View File
@@ -6,6 +6,9 @@
#include <Arduino.h>
#include "../hal/rudder_sensor.h"
#include "../pid/pid_outer_task.h"
#include "../protocols/nmea2000_consumer.h"
#include "../system/ar_log.h"
namespace arautopilot::modes {
@@ -45,9 +48,6 @@ Mode current_mode() {
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;
@@ -59,9 +59,38 @@ bool request_mode(Mode m) {
return true;
}
// ----- HEADING_HOLD activation (Sprint 3) -----
if (m == Mode::HEADING_HOLD) {
const auto n2k = protocols::nmea2000::nmea2000_latest();
const auto rd = hal::rudder_sensor_latest();
if (!n2k.heading_valid) {
AR_LOGW(TAG, "request_mode(HEADING_HOLD) rejected: heading sensor "
"not valid (no fresh PGN 127250)");
return false;
}
if (!rd.valid) {
AR_LOGW(TAG, "request_mode(HEADING_HOLD) rejected: rudder sensor "
"not valid yet (median filter still filling)");
return false;
}
// Capture the current heading as the initial setpoint so the
// operator doesn't get a sudden swing toward whatever was in
// the holding register beforehand.
pid::pid_outer_set_heading_setpoint_deg(n2k.heading_deg);
portENTER_CRITICAL(&g_mux);
Mode prev = g_mode;
g_mode = Mode::HEADING_HOLD;
portEXIT_CRITICAL(&g_mux);
AR_LOGI(TAG, "mode change: %s -> HEADING_HOLD (setpoint captured at %.1f deg)",
mode_name(prev), n2k.heading_deg);
return true;
}
// TRUE_COURSE / TRACK_KEEPING / DODGE: Sprint 5.
AR_LOGW(TAG,
"request_mode(%s) rejected: only STANDBY is implemented in "
"Sprint 1; current mode remains %s",
"request_mode(%s) rejected: mode not yet implemented "
"(Sprint 5+); current mode remains %s",
mode_name(m), mode_name(current_mode()));
return false;
}
firmware/ar_autopilot_v1/src/pid/pid_outer.h
+240
View File
@@ -0,0 +1,240 @@
// =============================================================================
// pid_outer.h -- outer heading-control PID (header-only, host-testable)
// =============================================================================
//
// Line-by-line port of arautopilot/studio/simulator/pid_outer.py. Same
// algorithm, same variables, same numerics. The Python module is the
// reference; this header must stay byte-equivalent in behaviour.
//
// No Arduino dependencies -- compiles on ESP32 + host g++/clang/MSVC.
// =============================================================================
#pragma once
#include <cstdint>
namespace arautopilot::pid {
struct OuterGainPoint {
float speed_kn;
float kp;
float ki;
float kd;
};
constexpr std::size_t MAX_OUTER_SCHEDULE_POINTS = 8;
struct PidOuterConfig {
// Sampling
float freq_hz{10.0f};
// Base gains (used if schedule_size == 0)
float base_kp{0.9f};
float base_ki{0.02f};
float base_kd{1.2f};
// SOG-indexed gain schedule (fixed-capacity array; schedule_size <= MAX)
OuterGainPoint schedule[MAX_OUTER_SCHEDULE_POINTS]{};
std::size_t schedule_size{0};
// Setpoint handling
float deadband_deg{0.5f};
float integral_clamp{35.0f};
// Output saturation
float max_rudder_deg{35.0f};
float rate_limit_dps{30.0f};
// Anti-windup
float aw_gain{-1.0f}; // -1 sentinel -> use 1/active_kp
// Derivative LPF
float d_lpf_tau_s{0.10f};
// ROT feed-forward (brief sec. 6)
float rot_ff_gain{1.5f};
float dt() const { return 1.0f / freq_hz; }
};
struct PidOuterState {
float integral{0.0f};
float prev_error{0.0f};
float prev_d_term{0.0f};
float prev_rudder_setpoint{0.0f};
float last_output_deg{0.0f};
};
namespace detail_outer {
inline float clamp_f(float x, float lo, float hi) {
if (x < lo) return lo;
if (x > hi) return hi;
return x;
}
inline float lpf_alpha(float tau_s, float dt) {
if (tau_s <= 0.0f) return 1.0f;
return dt / (tau_s + dt);
}
/// Shortest-arc signed error between two compass headings in degrees.
/// Returns value in (-180, +180]; positive means turn starboard.
inline float heading_error_deg(float setpoint, float measured) {
float delta = setpoint - measured;
// wrap to [0, 360)
delta = delta - 360.0f * (float)((int)(delta / 360.0f));
if (delta < 0.0f) delta += 360.0f;
if (delta > 180.0f) delta -= 360.0f;
return delta;
}
inline void interpolate_gains(
const PidOuterConfig& cfg, float speed_kn,
float& kp_out, float& ki_out, float& kd_out
) {
if (cfg.schedule_size == 0) {
kp_out = cfg.base_kp;
ki_out = cfg.base_ki;
kd_out = cfg.base_kd;
return;
}
// Schedule entries are expected to be sorted by speed_kn ascending
// (enforced by the constructor / config-update path).
if (speed_kn <= cfg.schedule[0].speed_kn) {
kp_out = cfg.schedule[0].kp;
ki_out = cfg.schedule[0].ki;
kd_out = cfg.schedule[0].kd;
return;
}
const std::size_t last = cfg.schedule_size - 1;
if (speed_kn >= cfg.schedule[last].speed_kn) {
kp_out = cfg.schedule[last].kp;
ki_out = cfg.schedule[last].ki;
kd_out = cfg.schedule[last].kd;
return;
}
for (std::size_t i = 0; i < last; ++i) {
const auto& a = cfg.schedule[i];
const auto& b = cfg.schedule[i + 1];
if (a.speed_kn <= speed_kn && speed_kn <= b.speed_kn) {
const float span = b.speed_kn - a.speed_kn;
const float t = (span == 0.0f) ? 0.0f
: (speed_kn - a.speed_kn) / span;
kp_out = a.kp + t * (b.kp - a.kp);
ki_out = a.ki + t * (b.ki - a.ki);
kd_out = a.kd + t * (b.kd - a.kd);
return;
}
}
// Unreachable.
kp_out = cfg.base_kp;
ki_out = cfg.base_ki;
kd_out = cfg.base_kd;
}
} // namespace detail_outer
class PidOuter {
public:
PidOuter() = default;
explicit PidOuter(const PidOuterConfig& config) : config_(config) {}
void reset() { state_ = PidOuterState{}; }
void update_config(const PidOuterConfig& config) { config_ = config; }
const PidOuterConfig& config() const { return config_; }
const PidOuterState& state() const { return state_; }
/// One controller tick. Returns the signed rudder setpoint in degrees,
/// already clamped to [-max_rudder_deg, +max_rudder_deg] and
/// rate-limited.
float step(
float heading_setpoint_deg,
float heading_measured_deg,
float rate_of_turn_dps,
float speed_kn,
bool allowed = true
) {
const PidOuterConfig& cfg = config_;
PidOuterState& st = state_;
const float dt = cfg.dt();
// Error with shortest-arc + deadband.
const float raw_err = detail_outer::heading_error_deg(
heading_setpoint_deg, heading_measured_deg
);
float error;
if (raw_err >= -cfg.deadband_deg && raw_err <= cfg.deadband_deg) {
error = 0.0f;
} else {
const float sign = (raw_err > 0.0f) ? 1.0f : -1.0f;
error = raw_err - sign * cfg.deadband_deg;
}
if (!allowed) {
st.integral *= 0.95f;
st.prev_error = error;
st.last_output_deg = 0.0f;
return 0.0f;
}
float kp = 0.0f, ki = 0.0f, kd = 0.0f;
detail_outer::interpolate_gains(cfg, speed_kn, kp, ki, kd);
const float p_term = kp * error;
st.integral += ki * error * dt;
st.integral = detail_outer::clamp_f(
st.integral, -cfg.integral_clamp, cfg.integral_clamp
);
const float d_raw = (dt > 0.0f) ? kd * (error - st.prev_error) / dt
: 0.0f;
const float alpha = detail_outer::lpf_alpha(cfg.d_lpf_tau_s, dt);
const float d_term = (1.0f - alpha) * st.prev_d_term + alpha * d_raw;
st.prev_d_term = d_term;
const float rot_ff_term = -cfg.rot_ff_gain * rate_of_turn_dps;
const float raw_rudder = p_term + st.integral + d_term + rot_ff_term;
float rudder = detail_outer::clamp_f(raw_rudder, -cfg.max_rudder_deg,
cfg.max_rudder_deg);
if (raw_rudder != rudder) {
float aw;
if (cfg.aw_gain >= 0.0f) {
aw = cfg.aw_gain;
} else if (kp != 0.0f) {
aw = 1.0f / kp;
} else {
aw = 0.0f;
}
st.integral -= aw * (raw_rudder - rudder) * dt;
st.integral = detail_outer::clamp_f(
st.integral, -cfg.integral_clamp, cfg.integral_clamp
);
}
// Rate-limit the produced rudder setpoint.
const float max_delta = cfg.rate_limit_dps * dt;
const float delta = rudder - st.prev_rudder_setpoint;
if (delta > max_delta) {
rudder = st.prev_rudder_setpoint + max_delta;
} else if (delta < -max_delta) {
rudder = st.prev_rudder_setpoint - max_delta;
}
st.prev_rudder_setpoint = rudder;
st.prev_error = error;
st.last_output_deg = rudder;
return rudder;
}
private:
PidOuterConfig config_{};
PidOuterState state_{};
};
} // namespace arautopilot::pid
firmware/ar_autopilot_v1/src/pid/pid_outer_task.cpp
+173
View File
@@ -0,0 +1,173 @@
// =============================================================================
// pid_outer_task.cpp -- 10 Hz outer-loop (heading control) task
// =============================================================================
#include "pid_outer_task.h"
#include <Arduino.h>
#include "../modes/standby.h"
#include "../protocols/nmea2000_consumer.h"
#include "../safety/watchdog.h"
#include "../system/ar_log.h"
#include "../system/task_config.h"
#include "pid_inner_task.h"
namespace arautopilot::pid {
namespace {
constexpr const char* TAG = "AR/PID";
portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
PidOuter g_outer{};
float g_heading_setpoint_deg = 0.0f;
float g_last_rudder_setpoint_deg = 0.0f;
float g_last_error_deg = 0.0f;
float g_speed_kn = 15.0f; // Sprint 3 default until PGN 129026 wiring (Sprint 5)
void OuterLoopTask(void* /*pv*/) {
AR_LOGI(TAG, "pid_outer task started on core %d (10 Hz)", xPortGetCoreID());
safety::watchdog_subscribe_current_task();
TickType_t last_wake = xTaskGetTickCount();
for (;;) {
// Snapshot inputs we need atomically.
float setpoint;
float speed_kn;
portENTER_CRITICAL(&g_mux);
setpoint = g_heading_setpoint_deg;
speed_kn = g_speed_kn;
portEXIT_CRITICAL(&g_mux);
const auto n2k = protocols::nmea2000::nmea2000_latest();
// Only active in HEADING_HOLD with valid heading sensor.
const bool in_hh =
modes::current_mode() == modes::Mode::HEADING_HOLD;
const bool allowed = in_hh && n2k.heading_valid;
const float rudder_sp = g_outer.step(
setpoint,
n2k.heading_deg,
n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
speed_kn,
allowed
);
// Always push the outer-loop output downstream. If `allowed` is
// false the output is zero, which corresponds to "rudder centred";
// the inner loop will pursue that or its own externally-supplied
// setpoint depending on the cascade configuration. In HH mode,
// the outer loop owns the inner setpoint.
if (in_hh) {
pid_inner_set_setpoint_deg(rudder_sp);
}
const float err =
n2k.heading_valid
? (setpoint - n2k.heading_deg)
: 0.0f;
portENTER_CRITICAL(&g_mux);
g_last_rudder_setpoint_deg = rudder_sp;
g_last_error_deg = err;
portEXIT_CRITICAL(&g_mux);
safety::watchdog_feed();
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_PID_OUTER));
}
}
} // namespace
void pid_outer_task_init() {
PidOuterConfig cfg;
// Seed the 3-point gain schedule from the 30 m yacht profile so the
// firmware has sensible defaults out of the box.
cfg.schedule_size = 3;
cfg.schedule[0] = {5.0f, 1.20f, 0.03f, 0.80f};
cfg.schedule[1] = {15.0f, 0.90f, 0.02f, 1.20f};
cfg.schedule[2] = {28.0f, 0.55f, 0.01f, 1.80f};
cfg.rot_ff_gain = 1.5f;
g_outer.update_config(cfg);
g_outer.reset();
AR_LOGI(TAG,
"pid_outer_init: schedule={5kn,15kn,28kn} rot_ff=%.2f freq=%.1fHz",
cfg.rot_ff_gain, cfg.freq_hz);
}
void pid_outer_task_start() {
xTaskCreatePinnedToCore(OuterLoopTask, "pid_outer", AR_TASK_STACK_PID_OUTER,
nullptr, AR_TASK_PRIO_PID_OUTER, nullptr,
AR_TASK_CORE_REALTIME);
}
void pid_outer_set_heading_setpoint_deg(float setpoint_deg) {
// Normalise to [0, 360).
float sp = setpoint_deg;
while (sp < 0.0f) sp += 360.0f;
while (sp >= 360.0f) sp -= 360.0f;
portENTER_CRITICAL(&g_mux);
g_heading_setpoint_deg = sp;
portEXIT_CRITICAL(&g_mux);
}
float pid_outer_heading_setpoint_deg() {
portENTER_CRITICAL(&g_mux);
float v = g_heading_setpoint_deg;
portEXIT_CRITICAL(&g_mux);
return v;
}
float pid_outer_last_rudder_setpoint_deg() {
portENTER_CRITICAL(&g_mux);
float v = g_last_rudder_setpoint_deg;
portEXIT_CRITICAL(&g_mux);
return v;
}
float pid_outer_last_error_deg() {
portENTER_CRITICAL(&g_mux);
float v = g_last_error_deg;
portEXIT_CRITICAL(&g_mux);
return v;
}
void pid_outer_set_speed_kn(float speed_kn) {
if (speed_kn < 0.0f) speed_kn = 0.0f;
if (speed_kn > 80.0f) speed_kn = 80.0f;
portENTER_CRITICAL(&g_mux);
g_speed_kn = speed_kn;
portEXIT_CRITICAL(&g_mux);
}
float pid_outer_speed_kn() {
portENTER_CRITICAL(&g_mux);
float v = g_speed_kn;
portEXIT_CRITICAL(&g_mux);
return v;
}
void pid_outer_update_gains(float kp, float ki, float kd) {
PidOuterConfig cfg = g_outer.config();
cfg.base_kp = kp;
cfg.base_ki = ki;
cfg.base_kd = kd;
cfg.schedule_size = 0; // explicit gains override the schedule
portENTER_CRITICAL(&g_mux);
g_outer.update_config(cfg);
portEXIT_CRITICAL(&g_mux);
AR_LOGI(TAG, "pid_outer base gains updated: kp=%.3f ki=%.3f kd=%.3f "
"(schedule disabled)", kp, ki, kd);
}
void pid_outer_get_gains(float& kp, float& ki, float& kd) {
portENTER_CRITICAL(&g_mux);
const auto& cfg = g_outer.config();
kp = cfg.base_kp;
ki = cfg.base_ki;
kd = cfg.base_kd;
portEXIT_CRITICAL(&g_mux);
}
} // namespace arautopilot::pid
firmware/ar_autopilot_v1/src/pid/pid_outer_task.h
+40
View File
@@ -0,0 +1,40 @@
// =============================================================================
// pid_outer_task.h -- 10 Hz outer-loop (heading control) task (Sprint 3)
// =============================================================================
//
// Reads heading + ROT from the NMEA 2000 snapshot, computes a rudder
// setpoint, hands it off to the inner loop. Active only in HEADING_HOLD
// mode; in any other mode the task ticks idle (allowed=false) which
// bleeds the integrator.
// =============================================================================
#pragma once
#include "pid_outer.h"
namespace arautopilot::pid {
void pid_outer_task_init();
void pid_outer_task_start();
/// Update the heading the outer loop pursues (degrees, 0..360).
void pid_outer_set_heading_setpoint_deg(float setpoint_deg);
/// Read the active heading setpoint (thread-safe).
float pid_outer_heading_setpoint_deg();
/// Read the last rudder setpoint the outer loop produced.
float pid_outer_last_rudder_setpoint_deg();
/// Read the last heading error the controller saw.
float pid_outer_last_error_deg();
/// Override the SOG used for gain scheduling (Sprint 3 keeps a default of
/// 15 kn until PGN 129026 wiring lands in Sprint 5).
void pid_outer_set_speed_kn(float speed_kn);
float pid_outer_speed_kn();
void pid_outer_update_gains(float kp, float ki, float kd);
void pid_outer_get_gains(float& kp, float& ki, float& kd);
} // namespace arautopilot::pid
firmware/ar_autopilot_v1/src/protocols/modbus_registers.h
+40 -4
View File
@@ -77,8 +77,8 @@ constexpr uint16_t COIL_CMD_ACK_ALL_ALARMS = 2;
constexpr uint16_t COIL_CMD_KNOB_ARM = 3;
// ----- Input registers (read-only words) -----
constexpr uint16_t INPUT_COUNT = 23;
constexpr uint16_t INPUT_MAX_ADDR = 45;
constexpr uint16_t INPUT_COUNT = 30;
constexpr uint16_t INPUT_MAX_ADDR = 56;
// Firmware major version
constexpr uint16_t INPUT_FW_VERSION_MAJOR = 0;
@@ -143,10 +143,31 @@ constexpr uint16_t INPUT_PID_INNER_KI_X1000 = 44;
// Inner-loop kd * 1000 (unsigned)
// scale=0.001
constexpr uint16_t INPUT_PID_INNER_KD_X1000 = 45;
// Outer-loop heading setpoint, deg*100 (0..35999)
// unit=deg, scale=0.01
constexpr uint16_t INPUT_PID_OUTER_HEADING_SP_X100 = 50;
// Rudder setpoint produced by outer loop, deg*100 (signed int16)
// unit=deg, scale=0.01
constexpr uint16_t INPUT_PID_OUTER_RUDDER_SP_X100 = 51;
// Outer-loop heading error, deg*100 (signed int16)
// unit=deg, scale=0.01
constexpr uint16_t INPUT_PID_OUTER_ERROR_X100 = 52;
// SOG currently used for gain scheduling, knots*10
// unit=kn, scale=0.1
constexpr uint16_t INPUT_PID_OUTER_SPEED_KN_X10 = 53;
// Outer-loop active kp * 1000
// scale=0.001
constexpr uint16_t INPUT_PID_OUTER_KP_X1000 = 54;
// Outer-loop active ki * 1000
// scale=0.001
constexpr uint16_t INPUT_PID_OUTER_KI_X1000 = 55;
// Outer-loop active kd * 1000
// scale=0.001
constexpr uint16_t INPUT_PID_OUTER_KD_X1000 = 56;
// ----- Holding registers (read-write words) -----
constexpr uint16_t HOLDING_COUNT = 9;
constexpr uint16_t HOLDING_MAX_ADDR = 19;
constexpr uint16_t HOLDING_COUNT = 14;
constexpr uint16_t HOLDING_MAX_ADDR = 28;
// Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)
constexpr uint16_t HOLDING_MODE_REQUEST = 0;
@@ -174,5 +195,20 @@ constexpr uint16_t HOLDING_PID_INNER_KI_REQ_X1000 = 18;
// Requested inner-loop kd * 1000 (unsigned)
// scale=0.001
constexpr uint16_t HOLDING_PID_INNER_KD_REQ_X1000 = 19;
// Requested outer-loop heading setpoint, deg*100 (0..35999)
// unit=deg, scale=0.01
constexpr uint16_t HOLDING_PID_OUTER_HEADING_SP_REQ_X100 = 24;
// Requested SOG for gain scheduling, knots*10
// unit=kn, scale=0.1
constexpr uint16_t HOLDING_PID_OUTER_SPEED_KN_REQ_X10 = 25;
// Requested outer-loop base kp * 1000
// scale=0.001
constexpr uint16_t HOLDING_PID_OUTER_KP_REQ_X1000 = 26;
// Requested outer-loop base ki * 1000
// scale=0.001
constexpr uint16_t HOLDING_PID_OUTER_KI_REQ_X1000 = 27;
// Requested outer-loop base kd * 1000
// scale=0.001
constexpr uint16_t HOLDING_PID_OUTER_KD_REQ_X1000 = 28;
} // namespace arautopilot::protocols::modbus
firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp
+82
View File
@@ -28,6 +28,7 @@
#include "../hal/rudder_sensor.h"
#include "../modes/standby.h"
#include "../pid/pid_inner_task.h"
#include "../pid/pid_outer_task.h"
#include "../system/ar_log.h"
#include "../system/task_config.h"
#include "modbus_registers.h"
@@ -60,6 +61,11 @@ struct HoldingStorage {
uint16_t pid_inner_kp_req_x1000 = 0;
uint16_t pid_inner_ki_req_x1000 = 0;
uint16_t pid_inner_kd_req_x1000 = 0;
uint16_t pid_outer_heading_sp_req_x100 = 0;
uint16_t pid_outer_speed_kn_req_x10 = 150; // 15.0 kn default
uint16_t pid_outer_kp_req_x1000 = 0;
uint16_t pid_outer_ki_req_x1000 = 0;
uint16_t pid_outer_kd_req_x1000 = 0;
};
HoldingStorage g_holding;
@@ -163,6 +169,46 @@ uint16_t read_input_register(uint16_t addr) {
return (uint16_t)scaled;
}
// ----- PID outer-loop telemetry (Sprint 3) -----
case INPUT_PID_OUTER_HEADING_SP_X100: {
int v = (int)(pid::pid_outer_heading_setpoint_deg() * 100.0f);
if (v < 0) v = 0;
if (v > 35999) v = 35999;
return (uint16_t)v;
}
case INPUT_PID_OUTER_RUDDER_SP_X100: {
int v = (int)(pid::pid_outer_last_rudder_setpoint_deg() * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_PID_OUTER_ERROR_X100: {
int v = (int)(pid::pid_outer_last_error_deg() * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_PID_OUTER_SPEED_KN_X10: {
int v = (int)(pid::pid_outer_speed_kn() * 10.0f);
if (v < 0) v = 0;
if (v > 65535) v = 65535;
return (uint16_t)v;
}
case INPUT_PID_OUTER_KP_X1000:
case INPUT_PID_OUTER_KI_X1000:
case INPUT_PID_OUTER_KD_X1000: {
float kp, ki, kd;
pid::pid_outer_get_gains(kp, ki, kd);
float v;
if (addr == INPUT_PID_OUTER_KP_X1000) v = kp;
else if (addr == INPUT_PID_OUTER_KI_X1000) v = ki;
else v = kd;
int scaled = (int)(v * 1000.0f);
if (scaled < 0) scaled = 0;
if (scaled > 65535) scaled = 65535;
return (uint16_t)scaled;
}
default:
return 0;
}
@@ -207,6 +253,11 @@ uint16_t read_holding(uint16_t addr) {
case HOLDING_PID_INNER_KP_REQ_X1000: return g_holding.pid_inner_kp_req_x1000;
case HOLDING_PID_INNER_KI_REQ_X1000: return g_holding.pid_inner_ki_req_x1000;
case HOLDING_PID_INNER_KD_REQ_X1000: return g_holding.pid_inner_kd_req_x1000;
case HOLDING_PID_OUTER_HEADING_SP_REQ_X100: return g_holding.pid_outer_heading_sp_req_x100;
case HOLDING_PID_OUTER_SPEED_KN_REQ_X10: return g_holding.pid_outer_speed_kn_req_x10;
case HOLDING_PID_OUTER_KP_REQ_X1000: return g_holding.pid_outer_kp_req_x1000;
case HOLDING_PID_OUTER_KI_REQ_X1000: return g_holding.pid_outer_ki_req_x1000;
case HOLDING_PID_OUTER_KD_REQ_X1000: return g_holding.pid_outer_kd_req_x1000;
default: return 0;
}
}
@@ -241,6 +292,37 @@ Modbus::Error write_holding(uint16_t addr, uint16_t value) {
pid::pid_inner_set_setpoint_deg((float)sv * 0.01f);
return Modbus::Error::SUCCESS;
}
case HOLDING_PID_OUTER_HEADING_SP_REQ_X100: {
if (value > 35999) return Modbus::Error::ILLEGAL_DATA_VALUE;
g_holding.pid_outer_heading_sp_req_x100 = value;
pid::pid_outer_set_heading_setpoint_deg((float)value * 0.01f);
return Modbus::Error::SUCCESS;
}
case HOLDING_PID_OUTER_SPEED_KN_REQ_X10: {
if (value > 800) return Modbus::Error::ILLEGAL_DATA_VALUE; // 80 kn cap
g_holding.pid_outer_speed_kn_req_x10 = value;
pid::pid_outer_set_speed_kn((float)value * 0.1f);
return Modbus::Error::SUCCESS;
}
case HOLDING_PID_OUTER_KP_REQ_X1000:
case HOLDING_PID_OUTER_KI_REQ_X1000:
case HOLDING_PID_OUTER_KD_REQ_X1000: {
if (addr == HOLDING_PID_OUTER_KP_REQ_X1000) {
g_holding.pid_outer_kp_req_x1000 = value;
} else if (addr == HOLDING_PID_OUTER_KI_REQ_X1000) {
g_holding.pid_outer_ki_req_x1000 = value;
} else {
g_holding.pid_outer_kd_req_x1000 = value;
}
float kp = (float)g_holding.pid_outer_kp_req_x1000 * 0.001f;
float ki = (float)g_holding.pid_outer_ki_req_x1000 * 0.001f;
float kd = (float)g_holding.pid_outer_kd_req_x1000 * 0.001f;
if (kp <= 0.0f) {
return Modbus::Error::ILLEGAL_DATA_VALUE;
}
pid::pid_outer_update_gains(kp, ki, kd);
return Modbus::Error::SUCCESS;
}
case HOLDING_PID_INNER_KP_REQ_X1000:
case HOLDING_PID_INNER_KI_REQ_X1000:
case HOLDING_PID_INNER_KD_REQ_X1000: {