Autopilot: rescue as a synthesised mission, TAKEOFF and hold patterns, SITL fidelity - #15413
Conversation
SITL now runs the Mahony estimator against the virtual gyro/acc/mag feeds instead of setting attitude straight from the FDM quaternion, making IMU heading recovery (GPS course-over-ground fusion) testable; build with -DSITL_ATTITUDE_DIRECT to restore the legacy path. The virtual mag is detected like any other (mag_hardware = NONE opts out) and fed a synthetic earth field rotated by the true attitude, with enough inclination and declination that every body component stays nonzero for the calibration gate. The harness signals GPS loss with out-of-range lat/lon (isnan is dead code under -Ofast) and the virtual GPS tracks feed freshness with a counter the GPS task checks on its own clock (SITL micros() is not safe from the feeder thread), so a dark feed trips the normal receive timeout and recovers when data returns. The harness also gains a 10 Hz trajectory recorder with metric helpers, and an initial-yaw option for wrong-heading scenarios.
…CUE_PLAN With ENABLE_RESCUE_PLAN (default off, byte-identical builds otherwise) the failsafe GPS-RESCUE procedure stages a 3-waypoint mission — altitude-gated climb-in-place HOLD, FLYOVER home, LAND — that flightPlanNavEngage consumes in place of the PG plan; core.c engages the executor via the existing FAILSAFE_AUTOPILOT phase with a 1 s grace window, and a mission that cannot start or aborts degrades to the baro-only auto-landing rather than re-staging itself. Return altitude mirrors legacy initRescueValues per altitude mode (a behaviour-neutral getter exposes the tracked maximum) and never commands an en-route descent. While the IMU heading is untrusted at the top of the climb the executor holds the plan and pitches forward until GPS course-over-ground teaches the estimator its heading, with the legacy 15 s give-up aborting to the failsafe fallback. The BOXGPSRESCUE switch keeps flying legacy rescue and preempts the mission. Engage now captures the estimator-vs-GPS altitude frame offset instead of the raw estimator reading, which shifted every waypoint up by the current height when a rescue engaged mid-flight. SITL A/B harness: --binary-b runs rescue scenarios against both implementations and compares qualitative outcome metrics; scenarios cover the plain return, heading recovery from a 90 degree error without a mag, and GPS loss mid-return. Blackbox .BFL artifacts are captured per leg.
TAKEOFF waypoints climb at the current position to the waypoint altitude (lat/lon advisory, matching MAV_CMD_NAV_TAKEOFF), gated on altitude arrival, with duration as a post-climb loiter. HOLD waypoints with a pattern chase a time-parametrised carrot around the hold point at ap_waypoint_hold_radius. The carrot command never completes (completionSpeed 0) so the hold timer and callback are undisturbed, and positionNavMoveTargetEf() moves the target each step without resetting the velocity ramp. Pattern start is deferred until the arrival braking settles, and the carrot rate is capped at 0.25 rad/s; entry momentum or faster rotation than the control chain can phase-track balloons the pursuit orbit well outside the ring. WAYPOINT_PATTERN_NONE = 0 keeps stored missions station-keeping. MAVLink LOITER_TURNS now maps to an ORBIT hold, with the turn count converted to and from a hold duration at the executor's orbit rate.
|
Do you want to test this code? You can flash it directly from the Betaflight App:
WARNING: It may be unstable. Use only for testing! |
|
No actionable comments were generated in the recent review. 🎉 ℹ️ Recent review info⚙️ Run configurationConfiguration used: Path: .coderabbit.yaml Review profile: CHILL Plan: Pro Run ID: 📒 Files selected for processing (2)
🚧 Files skipped from review as they are similar to previous changes (1)
WalkthroughGPS rescue can optionally execute as a synthesized flight plan. The changes add rescue-aware failsafe transitions, waypoint hold patterns, navigation target updates, MAVLink support, simulator sensor handling, and unit/SITL coverage. ChangesRescue navigation and flight-plan execution
Estimated code review effort: 5 (Critical) | ~120 minutes Sequence Diagram(s)sequenceDiagram
participant Pilot
participant Failsafe
participant FlightPlanNav
participant PositionNav
participant Simulator
Pilot->>Failsafe: trigger RX-loss rescue
Failsafe->>FlightPlanNav: stage rescue plan
FlightPlanNav->>PositionNav: dispatch climb target
PositionNav->>FlightPlanNav: report waypoint reached
FlightPlanNav->>PositionNav: move hold-pattern target
Simulator->>FlightPlanNav: provide GPS and attitude state
FlightPlanNav->>Failsafe: complete or abort rescue mission
Possibly related issues
Possibly related PRs
Suggested labels: Suggested reviewers: 🚥 Pre-merge checks | ✅ 4 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (4 passed)
✨ Finishing Touches🧪 Generate unit tests (beta)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
There was a problem hiding this comment.
Pull request overview
This PR advances the autopilot program by improving SITL fidelity, adding waypoint-type behavior (TAKEOFF and patterned HOLD), and implementing failsafe GPS rescue as a synthesized flight-plan mission behind ENABLE_RESCUE_PLAN (default off), with substantial unit + SITL harness coverage.
Changes:
- Add rescue-as-mission staging/engagement and failsafe integration behind
ENABLE_RESCUE_PLAN, including heading-recovery hold/timeout behavior and fallback logic. - Implement TAKEOFF semantics (climb-in-place + optional loiter) and HOLD patterns (NONE/ORBIT/FIGURE8) with moving-target “carrot” support.
- Improve SITL realism and testability (real attitude estimator path, virtual mag feed, GPS-loss injection, trajectory recording, A/B harness runs, optional ground-truth telemetry fan-out).
Reviewed changes
Copilot reviewed 26 out of 26 changed files in this pull request and generated 2 comments.
Show a summary per file
| File | Description |
|---|---|
| src/test/unit/position_nav_unittest.cc | Adds unit coverage for moving a live position-nav target without restarting velocity ramping. |
| src/test/unit/flight_plan_rescue_unittest.cc | New unit tests for the rescue plan synthesis and its staged/active behavior under ENABLE_RESCUE_PLAN. |
| src/test/unit/flight_plan_nav_unittest.cc | Expands mission executor tests for TAKEOFF and HOLD patterns; adjusts defaults to WAYPOINT_PATTERN_NONE. |
| src/test/unit/flight_failsafe_rescue_unittest.cc | New unit tests validating failsafe GPS rescue staging, grace period, and fallback logic under ENABLE_RESCUE_PLAN. |
| src/test/sitl/sitl_harness.py | Enhances SITL harness with history recorder, GPS-loss injection, A/B scenario execution, and optional ground-truth telemetry output. |
| src/test/Makefile | Wires new unit test binaries and per-test compile flags/sources (including ENABLE_RESCUE_PLAN=1 variants). |
| src/platform/SIMULATOR/target/SITL/target.h | Makes real estimator the default for SITL, with an opt-out define for legacy direct attitude. |
| src/platform/SIMULATOR/sitl.c | Adds virtual mag feeding and GPS-loss sentinel behavior; refactors quaternion handling for consistency. |
| src/main/telemetry/mavlink_mission.c | Adds MAVLink mapping for ORBIT HOLD via LOITER_TURNS and fixes default pattern initialization to NONE. |
| src/main/target/common_post.h | Introduces ENABLE_RESCUE_PLAN defaulting to 0 with compile-time dependency checks. |
| src/main/sensors/compass.c | Adds virtual-mag defaults/detection paths for simulator use. |
| src/main/pg/flight_plan.h | Adds WAYPOINT_PATTERN_NONE=0 and WAYPOINT_PATTERN_COUNT for safer pattern handling. |
| src/main/osd/osd_warnings.c | Adds OSD warning text for FP_ABORT_HEADING. |
| src/main/io/gps.c | Improves virtual GPS freshness handling to allow “GPS goes dark” behavior and recovery from stale feeds. |
| src/main/io/gps_virtual.h | Exposes a virtual-GPS update counter for freshness detection. |
| src/main/io/gps_virtual.c | Implements the update counter used for virtual GPS freshness checks. |
| src/main/flight/position_nav.h | Adds positionNavMoveTargetEf() API for moving-target use cases (pattern carrots). |
| src/main/flight/position_nav.c | Implements positionNavMoveTargetEf() preserving command state and ramping. |
| src/main/flight/gps_rescue_multirotor.h | Exposes gpsRescueGetMaxAltitudeCm() for rescue plan synthesis. |
| src/main/flight/gps_rescue_multirotor.c | Implements gpsRescueGetMaxAltitudeCm() returning the running max altitude since arming. |
| src/main/flight/flight_plan_nav.h | Adds orbit-period API and rescue-plan staging APIs (guarded by ENABLE_RESCUE_PLAN). |
| src/main/flight/flight_plan_nav.c | Implements TAKEOFF behavior, HOLD patterns, orbit period computation, rescue plan synthesis + heading recovery hold. |
| src/main/flight/failsafe.h | Adds an engage-deadline field for the rescue-plan autopilot grace window. |
| src/main/flight/failsafe.c | Integrates rescue plan staging into failsafe GPS rescue (grace window + safe fallback behavior). |
| src/main/fc/core.c | Ensures GPS_RESCUE_MODE activation remains switch-only when ENABLE_RESCUE_PLAN is enabled. |
| src/main/cli/cli.c | Updates CLI pattern vocabulary to include NONE and asserts array sizing. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
There was a problem hiding this comment.
Actionable comments posted: 1
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@src/main/pg/flight_plan.h`:
- Around line 50-53: Preserve the persisted numeric values of waypoint_t.pattern
in PG_FLIGHT_PLAN_CONFIG by keeping the existing enum assignments stable, or
bump the flight-plan PG version and add migration logic that converts saved
pattern values before using them. Update the WAYPOINT_PATTERN_NONE,
WAYPOINT_PATTERN_ORBIT, WAYPOINT_PATTERN_FIGURE8, and WAYPOINT_PATTERN_COUNT
definitions accordingly.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Path: .coderabbit.yaml
Review profile: CHILL
Plan: Pro
Run ID: 71dcf477-6c4b-4759-ac82-345d17bb8a83
📒 Files selected for processing (26)
src/main/cli/cli.csrc/main/fc/core.csrc/main/flight/failsafe.csrc/main/flight/failsafe.hsrc/main/flight/flight_plan_nav.csrc/main/flight/flight_plan_nav.hsrc/main/flight/gps_rescue_multirotor.csrc/main/flight/gps_rescue_multirotor.hsrc/main/flight/position_nav.csrc/main/flight/position_nav.hsrc/main/io/gps.csrc/main/io/gps_virtual.csrc/main/io/gps_virtual.hsrc/main/osd/osd_warnings.csrc/main/pg/flight_plan.hsrc/main/sensors/compass.csrc/main/target/common_post.hsrc/main/telemetry/mavlink_mission.csrc/platform/SIMULATOR/sitl.csrc/platform/SIMULATOR/target/SITL/target.hsrc/test/Makefilesrc/test/sitl/sitl_harness.pysrc/test/unit/flight_failsafe_rescue_unittest.ccsrc/test/unit/flight_plan_nav_unittest.ccsrc/test/unit/flight_plan_rescue_unittest.ccsrc/test/unit/position_nav_unittest.cc
…G version setVirtualGPS() populates the struct before release-storing the freshness counter, and getVirtualGPSUpdateCount() acquire-loads it, so the SITL feeder thread and the GPS task no longer race on partially-written data. Bump PG_FLIGHT_PLAN_CONFIG to 1 so the WAYPOINT_PATTERN_NONE=0 renumber resets stored plans instead of reinterpreting persisted pattern values.
Next tranche of the autopilot programme, four commits.
ENABLE_RESCUE_PLAN(default 0, byte-identical when off): staged plan of climb hold, flyover home, land;FAILSAFE_AUTOPILOTphase with a short grace; aborts remapped to auto-landing; heading recovery gated by the pitch-forward override. Switch rescue keeps the legacy path. A/B SITL parity covers heading recovery from 90 degrees of initial error and GPS-loss degradation.ap_waypoint_hold_radius; the pattern starts once arrival braking settles and rotation is capped at 0.25 rad/s.WAYPOINT_PATTERN_NONE = 0keeps stored missions station-keeping. MAVLinkLOITER_TURNSuploads as an ORBIT hold (turns convert to a duration via the orbit period) and round-trips on download; the upload decoder's pattern default is fixed to NONE.Part of the autopilot programme tracked in #15411.
Summary by CodeRabbit
New Features
NONEwaypoint pattern option, including updates to CLI and MAVLink mission encoding/decoding.Bug Fixes
Tests