navigator: API changes, reparing to move manual modes to navigator, WIP

10 files changed, 87 insertions, 86 deletions

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index 5e7067b0e..f827e70c9 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -36,6 +36,7 @@
  * Helper class to loiter
  *
  * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <string.h>
@@ -51,14 +52,13 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 
 #include "loiter.h"
+#include "navigator.h"
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name)
 {
 	/* load initial params */
 	updateParams();
-	/* initial reset */
-	on_inactive();
 }
 
 Loiter::~Loiter()
@@ -71,16 +71,23 @@ Loiter::on_inactive()
 }
 
 void
-Loiter::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
+Loiter::on_activation()
 {
-	set_loiter_item(pos_sp_triplet);
+	/* set current mission item to loiter */
+	set_loiter_item(&_mission_item);
+
+	/* convert mission item to current setpoint */
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->previous.valid = false;
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
+
+	_navigator->set_position_setpoint_triplet_updated();
 }
 
-bool
-Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+void
+Loiter::on_active()
 {
-	return false;
 }
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
index 5ce86d2a7..37ab57a07 100644
--- a/src/modules/navigator/loiter.h
+++ b/src/modules/navigator/loiter.h
@@ -56,9 +56,9 @@ public:
 
 	virtual void on_inactive();
 
-	virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_activation();
 
-	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_active();
 };
 
 #endif
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index ab99a6b7e..1e86b1b6c 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -54,8 +54,8 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
-#include "navigator.h"
 #include "mission.h"
+#include "navigator.h"
 
 Mission::Mission(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
@@ -97,16 +97,14 @@ Mission::on_inactive()
 }
 
 void
-Mission::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
+Mission::on_activation()
 {
-	set_mission_items(pos_sp_triplet);
+	set_mission_items();
 }
 
-bool
-Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+void
+Mission::on_active()
 {
-	bool updated = false;
-
 	/* check if anything has changed */
 	bool onboard_updated;
 	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@@ -122,18 +120,18 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 
 	/* reset mission items if needed */
 	if (onboard_updated || offboard_updated) {
-		set_mission_items(pos_sp_triplet);
-		updated = true;
+		set_mission_items();
+
+		_navigator->set_position_setpoint_triplet_updated();
 	}
 
 	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
 		advance_mission();
-		set_mission_items(pos_sp_triplet);
-		updated = true;
-	}
+		set_mission_items();
 
-	return updated;
+		_navigator->set_position_setpoint_triplet_updated();
+	}
 }
 
 void
@@ -223,8 +221,10 @@ Mission::advance_mission()
 }
 
 void
-Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
+Mission::set_mission_items()
 {
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
 	set_previous_pos_setpoint(pos_sp_triplet);
 
 	/* try setting onboard mission item */
@@ -235,7 +235,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: onboard mission running");
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
-		_navigator->set_can_loiter_at_sp(false);
+		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
 
 	/* try setting offboard mission item */
 	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -245,7 +245,8 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: offboard mission running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
-		_navigator->set_can_loiter_at_sp(false);
+		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
+
 	} else {
 		if (_mission_type != MISSION_TYPE_NONE) {
 			mavlink_log_info(_navigator->get_mavlink_fd(),
@@ -255,12 +256,15 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: no mission available");
 		}
 		_mission_type = MISSION_TYPE_NONE;
+
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
 
-		set_loiter_item(pos_sp_triplet);
+		set_loiter_item(&_mission_item);
+
 		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();
 	}
@@ -279,7 +283,6 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current
 					&new_mission_item)) {
 			/* convert the current mission item and set it valid */
 			mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
-			current_pos_sp->valid = true;
 
 			reset_mission_item_reached();
 
@@ -306,7 +309,6 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren
 		if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) {
 			/* convert the current mission item and set it valid */
 			mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
-			current_pos_sp->valid = true;
 
 			reset_mission_item_reached();
 
@@ -330,7 +332,6 @@ Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
 		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
 			/* convert next mission item to position setpoint */
 			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
-			next_pos_sp->valid = true;
 			return;
 		}
 	}
@@ -358,7 +359,6 @@ Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
 		if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
 			/* convert next mission item to position setpoint */
 			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
-			next_pos_sp->valid = true;
 			return;
 		}
 	}
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 38a4f7612..ac293990f 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -71,9 +71,9 @@ public:
 
 	virtual void on_inactive();
 
-	virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_activation();
 
-	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_active();
 
 private:
 	/**
@@ -94,7 +94,7 @@ private:
 	/**
 	 * Set new mission items
 	 */
-	void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
+	void set_mission_items();
 
 	/**
 	 * Try to set the current position setpoint from an onboard mission item
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index acf3ad569..bf3c934a6 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -58,8 +58,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
-	_mission_item({0}),
-	_mission_item_valid(false)
+	_mission_item({0})
 {
 }
 
@@ -70,6 +69,10 @@ MissionBlock::~MissionBlock()
 bool
 MissionBlock::is_mission_item_reached()
 {
+	if (_mission_item.nav_cmd == NAV_CMD_IDLE) {
+		return false;
+	}
+
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
 		return _navigator->get_vstatus()->condition_landed;
 	}
@@ -83,7 +86,6 @@ MissionBlock::is_mission_item_reached()
 	hrt_abstime now = hrt_absolute_time();
 
 	if (!_waypoint_position_reached) {
-
 		float dist = -1.0f;
 		float dist_xy = -1.0f;
 		float dist_z = -1.0f;
@@ -209,43 +211,38 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_
 }
 
 void
-MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
+MissionBlock::set_loiter_item(struct mission_item_s *item)
 {
 	if (_navigator->get_vstatus()->condition_landed) {
 		/* landed, don't takeoff, but switch to IDLE mode */
-		_mission_item.nav_cmd = NAV_CMD_IDLE;
-
-		_navigator->set_can_loiter_at_sp(false);
-
-		mavlink_log_info(_navigator->get_mavlink_fd(), "landed, switch to IDLE");
+		item->nav_cmd = NAV_CMD_IDLE;
 
 	} else {
-		_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+		item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+
+		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
 		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;
+			item->lat = pos_sp_triplet->current.lat;
+			item->lon = pos_sp_triplet->current.lon;
+			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;
+			item->lat = _navigator->get_global_position()->lat;
+			item->lon = _navigator->get_global_position()->lon;
+			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");
+		item->altitude_is_relative = false;
+		item->yaw = NAN;
+		item->loiter_radius = _navigator->get_loiter_radius();
+		item->loiter_direction = 1;
+		item->acceptance_radius = _navigator->get_acceptance_radius();
+		item->time_inside = 0.0f;
+		item->pitch_min = 0.0f;
+		item->autocontinue = false;
+		item->origin = ORIGIN_ONBOARD;
 	}
 }
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 4f79238f4..22f42cc31 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -91,11 +91,9 @@ protected:
 	/**
 	 * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
 	 */
-	// 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);
+	void set_loiter_item(struct mission_item_s *item);
 
 	mission_item_s _mission_item;
-	bool _mission_item_valid;
 	bool _waypoint_position_reached;
 	bool _waypoint_yaw_reached;
 	hrt_abstime _time_first_inside_orbit;
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 184ecd365..5dcd8859e 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -103,6 +103,7 @@ public:
 	 * Setters
 	 */
 	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
+	void		set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
 
 	/**
 	 * Getters
@@ -110,6 +111,7 @@ public:
 	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
 	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
+	struct position_setpoint_triplet_s*		    get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
 	Geofence&	get_geofence() { return _geofence; }
@@ -162,7 +164,7 @@ private:
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
 	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
-	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
+	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 266114e38..bb48db829 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -121,7 +121,7 @@ Navigator::Navigator() :
 	_mission(this, "MIS"),
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
-	_update_triplet(false),
+	_pos_sp_triplet_updated(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD")
 {
@@ -371,20 +371,21 @@ Navigator::task_main()
 
 		/* iterate through navigation modes and set active/inactive for each */
 		for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
-			_update_triplet = _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i], &_pos_sp_triplet);
+			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
 		}
 
 		/* if nothing is running, set position setpoint triplet invalid */
 		if (_navigation_mode == nullptr) {
+			// TODO publish empty sp only once
 			_pos_sp_triplet.previous.valid = false;
 			_pos_sp_triplet.current.valid = false;
 			_pos_sp_triplet.next.valid = false;
-			_update_triplet = true;
+			_pos_sp_triplet_updated = true;
 		}
 
-		if (_update_triplet) {
+		if (_pos_sp_triplet_updated) {
 			publish_position_setpoint_triplet();
-			_update_triplet = false;
+			_pos_sp_triplet_updated = false;
 		}
 
 		perf_end(_loop_perf);
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index 6361ea9c8..f43215665 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -40,6 +40,7 @@
  */
 
 #include "navigator_mode.h"
+#include "navigator.h"
 
 NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
 	SuperBlock(NULL, name),
@@ -56,18 +57,17 @@ NavigatorMode::~NavigatorMode()
 {
 }
 
-bool
-NavigatorMode::run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet) {
+void
+NavigatorMode::run(bool active) {
 	if (active) {
 		if (_first_run) {
 			/* first run */
 			_first_run = false;
-			on_activation(pos_sp_triplet);
-			return true;
+			on_activation();
 
 		} else {
 			/* periodic updates when active */
-			on_active(pos_sp_triplet);
+			on_active();
 		}
 
 	} else {
@@ -83,14 +83,13 @@ NavigatorMode::on_inactive()
 }
 
 void
-NavigatorMode::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
+NavigatorMode::on_activation()
 {
 	/* invalidate position setpoint by default */
-	pos_sp_triplet->current.valid = false;
+	_navigator->get_position_setpoint_triplet()->current.valid = false;
 }
 
-bool
-NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+void
+NavigatorMode::on_active()
 {
-	return false;
 }
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
index 5c36af1fe..a7ba79bba 100644
--- a/src/modules/navigator/navigator_mode.h
+++ b/src/modules/navigator/navigator_mode.h
@@ -66,7 +66,7 @@ public:
 	 */
 	virtual ~NavigatorMode();
 
-	bool run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet);
+	void run(bool active);
 
 	/**
 	 * This function is called while the mode is inactive
@@ -76,15 +76,12 @@ public:
 	/**
 	 * 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);
+	virtual void on_activation();
 
 	/**
 	 * This function is called while the mode is active
-	 *
-	 * @param position setpoint triplet to set
-	 * @return true if position setpoint triplet has been changed
 	 */
-	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_active();
 
 protected:
 	Navigator *_navigator;