From 0530af93aabeea2fcbc75498464a8ceaa85fc392 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Apr 2016 16:16:03 +1100 Subject: [PATCH] AP_Param: allow top level parameters to be pointers this will allow for the attitude_control variable in quadplane to be a pointer --- libraries/AP_Param/AP_Param.cpp | 194 ++++++++++++++++++++++++-------- libraries/AP_Param/AP_Param.h | 8 +- 2 files changed, 156 insertions(+), 46 deletions(-) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 991dc3c61f..a5c498ef25 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -221,6 +221,10 @@ bool AP_Param::check_var_info(void) if (duplicate_key(i, key)) { return false; } + if (type != AP_PARAM_GROUP && (_var_info[i].flags & AP_PARAM_FLAG_POINTER)) { + // only groups can be pointers + return false; + } } // we no longer check if total_size is larger than _eeprom_size, @@ -270,17 +274,36 @@ bool AP_Param::adjust_group_offset(uint16_t vindex, const struct GroupInfo &grou } if (group_info.flags & AP_PARAM_FLAG_POINTER) { // group_info.offset refers to a pointer - void **p = (void **)(((ptrdiff_t)_var_info[vindex].ptr) + new_offset + group_info.offset); + ptrdiff_t base; + if (!get_base(_var_info[vindex], base)) { + // the object is not allocated yet + return false; + } + void **p = (void **)(base + new_offset + group_info.offset); if (*p == nullptr) { // the object is not allocated yet return false; } // calculate offset that is needed to take base object and adjust for this object - new_offset = ((ptrdiff_t)*p) - (ptrdiff_t)_var_info[vindex].ptr; + new_offset = ((ptrdiff_t)*p) - base; } return true; } +/* + get the base pointer for a variable, accounting for AP_PARAM_FLAG_POINTER + */ +bool AP_Param::get_base(const struct Info &info, ptrdiff_t &base) +{ + if (info.flags & AP_PARAM_FLAG_POINTER) { + base = *(ptrdiff_t *)info.ptr; + return (base != (ptrdiff_t)0); + } + base = (ptrdiff_t)info.ptr; + return true; +} + + // find the info structure given a header and a group_info table // return the Info structure and a pointer to the variables storage const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header phdr, void **ptr, @@ -318,7 +341,11 @@ const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header } if (group_id(group_info, group_base, i, group_shift) == phdr.group_element && type == phdr.type) { // found a group element - *ptr = (void*)((ptrdiff_t)_var_info[vindex].ptr + group_info[i].offset + group_offset); + ptrdiff_t base; + if (!get_base(_var_info[vindex], base)) { + continue; + } + *ptr = (void*)(base + group_info[i].offset + group_offset); return &_var_info[vindex]; } } @@ -343,7 +370,11 @@ const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr, } if (type == phdr.type) { // found it - *ptr = (void*)_var_info[i].ptr; + ptrdiff_t base; + if (!get_base(_var_info[i], base)) { + return NULL; + } + *ptr = (void*)base; return &_var_info[i]; } } @@ -361,7 +392,10 @@ const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInf struct GroupNesting &group_nesting, uint8_t * idx) const { - ptrdiff_t base = (ptrdiff_t)_var_info[vindex].ptr; + ptrdiff_t base; + if (!get_base(_var_info[vindex], base)) { + return NULL; + } uint8_t type; for (uint8_t i=0; (type=group_info[i].type) != AP_PARAM_NONE; @@ -425,7 +459,10 @@ const struct AP_Param::Info *AP_Param::find_var_info(uint32_t * for (uint16_t i=0; i<_num_vars; i++) { uint8_t type = _var_info[i].type; - ptrdiff_t base = (ptrdiff_t)_var_info[i].ptr; + ptrdiff_t base; + if (!get_base(_var_info[i], base)) { + continue; + } if (type == AP_PARAM_GROUP) { const struct GroupInfo *group_info = _var_info[i].group_info; const struct AP_Param::Info *info; @@ -460,7 +497,10 @@ const struct AP_Param::Info *AP_Param::find_var_info_token(const ParamToken &tok { uint16_t i = token.key; uint8_t type = _var_info[i].type; - ptrdiff_t base = (ptrdiff_t)_var_info[i].ptr; + ptrdiff_t base; + if (!get_base(_var_info[i], base)) { + return NULL; + } group_ret = NULL; if (type == AP_PARAM_GROUP) { @@ -658,9 +698,12 @@ AP_Param::find_group(const char *name, uint16_t vindex, ptrdiff_t group_offset, return ap; } } else if (strcasecmp(name, group_info[i].name) == 0) { - ptrdiff_t p = (ptrdiff_t)_var_info[vindex].ptr; + ptrdiff_t base; + if (!get_base(_var_info[vindex], base)) { + continue; + } *ptype = (enum ap_var_type)type; - return (AP_Param *)(p + group_info[i].offset + group_offset); + return (AP_Param *)(base + group_info[i].offset + group_offset); } else if (type == AP_PARAM_VECTOR3F) { // special case for finding Vector3f elements uint8_t suffix_len = strnlen(group_info[i].name, AP_MAX_NAME_SIZE); @@ -669,8 +712,11 @@ AP_Param::find_group(const char *name, uint16_t vindex, ptrdiff_t group_offset, (name[suffix_len+1] == 'X' || name[suffix_len+1] == 'Y' || name[suffix_len+1] == 'Z')) { - ptrdiff_t p = (ptrdiff_t)_var_info[vindex].ptr; - AP_Float *v = (AP_Float *)(p + group_info[i].offset + group_offset); + ptrdiff_t base; + if (!get_base(_var_info[vindex], base)) { + continue; + } + AP_Float *v = (AP_Float *)(base + group_info[i].offset + group_offset); *ptype = AP_PARAM_FLOAT; switch (name[suffix_len+1]) { case 'X': @@ -709,7 +755,11 @@ AP_Param::find(const char *name, enum ap_var_type *ptype) // parameters, for example CAM_P_G } else if (strcasecmp(name, _var_info[i].name) == 0) { *ptype = (enum ap_var_type)type; - return (AP_Param *)_var_info[i].ptr; + ptrdiff_t base; + if (!get_base(_var_info[i], base)) { + return NULL; + } + return (AP_Param *)base; } } return NULL; @@ -757,17 +807,61 @@ AP_Param::find_by_index(uint16_t idx, enum ap_var_type *ptype, ParamToken *token /* - Find a variable by pointer, returning a ParamToken + Find a variable by pointer, returning key. This is used for loading pointer variables */ -bool AP_Param::find_by_pointer(AP_Param *p, ParamToken &token) +bool AP_Param::find_key_by_pointer_group(const void *ptr, uint16_t vindex, + const struct GroupInfo *group_info, + ptrdiff_t offset, uint16_t &key) { - AP_Param *ap; - enum ap_var_type ptype; - for (ap=AP_Param::first(&token, &ptype); - ap && ap != p; - ap=AP_Param::next(&token, &ptype)) { + for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) { + if (group_info[i].type != AP_PARAM_GROUP) { + continue; + } + ptrdiff_t base; + if (!get_base(_var_info[vindex], base)) { + continue; + } + if (group_info[i].flags & AP_PARAM_FLAG_POINTER) { + if (ptr == *(void **)(base+group_info[i].offset+offset)) { + key = _var_info[vindex].key; + return true; + } + } else if (ptr == (void *)(base+group_info[i].offset+offset)) { + key = _var_info[vindex].key; + return true; + } + ptrdiff_t new_offset = offset; + 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)) { + return true; + } + } + return false; +} + + +/* + Find a variable by pointer, returning key. This is used for loading pointer variables +*/ +bool AP_Param::find_key_by_pointer(const void *ptr, uint16_t &key) +{ + for (uint16_t i=0; i<_num_vars; i++) { + if (_var_info[i].type != AP_PARAM_GROUP) { + continue; + } + if ((_var_info[i].flags & AP_PARAM_FLAG_POINTER) && + ptr == *(void **)_var_info[i].ptr) { + key = _var_info[i].key; + return true; + } + ptrdiff_t offset = 0; + if (find_key_by_pointer_group(ptr, i, _var_info[i].group_info, offset, key)) { + return true; + } } - return ap == p; + return false; } @@ -778,7 +872,11 @@ AP_Param::find_object(const char *name) { for (uint16_t i=0; i<_num_vars; i++) { if (strcasecmp(name, _var_info[i].name) == 0) { - return (AP_Param *)_var_info[i].ptr; + ptrdiff_t base; + if (!get_base(_var_info[i], base)) { + return NULL; + } + return (AP_Param *)base; } } return NULL; @@ -933,9 +1031,12 @@ bool AP_Param::load(void) uint16_t ofs; if (!scan(&phdr, &ofs)) { // if the value isn't stored in EEPROM then set the default value - if (ginfo != NULL) { - ptrdiff_t base = (ptrdiff_t)info->ptr; + ptrdiff_t base; + if (!get_base(*info, base)) { + return false; + } + if (ginfo != NULL) { // add in nested group offset ptrdiff_t group_offset = 0; for (uint8_t i=0; ioffset + group_offset), get_default_value(&ginfo->def_value)); } else { - set_value((enum ap_var_type)phdr.type, (void*)info->ptr, + set_value((enum ap_var_type)phdr.type, (void*)base, get_default_value(&info->def_value)); } return false; @@ -1090,8 +1191,10 @@ void AP_Param::setup_sketch_defaults(void) for (uint16_t i=0; i<_num_vars; i++) { uint8_t type = _var_info[i].type; if (type <= AP_PARAM_FLOAT) { - void *ptr = (void*)_var_info[i].ptr; - set_value((enum ap_var_type)type, ptr, get_default_value(&_var_info[i].def_value)); + ptrdiff_t base; + if (get_base(_var_info[i], base)) { + set_value((enum ap_var_type)type, (void*)base, get_default_value(&_var_info[i].def_value)); + } } } } @@ -1146,27 +1249,17 @@ bool AP_Param::load_all(void) void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct GroupInfo *group_info) { struct Param_header phdr; - ParamToken token; - - // find first storable value - uint8_t i; - for (i=0; group_info[i].type != AP_PARAM_NONE; i++) { - if (group_info[i].type < AP_PARAM_GROUP) break; - } - if (group_info[i].type == AP_PARAM_NONE) { - // no storable parameters - return; - } + uint16_t key; - if (!find_by_pointer((AP_Param *)(((ptrdiff_t)object_pointer)+group_info[i].offset), token)) { + if (!find_key_by_pointer(object_pointer, key)) { hal.console->printf("ERROR: Unable to find param pointer\n"); return; } - for (i=0; group_info[i].type != AP_PARAM_NONE; i++) { + for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) { if (group_info[i].type == AP_PARAM_GROUP) { ptrdiff_t new_offset = 0; - if (!adjust_group_offset(token.key, group_info[i], new_offset)) { + 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); @@ -1181,7 +1274,7 @@ void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct // we've reached the sentinal break; } - if (get_key(phdr) == _var_info[token.key].key) { + if (get_key(phdr) == key) { const struct AP_Param::Info *info; void *ptr; @@ -1211,7 +1304,12 @@ AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype) if (ptype != NULL) { *ptype = (enum ap_var_type)_var_info[0].type; } - return (AP_Param *)(_var_info[0].ptr); + ptrdiff_t base; + if (!get_base(_var_info[0], base)) { + // should be impossible, first var needs to be non-pointer + return NULL; + } + return (AP_Param *)base; } /// Returns the next variable in a group, recursing into groups @@ -1252,7 +1350,11 @@ AP_Param *AP_Param::next_group(uint16_t vindex, const struct GroupInfo *group_in if (ptype != NULL) { *ptype = type; } - return (AP_Param*)((ptrdiff_t)_var_info[vindex].ptr + group_info[i].offset + group_offset); + ptrdiff_t base; + if (!get_base(_var_info[vindex], base)) { + continue; + } + return (AP_Param*)(base + group_info[i].offset + group_offset); } if (group_id(group_info, group_base, i, group_shift) == token->group_element) { *found_current = true; @@ -1263,7 +1365,11 @@ AP_Param *AP_Param::next_group(uint16_t vindex, const struct GroupInfo *group_in if (ptype != NULL) { *ptype = AP_PARAM_FLOAT; } - ptrdiff_t ofs = (ptrdiff_t)_var_info[vindex].ptr + group_info[i].offset + group_offset; + ptrdiff_t base; + if (!get_base(_var_info[vindex], base)) { + continue; + } + ptrdiff_t ofs = base + group_info[i].offset + group_offset; ofs += sizeof(float)*(token->idx - 1u); return (AP_Param *)ofs; } diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 8b6776dddd..2c3d25c3e8 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -118,6 +118,7 @@ public: const struct GroupInfo *group_info; const float def_value; }; + uint8_t flags; }; struct ConversionInfo { uint16_t old_key; // k_param_* @@ -216,8 +217,10 @@ public: /// /// /// @param p Pointer to variable - /// @return token for variable - static bool find_by_pointer(AP_Param *p, ParamToken &token); + /// @return key for variable + static bool find_key_by_pointer_group(const void *ptr, uint16_t vindex, const struct GroupInfo *group_info, + ptrdiff_t offset, uint16_t &key); + static bool find_key_by_pointer(const void *ptr, uint16_t &key); /// Find a object in the top level var_info table /// @@ -381,6 +384,7 @@ private: static bool duplicate_key(uint16_t vindex, uint16_t key); 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); const struct Info * find_var_info_group( const struct GroupInfo * group_info,