From aa4c6c038d14b8e21140336fa65071dbc69a6e82 Mon Sep 17 00:00:00 2001 From: Nicolas de Palezieux Date: Sun, 14 Jan 2018 23:19:57 +0530 Subject: [PATCH] navigator : add support for precision landing --- src/modules/navigator/CMakeLists.txt | 1 + src/modules/navigator/mission.cpp | 56 ++- src/modules/navigator/mission.h | 3 +- src/modules/navigator/navigation.h | 1 + src/modules/navigator/navigator.h | 5 +- src/modules/navigator/navigator_main.cpp | 11 +- src/modules/navigator/precland.cpp | 602 +++++++++++++++++++++++ src/modules/navigator/precland.h | 135 +++++ src/modules/navigator/precland_params.c | 121 +++++ 9 files changed, 925 insertions(+), 10 deletions(-) create mode 100644 src/modules/navigator/precland.cpp create mode 100644 src/modules/navigator/precland.h create mode 100644 src/modules/navigator/precland_params.c diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 6b17219c4e..378137aa3e 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -47,6 +47,7 @@ px4_add_module( rtl.cpp takeoff.cpp land.cpp + precland.cpp mission_feasibility_checker.cpp geofence.cpp datalinkloss.cpp diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 52d6b60657..ed4e36846d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -188,7 +188,6 @@ Mission::on_active() /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { - /* If we just completed a takeoff which was inserted before the right waypoint, there is no need to report that we reached it because we didn't. */ if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) { @@ -232,6 +231,17 @@ Mission::on_active() do_abort_landing(); } + + if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) { + // switch out of precision land once landed + if (_navigator->get_land_detected()->landed) { + _navigator->get_precland()->on_inactivation(); + _work_item_type = WORK_ITEM_TYPE_DEFAULT; + + } else { + _navigator->get_precland()->on_active(); + } + } } bool @@ -501,9 +511,12 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; } - /* we have a new position item so set previous position setpoint to current */ position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->previous = pos_sp_triplet->current; + + /* we have a new position item so set previous position setpoint to current */ + if (_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) { + pos_sp_triplet->previous = pos_sp_triplet->current; + } /* do takeoff before going to setpoint if needed and not already in takeoff */ /* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */ @@ -681,14 +694,41 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 0.0f; + + } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT) { + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + + } else { //_mission_item.land_precision == 2 + _navigator->get_precland()->set_mode(PrecLandMode::Required); + } + + _navigator->get_precland()->on_activation(); + + } } /* we just moved to the landing waypoint, now descend */ if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - new_work_item_type = WORK_ITEM_TYPE_DEFAULT; - /* XXX: noop */ + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + + } else { //_mission_item.land_precision == 2 + _navigator->get_precland()->set_mode(PrecLandMode::Required); + } + + _navigator->get_precland()->on_activation(); + + } + } /* ignore yaw for landing items */ @@ -760,8 +800,10 @@ Mission::set_mission_items() position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* set current position setpoint from mission item (is protected against non-position items) */ - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) { + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } /* issue command if ready (will do nothing for position mission items) */ issue_command(_mission_item); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 966d40fa4d..becbe3be15 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -264,7 +264,8 @@ private: WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ WORK_ITEM_TYPE_CMD_BEFORE_MOVE, WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, - WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION + WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, + WORK_ITEM_TYPE_PRECISION_LAND } _work_item_type{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ }; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 4617e9ab22..b6d947b36e 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -134,6 +134,7 @@ struct mission_item_s { union { uint16_t do_jump_current_count; /**< count how many times the jump has been done */ uint16_t vertex_count; /**< Polygon vertex count (geofence) */ + uint16_t land_precision; /**< Defines if landing should be precise: 0 = normal landing, 1 = opportunistic precision landing, 2 = required precision landing (with search) */ }; struct { uint16_t frame : 4, /**< mission frame */ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ce15ca431a..63dfc9baf2 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -48,6 +48,7 @@ #include "geofence.h" #include "gpsfailure.h" #include "land.h" +#include "precland.h" #include "loiter.h" #include "mission.h" #include "navigator_mode.h" @@ -76,7 +77,7 @@ /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 10 +#define NAVIGATOR_MODE_ARRAY_SIZE 11 class Navigator : public control::SuperBlock { @@ -148,6 +149,7 @@ public: struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; } struct vehicle_local_position_s *get_local_position() { return &_local_pos; } struct 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; } @@ -318,6 +320,7 @@ private: Loiter _loiter; /**< class that handles loiter */ Takeoff _takeoff; /**< class for handling takeoff commands */ Land _land; /**< class for handling land commands */ + PrecLand _precland; /**< class for handling precision land commands */ RTL _rtl; /**< class that handles RTL */ RCLoss _rcLoss; /**< class that handles RTL according to OBC rules (rc loss mode) */ DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f1bac41a63..3aad446c6f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -91,6 +91,7 @@ Navigator::Navigator() : _loiter(this, "LOI"), _takeoff(this, "TKF"), _land(this, "LND"), + _precland(this, "PLD"), _rtl(this, "RTL"), _rcLoss(this, "RCL"), _dataLinkLoss(this, "DLL"), @@ -122,7 +123,9 @@ Navigator::Navigator() : _navigation_mode_array[6] = &_rcLoss; _navigation_mode_array[7] = &_takeoff; _navigation_mode_array[8] = &_land; - _navigation_mode_array[9] = &_follow_target; + _navigation_mode_array[9] = &_precland; + _navigation_mode_array[10] = &_follow_target; + } Navigator::~Navigator() @@ -648,6 +651,12 @@ Navigator::task_main() navigation_mode_new = &_land; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + _pos_sp_triplet_published_invalid_once = false; + navigation_mode_new = &_precland; + _precland.set_mode(PrecLandMode::Required); + break; + case vehicle_status_s::NAVIGATION_STATE_DESCEND: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_land; diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp new file mode 100644 index 0000000000..cc5be68527 --- /dev/null +++ b/src/modules/navigator/precland.cpp @@ -0,0 +1,602 @@ +/**************************************************************************** + * + * 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.cpp + * + * Helper class to do precision landing with a landing target + * + * @author Nicolas de Palezieux (Sunflower Labs) + */ + +#include "precland.h" +#include "navigator.h" + +#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 + +PrecLand::PrecLand(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _targetPoseSub(0), + _target_pose_valid(false), + _state_start_time(0), + _search_cnt(0), + _approach_alt(0), + _param_timeout(this, "PLD_BTOUT", false), + _param_hacc_rad(this, "PLD_HACC_RAD", false), + _param_final_approach_alt(this, "PLD_FAPPR_ALT", false), + _param_search_alt(this, "PLD_SRCH_ALT", false), + _param_search_timeout(this, "PLD_SRCH_TOUT", false), + _param_max_searches(this, "PLD_MAX_SRCH", false), + _param_acceleration_hor(this, "MPC_ACC_HOR", false), + _param_xy_vel_cruise(this, "MPC_XY_CRUISE", false) + +{ + /* load initial params */ + updateParams(); + +} + +PrecLand::~PrecLand() +{ +} + +void +PrecLand::on_inactive() +{ +} + +void +PrecLand::on_activation() +{ + // We need to subscribe here and not in the constructor because constructor is called before the navigator task is spawned + if (!_targetPoseSub) { + _targetPoseSub = orb_subscribe(ORB_ID(landing_target_pose)); + } + + _state = PrecLandState::Start; + _search_cnt = 0; + _last_slewrate_time = 0; + + vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); + + if (!map_projection_initialized(&_map_ref)) { + map_projection_init(&_map_ref, 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; + + // Check that the current position setpoint is valid, otherwise land at current position + if (!pos_sp_triplet->current.valid) { + PX4_WARN("Resetting landing position to current position"); + 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; + } + + _sp_pev = matrix::Vector2f(0, 0); + _sp_pev_prev = matrix::Vector2f(0, 0); + _last_slewrate_time = 0; + + switch_to_state_start(); + +} + +void +PrecLand::on_active() +{ + // get new target measurement + bool updated = false; + orb_check(_targetPoseSub, &updated); + + if (updated) { + orb_copy(ORB_ID(landing_target_pose), _targetPoseSub, &_target_pose); + _target_pose_valid = true; + } + + if (hrt_absolute_time() - _target_pose.timestamp > (uint64_t)(_param_timeout.get()*SEC2USEC)) { + _target_pose_valid = false; + } + + // stop if we are landed + if (_navigator->get_land_detected()->landed) { + switch_to_state_done(); + } + + switch (_state) { + case PrecLandState::Start: + run_state_start(); + break; + + case PrecLandState::HorizontalApproach: + run_state_horizontal_approach(); + break; + + case PrecLandState::DescendAboveTarget: + run_state_descend_above_target(); + break; + + case PrecLandState::FinalApproach: + run_state_final_approach(); + break; + + case PrecLandState::Search: + run_state_search(); + break; + + case PrecLandState::Fallback: + run_state_fallback(); + break; + + case PrecLandState::Done: + // nothing to do + break; + + default: + // unknown state + break; + } + +} + +void +PrecLand::run_state_start() +{ + // check if target visible and go to horizontal approach + if (switch_to_state_horizontal_approach()) { + return; + } + + if (_mode == PrecLandMode::Opportunistic) { + // could not see the target immediately, so just fall back to normal landing + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to search or fallback landing"); + } + } + + 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); + + // 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_search_timeout.get() > 0) { + + if (hrt_absolute_time() - _point_reached_time > 2000000) { + if (!switch_to_state_search()) { + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to search or fallback landing"); + } + } + } + + } else { + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to search or fallback landing"); + } + } + } +} + +void +PrecLand::run_state_horizontal_approach() +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // check if target visible, if not go to start + if (!check_state_conditions(PrecLandState::HorizontalApproach)) { + PX4_WARN("Lost landing target while landig (horizontal approach)."); + + // 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; + + if (!switch_to_state_start()) { + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to fallback landing"); + } + } + + 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 (switch_to_state_descend_above_target()) { + return; + } + } + + } + + if (hrt_absolute_time() - _state_start_time > STATE_TIMEOUT) { + PX4_ERR("Precision landing took too long during horizontal approach phase."); + + if (switch_to_state_fallback()) { + return; + } + + PX4_ERR("Can't switch to fallback landing"); + } + + 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 + double lat, lon; + map_projection_reproject(&_map_ref, x, y, &lat, &lon); + + pos_sp_triplet->current.lat = lat; + pos_sp_triplet->current.lon = 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() +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // check if target visible + if (!check_state_conditions(PrecLandState::DescendAboveTarget)) { + if (!switch_to_state_final_approach()) { + PX4_WARN("Lost landing target while landing (descending)."); + + // 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; + + if (!switch_to_state_start()) { + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to fallback landing"); + } + } + } + + return; + } + + // XXX need to transform to GPS coords because mc_pos_control only looks at that + double lat, lon; + map_projection_reproject(&_map_ref, _target_pose.x_abs, _target_pose.y_abs, &lat, &lon); + + pos_sp_triplet->current.lat = lat; + pos_sp_triplet->current.lon = lon; + + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; + + _navigator->set_position_setpoint_triplet_updated(); + +} + +void +PrecLand::run_state_final_approach() +{ + // nothing to do, will land +} + +void +PrecLand::run_state_search() +{ + // 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(); + } + + } + + // 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 (switch_to_state_horizontal_approach()) { + return; + } + } + + // check if search timed out and go to fallback + if (hrt_absolute_time() - _state_start_time > _param_search_timeout.get()*SEC2USEC) { + PX4_WARN("Search timed out"); + + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to fallback landing"); + } + } +} + +void +PrecLand::run_state_fallback() +{ + // nothing to do, will land +} + +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; + + _state = PrecLandState::Start; + _state_start_time = hrt_absolute_time(); + return true; + } + + return false; +} + +bool +PrecLand::switch_to_state_horizontal_approach() +{ + if (check_state_conditions(PrecLandState::HorizontalApproach)) { + _approach_alt = _navigator->get_global_position()->alt; + + _point_reached_time = 0; + + _state = PrecLandState::HorizontalApproach; + _state_start_time = hrt_absolute_time(); + return true; + } + + return false; +} + +bool +PrecLand::switch_to_state_descend_above_target() +{ + if (check_state_conditions(PrecLandState::DescendAboveTarget)) { + _state = PrecLandState::DescendAboveTarget; + _state_start_time = hrt_absolute_time(); + return true; + } + + return false; +} + +bool +PrecLand::switch_to_state_final_approach() +{ + if (check_state_conditions(PrecLandState::FinalApproach)) { + _state = PrecLandState::FinalApproach; + _state_start_time = hrt_absolute_time(); + return true; + } + + return false; +} + +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_search_alt.get(); + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + _navigator->set_position_setpoint_triplet_updated(); + + _target_acquired_time = 0; + + _state = PrecLandState::Search; + _state_start_time = hrt_absolute_time(); + return true; +} + +bool +PrecLand::switch_to_state_fallback() +{ + PX4_WARN("Falling back to normal land."); + 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; +} + +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_max_searches.get(); + + case PrecLandState::HorizontalApproach: + + // 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_hacc_rad.get() + && fabsf(_target_pose.y_rel - vehicle_local_position->y) < _param_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; + + } 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 _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 (_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 + + } else { + return _target_pose_valid && _target_pose.abs_pos_valid; + } + + } else { + // if not already in this state, need to be above target to enter it + return _target_pose_valid && _target_pose.abs_pos_valid + && fabsf(_target_pose.x_rel) < _param_hacc_rad.get() && fabsf(_target_pose.y_rel) < _param_hacc_rad.get(); + } + + case PrecLandState::FinalApproach: + return _target_pose_valid && _target_pose.rel_pos_valid && _target_pose.z_rel < _param_final_approach_alt.get(); + + case PrecLandState::Search: + return true; + + case PrecLandState::Fallback: + return true; + + default: + return false; + } +} + +void PrecLand::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) { + // running the first time since switching to precland + + // assume dt will be about 50000us + dt = 50000 / SEC2USEC; + + // set a best guess for previous setpoints for smooth transition + map_projection_project(&_map_ref, _navigator->get_position_setpoint_triplet()->current.lat, + _navigator->get_position_setpoint_triplet()->current.lon, &_sp_pev(0), &_sp_pev(1)); + _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; + } + + _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/navigator/precland.h b/src/modules/navigator/precland.h new file mode 100644 index 0000000000..b8b9805e9a --- /dev/null +++ b/src/modules/navigator/precland.h @@ -0,0 +1,135 @@ +/*************************************************************************** + * + * 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.h + * + * Helper class to do precision landing with a landing target + * + * @author Nicolas de Palezieux (Sunflower Labs) + */ + +#ifndef NAVIGATOR_PRECLAND_H +#define NAVIGATOR_PRECLAND_H + +#include +#include +#include +#include + +#include "navigator_mode.h" +#include "mission_block.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: + PrecLand(Navigator *navigator, const char *name); + + ~PrecLand(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + + void set_mode(PrecLandMode mode) { _mode = mode; }; + + PrecLandMode get_mode() { return _mode; }; + +private: + // run the control loop for each state + void run_state_start(); + void run_state_horizontal_approach(); + void run_state_descend_above_target(); + void run_state_final_approach(); + void run_state_search(); + void run_state_fallback(); + + // attempt to switch to a different state. Returns true if state change was successful, false otherwise + bool switch_to_state_start(); + 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(); + + // 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); + + landing_target_pose_s _target_pose{}; /**< precision landing target position */ + int _targetPoseSub; + bool _target_pose_valid; /**< wether we have received a landing target position message */ + struct map_projection_reference_s _map_ref {}; /**< reference for local/global projections */ + uint64_t _state_start_time; /**< time when we entered current state */ + uint64_t _last_slewrate_time; /**< time when we last limited setpoint changes */ + uint64_t _target_acquired_time; /**< time when we first saw the landing target during search */ + uint64_t _point_reached_time; /**< time when we reached a setpoint */ + int _search_cnt; /**< counter of how many times we had to search for the landing target */ + float _approach_alt; /**< altitude at which to stay during horizontal approach */ + + matrix::Vector2f _sp_pev; + matrix::Vector2f _sp_pev_prev; + + PrecLandState _state; + + PrecLandMode _mode; + + control::BlockParamFloat _param_timeout; + control::BlockParamFloat _param_hacc_rad; + control::BlockParamFloat _param_final_approach_alt; + control::BlockParamFloat _param_search_alt; + control::BlockParamFloat _param_search_timeout; + control::BlockParamInt _param_max_searches; + control::BlockParamFloat _param_acceleration_hor; + control::BlockParamFloat _param_xy_vel_cruise; + +}; + +#endif diff --git a/src/modules/navigator/precland_params.c b/src/modules/navigator/precland_params.c new file mode 100644 index 0000000000..7d8489f000 --- /dev/null +++ b/src/modules/navigator/precland_params.c @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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_params.c + * + * Parameters for precision landing. + * + * @author Nicolas de Palezieux (Sunflower Labs) + */ + +/** + * Landing Target Timeout + * + * Time after which the landing target is considered lost without any new measurements. + * + * @unit s + * @min 0.0 + * @max 50 + * @decimal 1 + * @increment 0.5 + * @group Precision Land + */ +PARAM_DEFINE_FLOAT(PLD_BTOUT, 5.0f); + +/** + * Horizontal acceptance radius + * + * Start descending if closer above landing target than this. + * + * @unit m + * @min 0.0 + * @max 10 + * @decimal 2 + * @increment 0.1 + * @group Precision Land + */ +PARAM_DEFINE_FLOAT(PLD_HACC_RAD, 0.2f); + +/** + * Final approach altitude + * + * Allow final approach (without horizontal positioning) if losing landing target closer than this to the ground. + * + * @unit m + * @min 0.0 + * @max 10 + * @decimal 2 + * @increment 0.1 + * @group Precision Land + */ +PARAM_DEFINE_FLOAT(PLD_FAPPR_ALT, 0.1f); + +/** + * Search altitude + * + * Altitude above home to which to climb when searching for the landing target. + * + * @unit m + * @min 0.0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group Precision Land + */ +PARAM_DEFINE_FLOAT(PLD_SRCH_ALT, 10.0f); + +/** + * Search timeout + * + * Time allowed to search for the landing target before falling back to normal landing. + * + * @unit s + * @min 0.0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group Precision Land + */ +PARAM_DEFINE_FLOAT(PLD_SRCH_TOUT, 10.0f); + +/** + * Maximum number of search attempts + * + * Maximum number of times to seach for the landing target if it is lost during the precision landing. + * + * @min 0.0 + * @max 100 + * @group Precision Land + */ +PARAM_DEFINE_INT32(PLD_MAX_SRCH, 3);