From 456e628e129b446d18246ab8ad312a15beea5996 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 28 Jun 2014 00:54:27 +0200 Subject: navigator: NavigatorMode and MissionBlock API cleanup --- src/modules/navigator/loiter.cpp | 18 ++++++--- src/modules/navigator/loiter.h | 14 +------ src/modules/navigator/mission.cpp | 14 +++++-- src/modules/navigator/mission.h | 14 +------ src/modules/navigator/mission_block.cpp | 65 ++++++++++++++++++-------------- src/modules/navigator/mission_block.h | 14 +++---- src/modules/navigator/navigator_main.cpp | 6 +-- src/modules/navigator/navigator_mode.cpp | 34 +++++++++++++++-- src/modules/navigator/navigator_mode.h | 12 +++++- src/modules/navigator/rtl.cpp | 62 ++++++++++++++---------------- src/modules/navigator/rtl.h | 20 ++-------- 11 files changed, 144 insertions(+), 129 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 542483fb1..5e7067b0e 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -65,14 +65,22 @@ Loiter::~Loiter() { } -bool -Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Loiter::on_inactive() { - /* set loiter item, don't reuse an existing position setpoint */ - return set_loiter_item(pos_sp_triplet); } void -Loiter::on_inactive() +Loiter::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + set_loiter_item(pos_sp_triplet); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; +} + +bool +Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { + return false; } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 65ff5c31e..5ce86d2a7 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -50,24 +50,14 @@ class Loiter : public MissionBlock { public: - /** - * Constructor - */ Loiter(Navigator *navigator, const char *name); - /** - * Destructor - */ ~Loiter(); - /** - * This function is called while the mode is inactive - */ virtual void on_inactive(); - /** - * This function is called while the mode is active - */ + virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); }; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 72255103b..395669698 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -82,8 +82,6 @@ Mission::~Mission() void Mission::on_inactive() { - _first_run = true; - /* check anyway if missions have changed so that feedback to groundstation is given */ bool onboard_updated; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); @@ -98,6 +96,12 @@ Mission::on_inactive() } } +void +Mission::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + set_mission_items(pos_sp_triplet); +} + bool Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { @@ -117,10 +121,9 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) } /* reset mission items if needed */ - if (onboard_updated || offboard_updated || _first_run) { + if (onboard_updated || offboard_updated) { set_mission_items(pos_sp_triplet); updated = true; - _first_run = false; } /* lets check if we reached the current mission item */ @@ -255,6 +258,9 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); set_loiter_item(pos_sp_triplet); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; reset_mission_item_reached(); report_mission_finished(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6e4761946..38a4f7612 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -65,24 +65,14 @@ class Navigator; class Mission : public MissionBlock { public: - /** - * Constructor - */ Mission(Navigator *navigator, const char *name); - /** - * Destructor - */ virtual ~Mission(); - /** - * This function is called while the mode is inactive - */ virtual void on_inactive(); - /** - * This function is called while the mode is active - */ + virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); private: diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 9b8d3d9c7..acf3ad569 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -45,6 +45,7 @@ #include #include +#include #include @@ -207,36 +208,44 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_ } } -bool +void MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) { - /* don't change setpoint if 'can_loiter_at_sp' flag set */ - if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { - /* use 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.yaw = NAN; /* NAN means to use current yaw */ - - _navigator->set_can_loiter_at_sp(true); - } + if (_navigator->get_vstatus()->condition_landed) { + /* landed, don't takeoff, but switch to IDLE mode */ + _mission_item.nav_cmd = NAV_CMD_IDLE; - if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER - || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius() - || pos_sp_triplet->current.loiter_direction != 1 - || pos_sp_triplet->previous.valid - || !pos_sp_triplet->current.valid - || pos_sp_triplet->next.valid) { - /* position setpoint triplet should be updated */ - pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; - pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius(); - pos_sp_triplet->current.loiter_direction = 1; - - pos_sp_triplet->previous.valid = false; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->next.valid = false; - return true; - } + _navigator->set_can_loiter_at_sp(false); + + mavlink_log_info(_navigator->get_mavlink_fd(), "landed, switch to IDLE"); + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - return false; + if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) { + /* use current position setpoint */ + _mission_item.lat = pos_sp_triplet->current.lat; + _mission_item.lon = pos_sp_triplet->current.lon; + _mission_item.altitude = pos_sp_triplet->current.alt; + + } else { + /* use current position */ + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = _navigator->get_global_position()->alt; + } + + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + mavlink_log_info(_navigator->get_mavlink_fd(), "switch to LOITER"); + } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index f99002752..4f79238f4 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -64,6 +64,7 @@ public: */ virtual ~MissionBlock(); +protected: /** * Check if mission item has been reached * @return true if successfully reached @@ -88,19 +89,16 @@ public: void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); /** - * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position - * - * @param the position setpoint triplet to set - * @return true if setpoint has changed + * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position */ - bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet); + // TODO remove argument, get position setpoint from navigator, add to arguments pointer to mission item instead + void set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet); + mission_item_s _mission_item; + bool _mission_item_valid; bool _waypoint_position_reached; bool _waypoint_yaw_reached; hrt_abstime _time_first_inside_orbit; - - mission_item_s _mission_item; - bool _mission_item_valid; }; #endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 546602741..266114e38 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -371,11 +371,7 @@ Navigator::task_main() /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { - if (_navigation_mode == _navigation_mode_array[i]) { - _update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet); - } else { - _navigation_mode_array[i]->on_inactive(); - } + _update_triplet = _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i], &_pos_sp_triplet); } /* if nothing is running, set position setpoint triplet invalid */ diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 25e767c2b..6361ea9c8 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -33,9 +33,10 @@ /** * @file navigator_mode.cpp * - * Helper class for different modes in navigator + * Base class for different modes in navigator * * @author Julian Oes + * @author Anton Babushkin */ #include "navigator_mode.h" @@ -55,16 +56,41 @@ NavigatorMode::~NavigatorMode() { } +bool +NavigatorMode::run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet) { + if (active) { + if (_first_run) { + /* first run */ + _first_run = false; + on_activation(pos_sp_triplet); + return true; + + } else { + /* periodic updates when active */ + on_active(pos_sp_triplet); + } + + } else { + /* periodic updates when inactive */ + _first_run = true; + on_inactive(); + } +} + void NavigatorMode::on_inactive() { - _first_run = true; +} + +void +NavigatorMode::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + /* invalidate position setpoint by default */ + pos_sp_triplet->current.valid = false; } bool NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { - pos_sp_triplet->current.valid = false; - _first_run = false; return false; } diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index cbb53d91b..5c36af1fe 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -33,9 +33,10 @@ /** * @file navigator_mode.h * - * Helper class for different modes in navigator + * Base class for different modes in navigator * * @author Julian Oes + * @author Anton Babushkin */ #ifndef NAVIGATOR_MODE_H @@ -65,11 +66,18 @@ public: */ virtual ~NavigatorMode(); + bool run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet); + /** * This function is called while the mode is inactive */ virtual void on_inactive(); + /** + * This function is called one time when mode become active, poos_sp_triplet must be initialized here + */ + virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet); + /** * This function is called while the mode is active * @@ -80,6 +88,8 @@ public: protected: Navigator *_navigator; + +private: bool _first_run; }; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 043f773a4..7cf6bb1a8 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -75,53 +75,49 @@ RTL::~RTL() void RTL::on_inactive() { - _first_run = true; - /* reset RTL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { _rtl_state = RTL_STATE_NONE; } } -bool -RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet) { - bool updated = false; - - if (_first_run) { - _first_run = false; - - /* decide where to enter the RTL procedure when we switch into it */ - if (_rtl_state == RTL_STATE_NONE) { - /* for safety reasons don't go into RTL if landed */ - if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_LANDED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - /* if lower than return altitude, climb up first */ - } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - + _param_return_alt.get()) { - _rtl_state = RTL_STATE_CLIMB; - - /* otherwise go straight to return */ - } else { - /* set altitude setpoint to current altitude */ - _rtl_state = RTL_STATE_RETURN; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_global_position()->alt; - } + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_LANDED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + /* otherwise go straight to return */ + } else { + /* set altitude setpoint to current altitude */ + _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; } + } - set_rtl_item(pos_sp_triplet); - updated = true; + set_rtl_item(pos_sp_triplet); +} - } else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { +bool +RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { advance_rtl(); set_rtl_item(pos_sp_triplet); - updated = true; + return true; } - return updated; + return false; } void diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index b4b729e89..9c8b3fdfc 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -57,29 +57,15 @@ class Navigator; class RTL : public MissionBlock { public: - /** - * Constructor - */ RTL(Navigator *navigator, const char *name); - /** - * Destructor - */ ~RTL(); - /** - * This function is called while the mode is inactive - */ - void on_inactive(); + virtual void on_inactive(); - /** - * This function is called while the mode is active - * - * @param position setpoint triplet that needs to be set - * @return true if updated - */ - bool on_active(position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_activation(position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(position_setpoint_triplet_s *pos_sp_triplet); private: /** -- cgit v1.2.3