Waypoints/Navigator: Use two different dataman storage places, keep old waypoints until all new ones are written

src/modules/dataman/dataman.c

@@ -111,7 +111,8 @@ static unsigned g_func_counts[dm_number_of_funcs];
 static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
 	DM_KEY_SAFE_POINTS_MAX,
 	DM_KEY_FENCE_POINTS_MAX,
-	DM_KEY_WAYPOINTS_OFFBOARD_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
+	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
 	DM_KEY_WAYPOINTS_ONBOARD_MAX
 };
 

src/modules/dataman/dataman.h

@@ -50,7 +50,8 @@ extern "C" {
 	typedef enum {
 		DM_KEY_SAFE_POINTS = 0,		/* Safe points coordinates, safe point 0 is home point */
 		DM_KEY_FENCE_POINTS,		/* Fence vertex coordinates */
-		DM_KEY_WAYPOINTS_OFFBOARD,		/* Mission way point coordinates sent over mavlink */
+		DM_KEY_WAYPOINTS_OFFBOARD_0,	/* Mission way point coordinates sent over mavlink */
+		DM_KEY_WAYPOINTS_OFFBOARD_1,	/* (alernate between 0 and 1) */
 		DM_KEY_WAYPOINTS_ONBOARD,	/* Mission way point coordinates generated onboard */
 		DM_KEY_NUM_KEYS			/* Total number of item types defined */
 	} dm_item_t;
@@ -59,7 +60,8 @@ extern "C" {
 	enum {
 		DM_KEY_SAFE_POINTS_MAX = 8,
 		DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
-		DM_KEY_WAYPOINTS_OFFBOARD_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
 		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
 	};
 

src/modules/mavlink/waypoints.c

@@ -169,6 +169,8 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
 	state->timestamp_lastaction = 0;
 	// state->timestamp_last_send_setpoint = 0;
 	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
+
+	state->current_dataman_id = 0;
 	// state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
 	// state->idle = false;      				///< indicates if the system is following the waypoints or is waiting
 	// state->current_active_wp_id = -1;		///< id of current waypoint
@@ -612,6 +614,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						// wpm->yaw_reached = false;
 						// wpm->pos_reached = false;
 						
+
 						mission.current_index = wpc.seq;
 
 						publish_mission();
@@ -718,7 +721,15 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 					struct mission_item_s mission_item;
 					ssize_t len = sizeof(struct mission_item_s);
 					
-					if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, wpr.seq, &mission_item, len) == len) {
+					dm_item_t dm_current;
+
+					if (wpm->current_dataman_id == 0) {
+						dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+					} else {
+						dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+					}
+
+					if (dm_read(dm_current, wpr.seq, &mission_item, len) == len) {
 					
 						if (mission.current_index == wpr.seq) {
 							wp.current = true;
@@ -855,10 +866,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 						if (wpc.count > NUM_MISSIONS_SUPPORTED) {
 							warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
-						} else {
-							/* set count to 0 while copying */
-							mission.count = 0;
-							publish_mission();
 						}
 
 #ifdef MAVLINK_WPM_NO_PRINTF
@@ -992,7 +999,17 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 					size_t len = sizeof(struct mission_item_s);
 
-					if (dm_write(DM_KEY_WAYPOINTS_OFFBOARD, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
+					dm_item_t dm_next;
+
+					if (wpm->current_dataman_id == 0) {
+						dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
+						mission.dataman_id = 1;
+					} else {
+						dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
+						mission.dataman_id = 0;
+					}
+
+					if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
 						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 						break;
@@ -1038,6 +1055,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						
 						publish_mission();
 
+						wpm->current_dataman_id = mission.dataman_id;
 						wpm->size = wpm->current_count;
 
 						//get the new current waypoint
@@ -1132,9 +1150,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 				// wpm->pos_reached = false;
 
 				/* prepare mission topic */
+				mission.dataman_id = -1;
 				mission.count = 0;
+				mission.current_index = -1;
 
-				if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD) == OK) {
+				if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
 					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
 				} else {
 					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);

src/modules/mavlink/waypoints.h

@@ -110,6 +110,7 @@ struct mavlink_wpm_storage {
 	// uint64_t timestamp_firstinside_orbit;
 	// uint64_t timestamp_lastoutside_orbit;
 	uint32_t timeout;
+	int current_dataman_id;
 	// uint32_t delay_setpoint;
 	// float accept_range_yaw;
 	// float accept_range_distance;

src/modules/navigator/navigator_main.cpp

@@ -440,6 +440,7 @@ Navigator::offboard_mission_update()
 	struct mission_s offboard_mission;
 	if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
 
+		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
 		_mission.set_current_offboard_mission_index(offboard_mission.current_index);
 		_mission.set_offboard_mission_count(offboard_mission.count);
 

src/modules/navigator/navigator_mission.cpp

@@ -53,7 +53,8 @@ static const int ERROR = -1;
 
 
 Mission::Mission() : 
-
+	
+	_offboard_dataman_id(-1),
 	_current_offboard_mission_index(0),
 	_current_onboard_mission_index(0),
 	_offboard_mission_item_count(0),
@@ -67,6 +68,12 @@ Mission::~Mission()
 	
 }
 
+void
+Mission::set_offboard_dataman_id(int new_id)
+{
+	_offboard_dataman_id = new_id;
+}
+
 void
 Mission::set_current_offboard_mission_index(int new_index)
 {
@@ -132,8 +139,16 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool
 	/* otherwise fallback to offboard */
 	} else if (current_offboard_mission_available()) {
 
+		dm_item_t dm_current;
+
+		if (_offboard_dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+
 		const ssize_t len = sizeof(struct mission_item_s);
-		if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index, new_mission_item, len) != len) {
+		if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			_current_mission_type = MISSION_TYPE_NONE;
 			return ERROR;
@@ -166,8 +181,16 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
 	/* otherwise fallback to offboard */
 	} else if (next_offboard_mission_available()) {
 
+		dm_item_t dm_current;
+
+		if (_offboard_dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+
 		const ssize_t len = sizeof(struct mission_item_s);
-		if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
+		if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			return ERROR;
 		}

src/modules/navigator/navigator_mission.h

@@ -55,6 +55,7 @@ public:
 	 */
 	~Mission();
 
+	void		set_offboard_dataman_id(int new_id);
 	void		set_current_offboard_mission_index(int new_index);
 	void		set_current_onboard_mission_index(int new_index);
 	void		set_offboard_mission_count(unsigned new_count);
@@ -78,6 +79,7 @@ private:
 	bool		next_onboard_mission_available();
 	bool		next_offboard_mission_available();
 
+	int 		_offboard_dataman_id;
 	unsigned	_current_offboard_mission_index;
 	unsigned	_current_onboard_mission_index;
 	unsigned	_offboard_mission_item_count;		/** number of offboard mission items available */

src/modules/uORB/topics/mission.h

@@ -97,6 +97,7 @@ struct mission_item_s
 
 struct mission_s
 {
+	int dataman_id;			/**< default -1, there are two offboard storage places in the dataman: 0 or 1 */
 	unsigned count;			/**< count of the missions stored in the datamanager */
 	int current_index;		/**< default -1, start at the one changed latest */
 };