From a5663d4d3c4bea7271675b70f8fc6891cab688c8 Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 16 Nov 2022 13:08:20 +0100 Subject: [PATCH] Made a separate precision landing library. Now, the Precision Landing mode can use it the same as the RTL and the mission mode. --- src/modules/navigator/CMakeLists.txt | 1 + src/modules/navigator/mission.cpp | 45 +++-- src/modules/navigator/mission.h | 3 + src/modules/navigator/navigator.h | 6 +- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/precland.cpp | 218 ++++++++++------------- src/modules/navigator/precland.h | 80 ++++++--- src/modules/navigator/precland_mode.cpp | 101 +++++++++++ src/modules/navigator/precland_mode.h | 69 +++++++ src/modules/navigator/rtl.cpp | 54 ++++-- src/modules/navigator/rtl.h | 5 + 11 files changed, 400 insertions(+), 184 deletions(-) create mode 100644 src/modules/navigator/precland_mode.cpp create mode 100644 src/modules/navigator/precland_mode.h diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index ce5abd72bd..6e465119f9 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -46,6 +46,7 @@ px4_add_module( takeoff.cpp land.cpp precland.cpp + precland_mode.cpp mission_feasibility_checker.cpp geofence.cpp vtol_takeoff.cpp diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 04f02358f7..c31c9ded4e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -176,10 +176,6 @@ Mission::on_inactivation() _navigator->stop_capturing_images(); _navigator->release_gimbal_control(); - if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } - _time_mission_deactivated = hrt_absolute_time(); /* reset so current mission item gets restarted if mission was paused */ @@ -298,10 +294,25 @@ Mission::on_active() } if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) { - _navigator->get_precland()->on_active(); + // Update the position in the setpoint triplet. + _precland.setAcceptanceRadius(_navigator->get_acceptance_radius()); + _precland.update(); + PrecLand::Output prec_land_output{_precland.getOutput()}; - } else if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); + _mission_item.altitude = prec_land_output.alt; + _mission_item.lat = prec_land_output.pos_hor.lat; + _mission_item.lon = prec_land_output.pos_hor.lon; + _mission_item.nav_cmd = prec_land_output.nav_cmd; + + mission_apply_limitation(_mission_item); + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->next.valid = false; + + if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { + _navigator->set_position_setpoint_triplet_updated(); + } } } @@ -983,13 +994,17 @@ Mission::set_mission_items() new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + _precland.setMode(PrecLand::PrecLandMode::Opportunistic); } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); + _precland.setMode(PrecLand::PrecLandMode::Required); } - _navigator->get_precland()->on_activation(); + PrecLand::LandingPosition2D approximate_landing_pos{ + .lat = _mission_item.lat, + .lon = _mission_item.lon, + }; + _precland.initialize(approximate_landing_pos); } } @@ -1002,13 +1017,17 @@ Mission::set_mission_items() new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + _precland.setMode(PrecLand::PrecLandMode::Opportunistic); } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); + _precland.setMode(PrecLand::PrecLandMode::Required); } - _navigator->get_precland()->on_activation(); + PrecLand::LandingPosition2D approximate_landing_pos{ + .lat = _mission_item.lat, + .lon = _mission_item.lon, + }; + _precland.initialize(approximate_landing_pos); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 1215dbe8c6..4dd0c65522 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -49,6 +49,7 @@ #include "mission_block.h" #include "mission_feasibility_checker.h" #include "navigator_mode.h" +#include "precland.h" #include @@ -270,6 +271,8 @@ private: hrt_abstime _time_mission_deactivated{0}; + PrecLand _precland; + enum { MISSION_TYPE_NONE, MISSION_TYPE_MISSION diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 004f8e1f4a..da4d97e279 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -43,7 +43,7 @@ #include "geofence.h" #include "land.h" -#include "precland.h" +#include "precland_mode.h" #include "loiter.h" #include "mission.h" #include "navigator_mode.h" @@ -172,8 +172,6 @@ public: vehicle_local_position_s *get_local_position() { return &_local_pos; } vehicle_status_s *get_vstatus() { return &_vstatus; } - PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */ - const vehicle_roi_s &get_vroi() { return _vroi; } void reset_vroi() { _vroi = {}; } @@ -391,7 +389,7 @@ private: Takeoff _takeoff; /**< class for handling takeoff commands */ VtolTakeoff _vtol_takeoff; /**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */ Land _land; /**< class for handling land commands */ - PrecLand _precland; /**< class for handling precision land commands */ + PrecLandMode _precland; /**< class for handling precision land commands */ RTL _rtl; /**< class that handles RTL */ NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3a561083cf..c9778095a7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -706,7 +706,7 @@ void Navigator::run() case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_precland; - _precland.set_mode(PrecLandMode::Required); + _precland.set_mode(PrecLand::PrecLandMode::Required); break; case vehicle_status_s::NAVIGATION_STATE_MANUAL: diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 080491d37c..de1343123d 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -39,31 +39,18 @@ */ #include "precland.h" -#include "navigator.h" -#include -#include -#include -#include -#include +#include -#include -#include - -#include -#include -#include -#include +using namespace time_literals; #define SEC2USEC 1000000.0f -#define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state +static constexpr hrt_abstime state_timeout{10_s}; // 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) +PrecLand::PrecLand() : ModuleParams(nullptr) { _handle_param_acceleration_hor = param_find("MPC_ACC_HOR"); _handle_param_xy_vel_cruise = param_find("MPC_XY_CRUISE"); @@ -72,31 +59,17 @@ PrecLand::PrecLand(Navigator *navigator) : } void -PrecLand::on_activation() +PrecLand::initialize(const LandingPosition2D &approximate_landing_pos) { + _approximate_landing_pos = approximate_landing_pos; + _local_pos_sub.update(); _state = PrecLandState::Start; _search_cnt = 0; _last_slewrate_time = 0; - - vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); + _point_reached_time = 0u; 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(); + _map_ref.initReference(_local_pos_sub.get().ref_lat, _local_pos_sub.get().ref_lon); } _sp_pev = matrix::Vector2f(0, 0); @@ -104,26 +77,27 @@ PrecLand::on_activation() _last_slewrate_time = 0; switch_to_state_start(); - - _is_activated = true; } void -PrecLand::on_active() +PrecLand::update() { - // get new target measurement - _target_pose_updated = _target_pose_sub.update(&_target_pose); + // get new input measurement + _target_pose_updated = _target_pose_sub.update(); + _land_detected_sub.update(); + _global_pos_sub.update(); + _local_pos_sub.update(); if (_target_pose_updated) { _target_pose_valid = true; } - if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) > _param_pld_btout.get()) { + if ((hrt_elapsed_time(&_target_pose_sub.get().timestamp) / 1e6f) > _param_pld_btout.get()) { _target_pose_valid = false; } // stop if we are landed - if (_navigator->get_land_detected()->landed) { + if (_land_detected_sub.get().landed) { switch_to_state_done(); } @@ -153,20 +127,13 @@ PrecLand::on_active() break; case PrecLandState::Done: - // nothing to do + run_state_done(); break; default: // unknown state break; } - -} - -void -PrecLand::on_inactivation() -{ - _is_activated = false; } void @@ -186,6 +153,11 @@ PrecLand::updateParams() void PrecLand::run_state_start() { + _output.nav_cmd = NAV_CMD_WAYPOINT; + _output.pos_hor.lat = _approximate_landing_pos.lat; + _output.pos_hor.lon = _approximate_landing_pos.lon; + _output.alt = _global_pos_sub.get().alt; + // check if target visible and go to horizontal approach if (switch_to_state_horizontal_approach()) { return; @@ -196,20 +168,19 @@ PrecLand::run_state_start() 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); + float dist = get_distance_to_next_waypoint(_approximate_landing_pos.lat, _approximate_landing_pos.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); // check if we've reached the start point - if (dist < _navigator->get_acceptance_radius()) { + if (dist < _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 (_param_pld_srch_tout.get() > FLT_EPSILON) { - if (hrt_absolute_time() - _point_reached_time > 2000000) { + if (hrt_absolute_time() - _point_reached_time > 1_s) { if (!switch_to_state_search()) { switch_to_state_fallback(); } @@ -224,16 +195,17 @@ PrecLand::run_state_start() void PrecLand::run_state_horizontal_approach() { - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + _output.nav_cmd = NAV_CMD_WAYPOINT; + _output.pos_hor.lat = _approximate_landing_pos.lat; + _output.pos_hor.lon = _approximate_landing_pos.lon; + _output.alt = _approach_alt; // 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; + // Stop at current altitude + _output.alt = _global_pos_sub.get().alt; if (!switch_to_state_start()) { switch_to_state_fallback(); @@ -247,44 +219,41 @@ PrecLand::run_state_horizontal_approach() _point_reached_time = hrt_absolute_time(); } - if (hrt_absolute_time() - _point_reached_time > 2000000) { + if (hrt_absolute_time() - _point_reached_time > 2_s) { // if close enough for descent above target go to descend above target if (switch_to_state_descend_above_target()) { return; } } - } - float x = _target_pose.x_abs; - float y = _target_pose.y_abs; + float x = _target_pose_sub.get().x_abs; + float y = _target_pose_sub.get().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(); + _map_ref.reproject(x, y, _output.pos_hor.lat, _output.pos_hor.lon); + _approximate_landing_pos.lat = _output.pos_hor.lat; + _approximate_landing_pos.lon = _output.pos_hor.lon; } void PrecLand::run_state_descend_above_target() { - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + _output.nav_cmd = NAV_CMD_LAND; + _output.pos_hor.lat = _approximate_landing_pos.lat; + _output.pos_hor.lon = _approximate_landing_pos.lon; + _output.alt = _global_pos_sub.get().alt; // check if target visible if (!check_state_conditions(PrecLandState::DescendAboveTarget)) { if (!switch_to_state_final_approach()) { 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; + // Stay at current altitude for searching for the target + _output.nav_cmd = NAV_CMD_WAYPOINT; if (!switch_to_state_start()) { switch_to_state_fallback(); @@ -295,37 +264,44 @@ PrecLand::run_state_descend_above_target() } // 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(); + _map_ref.reproject(_target_pose_sub.get().x_abs, _target_pose_sub.get().y_abs, _output.pos_hor.lat, + _output.pos_hor.lon); + _approximate_landing_pos.lat = _output.pos_hor.lat; + _approximate_landing_pos.lon = _output.pos_hor.lon; } void PrecLand::run_state_final_approach() { - // nothing to do, will land + _output.nav_cmd = NAV_CMD_LAND; + _output.pos_hor.lat = _approximate_landing_pos.lat; + _output.pos_hor.lon = _approximate_landing_pos.lon; + _output.alt = _global_pos_sub.get().alt; } void PrecLand::run_state_search() { + _output.nav_cmd = NAV_CMD_WAYPOINT; + _output.pos_hor.lat = _approximate_landing_pos.lat; + _output.pos_hor.lon = _approximate_landing_pos.lon; + // 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 just became visible. Stop climbing, but give it some margin so we don't stop too abruptly _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(); + float new_alt = _global_pos_sub.get().alt + 1.0f; + _output.alt = new_alt < _output.alt ? new_alt : _output.alt; } + } else { + _target_acquired_time = 0u; + _output.alt = _local_pos_sub.get().ref_alt + _param_pld_srch_alt.get(); } // stay at that height for a second to allow the vehicle to settle - if (_target_acquired_time && (hrt_absolute_time() - _target_acquired_time) > 1000000) { + if (_target_acquired_time && (hrt_absolute_time() - _target_acquired_time) > 1_s) { // try to switch to horizontal approach if (switch_to_state_horizontal_approach()) { return; @@ -333,7 +309,7 @@ PrecLand::run_state_search() } // check if search timed out and go to fallback - if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get()*SEC2USEC) { + if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get() * 1_s) { PX4_WARN("Search timed out"); switch_to_state_fallback(); @@ -343,16 +319,24 @@ PrecLand::run_state_search() void PrecLand::run_state_fallback() { - // nothing to do, will land + _output.nav_cmd = NAV_CMD_LAND; + _output.pos_hor.lat = _approximate_landing_pos.lat; + _output.pos_hor.lon = _approximate_landing_pos.lon; +} + +void +PrecLand::run_state_done() +{ + _output.nav_cmd = NAV_CMD_IDLE; + _output.pos_hor.lat = _global_pos_sub.get().lat; + _output.pos_hor.lon = _global_pos_sub.get().lon; + _output.alt = _global_pos_sub.get().alt; } bool PrecLand::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; @@ -370,7 +354,7 @@ PrecLand::switch_to_state_horizontal_approach() { if (check_state_conditions(PrecLandState::HorizontalApproach)) { print_state_switch_message("horizontal approach"); - _approach_alt = _navigator->get_global_position()->alt; + _approach_alt = _global_pos_sub.get().alt; _point_reached_time = 0; @@ -412,12 +396,6 @@ bool PrecLand::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(); _target_acquired_time = 0; @@ -430,12 +408,6 @@ bool PrecLand::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(); @@ -457,8 +429,6 @@ void PrecLand::print_state_switch_message(const char *state_name) bool PrecLand::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,10 +437,10 @@ 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 (fabsf(_target_pose_sub.get().x_abs - _local_pos_sub.get().x) < _param_pld_hacc_rad.get() + && fabsf(_target_pose_sub.get().y_abs - _local_pos_sub.get().y) < _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; + return _target_pose_valid && _target_pose_sub.get().abs_pos_valid; } else { // We've seen the target sometime during horizontal approach. @@ -480,7 +450,7 @@ 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_updated && _target_pose_valid && _target_pose_sub.get().abs_pos_valid; case PrecLandState::DescendAboveTarget: @@ -488,22 +458,22 @@ bool PrecLand::check_state_conditions(PrecLandState state) 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_sub.get().timestamp < 500_ms; // 0.5s } else { - return _target_pose_valid && _target_pose.abs_pos_valid; + return _target_pose_valid && _target_pose_sub.get().abs_pos_valid; } } 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_updated && _target_pose_sub.get().abs_pos_valid + && fabsf(_target_pose_sub.get().x_abs - _local_pos_sub.get().x) < _param_pld_hacc_rad.get() + && fabsf(_target_pose_sub.get().y_abs - _local_pos_sub.get().y) < _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(); + return _target_pose_valid && _target_pose_sub.get().abs_pos_valid + && (_target_pose_sub.get().z_abs - _local_pos_sub.get().z) < _param_pld_fappr_alt.get(); case PrecLandState::Search: return true; @@ -537,10 +507,10 @@ 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 = _map_ref.project(_output.pos_hor.lat, + _output.pos_hor.lon); + _sp_pev_prev(0) = _sp_pev(0) - _local_pos_sub.get().vx * dt; + _sp_pev_prev(1) = _sp_pev(1) - _local_pos_sub.get().vy * dt; } _last_slewrate_time = now; diff --git a/src/modules/navigator/precland.h b/src/modules/navigator/precland.h index df07ff7bd5..5dcad8ab6a 100644 --- a/src/modules/navigator/precland.h +++ b/src/modules/navigator/precland.h @@ -43,45 +43,61 @@ #include #include #include +#include #include #include +#include +#include +#include -#include "navigator_mode.h" -#include "mission_block.h" +#include "navigator/navigation.h" -enum class PrecLandState { - Start, // Starting state - HorizontalApproach, // Positioning over landing target while maintaining altitude - 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 -}; - -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 -}; - -class PrecLand : public MissionBlock, public ModuleParams +class PrecLand : public ModuleParams { public: - PrecLand(Navigator *navigator); - ~PrecLand() override = default; + 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 + }; - void on_activation() override; - void on_active() override; - void on_inactivation() override; + struct LandingPosition2D { + double lat; + double lon; + }; - void set_mode(PrecLandMode mode) { _mode = mode; }; + struct Output { + LandingPosition2D pos_hor; + float alt; + enum NAV_CMD nav_cmd; + }; - PrecLandMode get_mode() { return _mode; }; +public: + PrecLand(); + ~PrecLand() = default; - bool is_activated() { return _is_activated; }; + void initialize(const LandingPosition2D &approximate_landing_pos); + void update(); + + Output getOutput() {return _output;}; + + void setMode(PrecLandMode mode) { _mode = mode; }; + + void setAcceptanceRadius(float acceptance_radius) {_acceptance_radius = acceptance_radius;}; + + PrecLandMode getMode() { return _mode; }; private: + enum class PrecLandState { + Start, // Starting state + HorizontalApproach, // Positioning over landing target while maintaining altitude + 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 + }; +private: void updateParams() override; // run the control loop for each state @@ -91,6 +107,7 @@ private: void run_state_final_approach(); void run_state_search(); void run_state_fallback(); + void run_state_done(); // attempt to switch to a different state. Returns true if state change was successful, false otherwise bool switch_to_state_start(); @@ -107,9 +124,14 @@ private: bool check_state_conditions(PrecLandState state); void slewrate(float &sp_x, float &sp_y); - landing_target_pose_s _target_pose{}; /**< precision landing target position */ + LandingPosition2D _approximate_landing_pos{}; /**< Global position in WGS84 at which to find the landing target.*/ + float _acceptance_radius{}; + + uORB::SubscriptionData _target_pose_sub{ORB_ID(landing_target_pose)}; + uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::SubscriptionData _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */ - 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 */ @@ -130,7 +152,7 @@ private: PrecLandMode _mode{PrecLandMode::Opportunistic}; - bool _is_activated {false}; /**< indicates if precland is activated */ + Output _output; DEFINE_PARAMETERS( (ParamFloat) _param_pld_btout, diff --git a/src/modules/navigator/precland_mode.cpp b/src/modules/navigator/precland_mode.cpp new file mode 100644 index 0000000000..48c0c56451 --- /dev/null +++ b/src/modules/navigator/precland_mode.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file precland_mode.cpp + * + * Helper class to do precision landing with a landing target + * + * @author Nicolas de Palezieux (Sunflower Labs) + */ + +#include "precland_mode.h" +#include "navigator.h" + +#include + +PrecLandMode::PrecLandMode(Navigator *navigator) : + MissionBlock(navigator) +{ + +} + +void +PrecLandMode::on_activation() +{ + _global_pos_sub.update(); + // Get the landing position from current position_setpoint, else use the current vehicle position. + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + PrecLand::LandingPosition2D approximate_landing_pos{ .lat = pos_sp_triplet->current.lat, + .lon = pos_sp_triplet->current.lon}; + + if (!pos_sp_triplet->current.valid) { + PX4_WARN("No valid landing position for precision landing. Using current position"); + approximate_landing_pos.lat = _global_pos_sub.get().lat; + approximate_landing_pos.lon = _global_pos_sub.get().lon; + } + + _prec_land.initialize(approximate_landing_pos); +} + +void +PrecLandMode::on_active() +{ + _local_pos_sub.update(); + + _prec_land.setAcceptanceRadius(_navigator->get_acceptance_radius()); + _prec_land.update(); + + PrecLand::Output prec_land_output{_prec_land.getOutput()}; + + _mission_item.nav_cmd = prec_land_output.nav_cmd; + _mission_item.lat = prec_land_output.pos_hor.lat; + _mission_item.lon = prec_land_output.pos_hor.lon; + _mission_item.altitude = prec_land_output.alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = _local_pos_sub.get().heading; + + + mission_apply_limitation(_mission_item); + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->next.valid = false; + + if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { + _navigator->set_position_setpoint_triplet_updated(); + } + +} + + diff --git a/src/modules/navigator/precland_mode.h b/src/modules/navigator/precland_mode.h new file mode 100644 index 0000000000..99f83b0df8 --- /dev/null +++ b/src/modules/navigator/precland_mode.h @@ -0,0 +1,69 @@ +/*************************************************************************** + * + * Copyright (c) 2017 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file preclandMode.h + * + * Helper class to do precision landing with a landing target + * + * @author Nicolas de Palezieux (Sunflower Labs) + */ + +#pragma once + +#include "mission_block.h" +#include "precland.h" + +#include +#include +#include + +class PrecLandMode : public MissionBlock +{ +public: + PrecLandMode(Navigator *navigator); + ~PrecLandMode() override = default; + + void on_activation() override; + void on_active() override; + + void set_mode(PrecLand::PrecLandMode mode) { _prec_land.setMode(mode); }; + + PrecLand::PrecLandMode get_mode() { return _prec_land.getMode(); }; +private: + PrecLand _prec_land; + + uORB::SubscriptionData _local_pos_sub{ORB_ID(vehicle_local_position)}; + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; +}; + + diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index d4de6c18dd..e6427fef6b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -69,9 +69,7 @@ RTL::RTL(Navigator *navigator) : void RTL::on_inactivation() { - if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } + } void RTL::on_inactive() @@ -303,10 +301,8 @@ void RTL::on_active() } 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(); + // Need to update the position and type on the current setpoint triplet. + set_prec_land_item(); } // Limit rtl time calculation to 1Hz @@ -566,13 +562,22 @@ void RTL::set_rtl_item() _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(); + if (_mission_item.land_precision > 0u && _mission_item.land_precision <= 2u) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - } else if (_mission_item.land_precision == 2) { - _navigator->get_precland()->set_mode(PrecLandMode::Required); - _navigator->get_precland()->on_activation(); + if (_mission_item.land_precision == 1) { + _precland.setMode(PrecLand::PrecLandMode::Opportunistic); + + } else if (_mission_item.land_precision == 2) { + _precland.setMode(PrecLand::PrecLandMode::Required); + } + + PrecLand::LandingPosition2D approximate_landing_pos{ + .lat = _mission_item.lat, + .lon = _mission_item.lon, + }; + + _precland.initialize(approximate_landing_pos); } mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); @@ -605,6 +610,29 @@ void RTL::set_rtl_item() } } +void RTL::set_prec_land_item() +{ + _precland.setAcceptanceRadius(_navigator->get_acceptance_radius()); + _precland.update(); + PrecLand::Output prec_land_output{_precland.getOutput()}; + _mission_item.altitude = prec_land_output.alt; + _mission_item.lat = prec_land_output.pos_hor.lat; + _mission_item.lon = prec_land_output.pos_hor.lon; + _mission_item.nav_cmd = prec_land_output.nav_cmd; + + // Convert mission item to current position setpoint and make it valid. + mission_apply_limitation(_mission_item); + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->next.valid = false; + + if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { + _navigator->set_position_setpoint_triplet_updated(); + } +} + void RTL::advance_rtl() { // determines if the vehicle should loiter above land diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 7b9320a87a..297315eed0 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -45,6 +45,7 @@ #include "navigator_mode.h" #include "mission_block.h" +#include "precland.h" #include #include @@ -118,6 +119,8 @@ private: void set_rtl_item(); + void set_prec_land_item(); + void advance_rtl(); float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg); @@ -135,6 +138,8 @@ private: RTLState _rtl_state{RTL_STATE_NONE}; + PrecLand _precland; + struct RTLPosition { double lat; double lon;