Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 32 Packages Projects Releases Wiki Activity

rtl_direct: reversed changes in rtl time estimate regarding geofence

Browse Source 
avoidance for now, needs more thought

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2026-04-27 07:01:23 +03:00
parent 970d52f502
commit e69e4432ec
1 changed files with 3 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl_direct.cpp
+3 -14
View File
@@ -492,25 +492,14 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
} }
// FALLTHROUGH // FALLTHROUGH
case RTLState::MOVE_TO_INTERMEDIATE_POINT: { case RTLState::MOVE_TO_INTERMEDIATE_POINT:
matrix::Vector2f direction{};
matrix::Vector2d intermediate_point = _geofence_aware_return_path.getCurrentPoint();
get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, intermediate_point(0),
intermediate_point(1), &direction(0), &direction(1));
float move_to_land_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, intermediate_point(0), intermediate_point(1))};
_rtl_time_estimator.addDistance(move_to_land_dist, direction, 0.f);
}
// FALLTHROUGH // FALLTHROUGH
case RTLState::MOVE_TO_LOITER: { case RTLState::MOVE_TO_LOITER: {
matrix::Vector2f direction{}; matrix::Vector2f direction{};
matrix::Vector2d intermediate_point = _geofence_aware_return_path.getCurrentPoint(); get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat,
const matrix::Vector2d start_position = intermediate_point.isAllFinite() ? matrix::Vector2d(intermediate_point(0), intermediate_point(1))
: matrix::Vector2d(_global_pos_sub.get().lat, _global_pos_sub.get().lon);
get_vector_to_next_waypoint(start_position(0), start_position(1), land_approach.lat,
land_approach.lon, &direction(0), &direction(1)); land_approach.lon, &direction(0), &direction(1));
float move_to_land_dist{get_distance_to_next_waypoint(start_position(0), start_position(1), land_approach.lat, land_approach.lon)}; float move_to_land_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon)};
if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
move_to_land_dist = max(0.f, move_to_land_dist - land_approach.loiter_radius_m); move_to_land_dist = max(0.f, move_to_land_dist - land_approach.loiter_radius_m);