param lib: Provide used index lookup

2 files changed, 39 insertions, 2 deletions

diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 8dea6e6cc..5dfef9dce 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -92,7 +92,7 @@ struct param_wbuf_s {
 };
 
 // XXX this should be param_info_count, but need to work out linking
-uint8_t param_changed_storage[(600 / sizeof(uint8_t)) + 1] = {};
+uint8_t param_changed_storage[(700 / sizeof(uint8_t)) + 1] = {};
 
 /** flexible array holding modified parameter values */
 UT_array	*param_values;
@@ -265,8 +265,37 @@ param_count_used(void)
 param_t
 param_for_index(unsigned index)
 {
-	if (index < param_info_count)
+	if (index < param_info_count) {
 		return (param_t)index;
+	}
+
+	return PARAM_INVALID;
+}
+
+param_t
+param_for_used_index(unsigned index)
+{
+	if (index < param_info_count) {
+
+		/* walk all params and count */
+		int count = 0;
+
+		for (unsigned i = 0; i < (unsigned)param_info_count + 1; i++) {
+			for (unsigned j = 0; j < 8; j++) {
+				if (param_changed_storage[i] & (1 << j)) {
+
+					/* we found the right used count,
+					 * return the param value
+					 */
+					if (i == count) {
+						return (param_t)i;
+					}
+
+					count++;
+				}
+			}
+		}
+	}
 
 	return PARAM_INVALID;
 }
diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h
index b29a7e51d..9cbe3570b 100644
--- a/src/modules/systemlib/param/param.h
+++ b/src/modules/systemlib/param/param.h
@@ -130,6 +130,14 @@ __EXPORT bool		param_used(param_t param);
 __EXPORT param_t	param_for_index(unsigned index);
 
 /**
+ * Look up an used parameter by index.
+ *
+ * @param param		The parameter to obtain the index for.
+ * @return		The index of the parameter in use, or -1 if the parameter does not exist.
+ */
+__EXPORT param_t	param_for_used_index(unsigned index);
+
+/**
  * Look up the index of a parameter.
  *
  * @param param		The parameter to obtain the index for.