alro65/AR-Autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
42ee63b776118a541c1fd5ad9b23f1721283296e
AR-Autopilot/arautopilot/shared/modbus_register_map.py
T
alro65 42ee63b776 sprint-3: PID outer + Heading Hold + ROT feed-forward + gain scheduling 
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>
2026-05-18 18:20:23 -04:00

120 lines
11 KiB
Python
Raw Blame History

"""AR-Autopilot Modbus RTU register map -- generated mirror of the firmware.
AUTO-GENERATED. Do not edit by hand.
Source: firmware/ar_autopilot_v1/modbus_registers.yaml
Regenerate with: python tools/gen_modbus_registers.py
"""
from __future__ import annotations
from dataclasses import dataclass
@dataclass(frozen=True)
class Reg:
"""One Modbus register entry as seen from Python tooling."""
addr: int
name: str
desc: str = ""
unit: str = ""
scale: float = 1.0
offset: float = 0.0
MAP_SCHEMA_VERSION = "0.1.0"
SLAVE_ID = 1
BAUDRATE = 38400
PARITY = "N"
DATA_BITS = 8
STOP_BITS = 1
# DISCRETES
DISCRETES: dict[str, Reg] = {
"PILOT_ENGAGED": Reg(addr=0, name="PILOT_ENGAGED", desc='1 if the pilot is currently engaged (any mode other than STANDBY)', unit="", scale=1.0, offset=0.0),
"DI_DISENGAGE_BUTTON": Reg(addr=1, name="DI_DISENGAGE_BUTTON", desc='Live state of the physical Engage/Disengage push-button', unit="", scale=1.0, offset=0.0),
"DI_LIMIT_PORT": Reg(addr=2, name="DI_LIMIT_PORT", desc='Port-side rudder mechanical end-stop reached', unit="", scale=1.0, offset=0.0),
"DI_LIMIT_STBD": Reg(addr=3, name="DI_LIMIT_STBD", desc='Starboard-side rudder mechanical end-stop reached', unit="", scale=1.0, offset=0.0),
"DI_EXTERNAL_ALARM": Reg(addr=4, name="DI_EXTERNAL_ALARM", desc='External critical alarm (VMS / genset) asserted', unit="", scale=1.0, offset=0.0),
"DI_MANUAL_CONFIRM": Reg(addr=5, name="DI_MANUAL_CONFIRM", desc='Manual confirmation switch asserted (emergency override)', unit="", scale=1.0, offset=0.0),
"ACTUATOR_POWER": Reg(addr=8, name="ACTUATOR_POWER", desc='Master actuator power relay (DO3) commanded ON', unit="", scale=1.0, offset=0.0),
"ACTUATOR_DRIVING_PORT": Reg(addr=9, name="ACTUATOR_DRIVING_PORT", desc='Actuator direction output: driving to port', unit="", scale=1.0, offset=0.0),
"ACTUATOR_DRIVING_STBD": Reg(addr=10, name="ACTUATOR_DRIVING_STBD", desc='Actuator direction output: driving to starboard', unit="", scale=1.0, offset=0.0),
"ALARM_OFF_COURSE": Reg(addr=16, name="ALARM_OFF_COURSE", desc='Off-course warning active', unit="", scale=1.0, offset=0.0),
"ALARM_OFF_COURSE_SEVERE": Reg(addr=17, name="ALARM_OFF_COURSE_SEVERE", desc='Severe off-course alarm (auto-disengage)', unit="", scale=1.0, offset=0.0),
"ALARM_RUDDER_NOT_RESP": Reg(addr=18, name="ALARM_RUDDER_NOT_RESP", desc='Rudder command sent but no feedback motion', unit="", scale=1.0, offset=0.0),
"ALARM_HEADING_LOST": Reg(addr=19, name="ALARM_HEADING_LOST", desc='NMEA 2000 heading PGN not received >5 s', unit="", scale=1.0, offset=0.0),
"ALARM_ACTUATOR_OVERCURR": Reg(addr=20, name="ALARM_ACTUATOR_OVERCURR", desc='Actuator current over threshold', unit="", scale=1.0, offset=0.0),
"ALARM_VOLTAGE_LOW": Reg(addr=21, name="ALARM_VOLTAGE_LOW", desc='Supply voltage below safe threshold', unit="", scale=1.0, offset=0.0),
"ALARM_LIMIT_REACHED": Reg(addr=22, name="ALARM_LIMIT_REACHED", desc='Rudder reached mechanical end-stop', unit="", scale=1.0, offset=0.0),
"ALARM_WATCHDOG_TRIPPED": Reg(addr=23, name="ALARM_WATCHDOG_TRIPPED", desc='Firmware watchdog fired -- controller reset', unit="", scale=1.0, offset=0.0),
"ALARM_VMS_CRITICAL": Reg(addr=24, name="ALARM_VMS_CRITICAL", desc='VMS reported blackout or electrical fault', unit="", scale=1.0, offset=0.0),
"ANY_ALARM_ACTIVE": Reg(addr=25, name="ANY_ALARM_ACTIVE", desc='OR of all alarm bits (convenience)', unit="", scale=1.0, offset=0.0),
}
# COILS
COILS: dict[str, Reg] = {
"CMD_ENGAGE_REQUEST": Reg(addr=0, name="CMD_ENGAGE_REQUEST", desc='Rising edge requests pilot engagement (subject to interlocks)', unit="", scale=1.0, offset=0.0),
"CMD_DISENGAGE_REQUEST": Reg(addr=1, name="CMD_DISENGAGE_REQUEST", desc='Rising edge forces pilot to STANDBY', unit="", scale=1.0, offset=0.0),
"CMD_ACK_ALL_ALARMS": Reg(addr=2, name="CMD_ACK_ALL_ALARMS", desc='Rising edge acknowledges every active alarm', unit="", scale=1.0, offset=0.0),
"CMD_KNOB_ARM": Reg(addr=3, name="CMD_KNOB_ARM", desc='Knob arming request (Sprint 7+)', unit="", scale=1.0, offset=0.0),
}
# INPUTS
INPUTS: dict[str, Reg] = {
"FW_VERSION_MAJOR": Reg(addr=0, name="FW_VERSION_MAJOR", desc='Firmware major version', unit="", scale=1.0, offset=0.0),
"FW_VERSION_MINOR": Reg(addr=1, name="FW_VERSION_MINOR", desc='Firmware minor version', unit="", scale=1.0, offset=0.0),
"FW_VERSION_PATCH": Reg(addr=2, name="FW_VERSION_PATCH", desc='Firmware patch version', unit="", scale=1.0, offset=0.0),
"SCHEMA_VERSION": Reg(addr=3, name="SCHEMA_VERSION", desc='Modbus map schema version (0=v0.1.0)', unit="", scale=1.0, offset=0.0),
"UPTIME_SECONDS_LO": Reg(addr=4, name="UPTIME_SECONDS_LO", desc='Uptime seconds, low 16 bits', unit="s", scale=1.0, offset=0.0),
"UPTIME_SECONDS_HI": Reg(addr=5, name="UPTIME_SECONDS_HI", desc='Uptime seconds, high 16 bits', unit="s", scale=1.0, offset=0.0),
"CURRENT_MODE": Reg(addr=6, name="CURRENT_MODE", desc='Current AutopilotMode (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)', unit="", scale=1.0, offset=0.0),
"FREE_HEAP_KB": Reg(addr=7, name="FREE_HEAP_KB", desc='Current free heap, KiB', unit="KiB", scale=1.0, offset=0.0),
"MIN_FREE_HEAP_KB": Reg(addr=8, name="MIN_FREE_HEAP_KB", desc='Minimum free heap since boot', unit="KiB", scale=1.0, offset=0.0),
"RUDDER_ANGLE_DEG_X100": Reg(addr=16, name="RUDDER_ANGLE_DEG_X100", desc='Filtered rudder angle, deg * 100 (-3500..+3500)', unit="deg", scale=0.01, offset=0.0),
"RUDDER_RAW_ADC": Reg(addr=17, name="RUDDER_RAW_ADC", desc='Raw ADC reading after median filter (0..4095)', unit="counts", scale=1.0, offset=0.0),
"RUDDER_VALID": Reg(addr=18, name="RUDDER_VALID", desc='1 if median filter has filled (>=5 samples)', unit="", scale=1.0, offset=0.0),
"HEADING_DEG_X100": Reg(addr=24, name="HEADING_DEG_X100", desc='Current heading from NMEA 2000 PGN 127250, deg*100 (0..35999)', unit="deg", scale=0.01, offset=0.0),
"ROT_DPS_X100": Reg(addr=25, name="ROT_DPS_X100", desc='Rate of turn from PGN 127251, deg/s*100 (signed int16)', unit="deg/s", scale=0.01, offset=0.0),
"HEADING_AGE_MS": Reg(addr=26, name="HEADING_AGE_MS", desc='Milliseconds since the last heading update (0..60000)', unit="ms", scale=1.0, offset=0.0),
"BATTERY_VOLTAGE_X100": Reg(addr=32, name="BATTERY_VOLTAGE_X100", desc='System battery voltage, V*100', unit="V", scale=0.01, offset=0.0),
"ACTUATOR_CURRENT_X100": Reg(addr=33, name="ACTUATOR_CURRENT_X100", desc='Actuator current, A*100', unit="A", scale=0.01, offset=0.0),
"PID_INNER_SETPOINT_X100": Reg(addr=40, name="PID_INNER_SETPOINT_X100", desc='Inner-loop rudder setpoint, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
"PID_INNER_OUTPUT_X100": Reg(addr=41, name="PID_INNER_OUTPUT_X100", desc='Last PID command, %*100 (signed int16, -10000..+10000)', unit="%", scale=0.01, offset=0.0),
"PID_INNER_ERROR_X100": Reg(addr=42, name="PID_INNER_ERROR_X100", desc='Last PID error, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
"PID_INNER_KP_X1000": Reg(addr=43, name="PID_INNER_KP_X1000", desc='Inner-loop kp * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
"PID_INNER_KI_X1000": Reg(addr=44, name="PID_INNER_KI_X1000", desc='Inner-loop ki * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
"PID_INNER_KD_X1000": Reg(addr=45, name="PID_INNER_KD_X1000", desc='Inner-loop kd * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
"PID_OUTER_HEADING_SP_X100": Reg(addr=50, name="PID_OUTER_HEADING_SP_X100", desc='Outer-loop heading setpoint, deg*100 (0..35999)', unit="deg", scale=0.01, offset=0.0),
"PID_OUTER_RUDDER_SP_X100": Reg(addr=51, name="PID_OUTER_RUDDER_SP_X100", desc='Rudder setpoint produced by outer loop, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
"PID_OUTER_ERROR_X100": Reg(addr=52, name="PID_OUTER_ERROR_X100", desc='Outer-loop heading error, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
"PID_OUTER_SPEED_KN_X10": Reg(addr=53, name="PID_OUTER_SPEED_KN_X10", desc='SOG currently used for gain scheduling, knots*10', unit="kn", scale=0.1, offset=0.0),
"PID_OUTER_KP_X1000": Reg(addr=54, name="PID_OUTER_KP_X1000", desc='Outer-loop active kp * 1000', unit="", scale=0.001, offset=0.0),
"PID_OUTER_KI_X1000": Reg(addr=55, name="PID_OUTER_KI_X1000", desc='Outer-loop active ki * 1000', unit="", scale=0.001, offset=0.0),
"PID_OUTER_KD_X1000": Reg(addr=56, name="PID_OUTER_KD_X1000", desc='Outer-loop active kd * 1000', unit="", scale=0.001, offset=0.0),
}
# HOLDINGS
HOLDINGS: dict[str, Reg] = {
"MODE_REQUEST": Reg(addr=0, name="MODE_REQUEST", desc='Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)', unit="", scale=1.0, offset=0.0),
"HEADING_SETPOINT_X100": Reg(addr=1, name="HEADING_SETPOINT_X100", desc='Desired heading, deg*100', unit="deg", scale=0.01, offset=0.0),
"BRIGHTNESS_PCT": Reg(addr=2, name="BRIGHTNESS_PCT", desc='Display brightness 0..100', unit="%", scale=1.0, offset=0.0),
"ALARM_VOLUME_PCT": Reg(addr=3, name="ALARM_VOLUME_PCT", desc='Alarm volume 0..100', unit="%", scale=1.0, offset=0.0),
"DODGE_OFFSET_DEG_X100": Reg(addr=8, name="DODGE_OFFSET_DEG_X100", desc='Dodge mode heading offset, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
"PID_INNER_SETPOINT_REQ_X100": Reg(addr=16, name="PID_INNER_SETPOINT_REQ_X100", desc='Requested inner-loop rudder setpoint, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
"PID_INNER_KP_REQ_X1000": Reg(addr=17, name="PID_INNER_KP_REQ_X1000", desc='Requested inner-loop kp * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
"PID_INNER_KI_REQ_X1000": Reg(addr=18, name="PID_INNER_KI_REQ_X1000", desc='Requested inner-loop ki * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
"PID_INNER_KD_REQ_X1000": Reg(addr=19, name="PID_INNER_KD_REQ_X1000", desc='Requested inner-loop kd * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
"PID_OUTER_HEADING_SP_REQ_X100": Reg(addr=24, name="PID_OUTER_HEADING_SP_REQ_X100", desc='Requested outer-loop heading setpoint, deg*100 (0..35999)', unit="deg", scale=0.01, offset=0.0),
"PID_OUTER_SPEED_KN_REQ_X10": Reg(addr=25, name="PID_OUTER_SPEED_KN_REQ_X10", desc='Requested SOG for gain scheduling, knots*10', unit="kn", scale=0.1, offset=0.0),
"PID_OUTER_KP_REQ_X1000": Reg(addr=26, name="PID_OUTER_KP_REQ_X1000", desc='Requested outer-loop base kp * 1000', unit="", scale=0.001, offset=0.0),
"PID_OUTER_KI_REQ_X1000": Reg(addr=27, name="PID_OUTER_KI_REQ_X1000", desc='Requested outer-loop base ki * 1000', unit="", scale=0.001, offset=0.0),
"PID_OUTER_KD_REQ_X1000": Reg(addr=28, name="PID_OUTER_KD_REQ_X1000", desc='Requested outer-loop base kd * 1000', unit="", scale=0.001, offset=0.0),
}
ALL_GROUPS: dict[str, dict[str, Reg]] = {
"discretes": DISCRETES,
"coils": COILS,
"inputs": INPUTS,
"holdings": HOLDINGS,
}
Reference in New Issue View Git Blame Copy Permalink