From 7801806b1cd88619c719f09bd05500ce05ea48ce Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Fri, 31 Mar 2023 19:05:02 +0200 Subject: [PATCH] split logic into simpler functions --- .../FlightTaskPrecisionLanding.cpp | 228 +++++++++++------- .../FlightTaskPrecisionLanding.hpp | 11 +- 2 files changed, 144 insertions(+), 95 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.cpp b/src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.cpp index a8134d00e5..765cd84e4e 100644 --- a/src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.cpp +++ b/src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.cpp @@ -44,14 +44,11 @@ using namespace matrix; bool FlightTaskPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); - PX4_INFO("precland activated"); - _precland_state = PRECLAND_STATE::AUTORTL_CLIMB; _position_setpoint = _position; _search_count = 0; - // TODO: Use target orientation instead _initial_yaw = _yaw; _initial_position = _position; @@ -72,45 +69,118 @@ bool FlightTaskPrecisionLanding::inside_acceptance_radius() bool FlightTaskPrecisionLanding::precision_target_available() { - // TODO: Add timeout const bool ever_received = _landing_target_pose.timestamp != 0; const bool timed_out = hrt_absolute_time() - _landing_target_pose.timestamp > _param_pld_btout.get() * SEC2USEC; return ever_received && !timed_out; } -bool FlightTaskPrecisionLanding::update() +void FlightTaskPrecisionLanding::generate_pos_xy_setpoints() { - // Get setpoints from FlightTask and later override if necessary - bool ret = FlightTask::update(); + switch (_precland_state) { + case PRECLAND_STATE::AUTORTL_CLIMB: + _position_setpoint(0) = _initial_position(0); + _position_setpoint(1) = _initial_position(1); + break; - // Fetch uorb - if (_landing_target_pose_sub.updated()) { - _landing_target_pose_sub.copy(&_landing_target_pose); + case PRECLAND_STATE::AUTORTL_APPROACH: + case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE: - _precision_target_ned(0) = _landing_target_pose.x_abs; - _precision_target_ned(1) = _landing_target_pose.y_abs; + // Horizontal approach to home or precision target + if (precision_target_available()) { + _position_setpoint(0) = _landing_target_pose.x_abs; + _position_setpoint(1) = _landing_target_pose.y_abs; - // Supplement precision target altitude with home postion's altitude if necessary - _precision_target_ned(2) = PX4_ISFINITE(_landing_target_pose.z_rel) ? _landing_target_pose.z_rel : - _sub_home_position.get().z; + } else { + _position_setpoint(0) = _sub_home_position.get().x; + _position_setpoint(1) = _sub_home_position.get().y; + } + + break; + + case PRECLAND_STATE::SEARCHING_TARGET: + case PRECLAND_STATE::MOVING_ABOVE_TARGET: + case PRECLAND_STATE::LANDING: + _position_setpoint(0) = _landing_target_pose.x_abs; + _position_setpoint(1) = _landing_target_pose.y_abs; + break; } +} - // Fetch runtime parameters - const float max_search_duration = _param_pld_srch_tout.get(); +void FlightTaskPrecisionLanding::generate_pos_z_setpoints() +{ const float search_rel_altitude = _param_pld_srch_alt.get(); + switch (_precland_state) { + case PRECLAND_STATE::AUTORTL_CLIMB: + case PRECLAND_STATE::AUTORTL_APPROACH: + // Horizontal approach to home or precision target + _position_setpoint(2) = _sub_home_position.get().z - _param_rtl_return_alt.get(); + break; + case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE: + case PRECLAND_STATE::SEARCHING_TARGET: + case PRECLAND_STATE::MOVING_ABOVE_TARGET: + // TODO: Should use current altitude when state was entered as the altitude setpoint + if (PX4_ISFINITE(_landing_target_pose.z_abs)){ + // For safety reasons take whichever reference altitude is higher up (lower in NED) + _position_setpoint(2) = math::min(_sub_home_position.get().z, _landing_target_pose.z_abs) - search_rel_altitude; + } else { + _position_setpoint(2) = _sub_home_position.get().z - search_rel_altitude; + } + break; + + case PRECLAND_STATE::LANDING: + _position_setpoint(2) = NAN; + break; + } +} + +void FlightTaskPrecisionLanding::generate_vel_setpoints() +{ + switch (_precland_state) { + case PRECLAND_STATE::AUTORTL_CLIMB: + case PRECLAND_STATE::AUTORTL_APPROACH: + case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE: + case PRECLAND_STATE::SEARCHING_TARGET: + case PRECLAND_STATE::MOVING_ABOVE_TARGET: + _velocity_setpoint.setNaN(); + break; + + case PRECLAND_STATE::LANDING: + _velocity_setpoint(0) = 0; + _velocity_setpoint(1) = 0; + _velocity_setpoint(2) = _param_mpc_land_speed.get(); + break; + } +} + +void FlightTaskPrecisionLanding::generate_acc_setpoints() +{ + _acceleration_setpoint.setNaN(); +} + +void FlightTaskPrecisionLanding::generate_yaw_setpoint() +{ + switch (_precland_state) { + case PRECLAND_STATE::AUTORTL_CLIMB: + _yaw_setpoint = _initial_yaw; + break; + + case PRECLAND_STATE::AUTORTL_APPROACH: + // TODO: Point in direction of flight + case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE: + case PRECLAND_STATE::SEARCHING_TARGET: + case PRECLAND_STATE::MOVING_ABOVE_TARGET: + case PRECLAND_STATE::LANDING: + _yaw_setpoint = _target_yaw; + break; + } +} + +void FlightTaskPrecisionLanding::check_state_transitions() +{ switch (_precland_state) { case PRECLAND_STATE::AUTORTL_CLIMB: PX4_INFO("AUTORTL_CLIMB"); - _position_setpoint(0) = _initial_position(0); - _position_setpoint(1) = _initial_position(1); - _position_setpoint(2) = _sub_home_position.get().z - _param_rtl_return_alt.get(); - - _velocity_setpoint.setNaN(); // TODO: Is there a RTL climb speed? - _acceleration_setpoint.setNaN(); - - _yaw_setpoint = _initial_yaw; - // transition condition // Wait for drone to reach z position setpoint or be higher if (_position(2) <= _position_setpoint(2) + _param_nav_mc_alt_rad.get()) { @@ -123,21 +193,6 @@ bool FlightTaskPrecisionLanding::update() case PRECLAND_STATE::AUTORTL_APPROACH: { PX4_INFO("AUTORTL_APPROACH"); - // Horizontal approach to home or precision target - if (precision_target_available()) { - _position_setpoint(0) = _precision_target_ned(0); - _position_setpoint(1) = _precision_target_ned(1); - _position_setpoint(2) = _precision_target_ned(2); - - } else { - PX4_INFO("reverting to HOME..."); - _position_setpoint(0) = _sub_home_position.get().x; - _position_setpoint(1) = _sub_home_position.get().y; - _position_setpoint(2) = _sub_home_position.get().z - _param_rtl_return_alt.get(); - } - - _velocity_setpoint.setNaN(); - // transition condition // Wait for drone to reach position setpoint if (inside_acceptance_radius()) { @@ -159,16 +214,6 @@ bool FlightTaskPrecisionLanding::update() case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE: PX4_INFO("MOVE_TO_SEARCH_ALTITUDE"); - // Ascend/Descend to search altitude - _position_setpoint(0) = _sub_home_position.get().x; - _position_setpoint(1) = _sub_home_position.get().y; - _position_setpoint(2) = _sub_home_position.get().z - search_rel_altitude; - - _velocity_setpoint.setNaN(); - _acceleration_setpoint.setNaN(); - - _yaw_setpoint = _target_yaw; - // transition condition if (inside_acceptance_radius()) { mavlink_log_info(&_mavlink_log_pub, "Starting search"); @@ -178,44 +223,36 @@ bool FlightTaskPrecisionLanding::update() break; case PRECLAND_STATE::SEARCHING_TARGET: - PX4_INFO("SEARCHING_TARGET"); + { + PX4_INFO("SEARCHING_TARGET"); + const float max_search_duration = _param_pld_srch_tout.get(); - _yaw_setpoint = _target_yaw; + // 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"); - // 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); - 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::SEARCHING_TARGET); + } else { + mavlink_log_info(&_mavlink_log_pub, "Moving to search center"); + do_state_transition(PRECLAND_STATE::SEARCHING_TARGET); + } } - } - // transition condition - if (precision_target_available()) { - mavlink_log_info(&_mavlink_log_pub, "Moving above target"); - do_state_transition(PRECLAND_STATE::MOVING_ABOVE_TARGET); - } + // transition condition + if (precision_target_available()) { + mavlink_log_info(&_mavlink_log_pub, "Moving above target"); + do_state_transition(PRECLAND_STATE::MOVING_ABOVE_TARGET); + } - break; + break; + } case PRECLAND_STATE::MOVING_ABOVE_TARGET: PX4_INFO("MOVING_ABOVE_TARGET"); - // TODO: Should use current altitude when state was entered as the altitude setpoint - _position_setpoint(0) = _precision_target_ned(0); - _position_setpoint(1) = _precision_target_ned(1); - _position_setpoint(2) = _sub_home_position.get().z - search_rel_altitude; - - _velocity_setpoint.setNaN(); - _acceleration_setpoint.setNaN(); - - _yaw_setpoint = _target_yaw; // Transition condition // - Check acceptance radius and @@ -237,23 +274,28 @@ bool FlightTaskPrecisionLanding::update() break; - case PRECLAND_STATE::LANDING: PX4_INFO("LANDING"); - // Control XY position and Z velocity - _position_setpoint(0) = _precision_target_ned(0); - _position_setpoint(1) = _precision_target_ned(1); - _position_setpoint(2) = NAN; - - _velocity_setpoint(0) = 0; - _velocity_setpoint(1) = 0; - _velocity_setpoint(2) = _param_mpc_land_speed.get(); - _acceleration_setpoint(2) = NAN; - - _yaw_setpoint = _target_yaw; break; - } +} + +bool FlightTaskPrecisionLanding::update() +{ + // Get setpoints from FlightTask and later override if necessary + bool ret = FlightTask::update(); + + // Fetch uorb + if (_landing_target_pose_sub.updated()) { + _landing_target_pose_sub.copy(&_landing_target_pose); + } + + generate_pos_xy_setpoints(); + generate_pos_z_setpoints(); + generate_vel_setpoints(); + generate_acc_setpoints(); + generate_yaw_setpoint(); + check_state_transitions(); precision_landing_status_s precision_landing_status{}; precision_landing_status.timestamp = hrt_absolute_time(); diff --git a/src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.hpp b/src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.hpp index e5b3a6d5d0..f79ee57b52 100644 --- a/src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.hpp +++ b/src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.hpp @@ -92,6 +92,15 @@ private: bool precision_target_available(); + void generate_setpoints(); + void generate_pos_xy_setpoints(); + void generate_pos_z_setpoints(); + void generate_vel_setpoints(); + void generate_acc_setpoints(); + void generate_yaw_setpoint(); + + void check_state_transitions(); + PRECLAND_STATE _precland_state; uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; @@ -106,6 +115,4 @@ private: matrix::Vector3f _initial_position; orb_advert_t _mavlink_log_pub{nullptr}; - matrix::Vector3f _precision_target_ned = {NAN, NAN, NAN}; - };