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/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: {