navigator: NavigatorMode and MissionBlock API cleanup

11 files changed, 144 insertions, 129 deletions

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 <systemlib/err.h>
 #include <geo/geo.h>
+#include <mavlink/mavlink_log.h>
 
 #include <uORB/uORB.h>
 
@@ -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 <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #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 <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef NAVIGATOR_MODE_H
@@ -65,12 +66,19 @@ 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
 	 *
 	 * @param position setpoint triplet to set
@@ -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:
 	/**