sprint-2: PID inner loop + Python rudder simulator
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:
@@ -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 = 17;
|
||||
constexpr uint16_t INPUT_MAX_ADDR = 33;
|
||||
constexpr uint16_t INPUT_COUNT = 23;
|
||||
constexpr uint16_t INPUT_MAX_ADDR = 45;
|
||||
|
||||
// Firmware major version
|
||||
constexpr uint16_t INPUT_FW_VERSION_MAJOR = 0;
|
||||
@@ -125,10 +125,28 @@ 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;
|
||||
// Inner-loop rudder setpoint, deg*100 (signed int16)
|
||||
// unit=deg, scale=0.01
|
||||
constexpr uint16_t INPUT_PID_INNER_SETPOINT_X100 = 40;
|
||||
// Last PID command, %*100 (signed int16, -10000..+10000)
|
||||
// unit=%, scale=0.01
|
||||
constexpr uint16_t INPUT_PID_INNER_OUTPUT_X100 = 41;
|
||||
// Last PID error, deg*100 (signed int16)
|
||||
// unit=deg, scale=0.01
|
||||
constexpr uint16_t INPUT_PID_INNER_ERROR_X100 = 42;
|
||||
// Inner-loop kp * 1000 (unsigned)
|
||||
// scale=0.001
|
||||
constexpr uint16_t INPUT_PID_INNER_KP_X1000 = 43;
|
||||
// Inner-loop ki * 1000 (unsigned)
|
||||
// scale=0.001
|
||||
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;
|
||||
|
||||
// ----- Holding registers (read-write words) -----
|
||||
constexpr uint16_t HOLDING_COUNT = 5;
|
||||
constexpr uint16_t HOLDING_MAX_ADDR = 8;
|
||||
constexpr uint16_t HOLDING_COUNT = 9;
|
||||
constexpr uint16_t HOLDING_MAX_ADDR = 19;
|
||||
|
||||
// Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)
|
||||
constexpr uint16_t HOLDING_MODE_REQUEST = 0;
|
||||
@@ -144,5 +162,17 @@ 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;
|
||||
// Requested inner-loop rudder setpoint, deg*100 (signed int16)
|
||||
// unit=deg, scale=0.01
|
||||
constexpr uint16_t HOLDING_PID_INNER_SETPOINT_REQ_X100 = 16;
|
||||
// Requested inner-loop kp * 1000 (unsigned)
|
||||
// scale=0.001
|
||||
constexpr uint16_t HOLDING_PID_INNER_KP_REQ_X1000 = 17;
|
||||
// Requested inner-loop ki * 1000 (unsigned)
|
||||
// scale=0.001
|
||||
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;
|
||||
|
||||
} // namespace arautopilot::protocols::modbus
|
||||
|
||||
Reference in New Issue
Block a user