alro65
295efa2d83
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>
179 lines
6.8 KiB
C++
179 lines
6.8 KiB
C++
// =============================================================================
|
|
// modbus_registers.h -- AR-Autopilot Modbus RTU register map
|
|
// =============================================================================
|
|
//
|
|
// AUTO-GENERATED. Do not edit by hand.
|
|
// Source: firmware/ar_autopilot_v1/modbus_registers.yaml
|
|
// Regenerate with: python tools/gen_modbus_registers.py
|
|
// =============================================================================
|
|
|
|
#pragma once
|
|
|
|
#include <cstdint>
|
|
|
|
namespace arautopilot::protocols::modbus {
|
|
|
|
constexpr const char* MAP_SCHEMA_VERSION = "0.1.0";
|
|
constexpr uint8_t SLAVE_ID = 1;
|
|
constexpr uint32_t BAUDRATE = 38400;
|
|
constexpr char PARITY = 'N';
|
|
constexpr uint8_t DATA_BITS = 8;
|
|
constexpr uint8_t STOP_BITS = 1;
|
|
|
|
// ----- Discrete inputs (read-only bits) -----
|
|
constexpr uint16_t DISCRETE_COUNT = 19;
|
|
constexpr uint16_t DISCRETE_MAX_ADDR = 25;
|
|
|
|
// 1 if the pilot is currently engaged (any mode other than STANDBY)
|
|
constexpr uint16_t DISCRETE_PILOT_ENGAGED = 0;
|
|
// Live state of the physical Engage/Disengage push-button
|
|
constexpr uint16_t DISCRETE_DI_DISENGAGE_BUTTON = 1;
|
|
// Port-side rudder mechanical end-stop reached
|
|
constexpr uint16_t DISCRETE_DI_LIMIT_PORT = 2;
|
|
// Starboard-side rudder mechanical end-stop reached
|
|
constexpr uint16_t DISCRETE_DI_LIMIT_STBD = 3;
|
|
// External critical alarm (VMS / genset) asserted
|
|
constexpr uint16_t DISCRETE_DI_EXTERNAL_ALARM = 4;
|
|
// Manual confirmation switch asserted (emergency override)
|
|
constexpr uint16_t DISCRETE_DI_MANUAL_CONFIRM = 5;
|
|
// Master actuator power relay (DO3) commanded ON
|
|
constexpr uint16_t DISCRETE_ACTUATOR_POWER = 8;
|
|
// Actuator direction output: driving to port
|
|
constexpr uint16_t DISCRETE_ACTUATOR_DRIVING_PORT = 9;
|
|
// Actuator direction output: driving to starboard
|
|
constexpr uint16_t DISCRETE_ACTUATOR_DRIVING_STBD = 10;
|
|
// Off-course warning active
|
|
constexpr uint16_t DISCRETE_ALARM_OFF_COURSE = 16;
|
|
// Severe off-course alarm (auto-disengage)
|
|
constexpr uint16_t DISCRETE_ALARM_OFF_COURSE_SEVERE = 17;
|
|
// Rudder command sent but no feedback motion
|
|
constexpr uint16_t DISCRETE_ALARM_RUDDER_NOT_RESP = 18;
|
|
// NMEA 2000 heading PGN not received >5 s
|
|
constexpr uint16_t DISCRETE_ALARM_HEADING_LOST = 19;
|
|
// Actuator current over threshold
|
|
constexpr uint16_t DISCRETE_ALARM_ACTUATOR_OVERCURR = 20;
|
|
// Supply voltage below safe threshold
|
|
constexpr uint16_t DISCRETE_ALARM_VOLTAGE_LOW = 21;
|
|
// Rudder reached mechanical end-stop
|
|
constexpr uint16_t DISCRETE_ALARM_LIMIT_REACHED = 22;
|
|
// Firmware watchdog fired -- controller reset
|
|
constexpr uint16_t DISCRETE_ALARM_WATCHDOG_TRIPPED = 23;
|
|
// VMS reported blackout or electrical fault
|
|
constexpr uint16_t DISCRETE_ALARM_VMS_CRITICAL = 24;
|
|
// OR of all alarm bits (convenience)
|
|
constexpr uint16_t DISCRETE_ANY_ALARM_ACTIVE = 25;
|
|
|
|
// ----- Coils (read-write bits) -----
|
|
constexpr uint16_t COIL_COUNT = 4;
|
|
constexpr uint16_t COIL_MAX_ADDR = 3;
|
|
|
|
// Rising edge requests pilot engagement (subject to interlocks)
|
|
constexpr uint16_t COIL_CMD_ENGAGE_REQUEST = 0;
|
|
// Rising edge forces pilot to STANDBY
|
|
constexpr uint16_t COIL_CMD_DISENGAGE_REQUEST = 1;
|
|
// Rising edge acknowledges every active alarm
|
|
constexpr uint16_t COIL_CMD_ACK_ALL_ALARMS = 2;
|
|
// Knob arming request (Sprint 7+)
|
|
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;
|
|
|
|
// Firmware major version
|
|
constexpr uint16_t INPUT_FW_VERSION_MAJOR = 0;
|
|
// Firmware minor version
|
|
constexpr uint16_t INPUT_FW_VERSION_MINOR = 1;
|
|
// Firmware patch version
|
|
constexpr uint16_t INPUT_FW_VERSION_PATCH = 2;
|
|
// Modbus map schema version (0=v0.1.0)
|
|
constexpr uint16_t INPUT_SCHEMA_VERSION = 3;
|
|
// Uptime seconds, low 16 bits
|
|
// unit=s
|
|
constexpr uint16_t INPUT_UPTIME_SECONDS_LO = 4;
|
|
// Uptime seconds, high 16 bits
|
|
// unit=s
|
|
constexpr uint16_t INPUT_UPTIME_SECONDS_HI = 5;
|
|
// Current AutopilotMode (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)
|
|
constexpr uint16_t INPUT_CURRENT_MODE = 6;
|
|
// Current free heap, KiB
|
|
// unit=KiB
|
|
constexpr uint16_t INPUT_FREE_HEAP_KB = 7;
|
|
// Minimum free heap since boot
|
|
// unit=KiB
|
|
constexpr uint16_t INPUT_MIN_FREE_HEAP_KB = 8;
|
|
// Filtered rudder angle, deg * 100 (-3500..+3500)
|
|
// unit=deg, scale=0.01
|
|
constexpr uint16_t INPUT_RUDDER_ANGLE_DEG_X100 = 16;
|
|
// Raw ADC reading after median filter (0..4095)
|
|
// unit=counts
|
|
constexpr uint16_t INPUT_RUDDER_RAW_ADC = 17;
|
|
// 1 if median filter has filled (>=5 samples)
|
|
constexpr uint16_t INPUT_RUDDER_VALID = 18;
|
|
// Current heading from NMEA 2000 PGN 127250, deg*100 (0..35999)
|
|
// unit=deg, scale=0.01
|
|
constexpr uint16_t INPUT_HEADING_DEG_X100 = 24;
|
|
// Rate of turn from PGN 127251, deg/s*100 (signed int16)
|
|
// unit=deg/s, scale=0.01
|
|
constexpr uint16_t INPUT_ROT_DPS_X100 = 25;
|
|
// Milliseconds since the last heading update (0..60000)
|
|
// unit=ms
|
|
constexpr uint16_t INPUT_HEADING_AGE_MS = 26;
|
|
// System battery voltage, V*100
|
|
// unit=V, scale=0.01
|
|
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 = 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;
|
|
// Desired heading, deg*100
|
|
// unit=deg, scale=0.01
|
|
constexpr uint16_t HOLDING_HEADING_SETPOINT_X100 = 1;
|
|
// Display brightness 0..100
|
|
// unit=%
|
|
constexpr uint16_t HOLDING_BRIGHTNESS_PCT = 2;
|
|
// Alarm volume 0..100
|
|
// unit=%
|
|
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
|