Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Introduce global position relaxed #24280

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
Open

Conversation

RomanBapst
Copy link
Contributor

Solved Problem

Fixed wing vehicles with an airspeed sensor are able to constrain position drift in GNSS denied environments. This use case usually requires the flight controller to accept low position accuracy reported by the estimator (high eph).
The parameter COM_POS_FS_EPH defines a threshold for the accuracy above which a failsafe is triggered (e.g. descend mode). In the above described use case, this parameter is usually set to Infinity, in order to allow unlimited operation. Usually the operator handles the position drift by manually resetting the vehicle position to a known location, based on visual cues from the camera.
This however has undesired side effects for VTOL vehicles equipped with a flow sensor when they are coming back for landing.
As soon as the flow sensor gets into range (mostly during the descend phase after the backtransition), global position is considered valid again (because there is no uncertainty limit) and a mode like RTL, which requires global position, is resumed (assuming the vehicle came back in RTL mode). In practice however, this leads to the vehicle flying to an inaccurate land location and gives the user the impression that it's "flying away". It would be much more desirable for the vehicle to just land at the location after the backtransition, relying on the user to correct the error using the nudging feature.

It all boils down to the idea of having separate accuracy requirements for navigation modes for multicopter and fixed wing.

Solution

Add the concept of a relaxed global position validity. This refers to global position being valid when the associated uncertainty is below a threshold defined by the parameter COM_POS_R_FS_EPH.

Changelog Entry

For release notes:

Feature/Bugfix XYZ
New parameter: XYZ_Z
Documentation: Need to clarify page ... / done, read docs.px4.io/...

Alternatives

We could also ...

Test coverage

Context

Related links, screenshot before/after, video

@RomanBapst RomanBapst requested a review from sfuhrer January 31, 2025 06:49
Copy link

github-actions bot commented Jan 31, 2025

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 448 byte (0.02 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +448  +0.0%    +448    .text
  +186%    +164  +186%    +164    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  +0.3%     +96  +0.3%     +96    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +1.8%     +88  +1.8%     +88    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +0.0%     +37  +0.0%     +37    [section .text]
  +0.1%     +16  +0.1%     +16    ../../src/modules/commander/Commander.cpp
  +1.1%     +16  +1.1%     +16    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +0.6%     +16  +0.6%     +16    ../../src/modules/commander/failsafe/framework.cpp
  +1.5%     +12  +1.5%     +12    ../../src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
  +0.1%      +4  +0.1%      +4    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
  -0.2%      -4  -0.2%      -4    ../../src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp
+0.0%     +91  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +2.8%     +17  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  +0.4%     +18  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0% +2.83Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +53  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/Common.cpp
  +0.0%     +39  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/armPermissionCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/baroCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
  +0.2%    +134  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +0.2%     +72  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.cpp
 -99.9% +2.16Ki  [ = ]       0    [31 Others]
+0.0%    +731  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.1%     -18  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +0.1%     +12  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +1.5%    +170  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +0.6%     +25  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +1.4%     +58  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
   +40%    +407  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  +0.2%     +13  [ = ]       0    ../../src/modules/commander/failsafe/framework.cpp
  +0.2%     +90  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  -0.1%      -1  [ = ]       0    task/task_cancelpt.c
+0.0% +1.68Ki  [ = ]       0    .debug_loc
  +1.3%    +546  [ = ]       0    ../../src/modules/commander/Commander.cpp
  -0.4%     -15  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp
  -0.6%     -15  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/baroCheck.cpp
  -0.2%     -15  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp
  +0.5%     +98  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  -0.2%     -15  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp
  +2.9%     +77  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
  -0.3%     -15  [ = ]       0    ../../src/modules/commander/HomePosition.cpp
  +0.4%      +2  [ = ]       0    ../../src/modules/commander/ModeUtil/control_mode.cpp
  +373% +1.04Ki  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  -1.0%     -15  [ = ]       0    ../../src/modules/events/status_display.cpp
  -0.0%     -16  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
  +0.2%     +50  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  -0.0%     -17  [ = ]       0    [section .debug_loc]
+0.0%    +377  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.7%     -72  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +3.0%     +40  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +294%    +400  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  +0.1%     +16  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +1.5%      +1  [ = ]       0    task/task_cancelpt.c
+0.0%    +138  [ = ]       0    .debug_str
  +0.1%     +65  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +1.5%     +51  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
   +11%     +22  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
-0.0%     -12  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  -0.4%     -12  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
-2.4%    -448  [ = ]       0    [Unmapped]
+0.0% +5.79Ki  +0.0%    +448    TOTAL

px4_fmu-v6x [Total VM Diff: 432 byte (0.02 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +432  +0.0%    +432    .text
  +186%    +164  +186%    +164    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  +0.3%     +96  +0.3%     +96    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +1.8%     +88  +1.8%     +88    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +0.0%     +24  +0.0%     +24    [section .text]
  +0.1%     +16  +0.1%     +16    ../../src/modules/commander/Commander.cpp
  +1.1%     +16  +1.1%     +16    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +0.6%     +16  +0.6%     +16    ../../src/modules/commander/failsafe/framework.cpp
  +1.5%     +12  +1.5%     +12    ../../src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
  +0.1%      +4  +0.1%      +4    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  -0.2%      -4  -0.2%      -4    ../../src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp
+0.0%     +91  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +2.8%     +17  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  +0.4%     +18  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0% +2.83Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +54  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/Common.cpp
  +0.0%     +39  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/armPermissionCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/baroCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
  +0.2%    +134  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +0.2%     +72  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.cpp
 -99.9% +2.16Ki  [ = ]       0    [31 Others]
+0.0%    +738  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.1%     -18  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +0.1%     +12  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +1.5%    +170  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +0.6%     +25  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +1.4%     +58  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
   +40%    +407  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  +0.2%     +13  [ = ]       0    ../../src/modules/commander/failsafe/framework.cpp
  +0.2%     +90  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +0.5%      +6  [ = ]       0    task/task_cancelpt.c
+0.0% +1.77Ki  [ = ]       0    .debug_loc
  +1.3%    +561  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +0.5%     +98  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +2.9%     +77  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
  +0.4%      +2  [ = ]       0    ../../src/modules/commander/ModeUtil/control_mode.cpp
  +373% +1.04Ki  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  -1.0%     -15  [ = ]       0    ../../src/modules/events/status_display.cpp
  -0.2%     -15  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
  +0.0%     +16  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
  +0.2%     +50  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  -0.0%     -17  [ = ]       0    [section .debug_loc]
  -0.0%     -15  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
+0.0%    +373  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.7%     -72  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +3.0%     +40  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +294%    +400  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  +0.1%     +16  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  -4.5%      -3  [ = ]       0    task/task_cancelpt.c
+0.0%    +138  [ = ]       0    .debug_str
  +0.1%     +65  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +1.5%     +51  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
   +11%     +22  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
-0.0%     -12  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  -0.4%     -12  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
-0.6%    -432  [ = ]       0    [Unmapped]
+0.0% +5.89Ki  +0.0%    +432    TOTAL

Updated: 2025-02-21T17:10:01

@dagar dagar self-requested a review February 1, 2025 02:23
@dagar
Copy link
Member

dagar commented Feb 1, 2025

I'm definitely in favor of handling these differences contextually, it's long overdue.

The parameter naming isn't great (COM_POS_R_FS_EPH), I'm wondering if we should capture this at some level by vehicle type. Alternatively could it simply be a multiple of COM_POS_FS_EPH? There's already the magic 2.5x relaxation.

My initial reaction after a first pass is that if we're

@RomanBapst RomanBapst force-pushed the pr-global_pos_relaxed branch from d664ee5 to e55c4ab Compare February 13, 2025 07:52
@RomanBapst
Copy link
Contributor Author

@dagar I removed the parameter now and set the accuracy threshold for global position relaxed to infinity. This enables fixed wing vehicles to navigate indefinitely if they are at least wind dead-reckoning (otherwise global position is not valid).
This allows GPS denied navigation, however, the user still has an option to set the low accuracy threshold via COM_POS_FS_EPH and define the corresponding action.

@RomanBapst RomanBapst force-pushed the pr-global_pos_relaxed branch from e55c4ab to eea9764 Compare February 21, 2025 11:43
@sfuhrer sfuhrer force-pushed the pr-global_pos_relaxed branch from 328d871 to 3b0c38d Compare February 21, 2025 17:03
@sfuhrer
Copy link
Contributor

sfuhrer commented Feb 21, 2025

I've rebased on main and hopefully fix the CI ROS translation failure with 3b0c38d

@sfuhrer sfuhrer force-pushed the pr-global_pos_relaxed branch from 3b0c38d to 9e51571 Compare February 25, 2025 09:17
@hamishwillee
Copy link
Contributor

Docs update here? BTW, are we at the point where we should add a page on "Configuration/Flying for GPS denied environments?"

@sfuhrer sfuhrer force-pushed the pr-global_pos_relaxed branch 2 times, most recently from 01216f1 to 0d6409d Compare February 26, 2025 08:36
@RomanBapst RomanBapst force-pushed the pr-global_pos_relaxed branch from 0d6409d to d2f1025 Compare February 27, 2025 05:13
@RomanBapst
Copy link
Contributor Author

@hamishwillee Would this section be the right place to add the docs?

Currently the docs says the following:

The Position Loss Failsafe is triggered if the quality of the PX4 global position estimate falls below acceptable levels (this might be caused by GPS loss) while in a mode that requires an acceptable position estimate.

Maybe it's time to add a dedicated section which shows the requirements for each mode and what the differences between the vehicle types are?

The mode requirements are actually defined here, so we could potentially even auto-generate a table.

@hamishwillee
Copy link
Contributor

hamishwillee commented Feb 27, 2025

@hamishwillee Would this section be the right place to add the docs?

Currently the docs says the following:

The Position Loss Failsafe is triggered if the quality of the PX4 global position estimate falls below acceptable levels (this might be caused by GPS loss) while in a mode that requires an acceptable position estimate.

That would be a great starting point.

Other places include

Maybe it's time to add a dedicated section which shows the requirements for each mode and what the differences between the vehicle types are?

So at an entry point level, we're partially down this path with things like the boxes at the top of (some) modes, such as FW Land mode. This is where the information is most useful - as a reader of FW docs I don't actually care much what MC Land might have as its requirements.

image

Ideally we could generate that automatically!

If we can't do it per-page, a link to a page for "up to date information" or similar might help.

Note the requirements are mentioned in https://docs.px4.io/main/en/concept/flight_modes.html#mode-restrictions but it is not user friendly or focused to this particular need.

I'll have a bit more of a think, because we definitely need to do this a bit better. But in the meantime, updates would be appreciated.

RomanBapst and others added 5 commits March 4, 2025 15:07
- allow fixed wing vehicles to indefinitely navigate if they are at least
wind dead-reckoning

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Signed-off-by: RomanBapst <bapstroman@gmail.com>
Signed-off-by: Silvan <silvan@auterion.com>
Signed-off-by: Silvan <silvan@auterion.com>
Signed-off-by: Silvan <silvan@auterion.com>
@RomanBapst RomanBapst force-pushed the pr-global_pos_relaxed branch from d2f1025 to 0765f7d Compare March 4, 2025 12:07
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

4 participants