From 4435a57e687f55825388bb866203260a669b0de6 Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Wed, 29 Mar 2023 16:14:33 +0200 Subject: [PATCH] wip --- msg/PrecisionLandingStatus.msg | 2 +- .../flight_mode_manager/FlightModeManager.cpp | 1 + .../FlightTaskAutoPrecisionLanding.cpp | 581 +++++------------- .../FlightTaskAutoPrecisionLanding.hpp | 113 ++-- 4 files changed, 205 insertions(+), 492 deletions(-) diff --git a/msg/PrecisionLandingStatus.msg b/msg/PrecisionLandingStatus.msg index 9a2327726d..e5bd537560 100644 --- a/msg/PrecisionLandingStatus.msg +++ b/msg/PrecisionLandingStatus.msg @@ -6,4 +6,4 @@ uint8 PRECLAND_STATE_TOUCHING_DOWN = 4 # Same as descend, but if sight o uint8 PRECLAND_STATE_FALLBACK = 5 # Fallback to conventional landing, either because of opportunistic precision landing or because maximum number of precland attempts were reached uint64 timestamp # time since system start (microseconds) -uint8 precland_state # this flight-task's internal state +uint8 state # this flight-task's internal state diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 31d4bdd880..565964b6b1 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -180,6 +180,7 @@ void FlightModeManager::start_flight_task() should_disable_task = false; if (switchTask(FlightTaskIndex::AutoPrecisionLanding) != FlightTaskError::NoError) { + PX4_INFO("Switching to precland flight task"); task_failure = true; } diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp index 9e4058fccb..18ef19ce17 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,450 +30,201 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + /** * @file FlightTaskAutoPrecisionLanding.cpp - * - * @author Nicolas de Palezieux (Sunflower Labs) */ #include "FlightTaskAutoPrecisionLanding.hpp" +#include -#include -#include -#include -#include - -static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing"; +using namespace matrix; bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); + PX4_INFO("precland activated"); - _search_cnt = 0; - _last_slewrate_time = 0; + _position_setpoint = _position; + _search_count = 0; - try_switch_to_state_auto_rtl(); + // TODO: Use target orientation instead + _yaw_setpoint = _yaw; return ret; } +void FlightTaskAutoPrecisionLanding::do_state_transition(PRECLAND_STATE new_state) +{ + _precland_state = new_state; + _state_start_time = hrt_absolute_time(); +} + +bool FlightTaskAutoPrecisionLanding::inside_acceptance_radius() +{ + return Vector3f(_position_setpoint - _position).norm() <= _param_pld_hacc_rad.get(); +} + bool FlightTaskAutoPrecisionLanding::update() { bool ret = FlightTaskAuto::update(); + // printf("state: %u\n", _precland_state); + // Fetch uorb if (_landing_target_pose_sub.updated()) { _landing_target_pose_sub.copy(&_landing_target_pose); + + _precision_target_ned(0) = _landing_target_pose.x_rel; + _precision_target_ned(1) = _landing_target_pose.y_rel; + _precision_target_ned(2) = _landing_target_pose.z_rel; } - // target pose can become invalid when the message timed out - _landing_target_pose_valid = (hrt_elapsed_time(&_landing_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get(); + // Initialize precision target coordinates with home position if marker has not been spotted yet + if (_landing_target_pose.timestamp == 0) { + if (_sub_home_position.get().valid_lpos && _sub_home_position.get().valid_alt) { + _precision_target_ned(0) = _sub_home_position.get().x; + _precision_target_ned(1) = _sub_home_position.get().y; + _precision_target_ned(2) = _sub_home_position.get().z; // TODO: I think this is NAN + } else { + // Nothing to do without valid home location or target position + return ret; + } - switch (_state) { - case PrecLandState::AutoRTL: - run_state_auto_rtl(); - break; - - case PrecLandState::MoveAboveTarget: - run_state_move_above_target(); - break; - - case PrecLandState::DescendAboveTarget: - run_state_descend_above_target(); - break; - - case PrecLandState::TouchingDown: - run_state_touching_down(); - break; - - case PrecLandState::Search: - run_state_search(); - break; - - case PrecLandState::Fallback: - run_state_fallback(); - break; - - default: - // unknown state - break; } - // Publish status message for debugging precision_landing_status_s precision_landing_status{}; - precision_landing_status.timestamp = hrt_absolute_time(); - precision_landing_status.precland_state = (uint8_t) _state; - _precision_landing_status_pub.publish(precision_landing_status); - _constraints.want_takeoff = _checkTakeoff(); + // Fetch runtime parameters + const float max_search_duration = _param_pld_srch_tout.get(); + const float search_rel_altitude = _param_pld_srch_alt.get(); + + switch (_precland_state) { + case PRECLAND_STATE::AUTORTL_CLIMB: + PX4_INFO("AUTORTL_CLIMB"); + + _position_setpoint = _position; + _position_setpoint(2) = _precision_target_ned(2) - search_rel_altitude; + _velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN; + + // transition condition + // Wait for drone to reach z position setpoint + if (abs(_position(2) - _position_setpoint(2)) <= _param_pld_hacc_rad.get()) { + mavlink_log_info(&_mavlink_log_pub, "Moving above target"); + do_state_transition(PRECLAND_STATE::AUTORTL_APPROACH); + } + + break; + + case PRECLAND_STATE::AUTORTL_APPROACH: + PX4_INFO("AUTORTL_APPROACH"); + + _position_setpoint = _precision_target_ned; + _position_setpoint(2) -= search_rel_altitude; + _velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN; + + // transition condition + // Wait for drone to reach position setpoint + if (inside_acceptance_radius()) { + mavlink_log_info(&_mavlink_log_pub, "Moving to search center"); + do_state_transition(PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE); + } + + break; + + case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE: + PX4_INFO("MOVE_TO_SEARCH_ALTITUDE"); + + // Ascend/Descend to search altitude + _position_setpoint = _precision_target_ned; + _position_setpoint(2) -= search_rel_altitude; + _velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN; + _acceleration_setpoint(0) = _acceleration_setpoint(1) = _acceleration_setpoint(2) = NAN; + + // transition condition + if (inside_acceptance_radius()) { + mavlink_log_info(&_mavlink_log_pub, "Starting search"); + do_state_transition(PRECLAND_STATE::SEARCHING_TARGET); + } + + break; + + case PRECLAND_STATE::SEARCHING_TARGET: + PX4_INFO("SEARCHING_TARGET"); + + // Currently no search pattern is implemented. Just wait for target... + // Workaround for Orion app making unwanted changes to my parameters + if ((hrt_absolute_time() - _state_start_time) > max_search_duration * SEC2USEC) { + mavlink_log_info(&_mavlink_log_pub, "Search timed out"); + + if (++_search_count >= _param_pld_max_srch.get()) { + mavlink_log_info(&_mavlink_log_pub, "Abandonning search and landing immediately"); + do_state_transition(PRECLAND_STATE::LANDING); + + } else { + mavlink_log_info(&_mavlink_log_pub, "Moving to search center"); + do_state_transition(PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE); + } + } + + // transition condition + if (hrt_absolute_time() - _landing_target_pose.timestamp <= _param_pld_btout.get()*SEC2USEC) { + mavlink_log_info(&_mavlink_log_pub, "Moving above target"); + do_state_transition(PRECLAND_STATE::MOVING_ABOVE_TARGET); + } + + break; + + case PRECLAND_STATE::MOVING_ABOVE_TARGET: + PX4_INFO("MOVING_ABOVE_TARGET"); + + _position_setpoint(0) = _landing_target_pose.x_abs; + _position_setpoint(1) = _landing_target_pose.y_abs; + _position_setpoint(2) = _precision_target_ned(2) - search_rel_altitude; + + _velocity_setpoint(0) = NAN; + _velocity_setpoint(1) = NAN; + _velocity_setpoint(2) = 0; + + // Transition condition + // - Check acceptance radius and + // - wait long enough to observe if the landing_target_pose timed out before proceeding + if (abs(_position(0) - _position_setpoint(0)) <= _param_pld_hacc_rad.get() + && abs(_position(1) - _position_setpoint(1)) <= _param_pld_hacc_rad.get() + && _velocity.norm() <= HOVER_SPEED_THRESHOLD + && (hrt_absolute_time() - _state_start_time) > _param_pld_btout.get() * SEC2USEC) { + + if (hrt_absolute_time() - _landing_target_pose.timestamp < _param_pld_btout.get() * SEC2USEC) { + mavlink_log_info(&_mavlink_log_pub, "Landing on target..."); + do_state_transition(PRECLAND_STATE::LANDING); + + } else { + mavlink_log_info(&_mavlink_log_pub, "Lost target, back to searching"); + do_state_transition(PRECLAND_STATE::SEARCHING_TARGET); + } + } + + break; + + + case PRECLAND_STATE::LANDING: + PX4_INFO("LANDING"); + // Control XY position and Z velocity + _position_setpoint(0) = _landing_target_pose.x_abs; + _position_setpoint(1) = _landing_target_pose.y_abs; + _position_setpoint(2) = NAN; + + _velocity_setpoint(0) = 0; + _velocity_setpoint(1) = 0; + _velocity_setpoint(2) = _param_mpc_land_speed.get(); + _acceleration_setpoint(2) = NAN; + break; + + } + + precision_landing_status.timestamp = hrt_absolute_time(); + precision_landing_status.state = _precland_state; + _precision_landing_status_pub.publish(precision_landing_status); return ret; } - -void -FlightTaskAutoPrecisionLanding::run_state_auto_rtl() -{ - // In this state simply track the navigator setpoint triplet - // This ensures that the behaviour for precision landing during RTL / LAND - // is the same as without precision landing. - // This flight task will generate independent setpoints once the vehicle - // is in the vertical landing phase. - _position_setpoint = _target; - - if (try_switch_to_state_move_above_target()) { - // If target visible and go to horizontal approach directly - return; - - } else if ((PrecLandMode)_param_rtl_pld_md.get() == PrecLandMode::Opportunistic) { - // could not see the target immediately, so just fall back to normal landing - try_switch_to_state_fallback(); - - } else if (_type == WaypointType::land) { - // Navigator already entered land stage. Take over with precision landing - // This is where the vehicle starts to behave differently than in regular land! - try_switch_to_state_search(); - } -} - -void -FlightTaskAutoPrecisionLanding::run_state_move_above_target() -{ - float x = _landing_target_pose.x_abs; - float y = _landing_target_pose.y_abs; - slewrate(x, y); // TODO: Replace this with PX4's AlphaFilter - - // Fly to target XY position, but keep navigator's altitude setpoint - _position_setpoint(0) = x; - _position_setpoint(1) = y; - _position_setpoint(2) = _target(2); - - _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::MoveAboveTarget)) { - PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state); - - // Stay at current position for searching for the landing target - // TODO: This is not going to work after conversion to flight task, because - // the position_setpoint will be overwritten at the next iteration! - // Solution: An additional wait state - _position_setpoint = _position; - _velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN; - - if (!try_switch_to_state_auto_rtl()) { - try_switch_to_state_fallback(); - } - - return; - } - - if (check_state_conditions(PrecLandState::DescendAboveTarget)) { - if (!_point_reached_time) { - _point_reached_time = hrt_absolute_time(); - } - - if (hrt_absolute_time() - _point_reached_time > 2000000) { - // if close enough for descent above target go to descend above target - if (try_switch_to_state_descend_above_target()) { - - return; - } - } - - } -} - -void -FlightTaskAutoPrecisionLanding::run_state_descend_above_target() -{ - // Overwrite Auto setpoints in order to descend above target - _position_setpoint(0) = _landing_target_pose.x_abs; - _position_setpoint(1) = _landing_target_pose.y_abs; - _position_setpoint(2) = NAN; - - _velocity_setpoint(0) = 0; - _velocity_setpoint(1) = 0; - _velocity_setpoint(2) = _param_mpc_land_speed.get(); - - // check if target visible - if (!check_state_conditions(PrecLandState::DescendAboveTarget)) { - if (!try_switch_to_state_touching_down()) { - PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state); - - // Stay at current position for searching for the target - _position_setpoint = _position; - - if (!try_switch_to_state_auto_rtl()) { - try_switch_to_state_fallback(); - } - } - - return; - } -} - -void -FlightTaskAutoPrecisionLanding::run_state_touching_down() -{ - // Overwrite Auto setpoints in order to land at target's last known location - _position_setpoint(0) = _landing_target_pose.x_abs; - _position_setpoint(1) = _landing_target_pose.y_abs; - _position_setpoint(2) = NAN; - - _velocity_setpoint(0) = 0; - _velocity_setpoint(1) = 0; - _velocity_setpoint(2) = _param_mpc_land_speed.get(); -} - -void -FlightTaskAutoPrecisionLanding::run_state_search() -{ - // Overwrite Auto setpoints in order to hover at search altitude - _position_setpoint = _target; - _position_setpoint(2) = _sub_home_position.get().z - _param_pld_srch_alt.get(); - - _velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN; - - // check if we can see the target - if (check_state_conditions(PrecLandState::MoveAboveTarget)) { - if (!_target_acquired_time) { - // target just became visible. Stop climbing, but give it some margin so we don't stop too apruptly - _target_acquired_time = hrt_absolute_time(); - _position_setpoint = _position; - _position_setpoint(2) += 1.0f; - } - - } - - // stay at that height for a second to allow the vehicle to settle - if (_target_acquired_time && (hrt_absolute_time() - _target_acquired_time) > 1000000) { - // try to switch to horizontal approach - if (try_switch_to_state_move_above_target()) { - return; - } - - } else if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get()*SEC2USEC) { - // Search timed out and go to fallback - PX4_WARN("Search timed out"); - - try_switch_to_state_fallback(); - } -} - -void -FlightTaskAutoPrecisionLanding::run_state_fallback() -{ - // Try switching to horizontal approach. This works if the - // target meanwhile became visible - try_switch_to_state_move_above_target(); - - // Otherwise there is nothing to do, except for listening to navigator - _position_setpoint = _target; -} - -bool -FlightTaskAutoPrecisionLanding::try_switch_to_state_auto_rtl() -{ - if (check_state_conditions(PrecLandState::AutoRTL)) { - _search_cnt++; - - _point_reached_time = 0; - - _state = PrecLandState::AutoRTL; - _state_start_time = hrt_absolute_time(); - return true; - } - - return false; -} - -bool -FlightTaskAutoPrecisionLanding::try_switch_to_state_move_above_target() -{ - if (check_state_conditions(PrecLandState::MoveAboveTarget)) { - print_state_switch_message("horizontal approach"); - - _point_reached_time = 0; - - _state = PrecLandState::MoveAboveTarget; - _state_start_time = hrt_absolute_time(); - return true; - } - - return false; -} - -bool -FlightTaskAutoPrecisionLanding::try_switch_to_state_descend_above_target() -{ - if (check_state_conditions(PrecLandState::DescendAboveTarget)) { - print_state_switch_message("descend"); - _state = PrecLandState::DescendAboveTarget; - _state_start_time = hrt_absolute_time(); - return true; - } - - return false; -} - -bool -FlightTaskAutoPrecisionLanding::try_switch_to_state_touching_down() -{ - if (check_state_conditions(PrecLandState::TouchingDown)) { - print_state_switch_message("final approach"); - _state = PrecLandState::TouchingDown; - _state_start_time = hrt_absolute_time(); - return true; - } - - return false; -} - -void -FlightTaskAutoPrecisionLanding::try_switch_to_state_search() -{ - PX4_INFO("Climbing to search altitude"); - - _target_acquired_time = 0; - - _state = PrecLandState::Search; - _state_start_time = hrt_absolute_time(); -} - -void -FlightTaskAutoPrecisionLanding::try_switch_to_state_fallback() -{ - print_state_switch_message("fallback"); - - _state = PrecLandState::Fallback; - _state_start_time = hrt_absolute_time(); -} - -void FlightTaskAutoPrecisionLanding::print_state_switch_message(const char *state_name) -{ - PX4_INFO("Precland: switching to %s", state_name); -} - -bool FlightTaskAutoPrecisionLanding::check_state_conditions(PrecLandState state) -{ - switch (state) { - case PrecLandState::AutoRTL: - return _search_cnt <= _param_pld_max_srch.get(); - - case PrecLandState::MoveAboveTarget: - - // if we're already in this state, only want to make it invalid if we reached the target but can't see it anymore - if (_state == PrecLandState::MoveAboveTarget) { - if (hor_acc_radius_check()) { - // we've reached the position where we last saw the target. If we don't see it now, we need to do something - return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid; - - } else { - // We've seen the target sometime during horizontal approach. - // Even if we don't see it as we're moving towards it, continue approaching last known location - return true; - } - } - - // If we're trying to switch to this state, the target needs to be visible - return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid; - - case PrecLandState::DescendAboveTarget: - - // if we're already in this state, only leave it if target becomes unusable, don't care about horizontal offset to target - if (_state == PrecLandState::DescendAboveTarget) { - // if we're close to the ground, we're more critical of target timeouts so we quickly go into descend - if (check_state_conditions(PrecLandState::TouchingDown)) { - return hrt_absolute_time() - _landing_target_pose.timestamp < 500000; // 0.5s // TODO: Magic number! - - } else { - return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid; - } - - } else { - // if not already in this state, need to be above target to enter it - return _landing_target_pose.abs_pos_valid && hor_acc_radius_check(); - } - - case PrecLandState::TouchingDown: - return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid - && (_landing_target_pose.z_abs - _position(2)) < _param_pld_fappr_alt.get(); - - case PrecLandState::Search: - return true; - - case PrecLandState::Fallback: - return true; - - default: - return false; - } -} - -bool FlightTaskAutoPrecisionLanding::hor_acc_radius_check() -{ - return Vector2f(Vector2f(_landing_target_pose.x_abs, - _landing_target_pose.y_abs) - _position.xy()).norm() <= _param_pld_hacc_rad.get(); -} - -void FlightTaskAutoPrecisionLanding::slewrate(float &sp_x, float &sp_y) -{ - matrix::Vector2f sp_curr(sp_x, sp_y); - uint64_t now = hrt_absolute_time(); - - float dt = (now - _last_slewrate_time); - - if (dt < 1) { - // bad dt, can't divide by it - return; - } - - dt /= SEC2USEC; - - if (_last_slewrate_time == 0) { - // running the first time since switching to precland - _sp_pev_prev(0) = sp_x; - _sp_pev_prev(1) = sp_y; - _sp_pev(0) = sp_x; - _sp_pev(1) = sp_y; - } - - _last_slewrate_time = now; - - // limit the setpoint speed to the maximum cruise speed - matrix::Vector2f sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints - - if (sp_vel.length() > _param_xy_vel_cruise.get()) { - sp_vel = sp_vel.normalized() * _param_xy_vel_cruise.get(); - sp_curr = _sp_pev + sp_vel * dt; - } - - // limit the setpoint acceleration to the maximum acceleration - matrix::Vector2f sp_acc = (sp_curr - _sp_pev * 2 + _sp_pev_prev) / (dt * dt); // acceleration of the setpoints - - if (sp_acc.length() > _param_acceleration_hor.get()) { - sp_acc = sp_acc.normalized() * _param_acceleration_hor.get(); - sp_curr = _sp_pev * 2 - _sp_pev_prev + sp_acc * (dt * dt); - } - - // limit the setpoint speed such that we can stop at the setpoint given the maximum acceleration/deceleration - float max_spd = sqrtf(_param_acceleration_hor.get() * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x, - sp_y))).length()); - sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints - - if (sp_vel.length() > max_spd) { - sp_vel = sp_vel.normalized() * max_spd; - sp_curr = _sp_pev + sp_vel * dt; - } - - _sp_pev_prev = _sp_pev; - _sp_pev = sp_curr; - - sp_x = sp_curr(0); - sp_y = sp_curr(1); -} diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp index 73d4af50ba..c83abc00ec 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,41 +35,23 @@ * @file FlightTaskAutoPrecisionLanding.hpp * * Flight task for better precision landing - * - * @author Nicolas de Palezieux (Sunflower Labs) */ #pragma once #include "FlightTaskAuto.hpp" - -#include -#include -#include -#include -#include #include #include #include +// #include +#include +#include +// #include +#include -#define SEC2USEC 1000000.0f // TODO: Get the correct define from some header -#define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state -#define ACCEPTANCE_RADIUS 0.20f // Horizontal acceptance radius for the navigation to the landing target -// TODO: Get ACCEPTANCE_RADIUS from NAV_ACC_RAD - -enum class PrecLandState { - AutoRTL, // Starting state - Search, // Search for landing target - MoveAboveTarget, // Positioning over landing target while maintaining altitude - DescendAboveTarget, // Stay over landing target while descending - TouchingDown, // Final landing approach, even without landing target - Fallback // Fallback landing method -}; - -enum class PrecLandMode { - Opportunistic = 1, // only do precision landing if landing target visible at the beginning - Required = 2 // try to find landing target if not visible at the beginning -}; +// TODO: Get the correct define from some header +#define SEC2USEC 1000000.0f +#define HOVER_SPEED_THRESHOLD 0.20f // If drone has speed under this value it is considered to be hovering in place class FlightTaskAutoPrecisionLanding : public FlightTaskAuto { @@ -82,60 +64,39 @@ public: bool update() override; private: - // run the control loop for each state - void run_state_auto_rtl(); - void run_state_search(); - void run_state_move_above_target(); - void run_state_descend_above_target(); - void run_state_touching_down(); - void run_state_fallback(); - - // attempt to switch to a different state. Returns true if state change was successful, false otherwise - bool try_switch_to_state_auto_rtl(); - void try_switch_to_state_search(); - bool try_switch_to_state_move_above_target(); - bool try_switch_to_state_descend_above_target(); - bool try_switch_to_state_touching_down(); - void try_switch_to_state_fallback(); - - void print_state_switch_message(const char *state_name); - - // check if a given state could be changed into. Return true if possible to transition to state, false otherwise - bool check_state_conditions(PrecLandState state); - void slewrate(float &sp_x, float &sp_y); - - bool hor_acc_radius_check(); - - landing_target_pose_s _landing_target_pose{}; /**< precision landing target position */ - - uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; - uORB::PublicationMulti _precision_landing_status_pub{ORB_ID(precision_landing_status)}; - - bool _landing_target_pose_valid{false}; /**< whether we have received a landing target position message */ - - uint64_t _state_start_time{0}; /**< time when we entered current state */ - uint64_t _last_slewrate_time{0}; /**< time when we last limited setpoint changes */ - uint64_t _target_acquired_time{0}; /**< time when we first saw the landing target during search */ - uint64_t _point_reached_time{0}; /**< time when we reached a setpoint */ - - int _search_cnt{0}; /**< counter of how many times we had to search for the landing target */ - - matrix::Vector2f _sp_pev; - matrix::Vector2f _sp_pev_prev; - - PrecLandState _state{PrecLandState::AutoRTL}; - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, (ParamFloat) _param_mpc_land_speed, ///< velocity for controlled descend - (ParamFloat) _param_acceleration_hor, - (ParamFloat) _param_xy_vel_cruise, - (ParamFloat) _param_pld_btout, - (ParamFloat) _param_pld_hacc_rad, - (ParamFloat) _param_pld_fappr_alt, (ParamFloat) _param_pld_srch_alt, (ParamFloat) _param_pld_srch_tout, (ParamInt) _param_pld_max_srch, - (ParamInt) _param_rtl_pld_md + (ParamFloat) _param_pld_hacc_rad, + (ParamFloat) _param_pld_btout ) + enum PRECLAND_STATE { + AUTORTL_CLIMB, + AUTORTL_APPROACH, + MOVE_TO_SEARCH_ALTITUDE, + SEARCHING_TARGET, + MOVING_ABOVE_TARGET, + LANDING + }; + + void do_state_transition(PRECLAND_STATE new_state); + + bool inside_acceptance_radius(); + + PRECLAND_STATE _precland_state = PRECLAND_STATE::AUTORTL_CLIMB; + + uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; + landing_target_pose_s _landing_target_pose{}; /**< precision landing target position */ + + uORB::PublicationMulti _precision_landing_status_pub{ORB_ID(precision_landing_status)}; + + uint64_t _state_start_time{0}; /**< time when entering search state */ + int _search_count = 0; + + orb_advert_t _mavlink_log_pub{nullptr}; + matrix::Vector3f _precision_target_ned = {NAN, NAN, NAN}; + };