From facd4f4cfe3fb10d0619f79a9b38d71ed22112af Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Thu, 6 Jan 2022 12:46:37 +0100 Subject: [PATCH] convert precland to flight task --- msg/CMakeLists.txt | 1 + msg/precision_landing_status.msg | 10 + .../flight_mode_manager/CMakeLists.txt | 1 + .../flight_mode_manager/FlightModeManager.cpp | 10 + .../flight_mode_manager/FlightModeManager.hpp | 3 +- .../FlightTaskAutoPrecisionLanding.cpp | 324 ++++++------------ .../FlightTaskAutoPrecisionLanding.hpp | 83 ++--- .../AutoPrecisionLanding}/precland_params.c | 0 src/modules/logger/logged_topics.cpp | 1 + 9 files changed, 170 insertions(+), 263 deletions(-) create mode 100644 msg/precision_landing_status.msg rename src/modules/{navigator => flight_mode_manager/tasks/AutoPrecisionLanding}/precland_params.c (100%) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index f13b7fb9d7..499c359d92 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -146,6 +146,7 @@ set(msg_files PowerButtonState.msg PowerMonitor.msg PpsCapture.msg + precision_landing_status.msg PwmInput.msg Px4ioStatus.msg QshellReq.msg diff --git a/msg/precision_landing_status.msg b/msg/precision_landing_status.msg new file mode 100644 index 0000000000..927a0d4582 --- /dev/null +++ b/msg/precision_landing_status.msg @@ -0,0 +1,10 @@ +# ORBIT_YAW_BEHAVIOUR +uint8 PRECLAND_STATE_START = 0 +uint8 PRECLAND_STATE_HORIZONTAL_APPROACH = 1 +uint8 PRECLAND_STATE_DESCEND_ABOVE_TARGET = 2 +uint8 PRECLAND_STATE_FINAL_APPROACH = 3 +uint8 PRECLAND_STATE_SEARCH = 4 +uint8 PRECLAND_STATE_FALLBACK = 5 + +uint64 timestamp # time since system start (microseconds) +uint8 precland_state # Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. [m] diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index bee40bbc66..59805b49d9 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -40,6 +40,7 @@ set(flight_tasks_all) list(APPEND flight_tasks_all Auto + AutoPrecisionLanding Descend Failsafe ManualAcceleration diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 9957b4c490..7e86f47046 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -164,6 +164,16 @@ void FlightModeManager::start_flight_task() task_failure = true; } + // 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 + && _param_rtl_pld_md.get() > 0) { + should_disable_task = false; + + if (switchTask(FlightTaskIndex::AutoPrecisionLanding) != FlightTaskError::NoError) { + task_failure = true; + } + } else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) { // Emergency descend diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index db5fb0bf56..5c281ddac5 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -157,6 +157,7 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_lndmc_alt_max, - (ParamInt) _param_mpc_pos_mode + (ParamInt) _param_mpc_pos_mode, + (ParamInt) _param_rtl_pld_md ); }; diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp index 080491d37c..4683e11e19 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 PX4 Development Team. All rights reserved. + * Copyright (c) 2022 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 @@ -31,101 +31,51 @@ * ****************************************************************************/ /** - * @file precland.cpp - * - * Helper class to do precision landing with a landing target + * @file FlightTaskAutoPrecisionLanding.cpp * * @author Nicolas de Palezieux (Sunflower Labs) */ -#include "precland.h" -#include "navigator.h" +#include "FlightTaskAutoPrecisionLanding.hpp" -#include #include #include #include #include -#include -#include - -#include -#include -#include -#include - -#define SEC2USEC 1000000.0f - -#define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state - static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing"; -PrecLand::PrecLand(Navigator *navigator) : - MissionBlock(navigator), - ModuleParams(navigator) +bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint) { - _handle_param_acceleration_hor = param_find("MPC_ACC_HOR"); - _handle_param_xy_vel_cruise = param_find("MPC_XY_CRUISE"); + bool ret = FlightTask::activate(last_setpoint); - updateParams(); -} + // This looks wrong at first, but is a mean little trick to avoid discontinuous setpoints. + // When this flight task is activated, _target might be outdated until the next navigator triplet + // comes in. Until then pretend that the current position setpoint is the navigator's setpoint: + _target = _position_setpoint; -void -PrecLand::on_activation() -{ - _state = PrecLandState::Start; _search_cnt = 0; _last_slewrate_time = 0; - vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); - - if (!_map_ref.isInitialized()) { - _map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon); - } - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - pos_sp_triplet->next.valid = false; - pos_sp_triplet->previous.valid = false; - - // Check that the current position setpoint is valid, otherwise land at current position - if (!pos_sp_triplet->current.valid) { - PX4_WARN("Reset"); - pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; - pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.timestamp = hrt_absolute_time(); - } - - _sp_pev = matrix::Vector2f(0, 0); - _sp_pev_prev = matrix::Vector2f(0, 0); - _last_slewrate_time = 0; + _sp_pev = matrix::Vector2f(last_setpoint.x, last_setpoint.y); + _sp_pev_prev = matrix::Vector2f(last_setpoint.x, last_setpoint.y); switch_to_state_start(); - _is_activated = true; + return ret; } -void -PrecLand::on_active() +bool FlightTaskAutoPrecisionLanding::update() { - // get new target measurement - _target_pose_updated = _target_pose_sub.update(&_target_pose); + bool ret = FlightTaskAuto::update(); - if (_target_pose_updated) { - _target_pose_valid = true; + // Fetch uorb + if (_target_pose_sub.updated()) { + _target_pose_sub.copy(&_target_pose); } - if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) > _param_pld_btout.get()) { - _target_pose_valid = false; - } - - // stop if we are landed - if (_navigator->get_land_detected()->landed) { - switch_to_state_done(); - } + // target pose can become invalid when the message timed out + _target_pose_valid = (hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get(); switch (_state) { case PrecLandState::Start: @@ -152,88 +102,66 @@ PrecLand::on_active() run_state_fallback(); break; - case PrecLandState::Done: - // nothing to do - 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(); + + return ret; } void -PrecLand::on_inactivation() +FlightTaskAutoPrecisionLanding::run_state_start() { - _is_activated = false; -} + _position_setpoint = _target; // Follow navigator triplet -void -PrecLand::updateParams() -{ - ModuleParams::updateParams(); - - if (_handle_param_acceleration_hor != PARAM_INVALID) { - param_get(_handle_param_acceleration_hor, &_param_acceleration_hor); - } - - if (_handle_param_xy_vel_cruise != PARAM_INVALID) { - param_get(_handle_param_xy_vel_cruise, &_param_xy_vel_cruise); - } -} - -void -PrecLand::run_state_start() -{ - // check if target visible and go to horizontal approach + // check if target visible and go to horizontal approach directly if (switch_to_state_horizontal_approach()) { return; - } - if (_mode == PrecLandMode::Opportunistic) { + } else if ((PrecLandMode)_param_rtl_pld_md.get() == PrecLandMode::Opportunistic) { + // could not see the target immediately, so just fall back to normal landing switch_to_state_fallback(); - } - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - float dist = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + } else if (_type == WaypointType::land) { - // check if we've reached the start point - if (dist < _navigator->get_acceptance_radius()) { - if (!_point_reached_time) { - _point_reached_time = hrt_absolute_time(); - } - - // if we don't see the target after 1 second, search for it - if (_param_pld_srch_tout.get() > 0) { - - if (hrt_absolute_time() - _point_reached_time > 2000000) { - if (!switch_to_state_search()) { - switch_to_state_fallback(); - } - } - - } else { - switch_to_state_fallback(); - } + // Navigator already entered land stage. Take over with precision landing + switch_to_state_search(); } } void -PrecLand::run_state_horizontal_approach() +FlightTaskAutoPrecisionLanding::run_state_horizontal_approach() { - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + float x = _target_pose.x_abs; + float y = _target_pose.y_abs; + slewrate(x, y); + + // 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) = _velocity_setpoint(2) = NAN; // check if target visible, if not go to start if (!check_state_conditions(PrecLandState::HorizontalApproach)) { PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state); // Stay at current position for searching for the landing target - pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; - pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; + // 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 (!switch_to_state_start()) { switch_to_state_fallback(); @@ -256,25 +184,18 @@ PrecLand::run_state_horizontal_approach() } } - - float x = _target_pose.x_abs; - float y = _target_pose.y_abs; - - slewrate(x, y); - - // XXX need to transform to GPS coords because mc_pos_control only looks at that - _map_ref.reproject(x, y, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon); - - pos_sp_triplet->current.alt = _approach_alt; - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - - _navigator->set_position_setpoint_triplet_updated(); } void -PrecLand::run_state_descend_above_target() +FlightTaskAutoPrecisionLanding::run_state_descend_above_target() { - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + // Overwrite Auto setpoints in order to descend above target + _position_setpoint(0) = _target_pose.x_abs; + _position_setpoint(1) = _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)) { @@ -282,9 +203,7 @@ PrecLand::run_state_descend_above_target() PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state); // Stay at current position for searching for the target - pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; - pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; + _position_setpoint = _position; if (!switch_to_state_start()) { switch_to_state_fallback(); @@ -293,33 +212,35 @@ PrecLand::run_state_descend_above_target() return; } - - // XXX need to transform to GPS coords because mc_pos_control only looks at that - _map_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon); - - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; - - _navigator->set_position_setpoint_triplet_updated(); } void -PrecLand::run_state_final_approach() +FlightTaskAutoPrecisionLanding::run_state_final_approach() { - // nothing to do, will land + // Overwrite Auto setpoints in order to land at target's last known location + _position_setpoint(0) = _target_pose.x_abs; + _position_setpoint(1) = _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 -PrecLand::run_state_search() +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::HorizontalApproach)) { 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_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - float new_alt = _navigator->get_global_position()->alt + 1.0f; - pos_sp_triplet->current.alt = new_alt < pos_sp_triplet->current.alt ? new_alt : pos_sp_triplet->current.alt; - _navigator->set_position_setpoint_triplet_updated(); + _position_setpoint = _position; + _position_setpoint(2) += 1.0f; } } @@ -341,18 +262,19 @@ PrecLand::run_state_search() } void -PrecLand::run_state_fallback() +FlightTaskAutoPrecisionLanding::run_state_fallback() { - // nothing to do, will land + // nothing to do, just listen to navigator + _position_setpoint = _target; + _velocity_setpoint(0) = 0; + _velocity_setpoint(1) = 0; + _velocity_setpoint(2) = _param_mpc_land_speed.get(); } bool -PrecLand::switch_to_state_start() +FlightTaskAutoPrecisionLanding::switch_to_state_start() { if (check_state_conditions(PrecLandState::Start)) { - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - _navigator->set_position_setpoint_triplet_updated(); _search_cnt++; _point_reached_time = 0; @@ -366,11 +288,10 @@ PrecLand::switch_to_state_start() } bool -PrecLand::switch_to_state_horizontal_approach() +FlightTaskAutoPrecisionLanding::switch_to_state_horizontal_approach() { if (check_state_conditions(PrecLandState::HorizontalApproach)) { print_state_switch_message("horizontal approach"); - _approach_alt = _navigator->get_global_position()->alt; _point_reached_time = 0; @@ -383,7 +304,7 @@ PrecLand::switch_to_state_horizontal_approach() } bool -PrecLand::switch_to_state_descend_above_target() +FlightTaskAutoPrecisionLanding::switch_to_state_descend_above_target() { if (check_state_conditions(PrecLandState::DescendAboveTarget)) { print_state_switch_message("descend"); @@ -396,7 +317,7 @@ PrecLand::switch_to_state_descend_above_target() } bool -PrecLand::switch_to_state_final_approach() +FlightTaskAutoPrecisionLanding::switch_to_state_final_approach() { if (check_state_conditions(PrecLandState::FinalApproach)) { print_state_switch_message("final approach"); @@ -408,57 +329,33 @@ PrecLand::switch_to_state_final_approach() return false; } -bool -PrecLand::switch_to_state_search() +void +FlightTaskAutoPrecisionLanding::switch_to_state_search() { - PX4_INFO("Climbing to search altitude."); - vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->current.alt = vehicle_local_position->ref_alt + _param_pld_srch_alt.get(); - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - _navigator->set_position_setpoint_triplet_updated(); + PX4_INFO("Climbing to search altitude"); _target_acquired_time = 0; _state = PrecLandState::Search; _state_start_time = hrt_absolute_time(); - return true; } -bool -PrecLand::switch_to_state_fallback() +void +FlightTaskAutoPrecisionLanding::switch_to_state_fallback() { print_state_switch_message("fallback"); - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; - pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; - pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; - _navigator->set_position_setpoint_triplet_updated(); _state = PrecLandState::Fallback; _state_start_time = hrt_absolute_time(); - return true; } -bool -PrecLand::switch_to_state_done() -{ - _state = PrecLandState::Done; - _state_start_time = hrt_absolute_time(); - return true; -} - -void PrecLand::print_state_switch_message(const char *state_name) +void FlightTaskAutoPrecisionLanding::print_state_switch_message(const char *state_name) { PX4_INFO("Precland: switching to %s", state_name); } -bool PrecLand::check_state_conditions(PrecLandState state) +bool FlightTaskAutoPrecisionLanding::check_state_conditions(PrecLandState state) { - vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); - switch (state) { case PrecLandState::Start: return _search_cnt <= _param_pld_max_srch.get(); @@ -467,8 +364,7 @@ bool PrecLand::check_state_conditions(PrecLandState state) // 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::HorizontalApproach) { - if (fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get() - && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get()) { + if (Vector2f(Vector2f(_target_pose.x_abs, _target_pose.y_abs) - _position.xy()).norm() <= _param_pld_hacc_rad.get()) { // we've reached the position where we last saw the target. If we don't see it now, we need to do something return _target_pose_valid && _target_pose.abs_pos_valid; @@ -480,15 +376,15 @@ bool PrecLand::check_state_conditions(PrecLandState state) } // If we're trying to switch to this state, the target needs to be visible - return _target_pose_updated && _target_pose_valid && _target_pose.abs_pos_valid; + return _target_pose_valid && _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 horizontall offset to target + // 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::FinalApproach)) { - return hrt_absolute_time() - _target_pose.timestamp < 500000; // 0.5s + return hrt_absolute_time() - _target_pose.timestamp < 500000; // 0.5s // TODO: Magic number! } else { return _target_pose_valid && _target_pose.abs_pos_valid; @@ -496,14 +392,14 @@ bool PrecLand::check_state_conditions(PrecLandState state) } else { // if not already in this state, need to be above target to enter it - return _target_pose_updated && _target_pose.abs_pos_valid - && fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get() - && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get(); + return _target_pose.abs_pos_valid + && fabsf(_target_pose.x_abs - _position(0)) < _param_pld_hacc_rad.get() + && fabsf(_target_pose.y_abs - _position(1)) < _param_pld_hacc_rad.get(); } case PrecLandState::FinalApproach: return _target_pose_valid && _target_pose.abs_pos_valid - && (_target_pose.z_abs - vehicle_local_position->z) < _param_pld_fappr_alt.get(); + && (_target_pose.z_abs - _position(2)) < _param_pld_fappr_alt.get(); case PrecLandState::Search: return true; @@ -516,7 +412,7 @@ bool PrecLand::check_state_conditions(PrecLandState state) } } -void PrecLand::slewrate(float &sp_x, float &sp_y) +void FlightTaskAutoPrecisionLanding::slewrate(float &sp_x, float &sp_y) { matrix::Vector2f sp_curr(sp_x, sp_y); uint64_t now = hrt_absolute_time(); @@ -537,10 +433,8 @@ void PrecLand::slewrate(float &sp_x, float &sp_y) dt = 50000 / SEC2USEC; // set a best guess for previous setpoints for smooth transition - _sp_pev = _map_ref.project(_navigator->get_position_setpoint_triplet()->current.lat, - _navigator->get_position_setpoint_triplet()->current.lon); - _sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt; - _sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt; + _sp_pev_prev(0) = _sp_pev(0) - _velocity(0) * dt; + _sp_pev_prev(1) = _sp_pev(1) - _velocity(1) * dt; } _last_slewrate_time = now; @@ -548,21 +442,21 @@ void PrecLand::slewrate(float &sp_x, float &sp_y) // 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) { - sp_vel = sp_vel.normalized() * _param_xy_vel_cruise; + 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) { - sp_acc = sp_acc.normalized() * _param_acceleration_hor; + 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 * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x, + 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 diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp index df07ff7bd5..d2a320923d 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 PX4 Development Team. All rights reserved. + * Copyright (c) 2022 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,24 +30,32 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + /** - * @file precland.h + * @file FlightTaskAutoPrecisionLanding.hpp * - * Helper class to do precision landing with a landing target + * 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 "navigator_mode.h" -#include "mission_block.h" +#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 { Start, // Starting state @@ -55,8 +63,7 @@ enum class PrecLandState { DescendAboveTarget, // Stay over landing target while descending FinalApproach, // Final landing approach, even without landing target Search, // Search for landing target - Fallback, // Fallback landing method - Done // Done landing + Fallback // Fallback landing method }; enum class PrecLandMode { @@ -64,26 +71,17 @@ enum class PrecLandMode { Required = 2 // try to find landing target if not visible at the beginning }; -class PrecLand : public MissionBlock, public ModuleParams +class FlightTaskAutoPrecisionLanding : public FlightTaskAuto { public: - PrecLand(Navigator *navigator); - ~PrecLand() override = default; + FlightTaskAutoPrecisionLanding() = default; + virtual ~FlightTaskAutoPrecisionLanding() = default; - void on_activation() override; - void on_active() override; - void on_inactivation() override; + bool activate(const trajectory_setpoint_s &last_setpoint) override; - void set_mode(PrecLandMode mode) { _mode = mode; }; - - PrecLandMode get_mode() { return _mode; }; - - bool is_activated() { return _is_activated; }; + bool update() override; private: - - void updateParams() override; - // run the control loop for each state void run_state_start(); void run_state_horizontal_approach(); @@ -97,9 +95,8 @@ private: bool switch_to_state_horizontal_approach(); bool switch_to_state_descend_above_target(); bool switch_to_state_final_approach(); - bool switch_to_state_search(); - bool switch_to_state_fallback(); - bool switch_to_state_done(); + void switch_to_state_search(); + void switch_to_state_fallback(); void print_state_switch_message(const char *state_name); @@ -110,10 +107,9 @@ private: landing_target_pose_s _target_pose{}; /**< precision landing target position */ uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)}; - bool _target_pose_valid{false}; /**< whether we have received a landing target position message */ - bool _target_pose_updated{false}; /**< wether the landing target position message is updated */ + uORB::PublicationMulti _precision_landing_status_pub{ORB_ID(precision_landing_status)}; - MapProjection _map_ref{}; /**< class for local/global projections */ + bool _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 */ @@ -121,30 +117,23 @@ private: 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 */ - float _approach_alt{0.0f}; /**< altitude at which to stay during horizontal approach */ matrix::Vector2f _sp_pev; matrix::Vector2f _sp_pev_prev; PrecLandState _state{PrecLandState::Start}; - PrecLandMode _mode{PrecLandMode::Opportunistic}; - - bool _is_activated {false}; /**< indicates if precland is activated */ - - DEFINE_PARAMETERS( - (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 - ) - - // non-navigator parameters - param_t _handle_param_acceleration_hor{PARAM_INVALID}; - param_t _handle_param_xy_vel_cruise{PARAM_INVALID}; - float _param_acceleration_hor{0.0f}; - float _param_xy_vel_cruise{0.0f}; + 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 + ) }; diff --git a/src/modules/navigator/precland_params.c b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/precland_params.c similarity index 100% rename from src/modules/navigator/precland_params.c rename to src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/precland_params.c diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 0545057d8a..ef82e16546 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -96,6 +96,7 @@ void LoggedTopics::add_default_topics() add_topic("position_controller_status", 500); add_topic("position_controller_landing_status", 100); add_topic("position_setpoint_triplet", 200); + add_topic("precision_landing_status", 1000); add_optional_topic("px4io_status"); add_topic("radio_status"); add_topic("rtl_time_estimate", 1000);