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.

Sign up for GitHub

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

Jump to bottom

Add hysteresis to max height above ground failsafe logic #24132

Closed
wants to merge 1 commit into from
Closed

Add hysteresis to max height above ground failsafe logic #24132

wants to merge 1 commit into from

Conversation

RomanBapst
Copy link
Contributor

Solved Problem

There was a race condition which happened when transitioning with a VTOL from hover to forward flight, and when we only rely on an optical flow sensor for velocity aiding during the hover phase.
In this case the estimator was not able to initialize airspeed aiding, before the logic for the maximum height above the ground triggered.

Fixes #{Github issue ID}

Solution

Add a hysteresis of 1 second in order to avoid the above described race condition and to avoid short false positives in general.

Changelog Entry

For release notes:

Bugfix: Fix race condition, which could cause a max height above ground failsafe to be triggered right after a VTOL front transition.

@RomanBapst
navigator: set 1 second hysteresis time before triggering navigator_f…
ba6f3f1 
…ail_max_hagl

- this is needed to avoid a race condition with the estimator in cases where
we transition from hover to forward flight and expect airspeed aiding to
activate after the transition.

Signed-off-by: RomanBapst <[email protected]>
@RomanBapst RomanBapst requested a review from bresch December 19, 2024 11:16
@RomanBapst RomanBapst changed the title Add hysteresis to max height above ground failsafe loginc Add hysteresis to max height above ground failsafe logic Dec 19, 2024
@github-actions GitHub Actions
Copy link

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 104 byte (0.01 %)] 
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +104  +0.0%    +104    .text
  +1.4%     +68  +1.4%     +68    ../../src/modules/navigator/mission_block.cpp
  +0.3%     +36  +0.3%     +36    ../../src/modules/navigator/navigator_main.cpp
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +20  [ = ]       0    .debug_frame
+0.0% +1.36Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/geofence.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/land.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/loiter.cpp
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/mission.cpp
  +0.0%     +10  [ = ]       0    ../../src/modules/navigator/mission_base.cpp
  +1.3%    +728  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/mission_feasibility_checker.cpp
  +0.4%    +540  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/navigator_mode.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/precland.cpp
  +0.0%     +10  [ = ]       0    ../../src/modules/navigator/rtl.cpp
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/rtl_direct.cpp
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/rtl_direct_mission_land.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast_reverse.cpp
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/takeoff.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/vtol_takeoff.cpp
+0.0%    +910  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.5%     +41  [ = ]       0    ../../src/modules/navigator/geofence.cpp
  +1.2%     +41  [ = ]       0    ../../src/modules/navigator/land.cpp
  +1.0%     +41  [ = ]       0    ../../src/modules/navigator/loiter.cpp
  +0.7%     +41  [ = ]       0    ../../src/modules/navigator/mission.cpp
  +0.2%     +25  [ = ]       0    ../../src/modules/navigator/mission_base.cpp
  +2.9%    +296  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  +1.1%     +41  [ = ]       0    ../../src/modules/navigator/mission_feasibility_checker.cpp
  +0.2%     +36  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  +1.5%     +41  [ = ]       0    ../../src/modules/navigator/navigator_mode.cpp
  +0.4%     +41  [ = ]       0    ../../src/modules/navigator/precland.cpp
  +0.4%     +41  [ = ]       0    ../../src/modules/navigator/rtl.cpp
  +0.4%     +41  [ = ]       0    ../../src/modules/navigator/rtl_direct.cpp
  +0.6%     +41  [ = ]       0    ../../src/modules/navigator/rtl_direct_mission_land.cpp
  +0.9%     +41  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast.cpp
  +0.8%     +41  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast_reverse.cpp
  +1.0%     +41  [ = ]       0    ../../src/modules/navigator/takeoff.cpp
  +0.9%     +41  [ = ]       0    ../../src/modules/navigator/vtol_takeoff.cpp
  +0.4%      +4  [ = ]       0    task/task_cancelpt.c
+0.0%    +384  [ = ]       0    .debug_loc
  +1.7%    +138  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  +1.0%    +244  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  +0.2%     +15  [ = ]       0    ../../src/modules/navigator/rtl_direct.cpp
  -0.5%     -15  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast.cpp
  +0.0%      +2  [ = ]       0    [section .debug_loc]
-0.0%     -17  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.5%     -16  [ = ]       0    ../../src/modules/navigator/mission_base.cpp
  +1.2%     +24  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  -0.2%     -16  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  -1.5%      -1  [ = ]       0    task/task_cancelpt.c
+0.0%    +133  [ = ]       0    .debug_str
  -0.3%      -9  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
  +0.4%      +9  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  +0.2%    +133  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
-0.6%    -104  [ = ]       0    [Unmapped]
+0.0% +2.80Ki  +0.0%    +104    TOTAL
px4_fmu-v6x [Total VM Diff: 120 byte (0.01 %)] 
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +120  +0.0%    +120    .text
  +1.4%     +68  +1.4%     +68    ../../src/modules/navigator/mission_block.cpp
  +0.3%     +36  +0.3%     +36    ../../src/modules/navigator/navigator_main.cpp
  +0.0%     +13  +0.0%     +13    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +20  [ = ]       0    .debug_frame
+0.0% +1.36Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/geofence.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/land.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/loiter.cpp
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/mission.cpp
  +0.0%     +10  [ = ]       0    ../../src/modules/navigator/mission_base.cpp
  +1.3%    +728  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/mission_feasibility_checker.cpp
  +0.4%    +540  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/navigator_mode.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/precland.cpp
  +0.0%     +10  [ = ]       0    ../../src/modules/navigator/rtl.cpp
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/rtl_direct.cpp
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/rtl_direct_mission_land.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast_reverse.cpp
  +0.0%      +9  [ = ]       0    ../../src/modules/navigator/takeoff.cpp
  +0.0%      +8  [ = ]       0    ../../src/modules/navigator/vtol_takeoff.cpp
+0.0%    +910  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.5%     +41  [ = ]       0    ../../src/modules/navigator/geofence.cpp
  +1.2%     +41  [ = ]       0    ../../src/modules/navigator/land.cpp
  +1.0%     +41  [ = ]       0    ../../src/modules/navigator/loiter.cpp
  +0.7%     +41  [ = ]       0    ../../src/modules/navigator/mission.cpp
  +0.2%     +25  [ = ]       0    ../../src/modules/navigator/mission_base.cpp
  +2.9%    +296  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  +1.1%     +41  [ = ]       0    ../../src/modules/navigator/mission_feasibility_checker.cpp
  +0.2%     +36  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  +1.5%     +41  [ = ]       0    ../../src/modules/navigator/navigator_mode.cpp
  +0.4%     +41  [ = ]       0    ../../src/modules/navigator/precland.cpp
  +0.4%     +41  [ = ]       0    ../../src/modules/navigator/rtl.cpp
  +0.4%     +41  [ = ]       0    ../../src/modules/navigator/rtl_direct.cpp
  +0.6%     +41  [ = ]       0    ../../src/modules/navigator/rtl_direct_mission_land.cpp
  +0.9%     +41  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast.cpp
  +0.8%     +41  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast_reverse.cpp
  +1.0%     +41  [ = ]       0    ../../src/modules/navigator/takeoff.cpp
  +0.9%     +41  [ = ]       0    ../../src/modules/navigator/vtol_takeoff.cpp
  +0.4%      +4  [ = ]       0    task/task_cancelpt.c
+0.0%    +369  [ = ]       0    .debug_loc
  +1.7%    +138  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  +0.9%    +229  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  -0.1%     -15  [ = ]       0    ../../src/modules/navigator/rtl.cpp
  +0.2%     +15  [ = ]       0    ../../src/modules/navigator/rtl_direct.cpp
  +0.0%      +2  [ = ]       0    [section .debug_loc]
-0.0%     -18  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.5%     -16  [ = ]       0    ../../src/modules/navigator/mission_base.cpp
  +1.2%     +24  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  -0.2%     -16  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  -3.0%      -2  [ = ]       0    task/task_cancelpt.c
+0.0%    +133  [ = ]       0    .debug_str
  -0.3%      -9  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
  +0.4%      +9  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  +0.2%    +133  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
-0.2%    -120  [ = ]       0    [Unmapped]
+0.0% +2.79Ki  +0.0%    +120    TOTAL

Updated: 2024-12-19T11:21:40

@bresch bresch marked this pull request as draft December 19, 2024 13:59
@bresch
Copy link
Member

bresch commented Dec 19, 2024

The estimator only starts fusing airspeed while in fixedwing and since the vehicle is considered a multirotor during front transition, there will always be that gap. I would suggest that we instead allow fusing airspeed in all modes (but keeping the minimum airspeed threshold)

@RomanBapst RomanBapst closed this Dec 20, 2024
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@bresch bresch Awaiting requested review from bresch

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@RomanBapst @bresch