/// 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; }