5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 22:48:29 -04:00

AP_Param: allow defualt values to be given by const float var

This commit is contained in:
Iampete1 2022-12-30 02:14:05 +00:00 committed by Andrew Tridgell
parent d30f53505c
commit 02af134ba6
2 changed files with 47 additions and 21 deletions
libraries/AP_Param

View File

@ -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<num_param_overrides; i++) {
if (vp == param_overrides[i].object_ptr) {
return param_overrides[i].value;
}
}
return *def_value_ptr;
if ((info.flags & AP_PARAM_FLAG_DEFAULT_POINTER) != 0) {
return *((float*)((ptrdiff_t)vp - info.def_value_offset));
}
return info.def_value;
}
float AP_Param::get_default_value(const AP_Param *vp, const struct Info &info)
{
for (uint16_t i=0; i<num_param_overrides; i++) {
if (vp == param_overrides[i].object_ptr) {
return param_overrides[i].value;
}
}
if ((info.flags & AP_PARAM_FLAG_DEFAULT_POINTER) != 0) {
return *((float*)((ptrdiff_t)vp - info.def_value_offset));
}
return info.def_value;
}

View File

@ -90,13 +90,16 @@
// hide parameter from param download
#define AP_PARAM_FLAG_HIDDEN (1<<6)
// Default value is a "pointer" actually its a offest from the base value, but the idea is the same
#define AP_PARAM_FLAG_DEFAULT_POINTER (1<<7)
// keep all flags before the FRAME tags
// vehicle and frame type flags, used to hide parameters when not
// relevent to a vehicle type. Use AP_Param::set_frame_type_flags() to
// enable parameters flagged in this way. frame type flags are stored
// in flags field, shifted by AP_PARAM_FRAME_TYPE_SHIFT.
#define AP_PARAM_FRAME_TYPE_SHIFT 7
#define AP_PARAM_FRAME_TYPE_SHIFT 8
// supported frame types for parameters
#define AP_PARAM_FRAME_COPTER (1<<0)
@ -124,6 +127,9 @@
// declare a group var_info line with both flags and frame type mask
#define AP_GROUPINFO_FLAGS_FRAME(name, idx, clazz, element, def, flags, frame_flags) AP_GROUPINFO_FLAGS(name, idx, clazz, element, def, flags|((frame_flags)<<AP_PARAM_FRAME_TYPE_SHIFT) )
// declare a group var_info line with a default "pointer"
#define AP_GROUPINFO_FLAGS_DEFAULT_POINTER(name, idx, clazz, element, def) { name, AP_VAROFFSET(clazz, element), {def_value_offset : AP_VAROFFSET(clazz, element) - AP_VAROFFSET(clazz, def)}, AP_PARAM_FLAG_DEFAULT_POINTER, idx, AP_CLASSTYPE(clazz, element) }
// declare a group var_info line
#define AP_GROUPINFO(name, idx, clazz, element, def) AP_GROUPINFO_FLAGS(name, idx, clazz, element, def, 0)
@ -186,6 +192,7 @@ public:
const struct GroupInfo *group_info;
const struct GroupInfo **group_info_ptr; // when AP_PARAM_FLAG_INFO_POINTER is set in flags
const float def_value;
ptrdiff_t def_value_offset; // Default value offset from param object, when AP_PARAM_FLAG_DEFAULT_POINTER is set in flags
};
uint16_t flags;
uint8_t idx; // identifier within the group
@ -198,6 +205,7 @@ public:
const struct GroupInfo *group_info;
const struct GroupInfo **group_info_ptr; // when AP_PARAM_FLAG_INFO_POINTER is set in flags
const float def_value;
ptrdiff_t def_value_offset; // Default value offset from param object, when AP_PARAM_FLAG_DEFAULT_POINTER is set in flags
};
uint16_t flags;
uint16_t key; // k_param_*
@ -706,7 +714,8 @@ private:
float *default_val);
// find a default value given a pointer to a default value in flash
static float get_default_value(const AP_Param *object_ptr, const float *def_value_ptr);
static float get_default_value(const AP_Param *object_ptr, const struct GroupInfo &info);
static float get_default_value(const AP_Param *object_ptr, const struct Info &info);
static bool parse_param_line(char *line, char **vname, float &value, bool &read_only);