diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-07-04 17:29:20 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-07-04 17:29:20 +0200 |
commit | 3f307fa0d7d6e8901eefca28dac039af69a0a92b (patch) | |
tree | 80c9ae37cb3a70b70dd0c1ed3ac9a03e5b51262e /src/modules/dataman | |
parent | 829a317d23093894c3d8974cce887ab3c19eba08 (diff) | |
parent | 39dab45ac5f9321221eb86e23b0e203ae63f0936 (diff) | |
download | px4-firmware-3f307fa0d7d6e8901eefca28dac039af69a0a92b.tar.gz px4-firmware-3f307fa0d7d6e8901eefca28dac039af69a0a92b.tar.bz2 px4-firmware-3f307fa0d7d6e8901eefca28dac039af69a0a92b.zip |
Merge branch 'dataman_state_nav_rewrite' into follower2
Diffstat (limited to 'src/modules/dataman')
-rw-r--r-- | src/modules/dataman/dataman.c | 54 | ||||
-rw-r--r-- | src/modules/dataman/dataman.h | 16 |
2 files changed, 63 insertions, 7 deletions
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 406293bda..ca1fe9bbb 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -62,6 +62,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 */ @@ -114,12 +116,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_MISSION_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"; @@ -139,22 +146,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 @@ -318,7 +325,9 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v buffer[1] = persistence; buffer[2] = 0; buffer[3] = 0; - memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + if (count > 0) { + memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + } count += DM_SECTOR_HDR_SIZE; len = -1; @@ -568,6 +577,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) @@ -608,6 +643,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_MISSION_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_MISSION_STATE] = &g_sys_state_mutex; + g_task_should_exit = false; init_q(&g_work_q); @@ -743,6 +784,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..dbace21ef 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_MISSION_STATE, /* Persistent mission state */ 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_MISSION_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( |