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

3 files changed, 29 insertions, 3 deletions

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index d93ecc7cd..48f828ff7 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/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);
 
diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp
index 993f8f133..6576aae70 100644
--- a/src/modules/navigator/navigator_mission.cpp
+++ b/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),
@@ -68,6 +69,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)
 {
 	if (new_index != -1) {
@@ -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;
 		}
diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h
index e8e476382..15d4e86bf 100644
--- a/src/modules/navigator/navigator_mission.h
+++ b/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 */