AP_Param: allow a HAL_PARAM_DEFAULTS_PATH to override defaults

this allows a HAL_PARAM_DEFAULTS_PATH to be specified for a build to
override the default parameters for a build. This is useful to build a
firmware that has different default parameters

libraries/AP_Param/AP_Param.cpp

@@ -74,6 +74,9 @@ uint8_t AP_Param::_num_vars;
 // storage and naming information about all types that can be saved
 const AP_Param::Info *AP_Param::_var_info;
 
+struct AP_Param::param_override *AP_Param::param_overrides = NULL;
+uint16_t AP_Param::num_param_overrides = 0;
+
 // storage object
 StorageAccess AP_Param::_storage(StorageManager::StorageParam);
 
@@ -613,6 +616,30 @@ AP_Param::find(const char *name, enum ap_var_type *ptype)
     return NULL;
 }
 
+/*
+  find the def_value for a variable by name
+*/
+const float *
+AP_Param::find_def_value_ptr(const char *name)
+{
+    enum ap_var_type ptype;
+    AP_Param *vp = find(name, &ptype);
+    if (vp == NULL) {
+        return NULL;
+    }
+    uint32_t group_element;
+    const struct GroupInfo *ginfo;
+    uint8_t gidx;
+    const struct AP_Param::Info *info = vp->find_var_info(&group_element, &ginfo, &gidx);
+    if (info == NULL) {
+        return NULL;
+    }
+    if (ginfo != NULL) {
+        return &ginfo->def_value;
+    }
+    return &info->def_value;
+}
+
 // Find a variable by name.
 //
 AP_Param *
@@ -704,9 +731,9 @@ bool AP_Param::save(bool force_save)
         float v1 = cast_to_float((enum ap_var_type)phdr.type);
         float v2;
         if (ginfo != NULL) {
-            v2 = PGM_FLOAT(&ginfo->def_value);
+            v2 = get_default_value(&ginfo->def_value);
         } else {
-            v2 = PGM_FLOAT(&info->def_value);
+            v2 = get_default_value(&info->def_value);
         }
         if (v1 == v2 && !force_save) {
             return true;
@@ -763,9 +790,10 @@ bool AP_Param::load(void)
         if (ginfo != NULL) {
             uintptr_t base = PGM_POINTER(&info->ptr);
             set_value((enum ap_var_type)phdr.type, (void*)(base + PGM_UINT16(&ginfo->offset)),
-                      PGM_FLOAT(&ginfo->def_value));
+                      get_default_value(&ginfo->def_value));
         } else {
-            set_value((enum ap_var_type)phdr.type, (void*)PGM_POINTER(&info->ptr), PGM_FLOAT(&info->def_value));
+            set_value((enum ap_var_type)phdr.type, (void*)PGM_POINTER(&info->ptr), 
+                      get_default_value(&info->def_value));
         }
         return false;
     }
@@ -819,7 +847,7 @@ void AP_Param::setup_object_defaults(const void *object_pointer, const struct Gr
          i++) {
         if (type <= AP_PARAM_FLOAT) {
             void *ptr = (void *)(base + PGM_UINT16(&group_info[i].offset));
-            set_value((enum ap_var_type)type, ptr, PGM_FLOAT(&group_info[i].def_value));
+            set_value((enum ap_var_type)type, ptr, get_default_value(&group_info[i].def_value));
         }
     }
 }
@@ -852,7 +880,7 @@ void AP_Param::setup_sketch_defaults(void)
         uint8_t type = PGM_UINT8(&_var_info[i].type);
         if (type <= AP_PARAM_FLOAT) {
             void *ptr = (void*)PGM_POINTER(&_var_info[i].ptr);
-            set_value((enum ap_var_type)type, ptr, PGM_FLOAT(&_var_info[i].def_value));
+            set_value((enum ap_var_type)type, ptr, get_default_value(&_var_info[i].def_value));
         }
     }
 }
@@ -865,6 +893,14 @@ bool AP_Param::load_all(void)
     struct Param_header phdr;
     uint16_t ofs = sizeof(AP_Param::EEPROM_header);
 
+    /*
+      if the HAL specifies a defaults parameter file then override
+      defaults using that file
+     */
+#ifdef HAL_PARAM_DEFAULTS_PATH
+    load_defaults_file(HAL_PARAM_DEFAULTS_PATH);
+#endif
+
     while (ofs < _storage.size()) {
         _storage.read_block(&phdr, ofs, sizeof(phdr));
         // note that this is an || not an && for robustness
@@ -1214,3 +1250,121 @@ AP_Param *AP_Param::set_param_by_name(const char *pname, float value, enum ap_va
     return vp;
 }
 
+
+#if HAL_OS_POSIX_IO == 1
+#include <stdio.h>
+
+/*
+  parse a parameter file line
+ */
+bool AP_Param::parse_param_line(char *line, char **vname, float &value)
+{
+    if (line[0] == '#') {
+        return false;
+    }
+    char *saveptr = NULL;
+    char *pname = strtok_r(line, ", =\t", &saveptr);
+    if (pname == NULL) {
+        return false;
+    }
+    if (strlen(pname) > AP_MAX_NAME_SIZE) {
+        return false;
+    }
+    const char *value_s = strtok_r(NULL, ", =\t", &saveptr);
+    if (value_s == NULL) {
+        return false;
+    }
+    value = atof(value_s);
+    *vname = pname;
+    return true;
+}
+
+/*
+  load a default set of parameters from a file
+ */
+bool AP_Param::load_defaults_file(const char *filename)
+{
+    FILE *f = fopen(filename, "r");
+    if (f == NULL) {
+        return false;
+    }
+    char line[100];
+
+    /*
+      work out how many parameter default structures to allocate
+     */
+    uint16_t num_defaults = 0;
+    while (fgets(line, sizeof(line)-1, f)) {
+        char *pname;
+        float value;
+        if (!parse_param_line(line, &pname, value)) {
+            continue;
+        }
+        if (!find_def_value_ptr(pname)) {
+            fclose(f);
+            return false;
+        }
+        num_defaults++;
+    }
+    fclose(f);
+
+    if (param_overrides != NULL) {
+        free(param_overrides);
+    }
+    num_param_overrides = 0;
+
+    param_overrides = new param_override[num_defaults];
+    if (param_overrides == NULL) {
+        return false;
+    }
+
+    /* 
+       re-open to avoid possible seek issues with NuttX
+     */
+    f = fopen(filename, "r");
+    if (f == NULL) {
+        return false;
+    }
+
+    uint16_t idx = 0;
+    while (fgets(line, sizeof(line)-1, f)) {
+        char *pname;
+        float value;
+        if (!parse_param_line(line, &pname, value)) {
+            continue;
+        }
+        const float *def_value_ptr = find_def_value_ptr(pname);
+        if (!def_value_ptr) {
+            fclose(f);
+            return false;
+        }
+        param_overrides[idx].def_value_ptr = def_value_ptr;
+        param_overrides[idx].value = value;
+        idx++;
+        if (!set_param_by_name(pname, value, NULL)) {
+            fclose(f);
+            return false;
+        }
+    }
+    fclose(f);
+
+    num_param_overrides = num_defaults;
+
+    return true;
+}
+
+#endif // HAL_OS_POSIX_IO
+
+/* 
+   find a default value given a pointer to a default value in flash
+ */
+float AP_Param::get_default_value(const float *def_value_ptr)
+{
+    for (uint16_t i=0; i<num_param_overrides; i++) {
+        if (def_value_ptr == param_overrides[i].def_value_ptr) {
+            return param_overrides[i].value;
+        }
+    }
+    return PGM_FLOAT(def_value_ptr);
+}
+

libraries/AP_Param/AP_Param.h

@@ -353,10 +353,36 @@ private:
                                     ParamToken *token,
                                     enum ap_var_type *ptype);
 
+    // find a default value given a pointer to a default value in flash
+    static float get_default_value(const float *def_value_ptr);
+
+    /*
+      find the def_value for a variable by name
+    */
+    static const float *find_def_value_ptr(const char *name);
+
+#if HAL_OS_POSIX_IO == 1
+    /*
+      load a parameter defaults file. This happens as part of load_all()
+     */
+    static bool parse_param_line(char *line, char **vname, float &value);
+    static bool load_defaults_file(const char *filename);
+#endif
+
     static StorageAccess        _storage;
     static uint8_t              _num_vars;
     static const struct Info *  _var_info;
 
+    /*
+      list of overridden values from load_defaults_file()
+    */
+    struct param_override {
+        const float *def_value_ptr;
+        float value;
+    };
+    static struct param_override *param_overrides;
+    static uint16_t num_param_overrides;
+
     // values filled into the EEPROM header
     static const uint8_t        k_EEPROM_magic0      = 0x50;
     static const uint8_t        k_EEPROM_magic1      = 0x41; ///< "AP"