diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index c8ea10bf69..bf78e812a3 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -1177,9 +1177,9 @@ void AP_Param::save_sync(bool force_save, bool send_to_gcs) float v1 = cast_to_float((enum ap_var_type)phdr.type); float v2; if (ginfo != nullptr) { - v2 = get_default_value(this, &ginfo->def_value); + v2 = get_default_value(this, *ginfo); } else { - v2 = get_default_value(this, &info->def_value); + v2 = get_default_value(this, *info); } if (is_equal(v1,v2) && !force_save) { if (send_to_gcs) { @@ -1317,10 +1317,10 @@ bool AP_Param::load(void) group_offset += group_nesting.group_ret[i]->offset; } set_value((enum ap_var_type)phdr.type, (void*)(base + ginfo->offset + group_offset), - get_default_value(this, &ginfo->def_value)); + get_default_value(this, *ginfo)); } else { set_value((enum ap_var_type)phdr.type, (void*)base, - get_default_value(this, &info->def_value)); + get_default_value(this, *info)); } return false; } @@ -1448,11 +1448,11 @@ void AP_Param::setup_object_defaults(const void *object_pointer, const struct Gr if (type <= AP_PARAM_FLOAT) { void *ptr = (void *)(base + group_info[i].offset); set_value((enum ap_var_type)type, ptr, - get_default_value((const AP_Param *)ptr, &group_info[i].def_value)); + get_default_value((const AP_Param *)ptr, group_info[i])); } else if (type == AP_PARAM_VECTOR3F) { // Single default for all components void *ptr = (void *)(base + group_info[i].offset); - const float default_val = get_default_value((const AP_Param *)ptr, &group_info[i].def_value); + const float default_val = get_default_value((const AP_Param *)ptr, group_info[i]); ((AP_Vector3f *)ptr)->set(Vector3f{default_val, default_val, default_val}); } } @@ -1493,7 +1493,7 @@ void AP_Param::setup_sketch_defaults(void) ptrdiff_t base; if (get_base(info, base)) { set_value((enum ap_var_type)type, (void*)base, - get_default_value((const AP_Param *)base, &info.def_value)); + get_default_value((const AP_Param *)base, info)); } } } @@ -1647,7 +1647,7 @@ AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype, float *def } #if AP_PARAM_DEFAULTS_ENABLED if (default_val != nullptr) { - *default_val = var_info(0).def_value; + *default_val = get_default_value((AP_Param *)base, var_info(0)); } check_default((AP_Param *)base, default_val); #endif @@ -1716,7 +1716,7 @@ AP_Param *AP_Param::next_group(const uint16_t vindex, const struct GroupInfo *gr } #if AP_PARAM_DEFAULTS_ENABLED if (default_val != nullptr) { - *default_val = group_info[i].def_value; + *default_val = get_default_value(ret, group_info[i]); } #endif return ret; @@ -1738,13 +1738,13 @@ AP_Param *AP_Param::next_group(const uint16_t vindex, const struct GroupInfo *gr if (!get_base(var_info(vindex), base)) { continue; } -#if AP_PARAM_DEFAULTS_ENABLED - if (default_val != nullptr) { - *default_val = group_info[i].def_value; - } -#endif ptrdiff_t ofs = base + group_info[i].offset + group_offset; ofs += sizeof(float)*(token->idx - 1u); +#if AP_PARAM_DEFAULTS_ENABLED + if (default_val != nullptr) { + *default_val = get_default_value((AP_Param *)ofs, group_info[i]); + } +#endif return (AP_Param *)ofs; } } @@ -1772,12 +1772,13 @@ AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_d if (ptype != nullptr) { *ptype = AP_PARAM_FLOAT; } + AP_Param *ret = (AP_Param *)(((token->idx - 1u)*sizeof(float))+(ptrdiff_t)var_info(i).ptr); #if AP_PARAM_DEFAULTS_ENABLED if (default_val != nullptr) { - *default_val = var_info(i).def_value; + *default_val = get_default_value(ret, var_info(i)); } #endif - return (AP_Param *)(((token->idx - 1u)*sizeof(float))+(ptrdiff_t)var_info(i).ptr); + return ret; } if (type != AP_PARAM_GROUP) { @@ -1809,7 +1810,7 @@ AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_d } #if AP_PARAM_DEFAULTS_ENABLED if (default_val != nullptr) { - *default_val = info.def_value; + *default_val = get_default_value((AP_Param *)info.ptr, info); } #endif return (AP_Param *)(info.ptr); @@ -2417,14 +2418,30 @@ void AP_Param::load_embedded_param_defaults(bool last_pass) /* find a default value given a pointer to a default value in flash */ -float AP_Param::get_default_value(const AP_Param *vp, const float *def_value_ptr) +float AP_Param::get_default_value(const AP_Param *vp, const struct GroupInfo &info) { for (uint16_t i=0; i