diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c index 6f882cddc6..333fa3e885 100644 --- a/src/modules/systemlib/param/param_shmem.c +++ b/src/modules/systemlib/param/param_shmem.c @@ -1,5 +1,3 @@ - - /**************************************************************************** * * Copyright (c) 2015 Vijay Venkatraman. All rights reserved. @@ -125,6 +123,8 @@ uint64_t sync_other_prev_time = 0, sync_other_current_time = 0; extern void update_to_shmem(param_t param, union param_value_u value); extern int update_from_shmem(param_t param, union param_value_u *value); +extern void update_index_from_shmem(void); + static int param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved); unsigned char set_called_from_get = 0; @@ -968,6 +968,10 @@ param_export(int fd, bool only_unsaved) goto out; } + /* First of all, update the index which will call param_get for params + * that have recently been changed. */ + update_index_from_shmem(); + while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { int32_t i; diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c index 0a672d7280..c9ec801ca9 100644 --- a/src/platforms/posix/px4_layer/shmem_posix.c +++ b/src/platforms/posix/px4_layer/shmem_posix.c @@ -65,6 +65,7 @@ void init_shared_memory(void); void copy_params_to_shmem(struct param_info_s *); void update_to_shmem(param_t param, union param_value_u value); int update_from_shmem(param_t param, union param_value_u *value); +void update_index_from_shmem(void); uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0; static unsigned char adsp_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; @@ -184,7 +185,7 @@ void copy_params_to_shmem(struct param_info_s *param_info_base) #endif } - //PX4_DEBUG("written %u params to shmem offset %lu", param_count(), (unsigned char*)&shmem_info_p->params_count-(unsigned char*)shmem_info_p); + PX4_DEBUG("written %u params to shmem", param_count()); for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { shmem_info_p->krait_changed_index[i] = 0; @@ -230,7 +231,7 @@ void update_to_shmem(param_t param, union param_value_u value) } -static void update_index_from_shmem(void) +void update_index_from_shmem(void) { unsigned int i; diff --git a/src/platforms/qurt/px4_layer/shmem_qurt.c b/src/platforms/qurt/px4_layer/shmem_qurt.c index df77576f05..39a2e93d14 100644 --- a/src/platforms/qurt/px4_layer/shmem_qurt.c +++ b/src/platforms/qurt/px4_layer/shmem_qurt.c @@ -47,6 +47,8 @@ #include #include +//#define SHMEM_DEBUG + int mem_fd; unsigned char *map_base, *virt_addr; struct shmem_info *shmem_info_p; @@ -57,9 +59,19 @@ void init_shared_memory(void); void copy_params_to_shmem(struct param_info_s *); void update_to_shmem(param_t param, union param_value_u value); int update_from_shmem(param_t param, union param_value_u *value); +void update_index_from_shmem(void); uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0; static unsigned char krait_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; +// Small helper to get log2 for ints +static unsigned log2_for_int(unsigned v) { + unsigned r = 0; + while (v >>= 1) { + ++r; + } + return r; +} + struct param_wbuf_s { param_t param; union param_value_u val; @@ -206,7 +218,7 @@ void update_to_shmem(param_t param, union param_value_u value) } -static void update_index_from_shmem(void) +void update_index_from_shmem(void) { unsigned int i; @@ -215,10 +227,25 @@ static void update_index_from_shmem(void) return; } - //PX4_INFO("Updating index from shmem\n"); + PX4_DEBUG("Updating index from shmem\n"); for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - krait_changed_index[i] = shmem_info_p->krait_changed_index[i]; + // Check if any param has been changed. + if (krait_changed_index[i] != shmem_info_p->krait_changed_index[i]) { + + // If a param has changed, we need to find out which one. + // From the byte and bit that is different, we can resolve the param number. + unsigned bit = log2_for_int(krait_changed_index[i] ^ shmem_info_p->krait_changed_index[i]); + param_t param_to_get = i*8+bit; + + // Update our krait_changed_index as well. + krait_changed_index[i] = shmem_info_p->krait_changed_index[i]; + + // FIXME: this is a hack but it gets the param so that it gets added + // to the local list param_values in param_shmem.c. + int32_t dummy; + param_get(param_to_get, &dummy); + } } release_shmem_lock(); @@ -285,7 +312,7 @@ int update_from_shmem(param_t param, union param_value_u *value) //else {PX4_INFO("no change to param %s\n", param_name(param));} - //PX4_INFO("%s %d bit on krait changed index[%d]\n", (retval)?"cleared":"unchanged", bit_changed, byte_changed); + PX4_DEBUG("%s %d bit on krait changed index[%d]\n", (retval)?"cleared":"unchanged", bit_changed, byte_changed); return retval; }