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

AP_Param: allow top level parameters to be pointers

Browse Source 
this will allow for the attitude_control variable in quadplane to be a
pointer
This commit is contained in:
Andrew Tridgell 2016-04-01 16:16:03 +11:00
parent b96ea15220
commit 0530af93aa
2 changed files with 156 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

194
libraries/AP_Param/AP_Param.cpp
View File

@ -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 ap == p;
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 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; i<group_nesting.level; i++) {
@ -944,7 +1045,7 @@ bool AP_Param::load(void)
set_value((enum ap_var_type)phdr.type, (void*)(base + ginfo->offset + 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;
uint16_t key;
// 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;
}
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;
}

8
libraries/AP_Param/AP_Param.h
View File

@ -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,