From 5b290920a76b39a4fb4310a90cf72445c2919fe9 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 13 Jun 2022 17:04:41 +0100 Subject: [PATCH] AP_Param: keep track of defualt values in linked list --- libraries/AP_Param/AP_Param.cpp | 119 ++++++++++++++++++++++++++++---- libraries/AP_Param/AP_Param.h | 48 +++++++++++-- 2 files changed, 149 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index e22c2402d0..dc6a609519 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -99,6 +99,11 @@ uint16_t AP_Param::num_read_only = 0; ObjectBuffer_TS 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); diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index f3cc37cf77..e577ff539f 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -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) {