fix(navigator): rtl compute wind angle to select best land approach based on rally point location instead of home location by JonasPerolini · Pull Request #27004 · PX4/PX4-Autopilot

JonasPerolini · 2026-04-08T10:33:38Z

As a first step for: #26973 (fyi @mrpollo).

VTOL rally-point land approaches are defined around the rally-point land location, but RTL was choosing between them using the bearing from home. That works only when the landing point is effectively home. For remote safe points it can select the wrong loiter circle even when the available approach data is correct.

Behavior changes:

compute the wind-alignment bearing from the rally-point land location instead of from home

const float wind_angle = wrap_pi(get_bearing_to_next_waypoint(_home_pos_sub.get().lat,
							 _home_pos_sub.get().lon, vtol_land_approaches.approaches[i].lat,
							 vtol_land_approaches.approaches[i].lon) - wind_direction);

becomes:

const float wind_angle = wrap_pi(get_bearing_to_next_waypoint(vtol_land_approaches.land_location_lat_lon(0),
				 vtol_land_approaches.land_location_lat_lon(1), vtol_land_approaches.approaches[i].lat,
				 vtol_land_approaches.approaches[i].lon) - wind_direction);

validate the associated rally point before exposing attached approaches
stop collecting approaches once land_approaches_s::num_approaches_max is reached

github-actions · 2026-04-09T05:57:14Z

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 528 byte (0.03 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +528  +0.0%    +528    .text
    [NEW]    +248  [NEW]    +248    RTL::extractValidSafePointPosition()
    [NEW]    +224  [NEW]    +224    RTL::findAssociatedSafePointIndex()
    [NEW]    +192  [NEW]    +192    RTL::scanVtolLandApproachBlock()
    [NEW]    +132  [NEW]    +132    RTL::getVtolLandApproachesNearLocation()
    [NEW]    +100  [NEW]    +100    RTL::selectLandingApproach()
    [NEW]    +100  [NEW]    +100    land_approaches_s::land_approaches_s()
    [NEW]     +62  [NEW]     +62    RTL::hasVtolLandApproachesNearLocation()
     +18%     +44   +18%     +44    RTL::chooseBestLandingApproach()
    [NEW]     +40  [NEW]     +40    RTL::makeVtolLandApproachPoint()
    [NEW]     +34  [NEW]     +34    RTL::loadSafePointItemWait()
    +7.1%     +24  +7.1%     +24    RTL::findClosestSafePoint()
   -97.5%     +18 -97.5%     +18    [6 Others]
    +1.6%      +8  +1.6%      +8    RTL::findRtlDestination()
    [NEW]      +6  [NEW]      +6    RTL::hasVtolLandApproachesAtSafePointIndex()
    -0.0%     -24  -0.0%     -24    [section .text]
    [DEL]     -24  [DEL]     -24    land_approaches_s::isAnyApproachValid()
    [DEL]     -40  [DEL]     -40    RTL::hasVtolLandApproach()
   -10.1%     -44 -10.1%     -44    RTL::setRtlTypeAndDestination()
    -0.0%     -44  -0.0%     -44    g_cromfs_image
    [DEL]    -168  [DEL]    -168    RTL::setSafepointAsDestination()
    [DEL]    -360  [DEL]    -360    RTL::readVtolLandApproaches()
  +0.0%    +196  [ = ]       0    .debug_abbrev
  +0.0%     +48  [ = ]       0    .debug_aranges
  +0.1%    +260  [ = ]       0    .debug_frame
  +0.0% +3.36Ki  [ = ]       0    .debug_info
  +0.0%    +889  [ = ]       0    .debug_line
    [DEL]      -3  [ = ]       0    [Unmapped]
    +0.0%    +892  [ = ]       0    [section .debug_line]
  +0.0% +1.20Ki  [ = ]       0    .debug_loclists
  +0.0%    +119  [ = ]       0    .debug_rnglists
  +0.0%    +805  [ = ]       0    .debug_str
  +0.1%    +400  [ = ]       0    .strtab
    +1.8%      +1  [ = ]       0    RTL::chooseBestLandingApproach()
    [NEW]     +82  [ = ]       0    RTL::extractValidSafePointPosition()
    [NEW]     +83  [ = ]       0    RTL::findAssociatedSafePointIndex()
    [NEW]     +69  [ = ]       0    RTL::getVtolLandApproachesNearLocation()
    [DEL]     -54  [ = ]       0    RTL::hasVtolLandApproach()
    [NEW]     +51  [ = ]       0    RTL::hasVtolLandApproachesAtSafePointIndex()
    [NEW]     +69  [ = ]       0    RTL::hasVtolLandApproachesNearLocation()
    [NEW]     +52  [ = ]       0    RTL::loadSafePointItemWait()
    [NEW]     +56  [ = ]       0    RTL::makeVtolLandApproachPoint()
    [DEL]     -55  [ = ]       0    RTL::readVtolLandApproaches()
    [NEW]     +59  [ = ]       0    RTL::scanVtolLandApproachBlock()
    [NEW]     +56  [ = ]       0    RTL::selectLandingApproach()
    [DEL]     -77  [ = ]       0    RTL::setSafepointAsDestination()
   -40.0%     -16  [ = ]       0    __stm32_dmastart_veneer
    +100%     +16  [ = ]       0    __strcmp_veneer
    [DEL]     -46  [ = ]       0    land_approaches_s::isAnyApproachValid()
    [NEW]     +54  [ = ]       0    land_approaches_s::land_approaches_s()
  +0.0%    +256  [ = ]       0    .symtab
    [NEW]    +112  [ = ]       0    RTL::extractValidSafePointPosition()
    [NEW]     +48  [ = ]       0    RTL::findAssociatedSafePointIndex()
    [NEW]     +32  [ = ]       0    RTL::getVtolLandApproachesNearLocation()
    [DEL]     -32  [ = ]       0    RTL::hasVtolLandApproach()
    [NEW]     +48  [ = ]       0    RTL::hasVtolLandApproachesAtSafePointIndex()
    [NEW]     +16  [ = ]       0    RTL::hasVtolLandApproachesNearLocation()
     +50%     +16  [ = ]       0    RTL::isLanding()
    [NEW]     +32  [ = ]       0    RTL::loadSafePointItemWait()
    [NEW]     +32  [ = ]       0    RTL::makeVtolLandApproachPoint()
    [DEL]     -48  [ = ]       0    RTL::readVtolLandApproaches()
    [NEW]     +48  [ = ]       0    RTL::scanVtolLandApproachBlock()
    [NEW]     +48  [ = ]       0    RTL::selectLandingApproach()
   -50.0%     -16  [ = ]       0    RTL::setLandPosAsDestination()
    [DEL]    -112  [ = ]       0    RTL::setSafepointAsDestination()
     +33%     +16  [ = ]       0    __arp_ipin_veneer
   -40.0%     -32  [ = ]       0    __stm32_dmastart_veneer
   -25.0%     -16  [ = ]       0    __stm32_i2c_set_bytes_to_transfer_veneer
     +67%     +32  [ = ]       0    __strcmp_veneer
    [DEL]     -32  [ = ]       0    land_approaches_s::isAnyApproachValid()
    [NEW]     +64  [ = ]       0    land_approaches_s::land_approaches_s()
  -5.2%    -528  [ = ]       0    [Unmapped]
  +0.0% +7.47Ki  +0.0%    +528    TOTAL

px4_fmu-v6x [Total VM Diff: 520 byte (0.03 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +520  +0.0%    +520    .text
    [NEW]    +248  [NEW]    +248    RTL::extractValidSafePointPosition()
    [NEW]    +224  [NEW]    +224    RTL::findAssociatedSafePointIndex()
    [NEW]    +192  [NEW]    +192    RTL::scanVtolLandApproachBlock()
    [NEW]    +132  [NEW]    +132    RTL::getVtolLandApproachesNearLocation()
    [NEW]    +100  [NEW]    +100    RTL::selectLandingApproach()
    [NEW]    +100  [NEW]    +100    land_approaches_s::land_approaches_s()
    [NEW]     +62  [NEW]     +62    RTL::hasVtolLandApproachesNearLocation()
     +18%     +44   +18%     +44    RTL::chooseBestLandingApproach()
    [NEW]     +40  [NEW]     +40    RTL::makeVtolLandApproachPoint()
    [NEW]     +34  [NEW]     +34    RTL::loadSafePointItemWait()
    +7.1%     +24  +7.1%     +24    RTL::findClosestSafePoint()
    +1.6%      +8  +1.6%      +8    RTL::findRtlDestination()
    [NEW]      +6  [NEW]      +6    RTL::hasVtolLandApproachesAtSafePointIndex()
   -98.9%      +6 -98.9%      +6    [5 Others]
    -0.0%     -24  -0.0%     -24    [section .text]
    [DEL]     -24  [DEL]     -24    land_approaches_s::isAnyApproachValid()
    [DEL]     -40  [DEL]     -40    RTL::hasVtolLandApproach()
    -0.0%     -40  -0.0%     -40    g_cromfs_image
   -10.1%     -44 -10.1%     -44    RTL::setRtlTypeAndDestination()
    [DEL]    -168  [DEL]    -168    RTL::setSafepointAsDestination()
    [DEL]    -360  [DEL]    -360    RTL::readVtolLandApproaches()
  +0.0%    +196  [ = ]       0    .debug_abbrev
  +0.0%     +48  [ = ]       0    .debug_aranges
  +0.1%    +260  [ = ]       0    .debug_frame
  +0.0% +3.36Ki  [ = ]       0    .debug_info
  +0.0%    +889  [ = ]       0    .debug_line
   -60.0%      -3  [ = ]       0    [Unmapped]
    +0.0%    +892  [ = ]       0    [section .debug_line]
  +0.0% +1.20Ki  [ = ]       0    .debug_loclists
  +0.0%    +119  [ = ]       0    .debug_rnglists
  +0.0%    +805  [ = ]       0    .debug_str
  +0.1%    +400  [ = ]       0    .strtab
    +1.8%      +1  [ = ]       0    RTL::chooseBestLandingApproach()
    [NEW]     +82  [ = ]       0    RTL::extractValidSafePointPosition()
    [NEW]     +83  [ = ]       0    RTL::findAssociatedSafePointIndex()
    [NEW]     +69  [ = ]       0    RTL::getVtolLandApproachesNearLocation()
    [DEL]     -54  [ = ]       0    RTL::hasVtolLandApproach()
    [NEW]     +51  [ = ]       0    RTL::hasVtolLandApproachesAtSafePointIndex()
    [NEW]     +69  [ = ]       0    RTL::hasVtolLandApproachesNearLocation()
    [NEW]     +52  [ = ]       0    RTL::loadSafePointItemWait()
    [NEW]     +56  [ = ]       0    RTL::makeVtolLandApproachPoint()
    [DEL]     -55  [ = ]       0    RTL::readVtolLandApproaches()
    [NEW]     +59  [ = ]       0    RTL::scanVtolLandApproachBlock()
    [NEW]     +56  [ = ]       0    RTL::selectLandingApproach()
    [DEL]     -77  [ = ]       0    RTL::setSafepointAsDestination()
    [DEL]     -46  [ = ]       0    land_approaches_s::isAnyApproachValid()
    [NEW]     +54  [ = ]       0    land_approaches_s::land_approaches_s()
  +0.0%    +256  [ = ]       0    .symtab
    [NEW]    +112  [ = ]       0    RTL::extractValidSafePointPosition()
    [NEW]     +48  [ = ]       0    RTL::findAssociatedSafePointIndex()
    [NEW]     +32  [ = ]       0    RTL::getVtolLandApproachesNearLocation()
    [DEL]     -32  [ = ]       0    RTL::hasVtolLandApproach()
    [NEW]     +48  [ = ]       0    RTL::hasVtolLandApproachesAtSafePointIndex()
    [NEW]     +16  [ = ]       0    RTL::hasVtolLandApproachesNearLocation()
     +50%     +16  [ = ]       0    RTL::isLanding()
    [NEW]     +32  [ = ]       0    RTL::loadSafePointItemWait()
    [NEW]     +32  [ = ]       0    RTL::makeVtolLandApproachPoint()
    [DEL]     -48  [ = ]       0    RTL::readVtolLandApproaches()
    [NEW]     +48  [ = ]       0    RTL::scanVtolLandApproachBlock()
    [NEW]     +48  [ = ]       0    RTL::selectLandingApproach()
   -50.0%     -16  [ = ]       0    RTL::setLandPosAsDestination()
    [DEL]    -112  [ = ]       0    RTL::setSafepointAsDestination()
    [DEL]     -32  [ = ]       0    land_approaches_s::isAnyApproachValid()
    [NEW]     +64  [ = ]       0    land_approaches_s::land_approaches_s()
  -8.2%    -520  [ = ]       0    [Unmapped]
  +0.0% +7.47Ki  +0.0%    +520    TOTAL

Updated: 2026-04-11T02:40:23

mrpollo · 2026-04-09T17:30:48Z

Unrelated to your PR, but I just wanted to drop a message to say our CI actions are now able to post comments on fork PRs!

src/modules/navigator/test/test_RTL.cpp

sfuhrer · 2026-04-10T10:16:11Z

Looks reasonable high level, I would like to get @mbjd 's review on it.

JonasPerolini · 2026-04-10T10:26:02Z

src/modules/navigator/rtl.cpp

-			if (_vehicle_status_sub.get().is_vtol
-			    && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)
-			    && rtl_land_approaches.isAnyApproachValid()) {
-				landing_loiter = chooseBestLandingApproach(rtl_land_approaches);


Note that this check is now inside of selectLandingApproach to simplify setRtlTypeAndDestination and to avoid checking all approaches if we know we won't use them.

JonasPerolini · 2026-04-10T10:32:34Z

src/modules/navigator/rtl.cpp

 				const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};

 				PositionYawSetpoint safepoint_position;
-				setSafepointAsDestination(safepoint_position, mission_safe_point);


Note that setSafepointAsDestination was replaced by extractValidSafePointPosition which returns false if the altitude frame is not valid or if the position is not valid. This change ensures we do not fly to an invalid approach. Especially important if we had other valid approaches.

src/modules/navigator/test/test_RTL.cpp

JonasPerolini · 2026-04-10T11:00:09Z

src/modules/navigator/rtl.cpp

 				PositionYawSetpoint safepoint_position;
-				setSafepointAsDestination(safepoint_position, mission_safe_point);

-				const bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)};


Note that instead of calling hasVtolLandApproach(safepoint_position) we now call hasVtolLandApproach(current_seq, _home_pos_sub.get().alt). The important change here is the current_seq.

The old code would go through all land approahces starting from index zero (which is not efficient) meaning that if two rally points (A, B) are close enough, we could have ended up with the location from rally point B but the approach from rally point A.

src/modules/navigator/test/test_RTL.cpp

JonasPerolini · 2026-04-10T11:51:01Z

src/modules/navigator/rtl.cpp

-		bool success_mission_item = _dataman_cache_safepoint.loadWait(static_cast<dm_item_t>(_stats.dataman_id), current_seq,
-					    reinterpret_cast<uint8_t *>(&mission_item),
-					    sizeof(mission_item_s));


Note that the loadWait call was updated to include the standard 500_ms timeout

src/modules/navigator/test/test_RTL.cpp

github-actions · 2026-04-10T17:27:24Z

src/modules/navigator/test/test_RTL.cpp

+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>


[error] clang-diagnostic-error [error]
gtest/gtest.h file not found

is this a false positive?

Yes @mrpollo, I added a comment here earlier #27019 it's due to

if(BUILD_TESTING) add_subdirectory(test) endif()

JonasPerolini · 2026-04-10T17:56:40Z

The PR is ready to be reviewed @mbjd (please note that the PR is not very large, 960 lines come from the unit tests)

Summary of the main changes:

Compute the wind-alignment bearing from the rally-point land location instead of from home
Fix corner case in which the land approach could be chosen from a different rally point when rally points are close by
Validate the associated rally point before exposing attached approaches
stop collecting approaches once land_approaches_s::num_approaches_max is reached
Refactor code with smaller helpers to reduce computations
Add a 500_ms timeout to loadWait calls to avoid dead locks
Add unit test coverage

Tested only using unit tests: make tests TESTFILTER=test_RTL

src/modules/navigator/test/test_RTL.cpp

…ePointPosition

…llFinite() fails for empty approaches

src/modules/navigator/test/test_RTL.cpp

Superseded by newer run

… push Branch protection rules block the GITHUB_TOKEN from dismissing reviews (HTTP 403), so every push added another undismissable REQUEST_CHANGES review. PR #27004 accumulated 12 identical blocking reviews. Switch to COMMENT-only reviews. Findings still show inline on the diff but don't create blocking reviews that require manual maintainer dismissal. The CI check status (pass/fail) gates merging, not the review state. Also enable CMAKE_TESTING=ON in the clang-tidy build so test files get proper include paths in compile_commands.json. Without this, clang-tidy-diff runs on test files from the PR diff but can't resolve gtest headers, producing false positives. Fixes #27004 Signed-off-by: Ramon Roche <mrpollo@gmail.com>

Signed-off-by: Ramon Roche <mrpollo@gmail.com>

mrpollo · 2026-04-11T02:35:03Z

Sorry didnt mean to close the PR, I was trying to prevent the damn bot from spamming you.

github-actions bot previously requested changes Apr 9, 2026

View reviewed changes

src/modules/navigator/test/test_RTL.cpp Show resolved Hide resolved

dakejahl requested review from ryanjAA and sfuhrer April 10, 2026 00:16

JonasPerolini force-pushed the pr-simplify-vtol-land-approaches branch from 438485e to 4f722af Compare April 10, 2026 06:27

github-actions bot previously requested changes Apr 10, 2026

View reviewed changes

src/modules/navigator/test/test_RTL.cpp Show resolved Hide resolved

github-actions bot previously requested changes Apr 10, 2026

View reviewed changes

src/modules/navigator/test/test_RTL.cpp Show resolved Hide resolved

github-actions bot previously requested changes Apr 10, 2026

View reviewed changes

src/modules/navigator/test/test_RTL.cpp Show resolved Hide resolved

github-actions bot previously requested changes Apr 10, 2026

View reviewed changes

src/modules/navigator/test/test_RTL.cpp Show resolved Hide resolved

JonasPerolini force-pushed the pr-simplify-vtol-land-approaches branch from 3ddec68 to 35dc7a0 Compare April 10, 2026 08:43

github-actions bot previously requested changes Apr 10, 2026

View reviewed changes

src/modules/navigator/test/test_RTL.cpp Show resolved Hide resolved

sfuhrer requested a review from mbjd April 10, 2026 10:16

JonasPerolini commented Apr 10, 2026

View reviewed changes