/// Returns the next variable in a group, recursing into groups
/// as needed
AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_info,
bool *found_current,
uint8_t group_base,
uint8_t group_shift,
ParamToken *token,
enum ap_var_type *ptype)
{
enum ap_var_type type;
for (uint8_t i=0;
(type=(enum ap_var_type)PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
i++) {
#ifdef AP_NESTED_GROUPS_ENABLED
if (type == AP_PARAM_GROUP) {
// a nested group
const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);
AP_Param *ap;
ap = next_group(vindex, ginfo, found_current, GROUP_ID(group_info, group_base, i, group_shift),
group_shift + _group_level_shift, token, ptype);
if (ap != NULL) {
return ap;
}
} else
#endif // AP_NESTED_GROUPS_ENABLED
{
if (*found_current) {
// got a new one
token->key = vindex;
token->group_element = GROUP_ID(group_info, group_base, i, group_shift);
token->idx = 0;
if (ptype != NULL) {
*ptype = type;
}
return (AP_Param*)(PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset));
}
if (GROUP_ID(group_info, group_base, i, group_shift) == token->group_element) {
*found_current = true;
if (type == AP_PARAM_VECTOR3F && token->idx < 3) {
// return the next element of the vector as a
// float
token->idx++;
if (ptype != NULL) {
*ptype = AP_PARAM_FLOAT;
}
uintptr_t ofs = (uintptr_t)PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset);
ofs += sizeof(float)*(token->idx-1);
return (AP_Param *)ofs;
}
}
}
}
return NULL;
}
// Load the variable from EEPROM, if supported
//
bool AP_Param::load(void)
{
uint32_t group_element = 0;
const struct GroupInfo *ginfo;
uint8_t idx;
const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx);
if (info == NULL) {
// we don't have any info on how to load it
return false;
}
struct Param_header phdr;
// create the header we will use to match the variable
if (ginfo != NULL) {
phdr.type = PGM_UINT8(&ginfo->type);
} else {
phdr.type = PGM_UINT8(&info->type);
}
phdr.key = PGM_UINT8(&info->key);
phdr.group_element = group_element;
// scan EEPROM to find the right location
uint16_t ofs;
if (!scan(&phdr, &ofs)) {
// if the value isn't stored in EEPROM then set the default value
if (ginfo != NULL) {
uintptr_t base = PGM_POINTER(&info->ptr);
set_value((enum ap_var_type)phdr.type, (void*)(base + PGM_UINT16(&ginfo->offset)),
get_default_value(&ginfo->def_value));
} else {
set_value((enum ap_var_type)phdr.type, (void*)PGM_POINTER(&info->ptr),
get_default_value(&info->def_value));
}
return false;
}
if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {
// only vector3f can have non-zero idx for now
return false;
}
AP_Param *ap;
ap = this;
if (idx != 0) {
ap = (AP_Param *)((uintptr_t)ap) - (idx*sizeof(float));
}
// found it
_storage.read_block(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
return true;
}
// load default values for scalars in a group. This does not recurse
// into other objects. This is a static function that should be called
// in the objects constructor
void AP_Param::setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
{
uintptr_t base = (uintptr_t)object_pointer;
uint8_t type;
for (uint8_t i=0;
(type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
i++) {
if (type <= AP_PARAM_FLOAT) {
void *ptr = (void *)(base + PGM_UINT16(&group_info[i].offset));
set_value((enum ap_var_type)type, ptr, PGM_FLOAT(&group_info[i].def_value));
}
}
}
// set a value directly in an object. This should only be used by
// example code, not by mainline vehicle code
void AP_Param::set_object_value(const void *object_pointer,
const struct GroupInfo *group_info,
const char *name, float value)
{
uintptr_t base = (uintptr_t)object_pointer;
uint8_t type;
for (uint8_t i=0;
(type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
i++) {
if (strcmp(name, group_info[i].name) == 0 && type <= AP_PARAM_FLOAT) {
void *ptr = (void *)(base + PGM_UINT16(&group_info[i].offset));
set_value((enum ap_var_type)type, ptr, value);
}
}
}
// 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,
uint8_t vindex,
const struct GroupInfo *group_info,
uint8_t group_base,
uint8_t group_shift)
{
uint8_t type;
for (uint8_t i=0;
(type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE;
i++) {
#ifdef AP_NESTED_GROUPS_ENABLED
if (type == AP_PARAM_GROUP) {
// a nested group
if (group_shift + _group_level_shift >= _group_bits) {
// too deeply nested - this should have been caught by
// setup() !
return NULL;
}
const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info);
const struct AP_Param::Info *ret = find_by_header_group(phdr, ptr, vindex, ginfo,
GROUP_ID(group_info, group_base, i, group_shift),
group_shift + _group_level_shift);
if (ret != NULL) {
return ret;
}
continue;
}
#endif // AP_NESTED_GROUPS_ENABLED
if (GROUP_ID(group_info, group_base, i, group_shift) == phdr.group_element) {
// found a group element
*ptr = (void*)(PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset));
return &_var_info[vindex];
}
}
return NULL;
}
