Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Param: keep track of defualt values in linked list

This commit is contained in:
Iampete1 2022-06-13 17:04:41 +01:00 committed by Andrew Tridgell
parent 040e08f4b1
commit 5b290920a7
2 changed files with 149 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
libraries/AP_Param/AP_Param.cpp
View File

@ -99,6 +99,11 @@ uint16_t AP_Param::num_read_only = 0;
ObjectBuffer_TS<AP_Param::param_save> AP_Param::save_queue{30};
bool AP_Param::registered_save_handler;
AP_Param::defaults_list *AP_Param::default_list;
#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
AP_Param::defaults_list *AP_Param::embedded_default_list;
#endif
// we need a dummy object for the parameter save callback
static AP_Param save_dummy;
@ -1631,7 +1636,7 @@ void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct
// return the first variable in _var_info
AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype)
AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype, float *default_val)
{
token->key = 0;
token->group_element = 0;
@ -1639,14 +1644,20 @@ AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype)
if (_num_vars == 0) {
return nullptr;
}
if (ptype != nullptr) {
*ptype = (enum ap_var_type)var_info(0).type;
}
ptrdiff_t base;
if (!get_base(var_info(0), base)) {
// should be impossible, first var needs to be non-pointer
return nullptr;
}
if (ptype != nullptr) {
*ptype = (enum ap_var_type)var_info(0).type;
}
#if AP_PARAM_DEFAULTS_ENABLED
if (default_val != nullptr) {
*default_val = var_info(0).def_value;
}
check_default((AP_Param *)base, default_val);
#endif
return (AP_Param *)base;
}
@ -1659,7 +1670,8 @@ AP_Param *AP_Param::next_group(const uint16_t vindex, const struct GroupInfo *gr
const ptrdiff_t group_offset,
ParamToken *token,
enum ap_var_type *ptype,
bool skip_disabled)
bool skip_disabled,
float *default_val)
{
enum ap_var_type type;
for (uint8_t i=0;
@ -1682,7 +1694,7 @@ AP_Param *AP_Param::next_group(const uint16_t vindex, const struct GroupInfo *gr
}
ap = next_group(vindex, ginfo, found_current, group_id(group_info, group_base, i, group_shift),
group_shift + _group_level_shift, new_offset, token, ptype, skip_disabled);
group_shift + _group_level_shift, new_offset, token, ptype, skip_disabled, default_val);
if (ap != nullptr) {
return ap;
}
@ -1709,6 +1721,11 @@ AP_Param *AP_Param::next_group(const uint16_t vindex, const struct GroupInfo *gr
((AP_Int8 *)ret)->get() == 0) {
token->last_disabled = 1;
}
#if AP_PARAM_DEFAULTS_ENABLED
if (default_val != nullptr) {
*default_val = group_info[i].def_value;
}
#endif
return ret;
}
if (group_id(group_info, group_base, i, group_shift) == token->group_element) {
@ -1728,6 +1745,11 @@ 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);
return (AP_Param *)ofs;
@ -1740,7 +1762,7 @@ AP_Param *AP_Param::next_group(const uint16_t vindex, const struct GroupInfo *gr
/// Returns the next variable in _var_info, recursing into groups
/// as needed
AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_disabled)
AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_disabled, float *default_val)
{
uint16_t i = token->key;
bool found_current = false;
@ -1757,6 +1779,11 @@ AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_d
if (ptype != nullptr) {
*ptype = AP_PARAM_FLOAT;
}
#if AP_PARAM_DEFAULTS_ENABLED
if (default_val != nullptr) {
*default_val = var_info(i).def_value;
}
#endif
return (AP_Param *)(((token->idx - 1u)*sizeof(float))+(ptrdiff_t)var_info(i).ptr);
}
@ -1775,7 +1802,7 @@ AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_d
if (group_info == nullptr) {
continue;
}
AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, 0, token, ptype, skip_disabled);
AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, 0, token, ptype, skip_disabled, default_val);
if (ap != nullptr) {
return ap;
}
@ -1787,6 +1814,11 @@ AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_d
if (ptype != nullptr) {
*ptype = type;
}
#if AP_PARAM_DEFAULTS_ENABLED
if (default_val != nullptr) {
*default_val = info.def_value;
}
#endif
return (AP_Param *)(info.ptr);
}
}
@ -1795,17 +1827,20 @@ AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_d
/// Returns the next scalar in _var_info, recursing into groups
/// as needed
AP_Param *AP_Param::next_scalar(ParamToken *token, enum ap_var_type *ptype)
AP_Param *AP_Param::next_scalar(ParamToken *token, enum ap_var_type *ptype, float *default_val)
{
AP_Param *ap;
enum ap_var_type type;
while ((ap = next(token, &type, true)) != nullptr && type > AP_PARAM_FLOAT) ;
while ((ap = next(token, &type, true, default_val)) != nullptr && type > AP_PARAM_FLOAT) ;
if (ap != nullptr) {
if (ptype != nullptr) {
*ptype = type;
}
}
#if AP_PARAM_DEFAULTS_ENABLED
check_default(ap, default_val);
#endif
return ap;
}
@ -2380,6 +2415,9 @@ void AP_Param::load_embedded_param_defaults(bool last_pass)
num_read_only++;
}
idx++;
#if AP_PARAM_DEFAULTS_ENABLED
add_default(vp, value, embedded_default_list);
#endif
if (!vp->configured_in_storage()) {
vp->set_float(value, var_type);
}
@ -2651,6 +2689,59 @@ bool AP_Param::set_and_save_by_name_ifchanged(const char *name, float value)
return false;
}
#if AP_PARAM_DEFAULTS_ENABLED
void AP_Param::check_default(AP_Param *ap, float *default_value)
{
if (default_value == nullptr || ap == nullptr) {
return;
}
#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
// Check embedded list first
if (embedded_default_list != nullptr) {
for (defaults_list *item = embedded_default_list; item; item = item->next) {
if (item->ap == ap) {
*default_value = item->val;
return;
}
}
}
#endif
if (default_list != nullptr) {
for (defaults_list *item = default_list; item; item = item->next) {
if (item->ap == ap) {
*default_value = item->val;
return;
}
}
}
}
void AP_Param::add_default(AP_Param *ap, float v, defaults_list*& list)
{
if (list != nullptr) {
// check is param is already in list
for (defaults_list *item = list; item; item = item->next) {
// update existing entry
if (item->ap == ap) {
item->val = v;
return;
}
}
}
// add to list
defaults_list *new_item = new defaults_list;
if (new_item == nullptr) {
return;
}
new_item->ap = ap;
new_item->val = v;
new_item->next = list;
list = new_item;
}
#endif // AP_PARAM_DEFAULTS_ENABLED
#if AP_PARAM_KEY_DUMP
/*
do not remove this show_all() code, it is essential for debugging
@ -2695,12 +2786,14 @@ void AP_Param::show_all(AP_HAL::BetterStream *port, bool showKeyValues)
ParamToken token;
AP_Param *ap;
enum ap_var_type type;
float default_value = nanf("0x4152"); // from logger quiet_nanf
for (ap=AP_Param::first(&token, &type);
for (ap=AP_Param::first(&token, &type, &default_value);
ap;
ap=AP_Param::next_scalar(&token, &type)) {
ap=AP_Param::next_scalar(&token, &type, &default_value)) {
if (showKeyValues) {
::printf("Key %u: Index %u: GroupElement %u : ", (unsigned)var_info(token.key).key, (unsigned)token.idx, (unsigned)token.group_element);
::printf("Key %u: Index %u: GroupElement %u : Default %f :", (unsigned)var_info(token.key).key, (unsigned)token.idx, (unsigned)token.group_element, default_value);
default_value = nanf("0x4152");
}
show(ap, token, type, port);
hal.scheduler->delay(1);

48
libraries/AP_Param/AP_Param.h
View File

@ -35,6 +35,12 @@
// optionally enable debug code for dumping keys
#define AP_PARAM_KEY_DUMP 0
#if defined(HAL_GCS_ENABLED)
#define AP_PARAM_DEFAULTS_ENABLED HAL_GCS_ENABLED
#else
#define AP_PARAM_DEFAULTS_ENABLED 1
#endif
/*
maximum size of embedded parameter file
*/
@ -469,15 +475,16 @@ public:
/// @return The first variable in _var_info, or nullptr if
/// there are none.
///
static AP_Param * first(ParamToken *token, enum ap_var_type *ptype);
static AP_Param * first(ParamToken *token, enum ap_var_type *ptype, float *default_val = nullptr);
/// Returns the next variable in _var_info, recursing into groups
/// as needed
static AP_Param * next(ParamToken *token, enum ap_var_type *ptype, bool skip_disabled=false);
static AP_Param * next(ParamToken *token, enum ap_var_type *ptype) { return next(token, ptype, false); }
static AP_Param * next(ParamToken *token, enum ap_var_type *ptype, bool skip_disabled, float *default_val = nullptr);
/// Returns the next scalar variable in _var_info, recursing into groups
/// as needed
static AP_Param * next_scalar(ParamToken *token, enum ap_var_type *ptype);
static AP_Param * next_scalar(ParamToken *token, enum ap_var_type *ptype, float *default_val = nullptr);
/// get the size of a type in bytes
static uint8_t type_size(enum ap_var_type type);
@ -539,7 +546,12 @@ public:
static bool add_param(uint8_t key, uint8_t param_num, const char *pname, float default_value);
static bool load_int32(uint16_t key, uint32_t group_element, int32_t &value);
#endif
protected:
// store default value in linked list
static void add_default(AP_Param *ap, float v) { add_default(ap, v, default_list); }
private:
static AP_Param *_singleton;
@ -676,7 +688,8 @@ private:
const ptrdiff_t group_offset,
ParamToken *token,
enum ap_var_type *ptype,
bool skip_disabled);
bool skip_disabled,
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);
@ -761,6 +774,19 @@ private:
// background function for saving parameters
void save_io_handler(void);
// Store default values from add_default() calls in linked list
struct defaults_list {
AP_Param *ap;
float val;
defaults_list *next;
};
#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
static defaults_list *embedded_default_list;
#endif
static defaults_list *default_list;
static void add_default(AP_Param *ap, float v, defaults_list *&list);
static void check_default(AP_Param *ap, float *default_value);
};
namespace AP {
@ -804,11 +830,23 @@ public:
/// Sets if the parameter is unconfigured
///
void set_default(const T &v) {
#if AP_PARAM_DEFAULTS_ENABLED
add_default(this, (float)v);
#endif
if (!configured()) {
set(v);
}
}
/// Sets parameter and default
///
void set_and_default(const T &v) {
#if AP_PARAM_DEFAULTS_ENABLED
add_default(this, (float)v);
#endif
set(v);
}
/// Value setter - set value, tell GCS
///
void set_and_notify(const T &v) {