diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 1d7e1613a4..7bebf1a374 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -52,6 +52,14 @@ RTL::RTL(Navigator *navigator) : { } +void +RTL::on_inactivation() +{ + if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + void RTL::on_inactive() { @@ -223,6 +231,14 @@ RTL::on_active() if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { advance_rtl(); set_rtl_item(); + + } + + if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { + _navigator->get_precland()->on_active(); + + } else if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); } } @@ -389,6 +405,16 @@ RTL::set_rtl_item() _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.land_precision = _param_rtl_pld_md.get(); + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + _navigator->get_precland()->on_activation(); + + } else if (_mission_item.land_precision == 2) { + _navigator->get_precland()->set_mode(PrecLandMode::Required); + _navigator->get_precland()->on_activation(); + } mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination"); break; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index b186aec8cc..d68a5a1ad3 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -70,6 +70,7 @@ public: ~RTL() = default; + void on_inactivation() override; void on_inactive() override; void on_activation() override; void on_active() override; @@ -137,6 +138,7 @@ private: (ParamFloat) _param_rtl_land_delay, (ParamFloat) _param_rtl_min_dist, (ParamInt) _param_rtl_type, - (ParamInt) _param_rtl_cone_half_angle_deg + (ParamInt) _param_rtl_cone_half_angle_deg, + (ParamInt) _param_rtl_pld_md ) }; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index f97201c489..a3bf36de27 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -135,3 +135,15 @@ PARAM_DEFINE_INT32(RTL_TYPE, 0); * @group Return Mode */ PARAM_DEFINE_INT32(RTL_CONE_ANG, 0); + +/** + * RTL precision land mode + * + * Use precision landing when doing an RTL landing phase. + * + * @value 0 No precision landing + * @value 1 Opportunistic precision landing + * @value 2 Required precision landing + * @group Return To Land + */ +PARAM_DEFINE_INT32(RTL_PLD_MD, 0);