diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 7e1aa4d72e..8a368e4600 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/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 + +/* + 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