bool AP_Param::configured_in_storage(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; // only vector3f can have non-zero idx for now return scan(&phdr, &ofs) && (phdr.type == AP_PARAM_VECTOR3F || idx == 0); }
// Copy the variable's whole name to the supplied buffer. // // If the variable is a group member, prepend the group name. // void AP_Param::copy_name_token(const ParamToken *token, char *buffer, size_t buffer_size, bool force_scalar) { uint32_t group_element; const struct GroupInfo *ginfo; uint8_t idx; const struct AP_Param::Info *info = find_var_info_token(token, &group_element, &ginfo, &idx); if (info == NULL) { *buffer = 0; serialDebug("no info found"); return; } strncpy_P(buffer, info->name, buffer_size); if (ginfo != NULL) { uint8_t len = strnlen(buffer, buffer_size); if (len < buffer_size) { strncpy_P(&buffer[len], ginfo->name, buffer_size-len); } if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == PGM_UINT8(&ginfo->type)) { // the caller wants a specific element in a Vector3f add_vector3f_suffix(buffer, buffer_size, idx); } } else if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == PGM_UINT8(&info->type)) { add_vector3f_suffix(buffer, buffer_size, idx); } }
// convert one old vehicle parameter to new object parameter void AP_Param::convert_old_parameter(const struct ConversionInfo *info) { // find the old value in EEPROM. uint16_t pofs; AP_Param::Param_header header; header.type = PGM_UINT8(&info->type); header.key = PGM_UINT8(&info->old_key); header.group_element = PGM_UINT8(&info->old_group_element); if (!scan(&header, &pofs)) { // the old parameter isn't saved in the EEPROM. It was // probably still set to the default value, which isn't stored // no need to convert return; } // load the old value from EEPROM uint8_t old_value[type_size((enum ap_var_type)header.type)]; _storage.read_block(old_value, pofs+sizeof(header), sizeof(old_value)); const AP_Param *ap = (const AP_Param *)&old_value[0]; // find the new variable in the variable structures enum ap_var_type ptype; AP_Param *ap2; ap2 = find_P((const prog_char_t *)&info->new_name[0], &ptype); if (ap2 == NULL) { hal.console->printf_P(PSTR("Unknown conversion '%s'\n"), info->new_name); return; } // see if we can load it from EEPROM if (ap2->load()) { // the new parameter already has a value set by the user, or // has already been converted return; } // see if they are the same type if (ptype == (ap_var_type)header.type) { // copy the value over only if the new parameter does not already // have the old value (via a default). if (memcmp(ap2, ap, sizeof(old_value)) != 0) { memcpy(ap2, ap, sizeof(old_value)); // and save ap2->save(); } } else if (ptype <= AP_PARAM_FLOAT && header.type <= AP_PARAM_FLOAT) { // perform scalar->scalar conversion float v = ap->cast_to_float((enum ap_var_type)header.type); if (!is_equal(v,ap2->cast_to_float(ptype))) { // the value needs to change set_value(ptype, ap2, v); ap2->save(); } } else { // can't do vector<->scalar conversion, or different vector types hal.console->printf_P(PSTR("Bad conversion type '%s'\n"), info->new_name); } }
/* print FMT specifiers for log dumps where we have wrapped in the dataflash and so have no formats. This assumes the log being dumped using the same log formats as the current formats, but it is better than falling back to old defaults in the GCS */ void DataFlash_Block::_print_log_formats(AP_HAL::BetterStream *port) { for (uint8_t i=0; i<_num_types; i++) { const struct LogStructure *s = &_structures[i]; port->printf_P(PSTR("FMT, %u, %u, %S, %S, %S\n"), (unsigned)PGM_UINT8(&s->msg_type), (unsigned)PGM_UINT8(&s->msg_len), s->name, s->format, s->labels); } }
/* write a structure format to the log */ void DataFlash_Class::Log_Fill_Format(const struct LogStructure *s, struct log_Format &pkt) { memset(&pkt, 0, sizeof(pkt)); pkt.head1 = HEAD_BYTE1; pkt.head2 = HEAD_BYTE2; pkt.msgid = LOG_FORMAT_MSG; pkt.type = PGM_UINT8(&s->msg_type); pkt.length = PGM_UINT8(&s->msg_len); strncpy_P(pkt.name, s->name, sizeof(pkt.name)); strncpy_P(pkt.format, s->format, sizeof(pkt.format)); strncpy_P(pkt.labels, s->labels, sizeof(pkt.labels)); }
// 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; }
// Save the variable to EEPROM, if supported // bool AP_Param::save(void) { uint8_t group_element = 0; const struct GroupInfo *ginfo; uint8_t idx; const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx); const AP_Param *ap; if (info == NULL) { // we don't have any info on how to store it return false; } struct Param_header phdr; // create the header we will use to store 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; ap = this; if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) { // only vector3f can have non-zero idx for now return false; } if (idx != 0) { ap = (const AP_Param *)((uintptr_t)ap) - (idx*sizeof(float)); } // scan EEPROM to find the right location uint16_t ofs; if (scan(&phdr, &ofs)) { // found an existing copy of the variable eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type)); return true; } if (ofs == (uint16_t)~0) { return false; } // write a new sentinal, then the data, then the header write_sentinal(ofs + sizeof(phdr) + type_size((enum ap_var_type)phdr.type)); eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type)); eeprom_write_check(&phdr, ofs, sizeof(phdr)); return true; }
// validate a group info table bool AP_Param::check_group_info(const struct AP_Param::GroupInfo * group_info, uint16_t * total_size, uint8_t group_shift, uint8_t prefix_length) { uint8_t type; int8_t max_idx = -1; 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 const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&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 == NULL || !check_group_info(ginfo, total_size, group_shift + _group_level_shift, prefix_length + strlen_P(group_info[i].name))) { return false; } continue; } #endif // AP_NESTED_GROUPS_ENABLED uint8_t idx = PGM_UINT8(&group_info[i].idx); if (idx >= (1<<_group_level_shift)) { Debug("idx too large (%u) in %S", idx, group_info[i].name); return false; } if ((int8_t)idx <= max_idx) { Debug("indexes must be in increasing order in %S", group_info[i].name); return false; } max_idx = (int8_t)idx; uint8_t size = type_size((enum ap_var_type)type); if (size == 0) { Debug("invalid type in %S", group_info[i].name); return false; } if (prefix_length + strlen_P(group_info[i].name) > 16) { Debug("suffix is too long in %S", group_info[i].name); return false; } (*total_size) += size + sizeof(struct Param_header); } return true; }
// find the info structure for a variable const struct AP_Param::Info *AP_Param::find_var_info_token(const ParamToken *token, uint32_t * group_element, const struct GroupInfo ** group_ret, uint8_t * idx) { uint8_t i = token->key; uint8_t type = PGM_UINT8(&_var_info[i].type); uintptr_t base = PGM_POINTER(&_var_info[i].ptr); if (type == AP_PARAM_GROUP) { const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); const struct AP_Param::Info *info; info = find_var_info_group(group_info, i, 0, 0, group_element, group_ret, idx); if (info != NULL) { return info; } } else if (base == (uintptr_t) this) { *group_element = 0; *group_ret = NULL; *idx = 0; return &_var_info[i]; } else if (type == AP_PARAM_VECTOR3F && (base+sizeof(float) == (uintptr_t) this || base+2*sizeof(float) == (uintptr_t) this)) { // we are inside a Vector3f. Work out which element we are // referring to. *idx = (((uintptr_t) this) - base)/sizeof(float); *group_element = 0; *group_ret = NULL; return &_var_info[i]; } return NULL; }
// Find a variable by name. // AP_Param * AP_Param::find(const char *name, enum ap_var_type *ptype) { for (uint8_t i=0; i<_num_vars; i++) { uint8_t type = PGM_UINT8(&_var_info[i].type); if (type == AP_PARAM_GROUP) { uint8_t len = strnlen_P(_var_info[i].name, AP_MAX_NAME_SIZE); if (strncmp_P(name, _var_info[i].name, len) != 0) { continue; } const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); AP_Param *ap = find_group(name + len, i, group_info, ptype); if (ap != NULL) { return ap; } // we continue looking as we want to allow top level // parameter to have the same prefix name as group // parameters, for example CAM_P_G } else if (strcasecmp_P(name, _var_info[i].name) == 0) { *ptype = (enum ap_var_type)type; return (AP_Param *)PGM_POINTER(&_var_info[i].ptr); } } return NULL; }
void AP_Param::copy_name_info(const struct AP_Param::Info *info, const struct GroupInfo *ginfo, uint8_t idx, char *buffer, size_t buffer_size, bool force_scalar) const { strncpy(buffer, info->name, buffer_size); if (ginfo != NULL) { uint8_t len = strnlen(buffer, buffer_size); if (len < buffer_size) { strncpy(&buffer[len], ginfo->name, buffer_size-len); } if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == PGM_UINT8(&ginfo->type)) { // the caller wants a specific element in a Vector3f add_vector3f_suffix(buffer, buffer_size, idx); } } else if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == PGM_UINT8(&info->type)) { add_vector3f_suffix(buffer, buffer_size, idx); } }
/// Returns the next variable in _var_info, recursing into groups /// as needed AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype) { uint8_t i = token->key; bool found_current = false; if (i >= _num_vars) { // illegal token return NULL; } enum ap_var_type type = (enum ap_var_type)PGM_UINT8(&_var_info[i].type); // allow Vector3f to be seen as 3 variables. First as a vector, // then as 3 separate floats if (type == AP_PARAM_VECTOR3F && token->idx < 3) { token->idx++; if (ptype != NULL) { *ptype = AP_PARAM_FLOAT; } return (AP_Param *)(((token->idx-1)*sizeof(float))+(uintptr_t)PGM_POINTER(&_var_info[i].ptr)); } if (type != AP_PARAM_GROUP) { i++; found_current = true; } for (; i<_num_vars; i++) { type = (enum ap_var_type)PGM_UINT8(&_var_info[i].type); if (type == AP_PARAM_GROUP) { const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, token, ptype); if (ap != NULL) { return ap; } } else { // found the next one token->key = i; token->group_element = 0; token->idx = 0; if (ptype != NULL) { *ptype = type; } return (AP_Param *)(PGM_POINTER(&_var_info[i].ptr)); } } return NULL; }
// validate a group info table bool AP_Param::check_group_info(const struct AP_Param::GroupInfo * group_info, uint16_t * total_size, uint8_t group_shift) { uint8_t type; int8_t max_idx = -1; 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 const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info); if (group_shift + _group_level_shift >= _group_bits) { // double nesting of groups is not allowed return false; } if (ginfo == NULL || !check_group_info(ginfo, total_size, group_shift + _group_level_shift)) { return false; } continue; } #endif // AP_NESTED_GROUPS_ENABLED uint8_t idx = PGM_UINT8(&group_info[i].idx); if (idx >= (1<<_group_level_shift)) { // passed limit on table size return false; } if ((int8_t)idx <= max_idx) { // the indexes must be in increasing order return false; } max_idx = (int8_t)idx; uint8_t size = type_size((enum ap_var_type)type); if (size == 0) { // not a valid type return false; } (*total_size) += size + sizeof(struct Param_header); } return true; }
// check for duplicate key values bool AP_Param::duplicate_key(uint8_t vindex, uint8_t key) { for (uint8_t i=vindex+1; i<_num_vars; i++) { uint8_t key2 = PGM_UINT8(&_var_info[i].key); if (key2 == key) { // no duplicate keys allowed return true; } } return false; }
// load default values for all scalars in a sketch. This does not // recurse into sub-objects void AP_Param::setup_sketch_defaults(void) { setup(); for (uint8_t i=0; i<_num_vars; i++) { uint8_t type = PGM_UINT8(&_var_info[i].type);//返回参数类型,强制转换为uint8,包括PARAM_NONE等 if (type <= AP_PARAM_FLOAT) {//按道理讲uint8_t一定会小于float,这里可能是保护的作用?? void *ptr = (void*)PGM_POINTER(&_var_info[i].ptr);//返回指向参数在内存中位置的指针 set_value((enum ap_var_type)type, ptr, PGM_FLOAT(&_var_info[i].def_value));//把参数变量设为一个特定的值 } } }
// load default values for all scalars in a sketch. This does not // recurse into sub-objects void AP_Param::setup_sketch_defaults(void) { setup(); for (uint8_t i=0; i<_num_vars; i++) { uint8_t type = PGM_UINT8(&_var_info[i].type); if (type <= AP_PARAM_FLOAT) { void *ptr = (void*)PGM_POINTER(&_var_info[i].ptr); set_value((enum ap_var_type)type, ptr, PGM_FLOAT(&_var_info[i].def_value)); } } }
// find the info structure for a variable in a group const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInfo * group_info, uint8_t vindex, uint8_t group_base, uint8_t group_shift, uint32_t * group_element, const struct GroupInfo **group_ret, uint8_t * idx) { uintptr_t base = PGM_POINTER(&_var_info[vindex].ptr); uint8_t type; for (uint8_t i=0; (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE; i++) { uintptr_t ofs = PGM_POINTER(&group_info[i].offset); #ifdef AP_NESTED_GROUPS_ENABLED if (type == AP_PARAM_GROUP) { const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info); // 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 AP_Param::Info *info; info = find_var_info_group(ginfo, vindex, GROUP_ID(group_info, group_base, i, group_shift), group_shift + _group_level_shift, group_element, group_ret, idx); if (info != NULL) { return info; } } else // Forgive the poor formatting - if continues below. #endif // AP_NESTED_GROUPS_ENABLED if ((uintptr_t) this == base + ofs) { *group_element = GROUP_ID(group_info, group_base, i, group_shift); *group_ret = &group_info[i]; *idx = 0; return &_var_info[vindex]; } else if (type == AP_PARAM_VECTOR3F && (base+ofs+sizeof(float) == (uintptr_t) this || base+ofs+2*sizeof(float) == (uintptr_t) this)) { // we are inside a Vector3f. We need to work out which // element of the vector the current object refers to. *idx = (((uintptr_t) this) - (base+ofs))/sizeof(float); *group_element = GROUP_ID(group_info, group_base, i, group_shift); *group_ret = &group_info[i]; return &_var_info[vindex]; } } return NULL; }
// find the info structure given a header // return the Info structure and a pointer to the variables storage const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr, void **ptr) { // loop over all named variables for (uint8_t i=0; i<_num_vars; i++) { uint8_t type = PGM_UINT8(&_var_info[i].type); uint8_t key = PGM_UINT8(&_var_info[i].key); if (key != phdr.key) { // not the right key continue; } if (type != AP_PARAM_GROUP) { // if its not a group then we are done *ptr = (void*)PGM_POINTER(&_var_info[i].ptr); return &_var_info[i]; } const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); return find_by_header_group(phdr, ptr, i, group_info, 0, 0); } return NULL; }
/// 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; }
// return the first variable in _var_info AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype) { token->key = 0; token->group_element = 0; token->idx = 0; if (_num_vars == 0) { return NULL; } if (ptype != NULL) { *ptype = (enum ap_var_type)PGM_UINT8(&_var_info[0].type); } return (AP_Param *)(PGM_POINTER(&_var_info[0].ptr)); }
// 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)); } } }
// notify GCS of current value of parameter void AP_Param::notify() const { 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) { // this is probably very bad return; } char name[AP_MAX_NAME_SIZE+1]; copy_name_info(info, ginfo, idx, name, sizeof(name), true); uint32_t param_header_type; if (ginfo != NULL) { param_header_type = PGM_UINT8(&ginfo->type); } else { param_header_type = PGM_UINT8(&info->type); } send_parameter(name, (enum ap_var_type)param_header_type); }
// validate the _var_info[] table bool AP_Param::check_var_info(void) { uint16_t total_size = sizeof(struct EEPROM_header); for (uint8_t i=0; i<_num_vars; i++) { uint8_t type = PGM_UINT8(&_var_info[i].type); uint8_t key = PGM_UINT8(&_var_info[i].key); if (type == AP_PARAM_GROUP) { if (i == 0) { // first element can't be a group, for first() call return false; } const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); if (group_info == NULL || !check_group_info(group_info, &total_size, 0)) { return false; } } else { uint8_t size = type_size((enum ap_var_type)type); if (size == 0) { // not a valid type - the top level list can't contain // AP_PARAM_NONE return false; } total_size += size + sizeof(struct Param_header); } if (duplicate_key(i, key)) { return false; } } if (total_size > _eeprom_size) { serialDebug("total_size %u exceeds _eeprom_size %u", total_size, _eeprom_size); return false; } return true; }
// validate the _var_info[] table bool AP_Param::check_var_info(void) { uint16_t total_size = sizeof(struct EEPROM_header); for (uint8_t i=0; i<_num_vars; i++) { uint8_t type = PGM_UINT8(&_var_info[i].type); uint8_t key = PGM_UINT8(&_var_info[i].key); if (type == AP_PARAM_GROUP) { if (i == 0) { // first element can't be a group, for first() call return false; } const struct GroupInfo *group_info = (const struct GroupInfo *)PGM_POINTER(&_var_info[i].group_info); if (group_info == NULL || !check_group_info(group_info, &total_size, 0)) { return false; } } else { uint8_t size = type_size((enum ap_var_type)type); if (size == 0) { // not a valid type - the top level list can't contain // AP_PARAM_NONE return false; } total_size += size + sizeof(struct Param_header); } if (duplicate_key(i, key)) { return false; } } // we no longer check if total_size is larger than _eeprom_size, // as we allow for more variables than could fit, relying on not // saving default values return true; }
// 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 a variable by name in a group AP_Param * AP_Param::find_group(const char *name, uint8_t vindex, const struct GroupInfo *group_info, enum ap_var_type *ptype) { 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) { const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info); AP_Param *ap = find_group(name, vindex, ginfo, ptype); if (ap != NULL) { return ap; } } else #endif // AP_NESTED_GROUPS_ENABLED if (strcasecmp_P(name, group_info[i].name) == 0) { uintptr_t p = PGM_POINTER(&_var_info[vindex].ptr); *ptype = (enum ap_var_type)type; return (AP_Param *)(p + PGM_POINTER(&group_info[i].offset)); } else if (type == AP_PARAM_VECTOR3F) { // special case for finding Vector3f elements uint8_t suffix_len = strnlen_P(group_info[i].name, AP_MAX_NAME_SIZE); if (strncmp_P(name, group_info[i].name, suffix_len) == 0 && name[suffix_len] == '_' && (name[suffix_len+1] == 'X' || name[suffix_len+1] == 'Y' || name[suffix_len+1] == 'Z')) { uintptr_t p = PGM_POINTER(&_var_info[vindex].ptr); AP_Float *v = (AP_Float *)(p + PGM_POINTER(&group_info[i].offset)); *ptype = AP_PARAM_FLOAT; switch (name[suffix_len+1]) { case 'X': return (AP_Float *)&v[0]; case 'Y': return (AP_Float *)&v[1]; case 'Z': return (AP_Float *)&v[2]; } } } } return NULL; }
// 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; }
/* read and print a log entry using the format strings from the given structure */ void DataFlash_Class::_print_log_entry(uint8_t msg_type, print_mode_fn print_mode, AP_HAL::BetterStream *port) { uint8_t i; for (i=0; i<_num_types; i++) { if (msg_type == PGM_UINT8(&_structures[i].msg_type)) { break; } } if (i == _num_types) { port->printf_P(PSTR("UNKN, %u\n"), (unsigned)msg_type); return; } uint8_t msg_len = PGM_UINT8(&_structures[i].msg_len) - 3; uint8_t pkt[msg_len]; ReadBlock(pkt, msg_len); port->printf_P(PSTR("%S, "), _structures[i].name); for (uint8_t ofs=0, fmt_ofs=0; ofs<msg_len; fmt_ofs++) { char fmt = PGM_UINT8(&_structures[i].format[fmt_ofs]); switch (fmt) { case 'b': { port->printf_P(PSTR("%d"), (int)pkt[ofs]); ofs += 1; break; } case 'B': { port->printf_P(PSTR("%u"), (unsigned)pkt[ofs]); ofs += 1; break; } case 'h': { int16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%d"), (int)v); ofs += sizeof(v); break; } case 'H': { uint16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%u"), (unsigned)v); ofs += sizeof(v); break; } case 'i': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%ld"), (long)v); ofs += sizeof(v); break; } case 'I': { uint32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%lu"), (unsigned long)v); ofs += sizeof(v); break; } case 'f': { float v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%f"), (double)v); ofs += sizeof(v); break; } case 'c': { int16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'C': { uint16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'e': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'E': { uint32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'L': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); print_latlon(port, v); ofs += sizeof(v); break; } case 'n': { char v[5]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'N': { char v[17]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'Z': { char v[65]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'M': { print_mode(port, pkt[ofs]); ofs += 1; break; } default: ofs = msg_len; break; } if (ofs < msg_len) { port->printf_P(PSTR(", ")); } } port->println(); }
/* read and print a log entry using the format strings from the given structure - this really should in in the frontend, not the backend */ void DataFlash_Backend::_print_log_entry(uint8_t msg_type, print_mode_fn print_mode, AP_HAL::BetterStream *port) { uint8_t i; for (i=0; i<_num_types; i++) { if (msg_type == PGM_UINT8(&_structures[i].msg_type)) { break; } } if (i == _num_types) { port->printf_P(PSTR("UNKN, %u\n"), (unsigned)msg_type); return; } uint8_t msg_len = PGM_UINT8(&_structures[i].msg_len) - 3; uint8_t pkt[msg_len]; if (!ReadBlock(pkt, msg_len)) { return; } port->printf_P(PSTR("%S, "), _structures[i].name); for (uint8_t ofs=0, fmt_ofs=0; ofs<msg_len; fmt_ofs++) { char fmt = PGM_UINT8(&_structures[i].format[fmt_ofs]); switch (fmt) { case 'b': { port->printf_P(PSTR("%d"), (int)pkt[ofs]); ofs += 1; break; } case 'B': { port->printf_P(PSTR("%u"), (unsigned)pkt[ofs]); ofs += 1; break; } case 'h': { int16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%d"), (int)v); ofs += sizeof(v); break; } case 'H': { uint16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%u"), (unsigned)v); ofs += sizeof(v); break; } case 'i': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%ld"), (long)v); ofs += sizeof(v); break; } case 'I': { uint32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%lu"), (unsigned long)v); ofs += sizeof(v); break; } case 'q': { int64_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%lld"), (long long)v); ofs += sizeof(v); break; } case 'Q': { uint64_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%llu"), (unsigned long long)v); ofs += sizeof(v); break; } case 'f': { float v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%f"), (double)v); ofs += sizeof(v); break; } case 'd': { double v; memcpy(&v, &pkt[ofs], sizeof(v)); // note that %f here *really* means a single-precision // float, so we lose precision printing this double out // dtoa_engine needed.... port->printf_P(PSTR("%f"), (double)v); ofs += sizeof(v); break; } case 'c': { int16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'C': { uint16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'e': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'E': { uint32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'L': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); print_latlon(port, v); ofs += sizeof(v); break; } case 'n': { char v[5]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'N': { char v[17]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'Z': { char v[65]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'M': { print_mode(port, pkt[ofs]); ofs += 1; break; } default: ofs = msg_len; break; } if (ofs < msg_len) { port->printf_P(PSTR(", ")); } } port->println(); }
// Save the variable to EEPROM, if supported // bool AP_Param::save(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); const AP_Param *ap; if (info == NULL) { // we don't have any info on how to store it return false; } struct Param_header phdr; // create the header we will use to store 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; ap = this; if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) { // only vector3f can have non-zero idx for now return false; } if (idx != 0) { ap = (const AP_Param *)((uintptr_t)ap) - (idx*sizeof(float)); } // scan EEPROM to find the right location uint16_t ofs; if (scan(&phdr, &ofs)) { // found an existing copy of the variable eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type)); return true; } if (ofs == (uint16_t) ~0) { return false; } // if the value is the default value then don't save if (phdr.type <= AP_PARAM_FLOAT) { float v1 = cast_to_float((enum ap_var_type)phdr.type); float v2; if (ginfo != NULL) { v2 = PGM_FLOAT(&ginfo->def_value); } else { v2 = PGM_FLOAT(&info->def_value); } if (v1 == v2) { return true; } if (phdr.type != AP_PARAM_INT32 && (fabsf(v1-v2) < 0.0001f*fabsf(v1))) { // for other than 32 bit integers, we accept values within // 0.01 percent of the current value as being the same return true; } } if (ofs+type_size((enum ap_var_type)phdr.type)+2*sizeof(phdr) >= _eeprom_size) { // we are out of room for saving variables return false; } // write a new sentinal, then the data, then the header write_sentinal(ofs + sizeof(phdr) + type_size((enum ap_var_type)phdr.type)); eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type)); eeprom_write_check(&phdr, ofs, sizeof(phdr)); return true; }