diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 63b5cc1e81..df4db717ad 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -167,13 +167,15 @@ bool AP_Param::check_group_info(const struct AP_Param::GroupInfo * group_info, } if (type == AP_PARAM_GROUP) { // a nested group - const struct GroupInfo *ginfo = group_info[i].group_info; if (group_shift + _group_level_shift >= _group_bits) { Debug("double group nesting in %s", group_info[i].name); return false; } - if (ginfo == nullptr || - !check_group_info(ginfo, total_size, group_shift + _group_level_shift, prefix_length + strlen(group_info[i].name))) { + const struct GroupInfo *ginfo = get_group_info(group_info[i]); + if (ginfo == nullptr) { + continue; + } + if (!check_group_info(ginfo, total_size, group_shift + _group_level_shift, prefix_length + strlen(group_info[i].name))) { return false; } continue; @@ -210,6 +212,28 @@ bool AP_Param::duplicate_key(uint16_t vindex, uint16_t key) return false; } +/* + get group_info pointer for a group + */ +const struct AP_Param::GroupInfo *AP_Param::get_group_info(const struct GroupInfo &ginfo) +{ + if (ginfo.flags & AP_PARAM_FLAG_INFO_POINTER) { + return *ginfo.group_info_ptr; + } + return ginfo.group_info; +} + +/* + get group_info pointer for a group + */ +const struct AP_Param::GroupInfo *AP_Param::get_group_info(const struct Info &info) +{ + if (info.flags & AP_PARAM_FLAG_INFO_POINTER) { + return *info.group_info_ptr; + } + return info.group_info; +} + // validate the _var_info[] table bool AP_Param::check_var_info(void) { @@ -223,9 +247,11 @@ bool AP_Param::check_var_info(void) // first element can't be a group, for first() call return false; } - const struct GroupInfo *group_info = _var_info[i].group_info; - if (group_info == nullptr || - !check_group_info(group_info, &total_size, 0, strlen(_var_info[i].name))) { + const struct GroupInfo *group_info = get_group_info(_var_info[i]); + if (group_info == nullptr) { + continue; + } + if (!check_group_info(group_info, &total_size, 0, strlen(_var_info[i].name))) { return false; } } else { @@ -343,7 +369,10 @@ const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header // setup() ! return nullptr; } - const struct GroupInfo *ginfo = group_info[i].group_info; + const struct GroupInfo *ginfo = get_group_info(group_info[i]); + if (ginfo == nullptr) { + continue; + } ptrdiff_t new_offset = group_offset; if (!adjust_group_offset(vindex, group_info[i], new_offset)) { @@ -384,7 +413,10 @@ const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr, continue; } if (type == AP_PARAM_GROUP) { - const struct GroupInfo *group_info = _var_info[i].group_info; + const struct GroupInfo *group_info = get_group_info(_var_info[i]); + if (group_info == nullptr) { + continue; + } return find_by_header_group(phdr, ptr, i, group_info, 0, 0, 0); } if (type == phdr.type) { @@ -421,7 +453,10 @@ const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInf i++) { ptrdiff_t ofs = group_info[i].offset + group_offset; if (type == AP_PARAM_GROUP) { - const struct GroupInfo *ginfo = group_info[i].group_info; + const struct GroupInfo *ginfo = get_group_info(group_info[i]); + if (ginfo == nullptr) { + continue; + } // a nested group if (group_shift + _group_level_shift >= _group_bits) { // too deeply nested - this should have been caught by @@ -483,7 +518,10 @@ const struct AP_Param::Info *AP_Param::find_var_info(uint32_t * continue; } if (type == AP_PARAM_GROUP) { - const struct GroupInfo *group_info = _var_info[i].group_info; + const struct GroupInfo *group_info = get_group_info(_var_info[i]); + if (group_info == nullptr) { + continue; + } const struct AP_Param::Info *info; info = find_var_info_group(group_info, i, 0, 0, 0, group_element, group_ret, group_nesting, idx); if (info != nullptr) { @@ -523,7 +561,10 @@ const struct AP_Param::Info *AP_Param::find_var_info_token(const ParamToken &tok group_ret = nullptr; if (type == AP_PARAM_GROUP) { - const struct GroupInfo *group_info = _var_info[i].group_info; + const struct GroupInfo *group_info = get_group_info(_var_info[i]); + if (group_info == nullptr) { + return nullptr; + } const struct AP_Param::Info *info; info = find_var_info_group(group_info, i, 0, 0, 0, group_element, group_ret, group_nesting, idx); if (info != nullptr) { @@ -705,7 +746,10 @@ AP_Param::find_group(const char *name, uint16_t vindex, ptrdiff_t group_offset, if (strncasecmp(name, group_info[i].name, strlen(group_info[i].name)) != 0) { continue; } - const struct GroupInfo *ginfo = group_info[i].group_info; + const struct GroupInfo *ginfo = get_group_info(group_info[i]); + if (ginfo == nullptr) { + continue; + } ptrdiff_t new_offset = group_offset; if (!adjust_group_offset(vindex, group_info[i], new_offset)) { @@ -764,7 +808,10 @@ AP_Param::find(const char *name, enum ap_var_type *ptype) if (strncmp(name, _var_info[i].name, len) != 0) { continue; } - const struct GroupInfo *group_info = _var_info[i].group_info; + const struct GroupInfo *group_info = get_group_info(_var_info[i]); + if (group_info == nullptr) { + continue; + } AP_Param *ap = find_group(name + len, i, 0, group_info, ptype); if (ap != nullptr) { return ap; @@ -828,7 +875,11 @@ bool AP_Param::find_key_by_pointer_group(const void *ptr, uint16_t vindex, if (!adjust_group_offset(vindex, group_info[i], new_offset)) { continue; } - if (find_key_by_pointer_group(ptr, vindex, group_info[i].group_info, new_offset, key)) { + const struct GroupInfo *ginfo = get_group_info(group_info[i]); + if (ginfo == nullptr) { + continue; + } + if (find_key_by_pointer_group(ptr, vindex, ginfo, new_offset, key)) { return true; } } @@ -851,7 +902,11 @@ bool AP_Param::find_key_by_pointer(const void *ptr, uint16_t &key) return true; } ptrdiff_t offset = 0; - if (find_key_by_pointer_group(ptr, i, _var_info[i].group_info, offset, key)) { + const struct GroupInfo *ginfo = get_group_info(_var_info[i]); + if (ginfo == nullptr) { + continue; + } + if (find_key_by_pointer_group(ptr, i, ginfo, offset, key)) { return true; } } @@ -1258,6 +1313,9 @@ 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 + _parameter_count = 0; + if (!find_key_by_pointer(object_pointer, key)) { hal.console->printf("ERROR: Unable to find param pointer\n"); return; @@ -1269,8 +1327,10 @@ void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct if (!adjust_group_offset(key, group_info[i], new_offset)) { continue; } - load_object_from_eeprom((void *)(((ptrdiff_t)object_pointer)+new_offset), group_info[i].group_info); - continue; + const struct GroupInfo *ginfo = get_group_info(group_info[i]); + if (ginfo != nullptr) { + load_object_from_eeprom((void *)(((ptrdiff_t)object_pointer)+new_offset), ginfo); + } } uint16_t ofs = sizeof(AP_Param::EEPROM_header); while (ofs < _storage.size()) { @@ -1338,7 +1398,10 @@ AP_Param *AP_Param::next_group(uint16_t vindex, const struct GroupInfo *group_in } if (type == AP_PARAM_GROUP) { // a nested group - const struct GroupInfo *ginfo = group_info[i].group_info; + const struct GroupInfo *ginfo = get_group_info(group_info[i]); + if (ginfo == nullptr) { + continue; + } AP_Param *ap; ptrdiff_t new_offset = group_offset; @@ -1421,7 +1484,10 @@ AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype) } type = (enum ap_var_type)_var_info[i].type; if (type == AP_PARAM_GROUP) { - const struct GroupInfo *group_info = _var_info[i].group_info; + const struct GroupInfo *group_info = get_group_info(_var_info[i]); + if (group_info == nullptr) { + continue; + } AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, 0, token, ptype); if (ap != nullptr) { return ap; diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 5b7a3148a2..da78771ba0 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -49,11 +49,14 @@ // no conflict with the parent #define AP_PARAM_NO_SHIFT 8 +// the var_info is a pointer, allowing for dynamic definition of the var_info tree +#define AP_PARAM_FLAG_INFO_POINTER 16 + // 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 4 +#define AP_PARAM_FRAME_TYPE_SHIFT 5 // supported frame types for parameters #define AP_PARAM_FRAME_COPTER (1<<0) @@ -93,6 +96,9 @@ // declare a pointer subgroup entry in a group var_info #define AP_SUBGROUPPTR(element, name, idx, thisclazz, elclazz) { AP_PARAM_GROUP, idx, name, AP_VAROFFSET(thisclazz, element), { group_info : elclazz::var_info }, AP_PARAM_FLAG_POINTER } +// declare a pointer subgroup entry in a group var_info with a pointer var_info +#define AP_SUBGROUPVARPTR(element, name, idx, thisclazz, var_info) { AP_PARAM_GROUP, idx, name, AP_VAROFFSET(thisclazz, element), { group_info_ptr : &var_info }, AP_PARAM_FLAG_POINTER | AP_PARAM_FLAG_INFO_POINTER } + #define AP_GROUPEND { AP_PARAM_NONE, 0xFF, "", 0, { group_info : nullptr } } #define AP_VAREND { AP_PARAM_NONE, "", 0, nullptr, { group_info : nullptr } } @@ -124,6 +130,7 @@ public: ptrdiff_t offset; // offset within the object union { 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; }; uint16_t flags; @@ -135,6 +142,7 @@ public: const void *ptr; // pointer to the variable in memory union { 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; }; uint16_t flags; @@ -440,6 +448,12 @@ private: static bool adjust_group_offset(uint16_t vindex, const struct GroupInfo &group_info, ptrdiff_t &new_offset); static bool get_base(const struct Info &info, ptrdiff_t &base); + /// get group_info pointer based on flags + static const struct GroupInfo *get_group_info(const struct GroupInfo &ginfo); + + /// get group_info pointer based on flags + static const struct GroupInfo *get_group_info(const struct Info &ginfo); + const struct Info * find_var_info_group( const struct GroupInfo * group_info, uint16_t vindex,