From 9bb8b12f43de3aa4e7f24c516e7e8e7e9e6c196d Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Thu, 12 Jun 2014 15:50:06 -0700 Subject: [PATCH] Using a vector to store navigation modes --- src/modules/navigator/navigator.h | 4 ++++ src/modules/navigator/navigator_main.cpp | 29 ++++++++++-------------- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index fe7485f566..dfa939c38a 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -40,6 +40,8 @@ #ifndef NAVIGATOR_H #define NAVIGATOR_H +#include + #include #include @@ -153,6 +155,8 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ + std::vector _navigation_mode_vector; + bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ bool _update_triplet; /**< flags if position SP triplet needs to be published */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3c190c7f2..ac32d4b22d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -125,6 +125,11 @@ Navigator::Navigator() : _param_loiter_radius(this, "LOITER_RAD"), _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") { + /* Create a list of our possible navigation types */ + _navigation_mode_vector.push_back(&_mission); + _navigation_mode_vector.push_back(&_loiter); + _navigation_mode_vector.push_back(&_rtl); + updateParams(); } @@ -363,23 +368,13 @@ Navigator::task_main() break; } - /* TODO: make list of modes and loop through it */ - if (_navigation_mode == &_mission) { - _update_triplet = _mission.on_active(&_pos_sp_triplet); - } else { - _mission.on_inactive(); - } - - if (_navigation_mode == &_rtl) { - _update_triplet = _rtl.on_active(&_pos_sp_triplet); - } else { - _rtl.on_inactive(); - } - - if (_navigation_mode == &_loiter) { - _update_triplet = _loiter.on_active(&_pos_sp_triplet); - } else { - _loiter.on_inactive(); + /* iterate through navigation modes and set active/inactive for each */ + for(unsigned int i = 0; i < _navigation_mode_vector.size(); i++) { + if (_navigation_mode == _navigation_mode_vector[i]) { + _update_triplet = _navigation_mode_vector[i]->on_active(&_pos_sp_triplet); + } else { + _navigation_mode_vector[i]->on_inactive(); + } } /* if nothing is running, set position setpoint triplet invalid */