From 05993bee6fa523d6d8ecfcceb614fa45fe669956 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 15:57:27 +0200 Subject: [PATCH] Navigator: Provide better feedback if no mission present, enforce minimum altitude in loiter and in auto modes --- src/modules/navigator/loiter.cpp | 5 ++-- src/modules/navigator/loiter.h | 3 +++ src/modules/navigator/mission.cpp | 32 ++++++++++++++++++------- src/modules/navigator/mission_block.cpp | 8 +++++-- src/modules/navigator/mission_block.h | 2 +- 5 files changed, 37 insertions(+), 13 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index a744d58cf0..aabdb2b075 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -55,7 +55,8 @@ #include "navigator.h" Loiter::Loiter(Navigator *navigator, const char *name) : - MissionBlock(navigator, name) + MissionBlock(navigator, name), + _param_min_alt(this, "MIS_TAKEOFF_ALT", false) { /* load initial params */ updateParams(); @@ -74,7 +75,7 @@ void Loiter::on_activation() { /* set current mission item to loiter */ - set_loiter_item(&_mission_item); + set_loiter_item(&_mission_item, _param_min_alt.get()); /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 37ab57a078..0627c54129 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -59,6 +59,9 @@ public: virtual void on_activation(); virtual void on_active(); + +private: + control::BlockParamFloat _param_min_alt; }; #endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index fe876ee8b1..a74e042a91 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -377,6 +377,7 @@ Mission::set_mission_items() /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); + user_feedback_done = true; } _mission_type = MISSION_TYPE_ONBOARD; @@ -385,6 +386,7 @@ Mission::set_mission_items() /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); + user_feedback_done = true; } _mission_type = MISSION_TYPE_OFFBOARD; } else { @@ -392,21 +394,17 @@ Mission::set_mission_items() if (_mission_type != MISSION_TYPE_NONE) { /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + user_feedback_done = true; /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); - } else if (!user_feedback_done) { - /* only tell users that we got no mission if there has not been any - * better, more specific feedback yet - * https://en.wikipedia.org/wiki/Loiter_(aeronautics) - */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering"); } + _mission_type = MISSION_TYPE_NONE; - /* set loiter mission item */ - set_loiter_item(&_mission_item); + /* set loiter mission item and ensure that there is a minimum clearance from home */ + set_loiter_item(&_mission_item, _param_takeoff_alt.get()); /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; @@ -418,6 +416,24 @@ Mission::set_mission_items() set_mission_finished(); + if (!user_feedback_done) { + /* only tell users that we got no mission if there has not been any + * better, more specific feedback yet + * https://en.wikipedia.org/wiki/Loiter_(aeronautics) + */ + + if (_navigator->get_vstatus()->condition_landed) { + /* landed, refusing to take off without a mission */ + + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff"); + } else { + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering"); + } + + user_feedback_done = true; + + } + _navigator->set_position_setpoint_triplet_updated(); return; } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 42c74428ad..8e83a3329d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -228,7 +228,7 @@ MissionBlock::set_previous_pos_setpoint() } void -MissionBlock::set_loiter_item(struct mission_item_s *item) +MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) { if (_navigator->get_vstatus()->condition_landed) { /* landed, don't takeoff, but switch to IDLE mode */ @@ -246,10 +246,14 @@ MissionBlock::set_loiter_item(struct mission_item_s *item) item->altitude = pos_sp_triplet->current.alt; } else { - /* use current position */ + /* use current position and use return altitude as clearance */ item->lat = _navigator->get_global_position()->lat; item->lon = _navigator->get_global_position()->lon; item->altitude = _navigator->get_global_position()->alt; + + if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) { + item->altitude = _navigator->get_home_position()->alt + min_clearance; + } } item->altitude_is_relative = false; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index ec3e305825..4e6e99acb0 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -91,7 +91,7 @@ protected: /** * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position */ - void set_loiter_item(struct mission_item_s *item); + void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f); mission_item_s _mission_item; bool _waypoint_position_reached;