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 1 commit 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: 464 byte (0.02 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +464  +0.0%    +464    .text
  +186%    +164  +186%    +164    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  +0.4%     +92  +0.4%     +92    ../../src/modules/commander/Commander.cpp
  +1.8%     +88  +1.8%     +88    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +0.1%     +48  +0.1%     +48    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +0.0%     +24  +0.0%     +24    [section .text]
  +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.1%      +4  +0.1%      +4    ../../src/modules/commander/accelerometer_calibration.cpp
  -0.2%      -4  -0.2%      -4    ../../src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp
+0.0%     +73  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +2.8%     +17  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0% +2.71Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +40  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/Common.cpp
  +0.0%     +23  [ = ]       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.1%     +56  [ = ]       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.09Ki  [ = ]       0    [31 Others]
+0.0%    +776  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.1%     +28  [ = ]       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%     +87  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +0.1%      +1  [ = ]       0    task/task_cancelpt.c
+0.0% +1.10Ki  [ = ]       0    .debug_loc
  -0.4%    -174  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +0.5%     +98  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +0.0%      +1  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.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
  +0.0%     +15  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
  -0.0%     -13  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
  +0.2%     +15  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
  +0.0%     +27  [ = ]       0    [section .debug_loc]
  +0.0%     +15  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
+0.0%    +432  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +3.0%     +40  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +294%    +400  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
+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
-4.2%    -464  [ = ]       0    [Unmapped]
+0.0% +5.19Ki  +0.0%    +464    TOTAL

px4_fmu-v6x [Total VM Diff: 480 byte (0.02 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +480  +0.0%    +480    .text
  +186%    +164  +186%    +164    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
  +0.4%     +92  +0.4%     +92    ../../src/modules/commander/Commander.cpp
  +1.8%     +88  +1.8%     +88    ../../src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
  +0.1%     +48  +0.1%     +48    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  +0.0%     +37  +0.0%     +37    [section .text]
  +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.1%      +4  +0.1%      +4    ../../src/modules/commander/accelerometer_calibration.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%     +73  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +2.8%     +17  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.cpp
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0% +2.71Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +39  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/Common.cpp
  +0.0%     +23  [ = ]       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.1%     +56  [ = ]       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.09Ki  [ = ]       0    [31 Others]
+0.0%    +769  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.1%     +28  [ = ]       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%     +87  [ = ]       0    ../../src/modules/uxrce_dds_client/uxrce_dds_client.cpp
  -0.5%      -6  [ = ]       0    task/task_cancelpt.c
+0.0% +1.04Ki  [ = ]       0    .debug_loc
  -0.4%    -189  [ = ]       0    ../../src/modules/commander/Commander.cpp
  -0.2%     -15  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.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
  -0.2%     -15  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
  -0.0%     -13  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
  +0.0%     +27  [ = ]       0    [section .debug_loc]
  +0.0%     +30  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
+0.0%    +429  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +3.0%     +40  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +294%    +400  [ = ]       0    ../../src/modules/commander/ModeUtil/mode_requirements.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.7%    -480  [ = ]       0    [Unmapped]
+0.0% +5.12Ki  +0.0%    +480    TOTAL

Updated: 2025-02-13T07:58:00

@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

- allow fixed wing vehicles to indefinitely navigate if they are at least
wind dead-reckoning

Signed-off-by: RomanBapst <[email protected]>
@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.

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.

2 participants