dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

rtl_direct: Rtl estimate only needs valid destination, not home position

This commit is contained in:
Konrad 2024-03-08 17:22:44 +01:00 committed by Silvan Fuhrer
parent 91d1342f20
commit e9fda548fa
2 changed files with 41 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
src/modules/navigator/rtl_direct.cpp
View File

@ -121,7 +121,6 @@ void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s
// Only allow to set a new approach if the mode is not activated yet. // Only allow to set a new approach if the mode is not activated yet.
if (!isActive()) { if (!isActive()) {
_land_approach = loiter_pos;
_destination = rtl_position; _destination = rtl_position;
_force_heading = false; _force_heading = false;
@ -139,24 +138,12 @@ void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s
_destination.alt = _home_pos_sub.get().alt; _destination.alt = _home_pos_sub.get().alt;
} }
if (!PX4_ISFINITE(_land_approach.lat) || !PX4_ISFINITE(_land_approach.lon)) { _land_approach = sanitizeLandApproach(loiter_pos);
_land_approach.lat = _destination.lat;
_land_approach.lon = _destination.lon;
} else { const float dist_to_destination{get_distance_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon)};
const float dist_to_destination{get_distance_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon)};
if (dist_to_destination > _navigator->get_acceptance_radius()) { if (dist_to_destination > _navigator->get_acceptance_radius()) {
_force_heading = true; _force_heading = true;
}
}
if (!PX4_ISFINITE(_land_approach.height_m)) {
_land_approach.height_m = _destination.alt + _param_rtl_descend_alt.get();
}
if (!PX4_ISFINITE(_land_approach.loiter_radius_m) || fabsf(_land_approach.loiter_radius_m) <= FLT_EPSILON) {
_land_approach.loiter_radius_m = _param_rtl_loiter_rad.get();
} }
} }
} }
@ -383,11 +370,13 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
start_state_for_estimate = getActivationLandState(); start_state_for_estimate = getActivationLandState();
} }
// Calculate RTL time estimate only when there is a valid home position // Calculate RTL time estimate only when there is a valid destination
// TODO: Also check if vehicle position is valid // TODO: Also check if vehicle position is valid
if (_navigator->home_global_position_valid()) { if (PX4_ISFINITE(_destination.lat) && PX4_ISFINITE(_destination.lon) && PX4_ISFINITE(_destination.alt)) {
const float loiter_altitude = min(_land_approach.height_m, _rtl_alt); loiter_point_s land_approach = sanitizeLandApproach(_land_approach);
const float loiter_altitude = min(land_approach.height_m, _rtl_alt);
// Sum up time estimate for various segments of the landing procedure // Sum up time estimate for various segments of the landing procedure
switch (start_state_for_estimate) { switch (start_state_for_estimate) {
@ -401,10 +390,10 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
// FALLTHROUGH // FALLTHROUGH
case RTLState::MOVE_TO_LOITER: { case RTLState::MOVE_TO_LOITER: {
matrix::Vector2f direction{}; matrix::Vector2f direction{};
get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _land_approach.lat, get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat,
_land_approach.lon, &direction(0), &direction(1)); land_approach.lon, &direction(0), &direction(1));
_rtl_time_estimator.addDistance(get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _rtl_time_estimator.addDistance(get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
_land_approach.lat, _land_approach.lon), direction, 0.f); land_approach.lat, land_approach.lon), direction, 0.f);
} }
// FALLTHROUGH // FALLTHROUGH
@ -450,8 +439,8 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
} else { } else {
move_to_land_dist = get_distance_to_next_waypoint( move_to_land_dist = get_distance_to_next_waypoint(
_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon); land_approach.lat, land_approach.lon, _destination.lat, _destination.lon);
get_vector_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon, &direction(0), get_vector_to_next_waypoint(land_approach.lat, land_approach.lon, _destination.lat, _destination.lon, &direction(0),
&direction(1)); &direction(1));
} }
@ -500,3 +489,24 @@ void RtlDirect::parameters_update()
updateParams(); updateParams();
} }
} }
loiter_point_s RtlDirect::sanitizeLandApproach(loiter_point_s land_approach) const
{
loiter_point_s sanitized_land_approach{land_approach};
if (!PX4_ISFINITE(land_approach.lat) || !PX4_ISFINITE(land_approach.lon)) {
sanitized_land_approach.lat = _destination.lat;
sanitized_land_approach.lon = _destination.lon;
}
if (!PX4_ISFINITE(land_approach.height_m)) {
sanitized_land_approach.height_m = _destination.alt + _param_rtl_descend_alt.get();
}
if (!PX4_ISFINITE(land_approach.loiter_radius_m) || fabsf(land_approach.loiter_radius_m) <= FLT_EPSILON) {
sanitized_land_approach.loiter_radius_m = _param_rtl_loiter_rad.get();
}
return sanitized_land_approach;
}

6
src/modules/navigator/rtl_direct.h
View File

@ -126,6 +126,12 @@ private:
*/ */
void set_rtl_item(); void set_rtl_item();
/**
* @brief sanitize land_approach
*
*/
loiter_point_s sanitizeLandApproach(loiter_point_s land_approach) const;
/** /**
* Check for parameter changes and update them if needed. * Check for parameter changes and update them if needed.
*/ */