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

sprint-2: PID inner loop + Python rudder simulator

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

Builds: pio run -e esp32-dev SUCCESS, RAM 6.8%, Flash 26.8% (351 KB).
Tests: pytest 129/129 green (110 Sprint 1 + 19 Sprint 2).

Python (arautopilot/studio/simulator/):

- rudder_dynamics.py: marine-realistic physical model of a hydraulic
  rudder actuator. Defaults tuned so 100 % PWM produces steady-state
  v_max ~5 deg/s, matching the brief's "typical 3-6 dps" for a 30 m
  yacht. Includes deadband, min-useful PWM snap, port/stbd asymmetry,
  end-stops, optional external torque, RunRecorder helper.
- pid_inner.py: pure-Python reference PID. Anti-windup via back-
  calculation, setpoint rate limit, setpoint deadband, derivative LPF,
  actuator non-linearity compensation. This module is the algorithmic
  source of truth; C++ firmware is a line-by-line port.

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

- pid_inner.h: header-only C++17 controller, byte-equivalent port of
  pid_inner.py. Compiles on ESP32 toolchain AND on host g++/clang/MSVC
  (no Arduino dependencies) -- ready for native Unity cross-validation
  once a host compiler is installed.
- pid_inner_task.{h,cpp}: FreeRTOS task wrapper. 50 Hz on Core 1
  (real-time core). Subscribes to TWDT, bleeds integrator during
  STANDBY, surfaces telemetry + tunables via the Modbus slave.

Modbus map (regenerated from YAML):

- 6 new INPUT registers (40-45): setpoint, output, error, kp/ki/kd live
- 4 new HOLDING registers (16-19): writable setpoint + kp/ki/kd req
  (writes propagate atomically; zero kp rejected as ILLEGAL_DATA_VALUE)

Tests:

- test_rudder_simulator.py: 9 tests (zero-input rest, full deflection,
  end-stop saturation, deadband, min-useful snap, asymmetry, recorder
  API, invalid dt, end-stop velocity zeroing).
- test_pid_inner_python.py: 10 tests (positive/negative step response,
  setpoint deadband holds, anti-windup bounds under saturation,
  allowed=false bleeds integrator, actuator deadband + asymmetry
  compensation, output saturation, rate limit, disturbance rejection).

NOT in Sprint 2 (intentional per brief sec. 12):
  - Outer heading PID, gain scheduling by SOG, ROT feed-forward
    (those land in Sprint 3)
  - Cross-validation tests via ctypes (need host C++ compiler that
    this Windows machine lacks; algorithmic parity enforced by review)

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Alvaro Enrique Romero Donado
2026-05-18 14:18:41 -04:00
parent 65860948b4
commit 295efa2d83
16 changed files with 1477 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp
+76
View File
@@ -27,6 +27,7 @@
#include "../hal/pinout.h"
#include "../hal/rudder_sensor.h"
#include "../modes/standby.h"
#include "../pid/pid_inner_task.h"
#include "../system/ar_log.h"
#include "../system/task_config.h"
#include "modbus_registers.h"
@@ -55,6 +56,10 @@ struct HoldingStorage {
uint16_t brightness_pct = 80;
uint16_t alarm_volume_pct = 60;
int16_t dodge_offset_deg_x100 = 0;
int16_t pid_inner_setpoint_req_x100 = 0;
uint16_t pid_inner_kp_req_x1000 = 0;
uint16_t pid_inner_ki_req_x1000 = 0;
uint16_t pid_inner_kd_req_x1000 = 0;
};
HoldingStorage g_holding;
@@ -124,6 +129,40 @@ uint16_t read_input_register(uint16_t addr) {
case INPUT_ACTUATOR_CURRENT_X100:
return 0;
// ----- PID inner-loop telemetry (Sprint 2) -----
case INPUT_PID_INNER_SETPOINT_X100: {
int v = (int)(pid::pid_inner_setpoint_deg() * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_PID_INNER_OUTPUT_X100: {
int v = (int)(pid::pid_inner_last_output_pct() * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_PID_INNER_ERROR_X100: {
int v = (int)(pid::pid_inner_last_error_deg() * 100.0f);
if (v < -32768) v = -32768;
if (v > 32767) v = 32767;
return (uint16_t)(int16_t)v;
}
case INPUT_PID_INNER_KP_X1000:
case INPUT_PID_INNER_KI_X1000:
case INPUT_PID_INNER_KD_X1000: {
float kp, ki, kd;
pid::pid_inner_get_gains(kp, ki, kd);
float v;
if (addr == INPUT_PID_INNER_KP_X1000) v = kp;
else if (addr == INPUT_PID_INNER_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;
}
@@ -164,6 +203,10 @@ uint16_t read_holding(uint16_t addr) {
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;
case HOLDING_PID_INNER_SETPOINT_REQ_X100: return (uint16_t)g_holding.pid_inner_setpoint_req_x100;
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;
default: return 0;
}
}
@@ -190,6 +233,39 @@ Modbus::Error write_holding(uint16_t addr, uint16_t value) {
case HOLDING_DODGE_OFFSET_DEG_X100:
g_holding.dodge_offset_deg_x100 = (int16_t)value;
return Modbus::Error::SUCCESS;
// ----- PID inner-loop tunables (Sprint 2) -----
case HOLDING_PID_INNER_SETPOINT_REQ_X100: {
int16_t sv = (int16_t)value;
g_holding.pid_inner_setpoint_req_x100 = sv;
pid::pid_inner_set_setpoint_deg((float)sv * 0.01f);
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: {
// Update the requested-gain shadow, then push all three to the
// live controller. We do all three together so partial writes
// don't leave the gains inconsistent.
if (addr == HOLDING_PID_INNER_KP_REQ_X1000) {
g_holding.pid_inner_kp_req_x1000 = value;
} else if (addr == HOLDING_PID_INNER_KI_REQ_X1000) {
g_holding.pid_inner_ki_req_x1000 = value;
} else {
g_holding.pid_inner_kd_req_x1000 = value;
}
float kp = (float)g_holding.pid_inner_kp_req_x1000 * 0.001f;
float ki = (float)g_holding.pid_inner_ki_req_x1000 * 0.001f;
float kd = (float)g_holding.pid_inner_kd_req_x1000 * 0.001f;
// Refuse zero kp -- the rest of the algorithm assumes kp > 0
// for back-calculation anti-windup. If the operator writes 0
// we ignore it (leave whatever the firmware booted with).
if (kp <= 0.0f) {
return Modbus::Error::ILLEGAL_DATA_VALUE;
}
pid::pid_inner_update_gains(kp, ki, kd);
return Modbus::Error::SUCCESS;
}
default:
return Modbus::Error::ILLEGAL_DATA_ADDRESS;
}