Move MISSION_STATE read/write from mavlink to navigator and commander

6 files changed, 80 insertions, 118 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e992706ac..d2f8c2994 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -76,6 +76,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/safety.h>
+#include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
 #include <drivers/drv_led.h>
@@ -89,6 +90,7 @@
 #include <systemlib/cpuload.h>
 #include <systemlib/rc_check.h>
 #include <systemlib/state_table.h>
+#include <dataman/dataman.h>
 
 #include "px4_custom_mode.h"
 #include "commander_helper.h"
@@ -691,6 +693,11 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+	if (status_pub < 0) {
+		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
+		warnx("exiting.");
+		exit(ERROR);
+	}
 
 	/* armed topic */
 	orb_advert_t armed_pub;
@@ -708,10 +715,17 @@ int commander_thread_main(int argc, char *argv[])
 	struct home_position_s home;
 	memset(&home, 0, sizeof(home));
 
-	if (status_pub < 0) {
-		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
-		warnx("exiting.");
-		exit(ERROR);
+	/* init mission state */
+	mission_s mission;
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
+		if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
+			mavlink_log_info(mavlink_fd, "[cmd] dataman ID: %i, count: %u, current: %i",
+												mission.dataman_id, mission.count, mission.current_index);
+			orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
+			close(mission_pub);
+		} else {
+			mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed");
+		}
 	}
 
 	mavlink_log_info(mavlink_fd, "[cmd] started");
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index 215ec4c76..dbace21ef 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -53,7 +53,7 @@ extern "C" {
 		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_MISSION_STATE,			/* Persistent mission state */
+		DM_KEY_MISSION_STATE,		/* Persistent mission state */
 		DM_KEY_NUM_KEYS			/* Total number of item types defined */
 	} dm_item_t;
 
@@ -127,16 +127,6 @@ extern "C" {
 		dm_reset_reason restart_type	/* The last reset type */
 	);
 
-	/* NOTE: The following structure defines the persistent system state data stored in the single
-	   entry DM_KEY_SYSTEM_STATE_KEY item type. It contains global system state information that
-	   needs to survive restarts. This definition is application specific so it doesn't really belong
-	   in this header, but till I find it a better home here it is */
-
-	typedef struct {
-		char offboard_waypoint_id;	/* the index of the active offboard waypoint data */
-							/* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */
-	} persistent_system_state_t;
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 5c6ef68ee..bc4376b84 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -18,7 +18,6 @@
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
@@ -964,8 +963,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
 
 void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
 {
-	persistent_system_state_t sys_state;
-
 	/* init WPM state */
 	state->size = 0;
 	state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
@@ -980,40 +977,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
 	state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
 	state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT;
 
-	int sys_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state));
-	if (sys_state_size == sizeof(sys_state)) {
-		if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) {
-			state->dataman_id = sys_state.offboard_waypoint_id;
-
-			if (_verbose) { warnx("WPM init: using dataman ID %d", state->dataman_id); }
-
-			/* count waypoints in current waypoints storage */
-			dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
-			struct mission_item_s mission_item;
-
-			int seq = 0;
-			while (seq < state->max_size && dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
-				seq++;
-			}
-
-			if (_verbose) { warnx("WPM init: found %d items", seq); }
-
-			state->size = seq;
-
-		} else {
-			if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in MISSION_STATE", sys_state.offboard_waypoint_id); }
-		}
-
-	} else {
-		if (_verbose) { warnx("WPM init: dataman MISSION_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); }
-	}
-
-	/* init mission topic */
-	mission.count = state->size;
-	mission.dataman_id = state->dataman_id;
-	mission.current_index = -1;	// TODO store current index in dataman?
-
-	publish_mission();
+	
 }
 
 /*
@@ -1081,10 +1045,10 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin
 
 void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
 {
-	dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
+	dm_item_t dm_item = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
 	struct mission_item_s mission_item;
 
-	if (dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
+	if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
 
 		/* create mission_item_s from mavlink_mission_item_t */
 		mavlink_mission_item_t wp;
@@ -1103,7 +1067,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
 		mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
 		mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD");
 
-		if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, dm_id); }
+		if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, _wpm->dataman_id); }
 	}
 }
 
@@ -1214,9 +1178,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 						mission.current_index = wpc.seq;
 						publish_mission();
 
-						/* don't answer yet, wait for the navigator to respond, then publish the mission_result */
-//						mavlink_wpm_send_waypoint_current(wpc.seq);
-
 					} else {
 						if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); }
 
@@ -1392,6 +1353,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				_wpm->current_count = wpc.count;
 				_wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0;	// use inactive storage for transmission
 
+				mission.current_index = -1;
+
 				mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
 			}
 		}
@@ -1442,10 +1405,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					break;
 				}
 
-				dm_item_t dm_id = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
+				dm_item_t dm_item = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
 
-				if (dm_write(dm_id, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
-					if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, dm_id); }
+				if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
+					if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _wpm->current_dataman_id); }
 
 					mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
 					mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
@@ -1453,6 +1416,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					break;
 				}
 
+				/* waypoint marked as current */
+				if (wp.current) {
+					mission.current_index = wp.seq;
+				}
+
 				if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); }
 
 				_wpm->current_wp_id = wp.seq + 1;
@@ -1463,65 +1431,23 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 
-					/* write terminator item */
-					if (_wpm->current_count < _wpm->max_size) {
-						if (dm_write(dm_id, _wpm->current_count, DM_PERSIST_POWER_ON_RESET, nullptr, 0) != 0) {
-							if (_verbose) { warnx("WPM: MISSION_ITEM ERROR writing terminator item"); }
+					mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
 
-							mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
-							break;
-						}
-					}
+					/* update WPM state */
+					_wpm->dataman_id = _wpm->current_dataman_id;
+					_wpm->size = _wpm->current_count;
 
-					/* offboard mission data saved correctly, now update the persistent system state */
-					persistent_system_state_t state;
-					bool dm_result;
+					/* update mission topic */
+					mission.dataman_id = _wpm->dataman_id;
+					mission.count = _wpm->current_count;
 
-					/* since we are doing a read-modify-write we must lock the item type */
-					dm_lock(DM_KEY_MISSION_STATE);
-
-					/* first read in the current state data, there may eventually be data other than the offboard index
-					 * and we must preserve it */
-					ssize_t dm_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &state, sizeof(state));
-					if (dm_state_size != sizeof(state)) {
-						warnx("dataman MISSION_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state));
-					}
-
-					/* set new dataman storage ID */
-					state.offboard_waypoint_id = _wpm->current_dataman_id;
-
-					/* write back to dataman */
-					dm_result = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state);
-
-					dm_unlock(DM_KEY_MISSION_STATE);
-
-					if (dm_result) {
-						if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman MISSION_STATE", state.offboard_waypoint_id); }
-
-						mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
-						mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
-
-					} else {
-						mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
-
-						/* update WPM state */
-						_wpm->dataman_id = _wpm->current_dataman_id;
-						_wpm->size = _wpm->current_count;
-
-						/* update mission topic */
-						mission.dataman_id = _wpm->dataman_id;
-						mission.count = _wpm->current_count;
-						mission.current_index = 0;
-
-						publish_mission();
-					}
+					publish_mission();
 
 				} else {
 					/* request next item */
 					mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
 				}
 			}
-
 			break;
 		}
 
@@ -1545,6 +1471,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 						_wpm->size = 0;
 
 						/* update mission topic */
+						mission.dataman_id = _wpm->dataman_id;
 						mission.count = 0;
 						mission.current_index = -1;
 
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 9244063b1..d9d8353f6 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -431,6 +431,37 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 }
 
 void
+Mission::save_offboard_mission_state()
+{
+	mission_s mission_state;
+
+	/* lock MISSION_STATE item */
+	dm_lock(DM_KEY_MISSION_STATE);
+
+	/* read current state */
+	int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+
+	/* check if state actually changed to save flash write cycles */
+	if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id ||
+			mission_state.count != _offboard_mission.count ||
+	    		mission_state.current_index != _current_offboard_mission_index) {
+
+		mission_state.dataman_id = _offboard_mission.dataman_id;
+		mission_state.count = _offboard_mission.count;
+		mission_state.current_index = _current_offboard_mission_index;
+
+		/* write modifyed state only if changed */
+		if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state");
+
+		}
+	}
+
+	/* unlock MISSION_STATE item */
+	dm_unlock(DM_KEY_MISSION_STATE);
+}
+
+void
 Mission::report_mission_item_reached()
 {
 	if (_mission_type == MISSION_TYPE_OFFBOARD) {
@@ -445,6 +476,8 @@ Mission::report_current_offboard_mission_item()
 {
 	_mission_result.index_current_mission = _current_offboard_mission_index;
 	publish_mission_result();
+
+	save_offboard_mission_state();
 }
 
 void
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 322aaf96a..98b11bade 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -142,6 +142,11 @@ private:
 			       struct mission_item_s *new_mission_item);
 
 	/**
+	 * Save current offboard mission state to dataman
+	 */
+	void save_offboard_mission_state();
+
+	/**
 	 * Report that a mission item has been reached
 	 */
 	void report_mission_item_reached();
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 888c8e7f4..a3c190c7f 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -227,13 +227,6 @@ Navigator::task_main()
 			warnx("Could not clear geofence");
 	}
 
-	/* Get the last offboard mission id */
-	persistent_system_state_t sys_state;
-	if (dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) {
-		if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1))
-			_mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id);
-	}
-
 	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));