diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index bf78e812a3..b999d3beb2 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -102,9 +102,6 @@ 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; @@ -2404,9 +2401,6 @@ 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); } @@ -2700,17 +2694,6 @@ 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) { @@ -2721,11 +2704,18 @@ void AP_Param::check_default(AP_Param *ap, float *default_value) } } -void AP_Param::add_default(AP_Param *ap, float v, defaults_list*& list) +void AP_Param::add_default(AP_Param *ap, float v) { - if (list != nullptr) { + // Embedded defaults trump runtime, don't allow override + for (uint16_t i=0; inext) { + for (defaults_list *item = default_list; item; item = item->next) { // update existing entry if (item->ap == ap) { item->val = v; @@ -2741,8 +2731,8 @@ void AP_Param::add_default(AP_Param *ap, float v, defaults_list*& list) } new_item->ap = ap; new_item->val = v; - new_item->next = list; - list = new_item; + new_item->next = default_list; + default_list = new_item; } #endif // AP_PARAM_DEFAULTS_ENABLED diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 5dd43ef61f..a7f47c814b 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -572,7 +572,7 @@ public: protected: // store default value in linked list - static void add_default(AP_Param *ap, float v) { add_default(ap, v, default_list); } + static void add_default(AP_Param *ap, float v); private: static AP_Param *_singleton; @@ -801,11 +801,7 @@ private: 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); };