fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 11:18:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

RTL: only do calculations in is-inactive if global position is recent (#21208)

Browse Source 
* RTL: only do calculations in is-inactive if global position is recent

* RTL: refactor calcRtlTimeEstimate to only calc and not pub


Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-03-02 11:09:32 +01:00
committed by GitHub
parent 458c351585
commit 0c4b288973
2 changed files with 116 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+115 -102
View File
@@ -80,11 +80,23 @@ void RTL::on_inactive()
if ((hrt_absolute_time() - _destination_check_time) > 1_s) {
_destination_check_time = hrt_absolute_time();
if (_navigator->home_global_position_valid()) {
const vehicle_global_position_s &global_position = *_navigator->get_global_position();
const bool global_position_recently_updated = global_position.timestamp > 0
&& hrt_elapsed_time(&global_position.timestamp) < 10_s;
rtl_time_estimate_s rtl_time_estimate{};
rtl_time_estimate.valid = false;
// Calculate RTL destination and time estimate only when there is a valid home and global position
if (_navigator->home_global_position_valid() && global_position_recently_updated) {
find_RTL_destination();
calcRtlTimeEstimate(RTLState::RTL_STATE_NONE, rtl_time_estimate);
rtl_time_estimate.valid = true;
}
calc_and_pub_rtl_time_estimate(RTLState::RTL_STATE_NONE);
rtl_time_estimate.timestamp = hrt_absolute_time();
_rtl_time_estimate_pub.publish(rtl_time_estimate);
}
}
@@ -312,7 +324,24 @@ void RTL::on_active()
// Limit rtl time calculation to 1Hz
if ((hrt_absolute_time() - _destination_check_time) > 1_s) {
_destination_check_time = hrt_absolute_time();
calc_and_pub_rtl_time_estimate(_rtl_state);
const vehicle_global_position_s &global_position = *_navigator->get_global_position();
const bool global_position_recently_updated = global_position.timestamp > 0
&& hrt_elapsed_time(&global_position.timestamp) < 10_s;
rtl_time_estimate_s rtl_time_estimate{};
rtl_time_estimate.valid = false;
// Calculate RTL destination and time estimate only when there is a valid home and global position
if (_navigator->home_global_position_valid() && global_position_recently_updated) {
find_RTL_destination();
calcRtlTimeEstimate(_rtl_state, rtl_time_estimate);
rtl_time_estimate.valid = true;
}
rtl_time_estimate.timestamp = hrt_absolute_time();
_rtl_time_estimate_pub.publish(rtl_time_estimate);
}
}
@@ -709,114 +738,98 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
return max(return_altitude_amsl, gpos.alt);
}
void RTL::calc_and_pub_rtl_time_estimate(const RTLState rtl_state)
void RTL::calcRtlTimeEstimate(const RTLState rtl_state, rtl_time_estimate_s &rtl_time_estimate)
{
rtl_time_estimate_s rtl_time_estimate{};
const vehicle_global_position_s &gpos = *_navigator->get_global_position();
// Calculate RTL time estimate only when there is a valid home position
// TODO: Also check if vehicle position is valid
if (!_navigator->home_global_position_valid()) {
rtl_time_estimate.valid = false;
// Sum up time estimate for various segments of the landing procedure
switch (rtl_state) {
case RTL_STATE_NONE:
case RTL_STATE_CLIMB: {
// Climb segment is only relevant if the drone is below return altitude
const float climb_dist = gpos.alt < _rtl_alt ? (_rtl_alt - gpos.alt) : 0;
} else {
rtl_time_estimate.valid = true;
const vehicle_global_position_s &gpos = *_navigator->get_global_position();
// Sum up time estimate for various segments of the landing procedure
switch (rtl_state) {
case RTL_STATE_NONE:
case RTL_STATE_CLIMB: {
// Climb segment is only relevant if the drone is below return altitude
const float climb_dist = gpos.alt < _rtl_alt ? (_rtl_alt - gpos.alt) : 0;
if (climb_dist > 0) {
rtl_time_estimate.time_estimate += climb_dist / getClimbRate();
}
if (climb_dist > FLT_EPSILON) {
rtl_time_estimate.time_estimate += climb_dist / getClimbRate();
}
// FALLTHROUGH
case RTL_STATE_RETURN:
// Add cruise segment to home
rtl_time_estimate.time_estimate += get_distance_to_next_waypoint(
_destination.lat, _destination.lon, gpos.lat, gpos.lon) / getCruiseGroundSpeed();
// FALLTHROUGH
case RTL_STATE_HEAD_TO_CENTER:
case RTL_STATE_TRANSITION_TO_MC:
case RTL_STATE_DESCEND: {
// when descending, the target altitude is stored in the current mission item
float initial_altitude = 0;
float loiter_altitude = 0;
if (rtl_state == RTL_STATE_DESCEND) {
// Take current vehicle altitude as the starting point for calculation
initial_altitude = gpos.alt; // TODO: Check if this is in the right frame
loiter_altitude = _mission_item.altitude; // Next waypoint = loiter
} else {
// Take the return altitude as the starting point for the calculation
initial_altitude = _rtl_alt; // CLIMB and RETURN
loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
}
// Add descend segment (first landing phase: return alt to loiter alt)
rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate();
}
// FALLTHROUGH
case RTL_STATE_LOITER:
// Add land delay (the short pause for deploying landing gear)
// TODO: Check if landing gear is deployed or not
rtl_time_estimate.time_estimate += _param_rtl_land_delay.get();
// FALLTHROUGH
case RTL_MOVE_TO_LAND_HOVER_VTOL:
case RTL_STATE_LAND: {
float initial_altitude;
// Add land segment (second landing phase) which comes after LOITER
if (rtl_state == RTL_STATE_LAND) {
// If we are in this phase, use the current vehicle altitude instead
// of the altitude paramteter to get a continous time estimate
initial_altitude = gpos.alt;
} else {
// If this phase is not active yet, simply use the loiter altitude,
// which is where the LAND phase will start
const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
initial_altitude = loiter_altitude;
}
// Prevent negative times when close to the ground
if (initial_altitude > _destination.alt) {
rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed();
}
}
break;
case RTL_STATE_LANDED:
// Remaining time is 0
break;
}
// Prevent negative durations as phyiscally they make no sense. These can
// occur during the last phase of landing when close to the ground.
rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate);
// FALLTHROUGH
case RTL_STATE_RETURN:
// Use actual time estimate to compute the safer time estimate with additional scale factor and a margin
rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate
+ _param_rtl_time_margin.get();
// Add cruise segment to home
rtl_time_estimate.time_estimate += get_distance_to_next_waypoint(
_destination.lat, _destination.lon, gpos.lat, gpos.lon) / getCruiseGroundSpeed();
// FALLTHROUGH
case RTL_STATE_HEAD_TO_CENTER:
case RTL_STATE_TRANSITION_TO_MC:
case RTL_STATE_DESCEND: {
// when descending, the target altitude is stored in the current mission item
float initial_altitude = 0.f;
float loiter_altitude = 0.f;
if (rtl_state == RTL_STATE_DESCEND) {
// Take current vehicle altitude as the starting point for calculation
initial_altitude = gpos.alt; // TODO: Check if this is in the right frame
loiter_altitude = _mission_item.altitude; // Next waypoint = loiter
} else {
// Take the return altitude as the starting point for the calculation
initial_altitude = _rtl_alt; // CLIMB and RETURN
loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
}
// Add descend segment (first landing phase: return alt to loiter alt)
rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate();
}
// FALLTHROUGH
case RTL_STATE_LOITER:
// Add land delay (the short pause for deploying landing gear)
// TODO: Check if landing gear is deployed or not
rtl_time_estimate.time_estimate += _param_rtl_land_delay.get();
// FALLTHROUGH
case RTL_MOVE_TO_LAND_HOVER_VTOL:
case RTL_STATE_LAND: {
float initial_altitude;
// Add land segment (second landing phase) which comes after LOITER
if (rtl_state == RTL_STATE_LAND) {
// If we are in this phase, use the current vehicle altitude instead
// of the altitude paramteter to get a continous time estimate
initial_altitude = gpos.alt;
} else {
// If this phase is not active yet, simply use the loiter altitude,
// which is where the LAND phase will start
const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
initial_altitude = loiter_altitude;
}
// Prevent negative times when close to the ground
if (initial_altitude > _destination.alt) {
rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed();
}
}
break;
case RTL_STATE_LANDED:
// Remaining time is 0
break;
}
// Publish message
rtl_time_estimate.timestamp = hrt_absolute_time();
_rtl_time_estimate_pub.publish(rtl_time_estimate);
// Prevent negative durations as phyiscally they make no sense. These can
// occur during the last phase of landing when close to the ground.
rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate);
// Use actual time estimate to compute the safer time estimate with additional scale factor and a margin
rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate
+ _param_rtl_time_margin.get();
}
float RTL::getCruiseSpeed()
src/modules/navigator/rtl.h
+1 -1
View File
@@ -121,7 +121,7 @@ private:
void advance_rtl();
float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
void calc_and_pub_rtl_time_estimate(const RTLState rtl_state);
void calcRtlTimeEstimate(const RTLState rtl_state, rtl_time_estimate_s &rtl_time_estimate);
float getCruiseGroundSpeed();