From 9301288d1ff27879924706d85b7c7f2734a343ed Mon Sep 17 00:00:00 2001 From: mcsauder Date: Mon, 30 Aug 2021 14:30:55 -0600 Subject: [PATCH] Add parameter and logic to set RTL heading mode. --- src/modules/navigator/rtl.cpp | 98 ++++++++++++++++++++++-------- src/modules/navigator/rtl.h | 34 +++++++---- src/modules/navigator/rtl_params.c | 14 ++++- 3 files changed, 106 insertions(+), 40 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 6ab3a98dce..ee4b8f7263 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -346,6 +346,8 @@ void RTL::set_rtl_item() const float descend_altitude_target = min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt); const float loiter_altitude = min(descend_altitude_target, _rtl_alt); + const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); + switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -362,7 +364,14 @@ void RTL::set_rtl_item() _mission_item.lon = gpos.lon; _mission_item.altitude = _rtl_alt; _mission_item.altitude_is_relative = false; - _mission_item.yaw = _navigator->get_local_position()->heading; + + if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _navigator->get_local_position()->heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; @@ -384,14 +393,17 @@ void RTL::set_rtl_item() _mission_item.altitude = _rtl_alt; _mission_item.altitude_is_relative = false; - // Use destination yaw if close to _destination. - // Check if we are pretty close to the destination already. - if (destination_dist < _param_rtl_min_dist.get()) { + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING && + destination_dist > _param_rtl_min_dist.get()) { + _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING || + destination_dist < _param_rtl_min_dist.get()) { + // Use destination yaw if close to _destination. _mission_item.yaw = _destination.yaw; - } else { - // Use current heading to _destination. - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _navigator->get_local_position()->heading; } _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); @@ -410,7 +422,6 @@ void RTL::set_rtl_item() case RTL_STATE_DESCEND: { _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - _mission_item.lat = _destination.lat; _mission_item.lon = _destination.lon; _mission_item.altitude = loiter_altitude; @@ -422,6 +433,9 @@ void RTL::set_rtl_item() if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) { _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _navigator->get_local_position()->heading; + } else { _mission_item.yaw = _destination.yaw; } @@ -449,20 +463,6 @@ void RTL::set_rtl_item() case RTL_STATE_LOITER: { const bool autoland = (_param_rtl_land_delay.get() > FLT_EPSILON); - // Don't change altitude. - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; - _mission_item.altitude_is_relative = false; - _mission_item.yaw = _destination.yaw; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); - _mission_item.autocontinue = autoland; - _mission_item.origin = ORIGIN_ONBOARD; - - _navigator->set_can_loiter_at_sp(true); - if (autoland) { _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t", @@ -475,17 +475,46 @@ void RTL::set_rtl_item() events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); } + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; // Don't change altitude. + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _navigator->get_local_position()->heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); + _mission_item.autocontinue = autoland; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; } case RTL_STATE_HEAD_TO_CENTER: { _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.lat = _destination.lat; _mission_item.lon = _destination.lon; _mission_item.altitude = loiter_altitude; _mission_item.altitude_is_relative = false; - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { + _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _navigator->get_local_position()->heading; + } + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; @@ -507,7 +536,17 @@ void RTL::set_rtl_item() _mission_item.lon = _destination.lon; _mission_item.altitude = loiter_altitude; _mission_item.altitude_is_relative = false; - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { + _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _navigator->get_local_position()->heading; + } + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.origin = ORIGIN_ONBOARD; break; @@ -518,9 +557,16 @@ void RTL::set_rtl_item() _mission_item.nav_cmd = NAV_CMD_LAND; _mission_item.lat = _destination.lat; _mission_item.lon = _destination.lon; - _mission_item.yaw = _destination.yaw; _mission_item.altitude = _destination.alt; _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _navigator->get_local_position()->heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 1217f9b25b..b1306516a8 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -59,6 +59,10 @@ class Navigator; class RTL : public MissionBlock, public ModuleParams { public: + RTL(Navigator *navigator); + + ~RTL() = default; + enum RTLType { RTL_HOME = 0, RTL_LAND, @@ -72,9 +76,11 @@ public: RTL_DESTINATION_SAFE_POINT, }; - RTL(Navigator *navigator); - - ~RTL() = default; + enum RTLHeadingMode { + RTL_NAVIGATION_HEADING = 0, + RTL_DESTINATION_HEADING, + RTL_CURRENT_HEADING, + }; void on_inactivation() override; void on_inactive() override; @@ -148,21 +154,23 @@ private: hrt_abstime _destination_check_time{0}; float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position - bool _rtl_alt_min{false}; float _rtl_loiter_rad{50.0f}; // radius at which a fixed wing would loiter while descending + bool _climb_and_return_done{false}; // this flag is set to true if RTL is active and we are past the climb state and return state bool _deny_mission_landing{false}; + bool _rtl_alt_min{false}; DEFINE_PARAMETERS( - (ParamFloat) _param_rtl_return_alt, + (ParamFloat) _param_rtl_return_alt, (ParamFloat) _param_rtl_descend_alt, - (ParamFloat) _param_rtl_land_delay, - (ParamFloat) _param_rtl_min_dist, - (ParamInt) _param_rtl_type, - (ParamInt) _param_rtl_cone_half_angle_deg, - (ParamFloat) _param_rtl_flt_time, - (ParamInt) _param_rtl_pld_md, - (ParamFloat) _param_rtl_loiter_rad + (ParamFloat) _param_rtl_land_delay, + (ParamFloat) _param_rtl_min_dist, + (ParamInt) _param_rtl_type, + (ParamInt) _param_rtl_cone_half_angle_deg, + (ParamFloat) _param_rtl_flt_time, + (ParamInt) _param_rtl_pld_md, + (ParamFloat) _param_rtl_loiter_rad, + (ParamInt) _param_rtl_hdg_md ) // These need to point at different parameters depending on vehicle type. @@ -173,7 +181,7 @@ private: param_t _param_rtl_descent_speed{PARAM_INVALID}; uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; - uORB::Publication _rtl_flight_time_pub{ORB_ID(rtl_flight_time)}; + uORB::Publication _rtl_flight_time_pub{ORB_ID(rtl_flight_time)}; }; float time_to_home(const matrix::Vector3f &to_home_vec, diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 463ee1a435..23f73dbdbe 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -156,7 +156,7 @@ PARAM_DEFINE_FLOAT(RTL_FLT_TIME, 15); * @value 0 No precision landing * @value 1 Opportunistic precision landing * @value 2 Required precision landing - * @group Return To Land + * @group Return Mode */ PARAM_DEFINE_INT32(RTL_PLD_MD, 0); @@ -173,3 +173,15 @@ PARAM_DEFINE_INT32(RTL_PLD_MD, 0); * @group Return Mode */ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); + +/** + * RTL heading mode + * + * Defines the heading behavior during RTL + * + * @value 0 Towards next waypoint. + * @value 1 Heading matches destination. + * @value 2 Use current heading. + * @group Return Mode + */ +PARAM_DEFINE_INT32(RTL_HDG_MD, 0);