1 files changed, 42 insertions, 5 deletions

diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 8dea6e6cc..4ec885ab3 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 (index == count) {
+						return (param_t)i;
+					}
+
+					count++;
+				}
+			}
+		}
+	}
 
 	return PARAM_INVALID;
 }
@@ -274,8 +303,9 @@ param_for_index(unsigned index)
 int
 param_get_index(param_t param)
 {
-	if (handle_in_range(param))
+	if (handle_in_range(param)) {
 		return (unsigned)param;
+	}
 
 	return -1;
 }
@@ -283,7 +313,9 @@ param_get_index(param_t param)
 int
 param_get_used_index(param_t param)
 {
-	if (!handle_in_range(param)) {
+	int param_storage_index = param_get_index(param);
+
+	if (param_storage_index < 0) {
 		return -1;
 	}
 
@@ -293,12 +325,17 @@ param_get_used_index(param_t param)
 	for (unsigned i = 0; i < (unsigned)param + 1; i++) {
 		for (unsigned j = 0; j < 8; j++) {
 			if (param_changed_storage[i] & (1 << j)) {
+
+				if (param_storage_index == i) {
+					return count;
+				}
+
 				count++;
 			}
 		}
 	}
 
-	return count;
+	return -1;
 }
 
 const char *