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

2 files changed, 28 insertions, 7 deletions

diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 0998cd5eb..45e891434 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/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);
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index fc68285e9..801bc0bcf 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/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;