Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Param: greatly speed up param download with disabled parameters

Browse Source 
allows fast skip over disabled subgroups. This removes a long delay
with param download on a MatekF405-STD where the final parameters
associated with the OSD took 80ms to fetch, causing a long loop delay
This commit is contained in:
Andrew Tridgell 2021-01-21 16:57:19 +11:00
parent d19dca37d5
commit 6c684538d3
2 changed files with 44 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
libraries/AP_Param/AP_Param.cpp
View File

@ -1215,6 +1215,10 @@ void AP_Param::save_io_handler(void)
while (save_queue.pop(p)) {
p.param->save_sync(p.force_save);
}
if (hal.scheduler->is_system_initialized()) {
// pay the cost of parameter counting in the IO thread
count_parameters();
}
}
/*
@ -1531,9 +1535,6 @@ void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct
struct Param_header phdr;
uint16_t key;
// reset cached param counter as we may be loading a dynamic var_info
invalidate_count();
if (!find_key_by_pointer(object_pointer, key)) {
hal.console->printf("ERROR: Unable to find param pointer\n");
return;
@ -1575,6 +1576,9 @@ void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct
ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
}
}
// reset cached param counter as we may be loading a dynamic var_info
invalidate_count();
}
@ -1600,13 +1604,14 @@ AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype)
/// Returns the next variable in a group, recursing into groups
/// as needed
AP_Param *AP_Param::next_group(uint16_t vindex, const struct GroupInfo *group_info,
AP_Param *AP_Param::next_group(const uint16_t vindex, const struct GroupInfo *group_info,
bool *found_current,
uint32_t group_base,
uint8_t group_shift,
ptrdiff_t group_offset,
const uint32_t group_base,
const uint8_t group_shift,
const ptrdiff_t group_offset,
ParamToken *token,
enum ap_var_type *ptype)
enum ap_var_type *ptype,
bool skip_disabled)
{
enum ap_var_type type;
for (uint8_t i=0;
@ -1629,7 +1634,7 @@ AP_Param *AP_Param::next_group(uint16_t vindex, const struct GroupInfo *group_in
}
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);
group_shift + _group_level_shift, new_offset, token, ptype, skip_disabled);
if (ap != nullptr) {
return ap;
}
@ -1646,10 +1651,24 @@ AP_Param *AP_Param::next_group(uint16_t vindex, const struct GroupInfo *group_in
if (!get_base(_var_info[vindex], base)) {
continue;
}
return (AP_Param*)(base + group_info[i].offset + group_offset);
AP_Param *ret = (AP_Param*)(base + group_info[i].offset + group_offset);
if (skip_disabled &&
_hide_disabled_groups &&
group_info[i].type == AP_PARAM_INT8 &&
(group_info[i].flags & AP_PARAM_FLAG_ENABLE) &&
((AP_Int8 *)ret)->get() == 0) {
token->last_disabled = 1;
}
return ret;
}
if (group_id(group_info, group_base, i, group_shift) == token->group_element) {
*found_current = true;
if (token->last_disabled) {
token->last_disabled = 0;
return nullptr;
}
if (type == AP_PARAM_VECTOR3F && token->idx < 3) {
// return the next element of the vector as a
// float
@ -1673,7 +1692,7 @@ AP_Param *AP_Param::next_group(uint16_t vindex, const struct GroupInfo *group_in
/// Returns the next variable in _var_info, recursing into groups
/// as needed
AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype)
AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_disabled)
{
uint16_t i = token->key;
bool found_current = false;
@ -1707,7 +1726,7 @@ AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype)
if (group_info == nullptr) {
continue;
}
AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, 0, token, ptype);
AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, 0, token, ptype, skip_disabled);
if (ap != nullptr) {
return ap;
}
@ -1731,48 +1750,13 @@ AP_Param *AP_Param::next_scalar(ParamToken *token, enum ap_var_type *ptype)
{
AP_Param *ap;
enum ap_var_type type;
while ((ap = next(token, &type)) != nullptr && type > AP_PARAM_FLOAT) ;
while ((ap = next(token, &type, true)) != nullptr && type > AP_PARAM_FLOAT) ;
if (ap != nullptr && type == AP_PARAM_INT8) {
/*
check if this is an enable variable. To do that we need to
find the info structures for the variable
*/
uint32_t group_element;
const struct GroupInfo *ginfo;
struct GroupNesting group_nesting {};
uint8_t idx;
const struct AP_Param::Info *info = ap->find_var_info_token(*token, &group_element,
ginfo, group_nesting, &idx);
if (info && ginfo &&
(ginfo->flags & AP_PARAM_FLAG_ENABLE) &&
((AP_Int8 *)ap)->get() == 0 &&
_hide_disabled_groups) {
/*
this is a disabled parameter tree, include this
parameter but not others below it. We need to keep
looking until we go past the parameters in this object
*/
ParamToken token2 = *token;
enum ap_var_type type2;
AP_Param *ap2;
while ((ap2 = next(&token2, &type2)) != nullptr) {
if (token2.key != token->key) {
break;
}
if (group_nesting.level != 0 && (token->group_element & 0x3F) != (token2.group_element & 0x3F)) {
break;
}
// update the returned token so the next() call goes from this point
*token = token2;
}
if (ap != nullptr) {
if (ptype != nullptr) {
*ptype = type;
}
}
if (ap != nullptr && ptype != nullptr) {
*ptype = type;
}
return ap;
}

16
libraries/AP_Param/AP_Param.h
View File

@ -210,8 +210,9 @@ public:
// a token used for first()/next() state
typedef struct {
uint32_t key : 9;
uint32_t idx : 5; // offset into array types
uint32_t idx : 4; // offset into array types
uint32_t group_element : 18;
uint32_t last_disabled : 1;
} ParamToken;
@ -451,7 +452,7 @@ public:
/// Returns the next variable in _var_info, recursing into groups
/// as needed
static AP_Param * next(ParamToken *token, enum ap_var_type *ptype);
static AP_Param * next(ParamToken *token, enum ap_var_type *ptype, bool skip_disabled=false);
/// Returns the next scalar variable in _var_info, recursing into groups
/// as needed
@ -645,14 +646,15 @@ private:
uint16_t ofs,
uint8_t size);
static AP_Param * next_group(
uint16_t vindex,
const uint16_t vindex,
const struct GroupInfo *group_info,
bool *found_current,
uint32_t group_base,
uint8_t group_shift,
ptrdiff_t group_offset,
const uint32_t group_base,
const uint8_t group_shift,
const ptrdiff_t group_offset,
ParamToken *token,
enum ap_var_type *ptype);
enum ap_var_type *ptype,
bool skip_disabled);
// 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);