Sitelet https://github.com/betaflight/betaflight/pull/15413
Skip to content

Autopilot: rescue as a synthesised mission, TAKEOFF and hold patterns, SITL fidelity - #15413

Merged
blckmn merged 5 commits into
betaflight:masterfrom
blckmn:feat/flight-plan-rescue-and-waypoints
Jul 14, 2026
Merged

blckmn merged 5 commits into
betaflight:masterfrom
blckmn:feat/flight-plan-rescue-and-waypoints

Conversation

@blckmn

@blckmn blckmn commented Jul 13, 2026 •

Copy link
Copy Markdown
Member

Next tranche of the autopilot programme, four commits.

  • SITL fidelity: the harness now exercises the real attitude estimator with a truth-fed virtual mag, adds GPS-loss injection and a trajectory recorder, gains an A/B driver for flying the same scenario on two binaries, and can stream ground-truth JSON telemetry to an external visualiser.
  • Failsafe GPS rescue synthesised as a runtime mission behind ENABLE_RESCUE_PLAN (default 0, byte-identical when off): staged plan of climb hold, flyover home, land; FAILSAFE_AUTOPILOT phase 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.
  • Remaining waypoint types: TAKEOFF climbs in place to the waypoint altitude (lat/lon advisory, matching MAV_CMD_NAV_TAKEOFF) then loiters for its duration. HOLD gains ORBIT and FIGURE8 patterns, a time-parametrised carrot around the hold point at ap_waypoint_hold_radius; the pattern starts once arrival braking settles and rotation is capped at 0.25 rad/s. WAYPOINT_PATTERN_NONE = 0 keeps stored missions station-keeping. MAVLink LOITER_TURNS uploads 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

    • Added rescue-plan support for GPS Rescue (staged mission with climb/return/landing), plus heading-hold recovery and grace-window engagement behavior.
    • Added NONE waypoint pattern option, including updates to CLI and MAVLink mission encoding/decoding.
    • Enhanced navigation for TAKEOFF and HOLD/orbit/figure-eight patterns, with improved target updates.
    • Improved virtual sensor support (virtual compass, more robust virtual GPS freshness).
  • Bug Fixes

    • Improved failsafe rescue fallback/grace handling and added clearer “WP HEADING” warning behavior.
  • Tests

    • Expanded unit and simulator coverage for rescue plans, waypoint patterns, TAKEOFF, and target updates (including A/B rescue parity and optional telemetry output).

blckmn added 4 commits July 13, 2026 16:20
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.
Copilot AI review requested due to automatic review settings July 13, 2026 07:31
@github-actions

Copy link
Copy Markdown

Do you want to test this code? You can flash it directly from the Betaflight App:

  • Simply put #15413 (this pull request number) in the Select commit field in the Firmware Flasher tab (you need to Enable expert mode, Show release candidates and Development).

WARNING: It may be unstable. Use only for testing!

@coderabbitai

coderabbitai Bot commented Jul 13, 2026 •

Copy link
Copy Markdown
Contributor

Review Change Stack

No actionable comments were generated in the recent review. 🎉

ℹ️ Recent review info
⚙️ Run configuration

Configuration used: Path: .coderabbit.yaml

Review profile: CHILL

Plan: Pro

Run ID: 8d84469f-89ce-4e03-9878-0ee9a97ae8f0

📥 Commits

Reviewing files that changed from the base of the PR and between 020f1ff and f9b9ca2.

📒 Files selected for processing (2)
  • src/main/io/gps_virtual.c
  • src/main/pg/flight_plan.c
🚧 Files skipped from review as they are similar to previous changes (1)
  • src/main/io/gps_virtual.c

Walkthrough

GPS 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.

Changes

Rescue navigation and flight-plan execution

Layer / File(s) Summary
Navigation contracts and feature gates
src/main/target/common_post.h, src/main/pg/flight_plan.*, src/main/flight/flight_plan_nav.h, src/main/flight/position_nav.*, src/main/flight/gps_rescue_multirotor.*
Adds rescue-plan build constraints, the NONE waypoint pattern, heading abort reporting, orbit-period conversion, rescue-plan APIs, rescue altitude access, active-target movement, and parameter registration updates.
Rescue mission staging and failsafe control
src/main/flight/failsafe.*, src/main/flight/flight_plan_nav.c, src/main/fc/core.c
Stages HOLD→FLYOVER→LAND rescue missions, manages engagement grace periods and fallback behavior, handles heading recovery, and adjusts GPS rescue mode activation.
Waypoint targeting and hold-pattern execution
src/main/flight/flight_plan_nav.c, src/main/cli/cli.c, src/main/telemetry/mavlink_mission.c
Adds TAKEOFF targeting rules, moving HOLD-pattern carrots, orbit timing, CLI pattern names, and MAVLink orbit/time hold encoding and decoding.
GPS freshness and simulator sensor feeds
src/main/io/gps*, src/main/sensors/compass.c, src/platform/SIMULATOR/...
Tracks virtual GPS updates, supports virtual magnetometer detection, reconstructs SITL attitude, signals invalid GPS coordinates, and makes the estimator selectable.
Unit-test coverage and test builds
src/test/Makefile, src/test/unit/*
Adds rescue failsafe and flight-plan test targets and coverage for rescue staging, heading gates, landing, waypoint patterns, and target movement.
SITL telemetry and rescue parity scenarios
src/test/sitl/sitl_harness.py
Adds telemetry output, status polling, motion history, TAKEOFF/HOLD scenarios, paired rescue-binary comparisons, and expanded scenario orchestration.

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
Loading

Possibly related issues

Possibly related PRs

Suggested labels: RN: MAJOR FEATURE

Suggested reviewers: haslinghuis, ctzsnooze, karatebrot, sugaark, nerdcopter

🚥 Pre-merge checks | ✅ 4 | ❌ 1

❌ Failed checks (1 warning)

Check name Status Explanation Resolution
Docstring Coverage ⚠️ Warning Docstring coverage is 15.13% which is insufficient. The required threshold is 80.00%. Write docstrings for the functions missing them to satisfy the coverage threshold.
✅ Passed checks (4 passed)
Check name Status Explanation
Title check ✅ Passed The title clearly summarizes the main changes: synthetic rescue mission, new TAKEOFF/hold patterns, and SITL improvements.
Description check ✅ Passed The description is concise but covers the major feature areas and references the related issue.
Linked Issues check ✅ Passed Check skipped because no linked issues were found for this pull request.
Out of Scope Changes check ✅ Passed Check skipped because no linked issues were found for this pull request.
✨ Finishing Touches
🧪 Generate unit tests (beta)
  • Create PR with unit tests

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.

❤️ Share

Comment @coderabbitai help to get the list of available commands.

Copilot AI left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Comment thread src/main/io/gps_virtual.c Outdated
Comment thread src/main/io/gps_virtual.c

@coderabbitai coderabbitai Bot left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

📥 Commits

Reviewing files that changed from the base of the PR and between b9cfa49 and 020f1ff.

📒 Files selected for processing (26)
  • src/main/cli/cli.c
  • src/main/fc/core.c
  • src/main/flight/failsafe.c
  • src/main/flight/failsafe.h
  • src/main/flight/flight_plan_nav.c
  • src/main/flight/flight_plan_nav.h
  • src/main/flight/gps_rescue_multirotor.c
  • src/main/flight/gps_rescue_multirotor.h
  • src/main/flight/position_nav.c
  • src/main/flight/position_nav.h
  • src/main/io/gps.c
  • src/main/io/gps_virtual.c
  • src/main/io/gps_virtual.h
  • src/main/osd/osd_warnings.c
  • src/main/pg/flight_plan.h
  • src/main/sensors/compass.c
  • src/main/target/common_post.h
  • src/main/telemetry/mavlink_mission.c
  • src/platform/SIMULATOR/sitl.c
  • src/platform/SIMULATOR/target/SITL/target.h
  • src/test/Makefile
  • src/test/sitl/sitl_harness.py
  • src/test/unit/flight_failsafe_rescue_unittest.cc
  • src/test/unit/flight_plan_nav_unittest.cc
  • src/test/unit/flight_plan_rescue_unittest.cc
  • src/test/unit/position_nav_unittest.cc

Comment thread src/main/pg/flight_plan.h
…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.
@blckmn
blckmn merged commit ebce860 into betaflight:master Jul 14, 2026
53 checks passed
@blckmn
blckmn deleted the feat/flight-plan-rescue-and-waypoints branch July 14, 2026 11:05
@haslinghuis haslinghuis added this to the 2026.6 milestone Jul 14, 2026
@github-project-automation github-project-automation Bot moved this to Done in 2026.6.0 Jul 14, 2026
gwlim pushed a commit to gwlim/betaflight that referenced this pull request Jul 25, 2026
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Projects

Status: Done

Development

Successfully merging this pull request may close these issues.

3 participants