1 files changed, 186 insertions, 134 deletions

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 72255103b..2e5047e2c 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -54,25 +54,28 @@
 #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),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
+	_param_takeoff_alt(this, "TAKEOFF_ALT"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
 	_current_offboard_mission_index(-1),
 	_mission_result_pub(-1),
 	_mission_result({0}),
-	_mission_type(MISSION_TYPE_NONE)
+	_mission_type(MISSION_TYPE_NONE),
+	_need_takeoff(true),
+	_takeoff(false)
 {
 	/* load initial params */
 	updateParams();
+
 	/* set initial mission items */
 	on_inactive();
-
 }
 
 Mission::~Mission()
@@ -82,8 +85,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);
@@ -96,13 +97,21 @@ Mission::on_inactive()
 	if (offboard_updated) {
 		update_offboard_mission();
 	}
+
+	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
+		_need_takeoff = true;
+	}
 }
 
-bool
-Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+void
+Mission::on_activation()
 {
-	bool updated = false;
+	set_mission_items();
+}
 
+void
+Mission::on_active()
+{
 	/* check if anything has changed */
 	bool onboard_updated;
 	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@@ -117,20 +126,21 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 	}
 
 	/* reset mission items if needed */
-	if (onboard_updated || offboard_updated || _first_run) {
-		set_mission_items(pos_sp_triplet);
-		updated = true;
-		_first_run = false;
+	if (onboard_updated || offboard_updated) {
+		set_mission_items();
 	}
 
 	/* 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;
+	} else {
+		/* if waypoint position reached allow loiter on the setpoint */
+		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
+			_navigator->set_can_loiter_at_sp(true);
+		}
+	}
 }
 
 void
@@ -151,6 +161,7 @@ Mission::update_onboard_mission()
 			}
 			/* otherwise, just leave it */
 		}
+
 	} else {
 		_onboard_mission.count = 0;
 		_onboard_mission.current_index = 0;
@@ -167,10 +178,12 @@ Mission::update_offboard_mission()
 		if (_offboard_mission.current_index >= 0
 		    && _offboard_mission.current_index < (int)_offboard_mission.count) {
 			_current_offboard_mission_index = _offboard_mission.current_index;
+
 		} else {
 			/* if less WPs available, reset to first WP */
 			if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
 				_current_offboard_mission_index = 0;
+
 			/* if not initialized, set it to 0 */
 			} else if (_current_offboard_mission_index < 0) {
 				_current_offboard_mission_index = 0;
@@ -184,6 +197,7 @@ Mission::update_offboard_mission()
 
 		if (_offboard_mission.dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
@@ -192,11 +206,13 @@ Mission::update_offboard_mission()
 			                                      (size_t)_offboard_mission.count,
 							      _navigator->get_geofence(),
 							      _navigator->get_home_position()->alt);
+
 	} else {
 		_offboard_mission.count = 0;
 		_offboard_mission.current_index = 0;
 		_current_offboard_mission_index = 0;
 	}
+
 	report_current_offboard_mission_item();
 }
 
@@ -204,46 +220,55 @@ Mission::update_offboard_mission()
 void
 Mission::advance_mission()
 {
-	switch (_mission_type) {
-	case MISSION_TYPE_ONBOARD:
-		_current_onboard_mission_index++;
-		break;
-
-	case MISSION_TYPE_OFFBOARD:
-		_current_offboard_mission_index++;
-		break;
-
-	case MISSION_TYPE_NONE:
-	default:
-		break;
+	if (_takeoff) {
+		_takeoff = false;
+
+	} else {
+		switch (_mission_type) {
+		case MISSION_TYPE_ONBOARD:
+			_current_onboard_mission_index++;
+			break;
+
+		case MISSION_TYPE_OFFBOARD:
+			_current_offboard_mission_index++;
+			break;
+
+		case MISSION_TYPE_NONE:
+		default:
+			break;
+		}
 	}
 }
 
 void
-Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
+Mission::set_mission_items()
 {
-	set_previous_pos_setpoint(pos_sp_triplet);
+	/* make sure param is up to date */
+	updateParams();
+
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* set previous position setpoint to current */
+	set_previous_pos_setpoint();
 
 	/* try setting onboard mission item */
-	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
+	if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_ONBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_fd(),
-					"#audio: onboard mission running");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running");
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
-		_navigator->set_can_loiter_at_sp(false);
 
 	/* try setting offboard mission item */
-	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
+	} else if (read_mission_item(false, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_fd(),
-					"#audio: offboard mission running");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
-		_navigator->set_can_loiter_at_sp(false);
+
 	} else {
+		/* no mission available, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
 			mavlink_log_info(_navigator->get_mavlink_fd(),
 					"#audio: mission finished");
@@ -252,125 +277,151 @@ 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 mission item */
+		set_loiter_item(&_mission_item);
+
+		/* update 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);
+
 		reset_mission_item_reached();
 		report_mission_finished();
+
+		_navigator->set_position_setpoint_triplet_updated();
+		return;
 	}
-}
 
-bool
-Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
-{
-	/* make sure param is up to date */
-	updateParams();
-	if (_param_onboard_enabled.get() > 0 &&
-	    _current_onboard_mission_index >= 0&&
-	    _current_onboard_mission_index < (int)_onboard_mission.count) {
-		struct mission_item_s new_mission_item;
-		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_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();
-
-			/* TODO: report this somehow */
-			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
-			return true;
+	/* do takeoff on first waypoint for rotary wing vehicles */
+	if (_navigator->get_vstatus()->is_rotary_wing) {
+		/* force takeoff if landed (additional protection) */
+		if (!_takeoff && _navigator->get_vstatus()->condition_landed) {
+			_need_takeoff = true;
+		}
+
+		/* new current mission item set, check if we need takeoff */
+		if (_need_takeoff && (
+				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+				_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+				_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+				_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+				_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
+				_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
+			_takeoff = true;
+			_need_takeoff = false;
 		}
 	}
-	return false;
-}
 
-bool
-Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
-{
-	if (_current_offboard_mission_index >= 0 &&
-	    _current_offboard_mission_index < (int)_offboard_mission.count) {
-		dm_item_t dm_current;
-		if (_offboard_mission.dataman_id == 0) {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+	if (_takeoff) {
+		/* do takeoff before going to setpoint */
+		/* set mission item as next position setpoint */
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
+
+		/* calculate takeoff altitude */
+		float takeoff_alt = _mission_item.altitude;
+		if (_mission_item.altitude_is_relative) {
+			takeoff_alt += _navigator->get_home_position()->alt;
+		}
+
+		if (_navigator->get_vstatus()->condition_landed) {
+			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
+
 		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+			takeoff_alt = _navigator->get_home_position()->alt + _param_takeoff_alt.get();
+		}
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", takeoff_alt - _navigator->get_home_position()->alt);
+
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude = takeoff_alt;
+		_mission_item.altitude_is_relative = false;
+
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+
+	} else {
+		/* set current position setpoint from mission item */
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+
+		/* require takeoff after landing or idle */
+		if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
+			_need_takeoff = true;
 		}
-		struct mission_item_s new_mission_item;
-		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();
+		_navigator->set_can_loiter_at_sp(false);
+		reset_mission_item_reached();
 
+		if (_mission_type == MISSION_TYPE_OFFBOARD) {
 			report_current_offboard_mission_item();
-			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
-			return true;
 		}
-	}
-	return false;
-}
+		// TODO: report onboard mission item somehow
 
-void
-Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
-{
-	int next_temp_mission_index = _onboard_mission.current_index + 1;
-
-	/* try if there is a next onboard mission */
-	if (_onboard_mission.current_index >= 0 &&
-	    next_temp_mission_index < (int)_onboard_mission.count) {
-		struct mission_item_s new_mission_item;
-		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;
+		/* try to read next mission item */
+		struct mission_item_s mission_item_next;
+
+		if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
+			/* got next mission item, update setpoint triplet */
+			mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
+
+		} else {
+			/* next mission item is not available */
+			pos_sp_triplet->next.valid = false;
 		}
 	}
 
-	/* give up */
-	next_pos_sp->valid = false;
-	return;
+	_navigator->set_position_setpoint_triplet_updated();
 }
 
-void
-Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
+bool
+Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
 {
-	/* try if there is a next offboard mission */
-	int next_temp_mission_index = _offboard_mission.current_index + 1;
-	warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count);
-	if (_offboard_mission.current_index >= 0 &&
-	    next_temp_mission_index < (int)_offboard_mission.count) {
-		dm_item_t dm_current;
+	/* select onboard/offboard mission */
+	int *mission_index_ptr;
+	struct mission_s *mission;
+	dm_item_t dm_item;
+	int mission_index_next;
+
+	if (onboard) {
+		/* onboard mission */
+		mission_index_next = _current_onboard_mission_index + 1;
+		mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;
+
+		mission = &_onboard_mission;
+
+		dm_item = DM_KEY_WAYPOINTS_ONBOARD;
+
+	} else {
+		/* offboard mission */
+		mission_index_next = _current_offboard_mission_index + 1;
+		mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;
+
+		mission = &_offboard_mission;
+
 		if (_offboard_mission.dataman_id == 0) {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+			dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
 		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-		}
-		struct mission_item_s new_mission_item;
-		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;
+			dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 	}
-	/* give up */
-	next_pos_sp->valid = false;
-	return;
-}
 
-bool
-Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
-		           struct mission_item_s *new_mission_item)
-{
+	if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
+		/* mission item index out of bounds */
+		return false;
+	}
+
 	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
-	for (int i=0; i<10; i++) {
+	for (int i = 0; i < 10; i++) {
 		const ssize_t len = sizeof(struct mission_item_s);
 
+		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
+		struct mission_item_s mission_item_tmp;
+
 		/* read mission item from datamanager */
-		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
+		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			mavlink_log_critical(_navigator->get_mavlink_fd(),
 			                     "#audio: ERROR waypoint could not be read");
@@ -378,18 +429,17 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 		}
 
 		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
-		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
+		if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) {
 
 			/* do DO_JUMP as many times as requested */
-			if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
+			if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) {
 
 				/* only raise the repeat count if this is for the current mission item
 				* but not for the next mission item */
 				if (is_current) {
-					(new_mission_item->do_jump_current_count)++;
+					(mission_item_tmp.do_jump_current_count)++;
 					/* save repeat count */
-					if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
-						new_mission_item, len) != len) {
+					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
 						mavlink_log_critical(_navigator->get_mavlink_fd(),
@@ -399,16 +449,18 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 				}
 				/* set new mission item index and repeat
 				* we don't have to validate here, if it's invalid, we should realize this later .*/
-				*mission_index = new_mission_item->do_jump_mission_index;
+				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
+
 			} else {
 				mavlink_log_info(_navigator->get_mavlink_fd(),
 						 "#audio: DO JUMP repetitions completed");
 				/* no more DO_JUMPS, therefore just try to continue with next mission item */
-				(*mission_index)++;
+				(*mission_index_ptr)++;
 			}
 
 		} else {
 			/* if it's not a DO_JUMP, then we were successful */
+			memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s));
 			return true;
 		}
 	}