Merge branch 'navigator_rewrite' into navigator_rewrite_estimator

2 files changed, 20 insertions, 17 deletions

diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index fe7485f56..dadd15527 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -58,6 +58,12 @@
 #include "rtl.h"
 #include "geofence.h"
 
+/**
+ * Number of navigation modes that need on_active/on_inactive calls
+ * Currently: mission, loiter, and rtl
+ */
+#define NAVIGATOR_MODE_ARRAY_SIZE 3
+
 class Navigator : public control::SuperBlock
 {
 public:
@@ -153,6 +159,8 @@ private:
 	Loiter		_loiter;			/**< class that handles loiter */
 	RTL 		_rtl;				/**< class that handles RTL */
 
+	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
+
 	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 661d3d744..d762b8a9d 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_array[0] = &_mission;
+	_navigation_mode_array[1] = &_loiter;
+	_navigation_mode_array[2] = &_rtl;
+
 	updateParams();
 }
 
@@ -364,23 +369,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 < 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();
+			}
 		}
 
 		/* if nothing is running, set position setpoint triplet invalid */