Create system state entry in dataman - ATTN Anton

Create persistent system state id for data manager to store system state
that will persist across resets.

4 files changed, 131 insertions, 8 deletions

diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 1a65313e8..96dc98f2a 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -61,6 +61,8 @@ __EXPORT int dataman_main(int argc, char *argv[]);
 __EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen);
 __EXPORT ssize_t dm_write(dm_item_t  item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen);
 __EXPORT int dm_clear(dm_item_t item);
+__EXPORT void dm_lock(dm_item_t item);
+__EXPORT void dm_unlock(dm_item_t item);
 __EXPORT int dm_restart(dm_reset_reason restart_type);
 
 /** Types of function calls supported by the worker task */
@@ -113,12 +115,17 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
 	DM_KEY_FENCE_POINTS_MAX,
 	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
 	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
-	DM_KEY_WAYPOINTS_ONBOARD_MAX
+	DM_KEY_WAYPOINTS_ONBOARD_MAX,
+	DM_KEY_SYSTEM_STATE_MAX
 };
 
 /* Table of offset for index 0 of each item type */
 static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
 
+/* Item type lock mutexes */
+static sem_t *g_item_locks[DM_KEY_NUM_KEYS];
+static sem_t g_sys_state_mutex;
+
 /* The data manager store file handle and file name */
 static int g_fd = -1, g_task_fd = -1;
 static const char *k_data_manager_device_path = "/fs/microsd/dataman";
@@ -138,22 +145,22 @@ static work_q_t g_work_q;	/* pending work items. To be consumed by worker thread
 sem_t g_work_queued_sema;	/* To notify worker thread a work item has been queued */
 sem_t g_init_sema;
 
-static bool g_task_should_exit;		/**< if true, dataman task should exit */
+static bool g_task_should_exit;	/**< if true, dataman task should exit */
 
 #define DM_SECTOR_HDR_SIZE 4	/* data manager per item header overhead */
 static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */
 
 static void init_q(work_q_t *q)
 {
-	sq_init(&(q->q));				/* Initialize the NuttX queue structure */
+	sq_init(&(q->q));		/* Initialize the NuttX queue structure */
 	sem_init(&(q->mutex), 1, 1);	/* Queue is initially unlocked */
-	q->size = q->max_size = 0;		/* Queue is initially empty */
+	q->size = q->max_size = 0;	/* Queue is initially empty */
 }
 
 static inline void
 destroy_q(work_q_t *q)
 {
-	sem_destroy(&(q->mutex));		/* Destroy the queue lock */
+	sem_destroy(&(q->mutex));	/* Destroy the queue lock */
 }
 
 static inline void
@@ -567,6 +574,32 @@ dm_clear(dm_item_t item)
 	return enqueue_work_item_and_wait_for_result(work);
 }
 
+__EXPORT void
+dm_lock(dm_item_t item)
+{
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return;
+	if (item >= DM_KEY_NUM_KEYS)
+		return;
+	if (g_item_locks[item]) {
+		sem_wait(g_item_locks[item]);
+	}
+}
+
+__EXPORT void
+dm_unlock(dm_item_t item)
+{
+	/* Make sure data manager has been started and is not shutting down */
+	if ((g_fd < 0) || g_task_should_exit)
+		return;
+	if (item >= DM_KEY_NUM_KEYS)
+		return;
+	if (g_item_locks[item]) {
+		sem_post(g_item_locks[item]);
+	}
+}
+
 /* Tell the data manager about the type of the last reset */
 __EXPORT int
 dm_restart(dm_reset_reason reason)
@@ -607,6 +640,12 @@ task_main(int argc, char *argv[])
 	for (unsigned i = 0; i < dm_number_of_funcs; i++)
 		g_func_counts[i] = 0;
 
+	/* Initialize the item type locks, for now only DM_KEY_SYSTEM_STATE supports locking */
+	sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */
+	for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++)
+		g_item_locks[i] = NULL;
+	g_item_locks[DM_KEY_SYSTEM_STATE] = &g_sys_state_mutex;
+
 	g_task_should_exit = false;
 
 	init_q(&g_work_q);
@@ -742,6 +781,7 @@ task_main(int argc, char *argv[])
 	destroy_q(&g_work_q);
 	destroy_q(&g_free_q);
 	sem_destroy(&g_work_queued_sema);
+	sem_destroy(&g_sys_state_mutex);
 
 	return 0;
 }
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index 1dfb26f73..f2b6cd4d3 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -53,6 +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_SYSTEM_STATE,		/* Persistent system state storage */
 		DM_KEY_NUM_KEYS			/* Total number of item types defined */
 	} dm_item_t;
 
@@ -62,7 +63,8 @@ extern "C" {
 		DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
 		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
+		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_SYSTEM_STATE_MAX = 1
 	};
 
 	/** Data persistence levels */
@@ -101,6 +103,18 @@ extern "C" {
 		size_t buflen			/* Length in bytes of data to retrieve */
 	);
 
+	/** Lock all items of this type */
+	__EXPORT void
+	dm_lock(
+		dm_item_t item			/* The item type to clear */
+		);
+
+	/** Unlock all items of this type */
+	__EXPORT void
+	dm_unlock(
+		dm_item_t item			/* The item type to clear */
+		);
+
 	/** Erase all items of this type */
 	__EXPORT int
 	dm_clear(
@@ -113,6 +127,16 @@ 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 current_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 e300be074..cb1b762a7 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -952,6 +952,8 @@ 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;
+
 	state->size = 0;
 	state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
 	state->current_state = MAVLINK_WPM_STATE_IDLE;
@@ -962,6 +964,10 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
 	state->timestamp_last_send_request = 0;
 	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
 	state->current_dataman_id = 0;
+	if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) {
+		if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1))
+			state->current_dataman_id = sys_state.current_offboard_waypoint_id;
+	}
 }
 
 /*
@@ -1428,12 +1434,38 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					mission.dataman_id = 0;
 				}
 
-				if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
+				if (dm_write(dm_next, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, len) != len) {
 					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");
 					_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 					break;
 				}
+				// offboard mission data saved correctly, now update the persistent system state
+				{
+					persistent_system_state_t state;
+					bool dm_result;
+					// Since we are doing a read-modify-write we must lock the item type
+					dm_lock(DM_KEY_SYSTEM_STATE);
+					// first read in the current state data. There may eventually be data other than the offboard index
+					// and we must preserve it
+					if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) {
+						// Not sure how to handle this? It means that either the item was never
+						// written, or fields have been added to the system state struct. In any case
+						// fields that may not be ours need to be initialized to sane values.
+						// For now the offboard index is the only field, so for now there
+						// is nothing to do here.
+					}
+					state.current_offboard_waypoint_id = mission.dataman_id;
+					dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state);
+					dm_unlock(DM_KEY_SYSTEM_STATE);
+					if (dm_result) {
+						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");
+						_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
+						break;
+					}
+				}
+
 
 //				if (wp.current) {
 //					warnx("current is: %d", wp.seq);
@@ -1485,7 +1517,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					publish_mission();
 
 					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);
+						persistent_system_state_t state;
+						bool dm_result;
+						dm_lock(DM_KEY_SYSTEM_STATE);
+						// first read in the current state data. There may eventually be data other than the offboard index
+						// and we must preserve it
+						if (dm_read(DM_KEY_SYSTEM_STATE, 0, &state, sizeof(state)) != sizeof(state)) {
+							// Not sure how to handle this? It means that either the item was never
+							// written, or fields have been added to the system state struct. In any case
+							// fields that may not be ours need to be initialized to sane values.
+							// For now the offboard index is the only field, so we can deal with it here.
+						}
+						state.current_offboard_waypoint_id = -1;
+						dm_result = dm_write(DM_KEY_SYSTEM_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) == sizeof(state);
+						dm_unlock(DM_KEY_SYSTEM_STATE);
+						if (dm_result) {
+							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);
+						}
 
 					} else {
 						mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 06e0c5904..04ea7da0e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -603,6 +603,15 @@ Navigator::task_main()
 			warnx("Could not clear geofence");
 	}
 
+#if 0 // *** UNTESTED... Anton, this is for you
+	/* Get the last offboard mission id */
+	persistent_system_state_t sys_state;
+	if (dm_read(DM_KEY_SYSTEM_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) {
+		if ((sys_state.current_offboard_waypoint_id >= 0) && (sys_state.current_offboard_waypoint_id <= 1))
+			_mission.set_offboard_dataman_id(sys_state.current_offboard_waypoint_id);
+	}
+#endif
+
 	/*
 	 * do subscriptions
 	 */