Reset height and position data indoors from last known location by andyp1per · Pull Request #31177 · ArduPilot/ardupilot

andyp1per · 2025-09-28T15:21:32Z

Summary

This PR enables reliable indoor flight using non-GPS position sources (optical flow, external navigation/motion capture, etc.) by refactoring how flight modes declare their position requirements and adding persistent origin storage.

Problem

Currently, many Copter flight modes declare requires_GPS() which causes issues when flying indoors with alternative position sources like optical flow or external navigation systems. The EKF can provide valid position estimates from these sources, but the mode requirements and arming checks still demand GPS specifically. This can lead to:

Inability to arm or enter position-controlled modes when position is valid but GPS is unavailable
Incorrect failsafe triggers when the position source is working correctly
No way to restore the EKF origin after a reboot when flying indoors without GPS

Solution

1. Mode Requirement Refactoring: `requires_GPS()` → `requires_position()`

All Copter flight modes now declare whether they need position rather than specifically GPS. The AHRS/EKF is then consulted to determine if the position requirement can be satisfied by the current sensor suite (GPS, optical flow, external nav, etc.).

2. GPS Usage Detection

New using_gps() methods added to AP_AHRS, NavEKF2, and NavEKF3 allow the system to distinguish between:

Modes that need position (satisfied by any valid position source)
Checks that specifically require GPS (fence, supersimple mode, etc.)

3. Persistent Origin Storage

New AHRS parameters for saving/restoring the EKF origin:

AHRS_ORIGIN_LAT - Last known origin latitude
AHRS_ORIGIN_LNG - Last known origin longitude
AHRS_ORIGIN_ALT - Last known origin altitude

New AHRS_OPTIONS bit (3): AutoRecordOrigin - Automatically saves the origin to parameters when it becomes valid.

4. Origin Restoration on Startup/Arming

New ARMING_OPTIONS bits allow the origin to be restored from saved parameters:

Bit for setting origin from AHRS params at startup (1Hz check when disarmed)
Bit for setting origin from AHRS params on arming (last-ditch attempt)

This allows indoor flights to resume with a valid origin after power cycles.

5. Updated Arming Checks

Arming checks now properly handle position-only scenarios:

If a mode requires position and position is valid, GPS-specific checks are relaxed
If position is not valid or GPS is actually being used, full GPS checks apply
Origin availability is checked when modes require position

Affected Vehicles

Copter: Primary target - all flight modes updated
Rover: Arming checks updated for consistency
Blimp: Arming checks updated for consistency

Testing

Autotest updates included for optical flow scenarios to validate the new behavior.

Usage

For indoor flight with optical flow or external nav:

Fly outdoors first to establish a valid origin (or set AHRS_ORIGIN_* parameters manually)
Enable AHRS_OPTIONS bit 3 to auto-record the origin
Enable appropriate ARMING_OPTIONS bit for origin restoration
On subsequent indoor flights, the origin will be restored from parameters

This allows position-controlled modes (Loiter, Auto, etc.) to work indoors without GPS when a valid position estimate is available from alternative sources.

libraries/AP_AHRS/AP_AHRS.cpp

peterbarker · 2025-09-29T05:27:31Z

... also, we probably don't need extra parameters for this storage, we persist home already.

andyp1per · 2025-09-29T08:37:33Z

... also, we probably don't need extra parameters for this storage, we persist home already.

Where is that?

peterbarker · 2025-10-31T06:38:55Z

... also, we probably don't need extra parameters for this storage, we persist home already.

Where is that?

mission-slot-0.

Yeah this is not user friendly and in discussion with others we don't want this associated with a mission. I think what is in the PR is still the best way of doing this.

andyp1per · 2025-11-10T14:08:26Z

I have been using this a lot and it is great!

peterbarker

I think I quite like this as a concept now. Yes, I've changed my mind on the parameters :-)

ArduCopter/AP_Arming_Copter.cpp

libraries/AP_AHRS/AP_AHRS.cpp

peterbarker

Spams

LOITER> Received 1366 parameters
Saved 1344 parameters to mav.parm
AP: Field Elevation Set: 0m
AP: Field Elevation Set: 0m
AP: Field Elevation Set: 0m

LOITER> AP: Field Elevation Set: 0m
AP: EKF3 IMU0 is using GPS

LOITER> AP: EKF3 IMU1 is using GPS

LOITER> aAP: Field Elevation Set: 0m

libraries/AP_AHRS/AP_AHRS.cpp

andyp1per · 2025-11-12T08:52:43Z

Spams

LOITER> Received 1366 parameters
Saved 1344 parameters to mav.parm
AP: Field Elevation Set: 0m
AP: Field Elevation Set: 0m
AP: Field Elevation Set: 0m

LOITER> AP: Field Elevation Set: 0m
AP: EKF3 IMU0 is using GPS

LOITER> AP: EKF3 IMU1 is using GPS

LOITER> aAP: Field Elevation Set: 0m

Ah-ha! So that's where the spam is coming from! Okay, need to figure that out because it was annoying me as well.

andyp1per · 2025-11-12T14:02:02Z

Spam is pre-existing

rmackay9 · 2025-11-16T05:38:59Z

ArduCopter/mode.h

    void run() override;

-    bool requires_GPS() const override { return true; }
+    bool requires_GPS() const override { return false; }


Thanks for this, it would definitely be nice if users didn't need to turn off arming checks when using optical flow (or other non-GPS based position estimation).

I think "requires_GPS" and "requires_position()" should always be the same. The "requires_GPS" is just a hang-over from when GPS was the only viable option. If we agree on this then let's just rename, "requires_GPS" to "requires_position".

Andy and I have discussed this elsewhere.

I was pushing for requires_location and requires_position, using the same terms we use elsewhere - Location as in absolute lat/lon object and "position" as in offsets-from-origin.

requires_absolute_location and requires_position_relative_to_origin could also work :-)

... or requires_latitude_longitude_altitude?

I am unconvinced about this. requires_GPS() causes GPS checks to be run - they shouldn't be run if the vehicle only requires position and the EKF is ok. Or for instance follow mode requires GPS because position (in the way we mean here) is not good enough. So I don't think there can only be a single function, but I am not fussed about what the two functions are called. FWIW I prefer the current naming - absolute position is pretty much only provided by the GPS.

rmackay9 · 2025-11-16T05:47:58Z

ArduCopter/AP_Arming_Copter.cpp

    fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0;
 #endif

+    // check if we only require position and the EKF thinks we are ok


I think we should instead rename "gps_checks" to "position_checks" or maybe just add add a comment to explain that they're synonymous. Then within this function we should put the position checks at the top and GPS checks at the bottom. Just above the GPS checks we should ask the AHRS if it's using GPS as its position, velocity or yaw source and if "no" we can skip the GPS checks. The AHRS doesn't currently have an accessor to ask which sensor it is using but we have similar functions like, "using_extnav_for_yaw" so around the same area we could have add a "using_gps_for_position_or_velocity". I'm not sure immediately how we should implement this function but perhaps DCM would always return "true" and perhaps EKF3 could re-use the logic in AP_NavEKF_Source's pre-arm check

We should not care how the EKF is getting the data it is using to supply us with stuff. Well, as much as possible, anyway. Asking the EKF how it is getting the data is a layering violation and unnecessary AFAICS (and @andyp1per 's PR here doesn't require it!) Examples of where doing that will stuff you up - beacons which are rooted at a lat/lon, external AHRS of any sort and visual odometry input to the EKF supply lat/lng/alt (e.g. from a google-maps-image-mapping external source).

Hi @peterbarker, unfortunately we do care about the EKF's sources if we only want to check those sources that the EKF is currently using. So whether that's the GPS or visual odometry or whatever, if we only want to check that the currently used source is healthy, then we need to know what the source is.

Hi @peterbarker, unfortunately we do care about the EKF's sources if we only want to check those sources that the EKF is currently using. So whether that's the GPS or visual odometry or whatever, if we only want to check that the currently used source is healthy, then we need to know what the source is.

I can see that for some use cases.

But I don't think it's the case for optical flow; we don't (or shouldn't, thus this PR) actually need GPS to be healthy in Copter to arm and fly in loiter if you have good optical flow.

@rmackay9 this does check the sources via the EKF. If you try this indoors with sources requiring GPS you will get an error "EKF sources require GPS". So I think it behaves in the way you want, its just that the function is a little subtle.

@andyp1per,

And if the user is flying indoors and the EKF pos/vel source is not GPS it doesn't raise the GPS pre-arm failures?

@rmackay9 correct, the code as written only gives a GPS pre-arm check if the sources require the GPS and the mode only requires "position" and not "GPS". This is another reason why we need two functions. Note that I think we have to delegate a bunch of this to the EKF - the EKF check is in some ways stricter and in other ways looser as it not only looks at what has been configured but also whether its good enough as a source for whatever.

andyp1per · 2025-12-07T20:03:55Z

@rmackay9 this is clean now and I have updated the tests to appropriately check that we can't arm without GPS where necessary

andyp1per · 2026-01-29T16:27:56Z

@rmackay9 @peterbarker and I have reached agreement, branch updated to reflect

Tridge's request to add an option bit has been added

rmackay9

I've tested this fairly well in SITL to confirm it works as expected. Below are some screen shots of the testing

Tested that the origin is recorded and used when EKF sources do not include GPS

Tested that the AHRS_OPTIONS bit for "RecordOrigin" works.

Tested that the AHRS_OPTIONS bit for "UseRecordedOrigin" works

Tested that the AHRS_OPTIONS bit for "UseRecordedOrigin" has no effect if EK3_SRC1_POSXY or EK3_SRC1_VELXY is set to GPS

Confirmed that the Sub parameter conversions from ORIGIN_LAT/LON/ALT to AHRS_ORIGIN_LAT/LON/ALT works

rmackay9 · 2026-02-02T23:30:45Z

We discussed on the dev call and agreed that once @Williangalvani has had a chance to test and approve we will merge (assuming that WillianG is happy of course)

Co-authored-by: Randy Mackay <rmackay9@yahoo.com> option bit 3 controls whether the EKF origin is recorded to parameters option bit 4 controls whether a recorded origin is used to initialise the EKF when using non-GPS sources (e.g. sources without an absolute position)

Co-authored-by: Andy Piper <github@andypiper.com>

This gives more time for the AHRS update to consume the origin and output the global-position-int mavlink message

andyp1per · 2026-02-05T18:37:14Z

@Williangalvani ready to go when you approve from the sub side

Williangalvani

I am happy with the Sub changes.
Also just tested on real hw.

andyp1per · 2026-02-05T20:32:41Z

Thanks @Williangalvani and @rmackay9 !

andyp1per added the WIP label Sep 28, 2025

rmackay9 reviewed Sep 29, 2025

View reviewed changes

libraries/AP_AHRS/AP_AHRS.cpp Outdated Show resolved Hide resolved

peterbarker reviewed Sep 29, 2025

View reviewed changes

libraries/AP_AHRS/AP_AHRS.cpp Outdated Show resolved Hide resolved

libraries/AP_AHRS/AP_AHRS.cpp Outdated Show resolved Hide resolved

libraries/AP_AHRS/AP_AHRS.cpp Outdated Show resolved Hide resolved

libraries/AP_AHRS/AP_AHRS.cpp Outdated Show resolved Hide resolved

andyp1per force-pushed the pr-height-baro-reset-rng branch from ff82426 to 726d213 Compare September 30, 2025 11:09

andyp1per marked this pull request as ready for review November 10, 2025 14:08

peterbarker requested changes Nov 10, 2025

View reviewed changes

andyp1per force-pushed the pr-height-baro-reset-rng branch 2 times, most recently from b4bf319 to f020dbf Compare November 11, 2025 19:08

andyp1per requested a review from peterbarker November 11, 2025 19:08

peterbarker requested changes Nov 11, 2025

View reviewed changes

libraries/AP_AHRS/AP_AHRS.cpp Outdated Show resolved Hide resolved

libraries/AP_AHRS/AP_AHRS.cpp Show resolved Hide resolved

andyp1per requested a review from peterbarker November 12, 2025 14:00

andyp1per removed the WIP label Nov 12, 2025

rmackay9 reviewed Nov 16, 2025

View reviewed changes

andyp1per force-pushed the pr-height-baro-reset-rng branch from 7427e7d to 8c4fd7a Compare November 26, 2025 19:24

andyp1per force-pushed the pr-height-baro-reset-rng branch 3 times, most recently from ef73e05 to bc8b9f9 Compare December 7, 2025 20:03

andyp1per requested a review from rmackay9 December 7, 2025 20:03

andyp1per added Enhancement DevCallEU labels Dec 7, 2025

rmackay9 added the WikiNeeded needs wiki update label Dec 10, 2025

peterbarker removed the DevCallEU label Dec 10, 2025

rmackay9 requested a review from Williangalvani January 30, 2026 10:34

rmackay9 added Copter Sub labels Jan 30, 2026

andyp1per force-pushed the pr-height-baro-reset-rng branch from 8ad6da0 to 182166c Compare January 30, 2026 13:03

rmackay9 added the DevCallTopic label Jan 31, 2026

rmackay9 approved these changes Jan 31, 2026

View reviewed changes

CraigElder removed the DevCallTopic label Feb 2, 2026

peterbarker removed the DevCallEU label Feb 4, 2026

andyp1per and others added 13 commits February 4, 2026 08:24

AP_NavEKF2: add accessor for EKF2 using gps

1672c01

AP_NavEKF3: add accessor for EKF using GPS

a5b4775

AP_AHRS: add using_gps accessor

8155ed5

AP_AHRS: origin reported via mavlink when set

e506718

Tools: add AHRSOriginRecorded test

2e3054e

Copter: rename requires_GPS to requires_position

41fd65a

Co-authored-by: Andy Piper <github@andypiper.com>

Copter: separate GPS and position arming checks

85fb351

AP_AHRS: sub uses recorded origin by default

fee0be1

Sub: use AHRS_ORIGIN in place of local ORIGIN params

1eff027

Tools: Sub.BackupOrigin autotest uses AHRS_ORIGIN params

73a40be

Tools: copter and sub autotests delay 2 sec after setting origin

d0fa5db

This gives more time for the AHRS update to consume the origin and output the global-position-int mavlink message

autotest: fix spelling

784a1a1

andyp1per force-pushed the pr-height-baro-reset-rng branch from 182166c to 784a1a1 Compare February 4, 2026 07:25

Williangalvani approved these changes Feb 5, 2026

View reviewed changes

rmackay9 merged commit 2a26d3c into ArduPilot:master Feb 5, 2026
107 checks passed

andyp1per deleted the pr-height-baro-reset-rng branch February 5, 2026 20:32

Conversation

andyp1per commented Sep 28, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

Problem

Solution

1. Mode Requirement Refactoring: requires_GPS() → requires_position()

2. GPS Usage Detection

3. Persistent Origin Storage

4. Origin Restoration on Startup/Arming

5. Updated Arming Checks

Affected Vehicles

Testing

Usage

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

peterbarker commented Sep 29, 2025

Uh oh!

andyp1per commented Sep 29, 2025

Uh oh!

peterbarker commented Oct 31, 2025 • edited by andyp1per Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

andyp1per commented Nov 10, 2025

Uh oh!

peterbarker left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

peterbarker left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

andyp1per commented Nov 12, 2025

Uh oh!

andyp1per commented Nov 12, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

rmackay9 Nov 17, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

andyp1per Nov 26, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

andyp1per commented Dec 7, 2025

Uh oh!

andyp1per commented Jan 29, 2026

Uh oh!

rmackay9 left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

rmackay9 commented Feb 2, 2026

Uh oh!

andyp1per commented Sep 28, 2025 •

edited

Loading

1. Mode Requirement Refactoring: `requires_GPS()` → `requires_position()`

peterbarker commented Oct 31, 2025 •

edited by andyp1per

Loading

rmackay9 Nov 17, 2025 •

edited

Loading

andyp1per Nov 26, 2025 •

edited

Loading

rmackay9 left a comment •

edited

Loading