param_shmem: fix bug where params didn't stick

There was the case where a param was changed on the Linux side but the
change did not get saved on the DSP side because the param was not in
the local list of changed params on the DSP side. On save, the param
index is now refreshed, and param_get is called on all params that have
changed. This is a hacky workaround but resolves the problem for now.

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;

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;
 

src/platforms/qurt/px4_layer/shmem_qurt.c

@@ -47,6 +47,8 @@
 #include <shmem.h>
 #include <drivers/drv_hrt.h>
 
+//#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;
 }