rtl: reset rtl state only on activation

rtl state was getting reset on inactive, which meant that the state which triggered resuming e.g. mission landing would be overwritten, and the navigator mode would switch back and forth between rtl and mission. this commit:
1. moves the reset of rtl state to the on activation function (removing it from the on inactive function)
2. functionalizes the rtl state input to the rtl time estimator so that rtl time can still be calculated from state=none while inactive
This commit is contained in:
Thomas Stastny
2022-07-22 12:20:57 +02:00
committed by Silvan Fuhrer
parent e512d77b89
commit 6a0f394d46
2 changed files with 9 additions and 10 deletions
+8 -9
View File
@@ -76,9 +76,6 @@ void RTL::on_inactivation()
void RTL::on_inactive()
{
// Reset RTL state.
_rtl_state = RTL_STATE_NONE;
// Limit inactive calculation to 1Hz
if ((hrt_absolute_time() - _destination_check_time) > 1_s) {
_destination_check_time = hrt_absolute_time();
@@ -87,7 +84,7 @@ void RTL::on_inactive()
find_RTL_destination();
}
calc_and_pub_rtl_time_estimate();
calc_and_pub_rtl_time_estimate(RTLState::RTL_STATE_NONE);
}
}
@@ -241,6 +238,8 @@ void RTL::find_RTL_destination()
void RTL::on_activation()
{
_rtl_state = RTL_STATE_NONE;
// if a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode
// In multirotor mode no landing pattern is required so we can just navigate to the land point directly and don't need to run mission
_should_engange_mission_for_landing = (_destination.type == RTL_DESTINATION_MISSION_LANDING)
@@ -314,7 +313,7 @@ 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();
calc_and_pub_rtl_time_estimate(_rtl_state);
}
}
@@ -711,7 +710,7 @@ 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()
void RTL::calc_and_pub_rtl_time_estimate(const RTLState rtl_state)
{
rtl_time_estimate_s rtl_time_estimate{};
@@ -726,7 +725,7 @@ void RTL::calc_and_pub_rtl_time_estimate()
const vehicle_global_position_s &gpos = *_navigator->get_global_position();
// Sum up time estimate for various segments of the landing procedure
switch (_rtl_state) {
switch (rtl_state) {
case RTL_STATE_NONE:
case RTL_STATE_CLIMB: {
// Climb segment is only relevant if the drone is below return altitude
@@ -752,7 +751,7 @@ void RTL::calc_and_pub_rtl_time_estimate()
float initial_altitude = 0;
float loiter_altitude = 0;
if (_rtl_state == RTL_STATE_DESCEND) {
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
@@ -780,7 +779,7 @@ void RTL::calc_and_pub_rtl_time_estimate()
float initial_altitude;
// Add land segment (second landing phase) which comes after LOITER
if (_rtl_state == RTL_STATE_LAND) {
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;
+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();
void calc_and_pub_rtl_time_estimate(const RTLState rtl_state);
float getCruiseGroundSpeed();