alro65/AR-Autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
42ee63b776118a541c1fd5ad9b23f1721283296e
AR-Autopilot/firmware/ar_autopilot_v1/modbus_registers.yaml
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

137 lines
10 KiB
YAML
Raw Blame History

# =============================================================================
# AR-Autopilot -- Modbus RTU register map (single source of truth)
# =============================================================================
#
# This file is read by tools/gen_modbus_registers.py to produce:
#
# firmware/ar_autopilot_v1/src/protocols/modbus_registers.h
# -> compile-time constants used by the firmware
#
# arautopilot/shared/modbus_register_map.py
# -> runtime constants used by the Studio simulator, the dedicated
# display (via the Flutter side mirror), and the Python tools
#
# Do NOT edit the generated files by hand. Always edit this YAML and run:
#
# python tools/gen_modbus_registers.py
#
# Register types (Modbus RTU standard):
# discrete -- read-only single bit (function code 0x02) addr 10001+
# coil -- read-write single bit (function codes 0x01/0x05/0x0F) addr 1+
# input -- read-only 16-bit register (function code 0x04) addr 30001+
# holding -- read-write 16-bit register (function codes 0x03/0x06/0x10) addr 40001+
#
# Addresses below are zero-based offsets within their type. Modbus over the
# wire adds the type base (10001 / 1 / 30001 / 40001).
#
# Scale factor: physical value = raw * scale + offset
# e.g. heading_deg with scale=0.01 means raw 4500 -> 45.00 deg.
# =============================================================================
schema_version: "0.1.0"
slave_id: 1 # Modbus slave (server) address
baudrate: 38400 # bps; 8N1 framing assumed
parity: "N"
data_bits: 8
stop_bits: 1
# -----------------------------------------------------------------------------
# Discrete inputs (read-only bits) -- status flags published by the firmware
# -----------------------------------------------------------------------------
discretes:
- { addr: 0, name: PILOT_ENGAGED, desc: "1 if the pilot is currently engaged (any mode other than STANDBY)" }
- { addr: 1, name: DI_DISENGAGE_BUTTON, desc: "Live state of the physical Engage/Disengage push-button" }
- { addr: 2, name: DI_LIMIT_PORT, desc: "Port-side rudder mechanical end-stop reached" }
- { addr: 3, name: DI_LIMIT_STBD, desc: "Starboard-side rudder mechanical end-stop reached" }
- { addr: 4, name: DI_EXTERNAL_ALARM, desc: "External critical alarm (VMS / genset) asserted" }
- { addr: 5, name: DI_MANUAL_CONFIRM, desc: "Manual confirmation switch asserted (emergency override)" }
- { addr: 8, name: ACTUATOR_POWER, desc: "Master actuator power relay (DO3) commanded ON" }
- { addr: 9, name: ACTUATOR_DRIVING_PORT, desc: "Actuator direction output: driving to port" }
- { addr: 10, name: ACTUATOR_DRIVING_STBD, desc: "Actuator direction output: driving to starboard" }
- { addr: 16, name: ALARM_OFF_COURSE, desc: "Off-course warning active" }
- { addr: 17, name: ALARM_OFF_COURSE_SEVERE, desc: "Severe off-course alarm (auto-disengage)" }
- { addr: 18, name: ALARM_RUDDER_NOT_RESP, desc: "Rudder command sent but no feedback motion" }
- { addr: 19, name: ALARM_HEADING_LOST, desc: "NMEA 2000 heading PGN not received >5 s" }
- { addr: 20, name: ALARM_ACTUATOR_OVERCURR, desc: "Actuator current over threshold" }
- { addr: 21, name: ALARM_VOLTAGE_LOW, desc: "Supply voltage below safe threshold" }
- { addr: 22, name: ALARM_LIMIT_REACHED, desc: "Rudder reached mechanical end-stop" }
- { addr: 23, name: ALARM_WATCHDOG_TRIPPED, desc: "Firmware watchdog fired -- controller reset" }
- { addr: 24, name: ALARM_VMS_CRITICAL, desc: "VMS reported blackout or electrical fault" }
- { addr: 25, name: ANY_ALARM_ACTIVE, desc: "OR of all alarm bits (convenience)" }
# -----------------------------------------------------------------------------
# Coils (read-write bits) -- commands written by the display / Studio
# -----------------------------------------------------------------------------
coils:
- { addr: 0, name: CMD_ENGAGE_REQUEST, desc: "Rising edge requests pilot engagement (subject to interlocks)" }
- { addr: 1, name: CMD_DISENGAGE_REQUEST, desc: "Rising edge forces pilot to STANDBY" }
- { addr: 2, name: CMD_ACK_ALL_ALARMS, desc: "Rising edge acknowledges every active alarm" }
- { addr: 3, name: CMD_KNOB_ARM, desc: "Knob arming request (Sprint 7+)" }
# -----------------------------------------------------------------------------
# Input registers (read-only 16-bit words) -- telemetry
# -----------------------------------------------------------------------------
# Two-register values (e.g. timestamps, floats) occupy consecutive addresses.
# -----------------------------------------------------------------------------
inputs:
- { addr: 0, name: FW_VERSION_MAJOR, desc: "Firmware major version", unit: "" }
- { addr: 1, name: FW_VERSION_MINOR, desc: "Firmware minor version", unit: "" }
- { addr: 2, name: FW_VERSION_PATCH, desc: "Firmware patch version", unit: "" }
- { addr: 3, name: SCHEMA_VERSION, desc: "Modbus map schema version (0=v0.1.0)", unit: "" }
- { addr: 4, name: UPTIME_SECONDS_LO, desc: "Uptime seconds, low 16 bits", unit: "s" }
- { addr: 5, name: UPTIME_SECONDS_HI, desc: "Uptime seconds, high 16 bits", unit: "s" }
- { addr: 6, name: CURRENT_MODE, desc: "Current AutopilotMode (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)", unit: "" }
- { addr: 7, name: FREE_HEAP_KB, desc: "Current free heap, KiB", unit: "KiB" }
- { addr: 8, name: MIN_FREE_HEAP_KB, desc: "Minimum free heap since boot", unit: "KiB" }
- { addr: 16, name: RUDDER_ANGLE_DEG_X100, desc: "Filtered rudder angle, deg * 100 (-3500..+3500)", unit: "deg", scale: 0.01 }
- { addr: 17, name: RUDDER_RAW_ADC, desc: "Raw ADC reading after median filter (0..4095)", unit: "counts" }
- { addr: 18, name: RUDDER_VALID, desc: "1 if median filter has filled (>=5 samples)", unit: "" }
- { addr: 24, name: HEADING_DEG_X100, desc: "Current heading from NMEA 2000 PGN 127250, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
- { addr: 25, name: ROT_DPS_X100, desc: "Rate of turn from PGN 127251, deg/s*100 (signed int16)", unit: "deg/s", scale: 0.01 }
- { addr: 26, name: HEADING_AGE_MS, desc: "Milliseconds since the last heading update (0..60000)", unit: "ms" }
- { addr: 32, name: BATTERY_VOLTAGE_X100, desc: "System battery voltage, V*100", unit: "V", scale: 0.01 }
- { addr: 33, name: ACTUATOR_CURRENT_X100, desc: "Actuator current, A*100", unit: "A", scale: 0.01 }
# ----- PID inner loop telemetry (Sprint 2) -----
- { addr: 40, name: PID_INNER_SETPOINT_X100, desc: "Inner-loop rudder setpoint, deg*100 (signed int16)", unit: "deg", scale: 0.01 }
- { addr: 41, name: PID_INNER_OUTPUT_X100, desc: "Last PID command, %*100 (signed int16, -10000..+10000)", unit: "%", scale: 0.01 }
- { addr: 42, name: PID_INNER_ERROR_X100, desc: "Last PID error, deg*100 (signed int16)", unit: "deg", scale: 0.01 }
- { addr: 43, name: PID_INNER_KP_X1000, desc: "Inner-loop kp * 1000 (unsigned)", scale: 0.001 }
- { addr: 44, name: PID_INNER_KI_X1000, desc: "Inner-loop ki * 1000 (unsigned)", scale: 0.001 }
- { addr: 45, name: PID_INNER_KD_X1000, desc: "Inner-loop kd * 1000 (unsigned)", scale: 0.001 }
# ----- PID outer loop telemetry (Sprint 3) -----
- { addr: 50, name: PID_OUTER_HEADING_SP_X100, desc: "Outer-loop heading setpoint, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
- { addr: 51, name: PID_OUTER_RUDDER_SP_X100, desc: "Rudder setpoint produced by outer loop, deg*100 (signed int16)", unit: "deg", scale: 0.01 }
- { addr: 52, name: PID_OUTER_ERROR_X100, desc: "Outer-loop heading error, deg*100 (signed int16)", unit: "deg", scale: 0.01 }
- { addr: 53, name: PID_OUTER_SPEED_KN_X10, desc: "SOG currently used for gain scheduling, knots*10", unit: "kn", scale: 0.1 }
- { addr: 54, name: PID_OUTER_KP_X1000, desc: "Outer-loop active kp * 1000", scale: 0.001 }
- { addr: 55, name: PID_OUTER_KI_X1000, desc: "Outer-loop active ki * 1000", scale: 0.001 }
- { addr: 56, name: PID_OUTER_KD_X1000, desc: "Outer-loop active kd * 1000", scale: 0.001 }
# -----------------------------------------------------------------------------
# Holding registers (read-write 16-bit words) -- setpoints and config
# -----------------------------------------------------------------------------
holdings:
- { addr: 0, name: MODE_REQUEST, desc: "Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)", unit: "" }
- { addr: 1, name: HEADING_SETPOINT_X100, desc: "Desired heading, deg*100", unit: "deg", scale: 0.01 }
- { addr: 2, name: BRIGHTNESS_PCT, desc: "Display brightness 0..100", unit: "%" }
- { addr: 3, name: ALARM_VOLUME_PCT, desc: "Alarm volume 0..100", unit: "%" }
- { addr: 8, name: DODGE_OFFSET_DEG_X100, desc: "Dodge mode heading offset, deg*100 (signed int16)", unit: "deg", scale: 0.01 }
# ----- PID inner loop tunable holdings (Sprint 2) -----
- { addr: 16, name: PID_INNER_SETPOINT_REQ_X100, desc: "Requested inner-loop rudder setpoint, deg*100 (signed int16)", unit: "deg", scale: 0.01 }
- { addr: 17, name: PID_INNER_KP_REQ_X1000, desc: "Requested inner-loop kp * 1000 (unsigned)", scale: 0.001 }
- { addr: 18, name: PID_INNER_KI_REQ_X1000, desc: "Requested inner-loop ki * 1000 (unsigned)", scale: 0.001 }
- { addr: 19, name: PID_INNER_KD_REQ_X1000, desc: "Requested inner-loop kd * 1000 (unsigned)", scale: 0.001 }
# ----- PID outer loop tunable holdings (Sprint 3) -----
- { addr: 24, name: PID_OUTER_HEADING_SP_REQ_X100, desc: "Requested outer-loop heading setpoint, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
- { addr: 25, name: PID_OUTER_SPEED_KN_REQ_X10, desc: "Requested SOG for gain scheduling, knots*10", unit: "kn", scale: 0.1 }
- { addr: 26, name: PID_OUTER_KP_REQ_X1000, desc: "Requested outer-loop base kp * 1000", scale: 0.001 }
- { addr: 27, name: PID_OUTER_KI_REQ_X1000, desc: "Requested outer-loop base ki * 1000", scale: 0.001 }
- { addr: 28, name: PID_OUTER_KD_REQ_X1000, desc: "Requested outer-loop base kd * 1000", scale: 0.001 }
Reference in New Issue View Git Blame Copy Permalink