From a6cf04a6ff623b7d39a97c70f837198b6c064f5b Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 8 Jun 2014 14:08:22 -0400 Subject: 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. --- src/modules/dataman/dataman.c | 50 ++++++++++++++++++++++++++--- src/modules/dataman/dataman.h | 26 ++++++++++++++- src/modules/mavlink/mavlink_main.cpp | 54 ++++++++++++++++++++++++++++++-- src/modules/navigator/navigator_main.cpp | 9 ++++++ 4 files changed, 131 insertions(+), 8 deletions(-) (limited to 'src/modules') 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 */ -- cgit v1.2.3