From f08268adbc240debf82f9d441d9f9a1b1cc891dc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 25 Mar 2022 08:20:13 +1300 Subject: [PATCH] flight_modes: use precision landing when landing This also enables precision landing in landing mode, not just during RTL. This required an additional fix during the horizontal approach. It seems like the z setpoint is not set when landing, so we can't blindly use that setpoint but rather need to keep the altitude. --- src/modules/flight_mode_manager/FlightModeManager.cpp | 3 ++- .../FlightTaskAutoPrecisionLanding.cpp | 8 +++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 7e86f47046..0df602fa6b 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -166,7 +166,8 @@ void FlightModeManager::start_flight_task() // Take-over landing from navigator if precision landing is enabled - } else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL + } else if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL || + _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) && _param_rtl_pld_md.get() > 0) { should_disable_task = false; diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp index 4683e11e19..7d3aec719f 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp @@ -150,7 +150,13 @@ FlightTaskAutoPrecisionLanding::run_state_horizontal_approach() _position_setpoint(0) = x; _position_setpoint(1) = y; _position_setpoint(2) = _target(2); - _velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN; + + _velocity_setpoint(0) = _velocity_setpoint(1) = NAN; + + // If altitude setpoint is NAN, we stay at the current altitude + if (!PX4_ISFINITE(_position_setpoint(2))) { + _velocity_setpoint(2) = 0.0f; + } // check if target visible, if not go to start if (!check_state_conditions(PrecLandState::HorizontalApproach)) {