Load missions correctly after restart. Fixes #2564

src/modules/navigator/mission.cpp

@@ -70,8 +70,8 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_dist_1wp(this, "MIS_DIST_1WP", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
 	_param_yawmode(this, "MIS_YAWMODE", false),
-	_onboard_mission({0}),
-	_offboard_mission({0}),
+	_onboard_mission{},
+	_offboard_mission{},
 	_current_onboard_mission_index(-1),
 	_current_offboard_mission_index(-1),
 	_need_takeoff(true),
@@ -110,32 +110,31 @@ Mission::on_inactive()
 			update_offboard_mission();
 		}
 
-		/* check if the home position became valid in the meantime */
-		if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) &&
-			!_home_inited && _navigator->home_position_valid()) {
+	} else {
+
+		/* load missions from storage */
+		mission_s mission_state;
 
-			dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+		dm_lock(DM_KEY_MISSION_STATE);
 
-			_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
-					dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-					_navigator->get_home_position()->alt, _navigator->home_position_valid(),
-					_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-					_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
+		/* read current state */
+		int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
 
-			_navigator->increment_mission_instance_count();
-			_navigator->set_mission_result_updated();
+		dm_unlock(DM_KEY_MISSION_STATE);
 
-			_home_inited = true;
+		if (read_res == sizeof(mission_s)) {
+			_offboard_mission.dataman_id = mission_state.dataman_id;
+			_offboard_mission.count = mission_state.count;
+			_current_offboard_mission_index = mission_state.current_seq;
+
+			warnx("LOADED MISSION STATE FROM STORAGE");
 		}
 
-	} else {
-		/* read mission topics on initialization */
 		_inited = true;
-
-		update_onboard_mission();
-		update_offboard_mission();
 	}
 
+	check_mission_valid();
+
 	/* require takeoff after non-loiter or landing */
 	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
 		_need_takeoff = true;
@@ -151,6 +150,8 @@ Mission::on_activation()
 void
 Mission::on_active()
 {
+	check_mission_valid();
+
 	/* check if anything has changed */
 	bool onboard_updated = false;
 	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@@ -272,6 +273,8 @@ Mission::update_offboard_mission()
 		_offboard_mission.count = 0;
 		_offboard_mission.current_seq = 0;
 		_current_offboard_mission_index = 0;
+
+		warnx("mission check failed");
 	}
 
 	set_current_offboard_mission_item();
@@ -652,6 +655,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 
 		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
 			/* mission item index out of bounds */
+			warnx("err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
 			return false;
 		}
 
@@ -795,3 +799,26 @@ Mission::set_mission_finished()
 	_navigator->get_mission_result()->finished = true;
 	_navigator->set_mission_result_updated();
 }
+
+bool
+Mission::check_mission_valid()
+{
+	/* check if the home position became valid in the meantime */
+	if (!_home_inited && _navigator->home_position_valid()) {
+
+		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+
+		_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
+				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
+				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
+				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+				_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
+
+		_navigator->increment_mission_instance_count();
+		_navigator->set_mission_result_updated();
+
+		_home_inited = true;
+	}
+
+	return _navigator->get_mission_result()->valid;
+}

src/modules/navigator/mission.h

 /****************************************************************************
  *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,6 +39,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Ban Siesta <bansiesta@gmail.com>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -165,6 +166,11 @@ private:
 	 */
 	void set_mission_finished();
 
+	/**
+	 * Check wether a mission is ready to go
+	 */
+	bool check_mission_valid();
+
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;