Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Param: allow for up to 512 top level vehicle parameters

Browse Source 
this will make life a bit easier for copter
This commit is contained in:
Andrew Tridgell 2015-12-31 08:27:22 +11:00
parent 0831661b3c
commit 1b8cf84801
2 changed files with 82 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
libraries/AP_Param/AP_Param.cpp
View File

@ -65,7 +65,7 @@ extern const AP_HAL::HAL &hal;
// //
// number of rows in the _var_info[] table // number of rows in the _var_info[] table
uint8_t AP_Param::_num_vars; uint16_t AP_Param::_num_vars;
// storage and naming information about all types that can be saved // storage and naming information about all types that can be saved
const AP_Param::Info *AP_Param::_var_info; const AP_Param::Info *AP_Param::_var_info;
@ -88,7 +88,7 @@ void AP_Param::write_sentinal(uint16_t ofs)
{ {
struct Param_header phdr; struct Param_header phdr;
phdr.type = _sentinal_type; phdr.type = _sentinal_type;
phdr.key = _sentinal_key; set_key(phdr, _sentinal_key);
phdr.group_element = _sentinal_group; phdr.group_element = _sentinal_group;
eeprom_write_check(&phdr, ofs, sizeof(phdr)); eeprom_write_check(&phdr, ofs, sizeof(phdr));
} }
@ -161,10 +161,10 @@ bool AP_Param::check_group_info(const struct AP_Param::GroupInfo * group_info,
} }
// check for duplicate key values // check for duplicate key values
bool AP_Param::duplicate_key(uint8_t vindex, uint8_t key) bool AP_Param::duplicate_key(uint16_t vindex, uint16_t key)
{ {
for (uint8_t i=vindex+1; i<_num_vars; i++) { for (uint16_t i=vindex+1; i<_num_vars; i++) {
uint8_t key2 = _var_info[i].key; uint16_t key2 = _var_info[i].key;
if (key2 == key) { if (key2 == key) {
// no duplicate keys allowed // no duplicate keys allowed
return true; return true;
@ -178,9 +178,9 @@ bool AP_Param::check_var_info(void)
{ {
uint16_t total_size = sizeof(struct EEPROM_header); uint16_t total_size = sizeof(struct EEPROM_header);
for (uint8_t i=0; i<_num_vars; i++) { for (uint16_t i=0; i<_num_vars; i++) {
uint8_t type = _var_info[i].type; uint8_t type = _var_info[i].type;
uint8_t key = _var_info[i].key; uint16_t key = _var_info[i].key;
if (type == AP_PARAM_GROUP) { if (type == AP_PARAM_GROUP) {
if (i == 0) { if (i == 0) {
// first element can't be a group, for first() call // first element can't be a group, for first() call
@ -246,7 +246,7 @@ bool AP_Param::initialised(void)
The new_offset variable is relative to the vindex base. This makes The new_offset variable is relative to the vindex base. This makes
dealing with pointer groups tricky dealing with pointer groups tricky
*/ */
bool AP_Param::adjust_group_offset(uint8_t vindex, const struct GroupInfo &group_info, ptrdiff_t &new_offset) bool AP_Param::adjust_group_offset(uint16_t vindex, const struct GroupInfo &group_info, ptrdiff_t &new_offset)
{ {
if (group_info.flags & AP_PARAM_FLAG_NESTED_OFFSET) { if (group_info.flags & AP_PARAM_FLAG_NESTED_OFFSET) {
new_offset += group_info.offset; new_offset += group_info.offset;
@ -268,7 +268,7 @@ bool AP_Param::adjust_group_offset(uint8_t vindex, const struct GroupInfo &group
// find the info structure given a header and a group_info table // find the info structure given a header and a group_info table
// return the Info structure and a pointer to the variables storage // 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, const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header phdr, void **ptr,
uint8_t vindex, uint16_t vindex,
const struct GroupInfo *group_info, const struct GroupInfo *group_info,
uint8_t group_base, uint8_t group_base,
uint8_t group_shift, uint8_t group_shift,
@ -314,10 +314,10 @@ const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header
const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr, void **ptr) const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr, void **ptr)
{ {
// loop over all named variables // loop over all named variables
for (uint8_t i=0; i<_num_vars; i++) { for (uint16_t i=0; i<_num_vars; i++) {
uint8_t type = _var_info[i].type; uint8_t type = _var_info[i].type;
uint8_t key = _var_info[i].key; uint16_t key = _var_info[i].key;
if (key != phdr.key) { if (key != get_key(phdr)) {
// not the right key // not the right key
continue; continue;
} }
@ -336,7 +336,7 @@ const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr,
// find the info structure for a variable in a group // 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, const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInfo * group_info,
uint8_t vindex, uint16_t vindex,
uint8_t group_base, uint8_t group_base,
uint8_t group_shift, uint8_t group_shift,
ptrdiff_t group_offset, ptrdiff_t group_offset,
@ -403,7 +403,7 @@ const struct AP_Param::Info *AP_Param::find_var_info(uint32_t *
{ {
group_ret0 = group_ret = NULL; group_ret0 = group_ret = NULL;
for (uint8_t i=0; i<_num_vars; i++) { for (uint16_t i=0; i<_num_vars; i++) {
uint8_t type = _var_info[i].type; uint8_t type = _var_info[i].type;
ptrdiff_t base = (ptrdiff_t)_var_info[i].ptr; ptrdiff_t base = (ptrdiff_t)_var_info[i].ptr;
if (type == AP_PARAM_GROUP) { if (type == AP_PARAM_GROUP) {
@ -438,7 +438,7 @@ const struct AP_Param::Info *AP_Param::find_var_info_token(const ParamToken &tok
const struct GroupInfo * &group_ret0, const struct GroupInfo * &group_ret0,
uint8_t * idx) const uint8_t * idx) const
{ {
uint8_t i = token.key; uint16_t i = token.key;
uint8_t type = _var_info[i].type; uint8_t type = _var_info[i].type;
ptrdiff_t base = (ptrdiff_t)_var_info[i].ptr; ptrdiff_t base = (ptrdiff_t)_var_info[i].ptr;
group_ret0 = group_ret = NULL; group_ret0 = group_ret = NULL;
@ -483,15 +483,43 @@ uint8_t AP_Param::type_size(enum ap_var_type type)
return 4; return 4;
case AP_PARAM_VECTOR3F: case AP_PARAM_VECTOR3F:
return 3*4; return 3*4;
case AP_PARAM_VECTOR6F:
return 6*4;
case AP_PARAM_MATRIX3F:
return 3*3*4;
} }
Debug("unknown type %u\n", type); Debug("unknown type %u\n", type);
return 0; return 0;
} }
/*
extract 9 bit key from Param_header
*/
uint16_t AP_Param::get_key(const Param_header &phdr)
{
return ((uint16_t)phdr.key_high)<<8 | phdr.key_low;
}
/*
set 9 bit key in Param_header
*/
void AP_Param::set_key(Param_header &phdr, uint16_t key)
{
phdr.key_low = key & 0xFF;
phdr.key_high = key >> 8;
}
/*
return true if a header is the end of eeprom sentinal
*/
bool AP_Param::is_sentinal(const Param_header &phdr)
{
// note that this is an ||, not an &&, as this makes us more
// robust to power off while adding a variable to EEPROM
if (phdr.type == _sentinal_type ||
get_key(phdr) == _sentinal_key ||
phdr.group_element == _sentinal_group) {
return true;
}
return false;
}
// scan the EEPROM looking for a given variable by header content // scan the EEPROM looking for a given variable by header content
// return true if found, along with the offset in the EEPROM where // return true if found, along with the offset in the EEPROM where
// the variable is stored // the variable is stored
@ -504,17 +532,13 @@ bool AP_Param::scan(const AP_Param::Param_header *target, uint16_t *pofs)
while (ofs < _storage.size()) { while (ofs < _storage.size()) {
_storage.read_block(&phdr, ofs, sizeof(phdr)); _storage.read_block(&phdr, ofs, sizeof(phdr));
if (phdr.type == target->type && if (phdr.type == target->type &&
phdr.key == target->key && get_key(phdr) == get_key(*target) &&
phdr.group_element == target->group_element) { phdr.group_element == target->group_element) {
// found it // found it
*pofs = ofs; *pofs = ofs;
return true; return true;
} }
// note that this is an ||, not an &&, as this makes us more if (is_sentinal(phdr)) {
// robust to power off while adding a variable to EEPROM
if (phdr.type == _sentinal_type ||
phdr.key == _sentinal_key ||
phdr.group_element == _sentinal_group) {
// we've reached the sentinal // we've reached the sentinal
*pofs = ofs; *pofs = ofs;
return false; return false;
@ -591,7 +615,7 @@ void AP_Param::copy_name_info(const struct AP_Param::Info *info,
// Find a variable by name in a group // Find a variable by name in a group
AP_Param * AP_Param *
AP_Param::find_group(const char *name, uint8_t vindex, ptrdiff_t group_offset, AP_Param::find_group(const char *name, uint16_t vindex, ptrdiff_t group_offset,
const struct GroupInfo *group_info, enum ap_var_type *ptype) const struct GroupInfo *group_info, enum ap_var_type *ptype)
{ {
uint8_t type; uint8_t type;
@ -648,7 +672,7 @@ AP_Param::find_group(const char *name, uint8_t vindex, ptrdiff_t group_offset,
AP_Param * AP_Param *
AP_Param::find(const char *name, enum ap_var_type *ptype) AP_Param::find(const char *name, enum ap_var_type *ptype)
{ {
for (uint8_t i=0; i<_num_vars; i++) { for (uint16_t i=0; i<_num_vars; i++) {
uint8_t type = _var_info[i].type; uint8_t type = _var_info[i].type;
if (type == AP_PARAM_GROUP) { if (type == AP_PARAM_GROUP) {
uint8_t len = strnlen(_var_info[i].name, AP_MAX_NAME_SIZE); uint8_t len = strnlen(_var_info[i].name, AP_MAX_NAME_SIZE);
@ -732,7 +756,7 @@ bool AP_Param::find_by_pointer(AP_Param *p, ParamToken &token)
AP_Param * AP_Param *
AP_Param::find_object(const char *name) AP_Param::find_object(const char *name)
{ {
for (uint8_t i=0; i<_num_vars; i++) { for (uint16_t i=0; i<_num_vars; i++) {
if (strcasecmp(name, _var_info[i].name) == 0) { if (strcasecmp(name, _var_info[i].name) == 0) {
return (AP_Param *)_var_info[i].ptr; return (AP_Param *)_var_info[i].ptr;
} }
@ -791,7 +815,7 @@ bool AP_Param::save(bool force_save)
} else { } else {
phdr.type = info->type; phdr.type = info->type;
} }
phdr.key = info->key; set_key(phdr, info->key);
phdr.group_element = group_element; phdr.group_element = group_element;
ap = this; ap = this;
@ -877,7 +901,7 @@ bool AP_Param::load(void)
} else { } else {
phdr.type = info->type; phdr.type = info->type;
} }
phdr.key = info->key; set_key(phdr, info->key);
phdr.group_element = group_element; phdr.group_element = group_element;
// scan EEPROM to find the right location // scan EEPROM to find the right location
@ -931,7 +955,7 @@ bool AP_Param::configured_in_storage(void)
} else { } else {
phdr.type = info->type; phdr.type = info->type;
} }
phdr.key = info->key; set_key(phdr, info->key);
phdr.group_element = group_element; phdr.group_element = group_element;
// scan EEPROM to find the right location // scan EEPROM to find the right location
@ -1032,7 +1056,7 @@ void AP_Param::set_object_value(const void *object_pointer,
void AP_Param::setup_sketch_defaults(void) void AP_Param::setup_sketch_defaults(void)
{ {
setup(); setup();
for (uint8_t i=0; i<_num_vars; i++) { for (uint16_t i=0; i<_num_vars; i++) {
uint8_t type = _var_info[i].type; uint8_t type = _var_info[i].type;
if (type <= AP_PARAM_FLOAT) { if (type <= AP_PARAM_FLOAT) {
void *ptr = (void*)_var_info[i].ptr; void *ptr = (void*)_var_info[i].ptr;
@ -1061,9 +1085,7 @@ bool AP_Param::load_all(void)
_storage.read_block(&phdr, ofs, sizeof(phdr)); _storage.read_block(&phdr, ofs, sizeof(phdr));
// note that this is an || not an && for robustness // note that this is an || not an && for robustness
// against power off while adding a variable // against power off while adding a variable
if (phdr.type == _sentinal_type || if (is_sentinal(phdr)) {
phdr.key == _sentinal_key ||
phdr.group_element == _sentinal_group) {
// we've reached the sentinal // we've reached the sentinal
return true; return true;
} }
@ -1124,13 +1146,11 @@ void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct
_storage.read_block(&phdr, ofs, sizeof(phdr)); _storage.read_block(&phdr, ofs, sizeof(phdr));
// note that this is an || not an && for robustness // note that this is an || not an && for robustness
// against power off while adding a variable // against power off while adding a variable
if (phdr.type == _sentinal_type || if (is_sentinal(phdr)) {
phdr.key == _sentinal_key ||
phdr.group_element == _sentinal_group) {
// we've reached the sentinal // we've reached the sentinal
break; break;
} }
if (phdr.key == _var_info[token.key].key) { if (get_key(phdr) == _var_info[token.key].key) {
const struct AP_Param::Info *info; const struct AP_Param::Info *info;
void *ptr; void *ptr;
@ -1165,7 +1185,7 @@ AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype)
/// Returns the next variable in a group, recursing into groups /// Returns the next variable in a group, recursing into groups
/// as needed /// as needed
AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_info, AP_Param *AP_Param::next_group(uint16_t vindex, const struct GroupInfo *group_info,
bool *found_current, bool *found_current,
uint8_t group_base, uint8_t group_base,
uint8_t group_shift, uint8_t group_shift,
@ -1226,7 +1246,7 @@ AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_inf
/// as needed /// as needed
AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype) AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype)
{ {
uint8_t i = token->key; uint16_t i = token->key;
bool found_current = false; bool found_current = false;
if (i >= _num_vars) { if (i >= _num_vars) {
// illegal token // illegal token
@ -1397,7 +1417,7 @@ void AP_Param::convert_old_parameter(const struct ConversionInfo *info)
uint16_t pofs; uint16_t pofs;
AP_Param::Param_header header; AP_Param::Param_header header;
header.type = info->type; header.type = info->type;
header.key = info->old_key; set_key(header, info->old_key);
header.group_element = info->old_group_element; header.group_element = info->old_group_element;
if (!scan(&header, &pofs)) { if (!scan(&header, &pofs)) {
// the old parameter isn't saved in the EEPROM. It was // the old parameter isn't saved in the EEPROM. It was

45
libraries/AP_Param/AP_Param.h
View File

@ -80,8 +80,6 @@ enum ap_var_type {
AP_PARAM_INT32, AP_PARAM_INT32,
AP_PARAM_FLOAT, AP_PARAM_FLOAT,
AP_PARAM_VECTOR3F, AP_PARAM_VECTOR3F,
AP_PARAM_VECTOR6F,
AP_PARAM_MATRIX3F,
AP_PARAM_GROUP AP_PARAM_GROUP
}; };
@ -110,7 +108,7 @@ public:
struct Info { struct Info {
uint8_t type; // AP_PARAM_* uint8_t type; // AP_PARAM_*
const char name[AP_MAX_NAME_SIZE+1]; const char name[AP_MAX_NAME_SIZE+1];
uint8_t key; // k_param_* uint16_t key; // k_param_*
const void *ptr; // pointer to the variable in memory const void *ptr; // pointer to the variable in memory
union { union {
const struct GroupInfo *group_info; const struct GroupInfo *group_info;
@ -118,7 +116,7 @@ public:
}; };
}; };
struct ConversionInfo { struct ConversionInfo {
uint8_t old_key; // k_param_* uint16_t old_key; // k_param_*
uint8_t old_group_element; // index in old object uint8_t old_group_element; // index in old object
enum ap_var_type type; // AP_PARAM_* enum ap_var_type type; // AP_PARAM_*
const char new_name[AP_MAX_NAME_SIZE+1]; const char new_name[AP_MAX_NAME_SIZE+1];
@ -143,8 +141,8 @@ public:
// a token used for first()/next() state // a token used for first()/next() state
typedef struct { typedef struct {
uint32_t key : 8; uint32_t key : 9;
uint32_t idx : 6; // offset into array types uint32_t idx : 5; // offset into array types
uint32_t group_element : 18; uint32_t group_element : 18;
} ParamToken; } ParamToken;
@ -340,8 +338,10 @@ private:
* - type: the ap_var_type value for the variable * - type: the ap_var_type value for the variable
*/ */
struct Param_header { struct Param_header {
uint32_t key : 8; // to get 9 bits for key we needed to split it into two parts to keep binary compatibility
uint32_t type : 6; uint32_t key_low : 8;
uint32_t type : 5;
uint32_t key_high : 1;
uint32_t group_element : 18; uint32_t group_element : 18;
}; };
@ -349,19 +349,19 @@ private:
static const uint8_t _group_level_shift = 6; static const uint8_t _group_level_shift = 6;
static const uint8_t _group_bits = 18; static const uint8_t _group_bits = 18;
static const uint8_t _sentinal_key = 0xFF; static const uint16_t _sentinal_key = 0x1FF;
static const uint8_t _sentinal_type = 0x3F; static const uint8_t _sentinal_type = 0x1F;
static const uint8_t _sentinal_group = 0xFF; static const uint8_t _sentinal_group = 0xFF;
static bool check_group_info(const struct GroupInfo *group_info, uint16_t *total_size, static bool check_group_info(const struct GroupInfo *group_info, uint16_t *total_size,
uint8_t max_bits, uint8_t prefix_length); uint8_t max_bits, uint8_t prefix_length);
static bool duplicate_key(uint8_t vindex, uint8_t key); static bool duplicate_key(uint16_t vindex, uint16_t key);
static bool adjust_group_offset(uint8_t vindex, const struct GroupInfo &group_info, ptrdiff_t &new_offset); static bool adjust_group_offset(uint16_t vindex, const struct GroupInfo &group_info, ptrdiff_t &new_offset);
const struct Info * find_var_info_group( const struct Info * find_var_info_group(
const struct GroupInfo * group_info, const struct GroupInfo * group_info,
uint8_t vindex, uint16_t vindex,
uint8_t group_base, uint8_t group_base,
uint8_t group_shift, uint8_t group_shift,
ptrdiff_t group_offset, ptrdiff_t group_offset,
@ -381,7 +381,7 @@ private:
uint8_t * idx) const; uint8_t * idx) const;
static const struct Info * find_by_header_group( static const struct Info * find_by_header_group(
struct Param_header phdr, void **ptr, struct Param_header phdr, void **ptr,
uint8_t vindex, uint16_t vindex,
const struct GroupInfo *group_info, const struct GroupInfo *group_info,
uint8_t group_base, uint8_t group_base,
uint8_t group_shift, uint8_t group_shift,
@ -395,11 +395,14 @@ private:
uint8_t idx) const; uint8_t idx) const;
static AP_Param * find_group( static AP_Param * find_group(
const char *name, const char *name,
uint8_t vindex, uint16_t vindex,
ptrdiff_t group_offset, ptrdiff_t group_offset,
const struct GroupInfo *group_info, const struct GroupInfo *group_info,
enum ap_var_type *ptype); enum ap_var_type *ptype);
static void write_sentinal(uint16_t ofs); static void write_sentinal(uint16_t ofs);
static uint16_t get_key(const Param_header &phdr);
static void set_key(Param_header &phdr, uint16_t key);
static bool is_sentinal(const Param_header &phrd);
static bool scan( static bool scan(
const struct Param_header *phdr, const struct Param_header *phdr,
uint16_t *pofs); uint16_t *pofs);
@ -409,7 +412,7 @@ private:
uint16_t ofs, uint16_t ofs,
uint8_t size); uint8_t size);
static AP_Param * next_group( static AP_Param * next_group(
uint8_t vindex, uint16_t vindex,
const struct GroupInfo *group_info, const struct GroupInfo *group_info,
bool *found_current, bool *found_current,
uint8_t group_base, uint8_t group_base,
@ -435,7 +438,7 @@ private:
#endif #endif
static StorageAccess _storage; static StorageAccess _storage;
static uint8_t _num_vars; static uint16_t _num_vars;
static const struct Info * _var_info; static const struct Info * _var_info;
/* /*
@ -690,14 +693,6 @@ AP_PARAMDEF(int8_t, Int8, AP_PARAM_INT8); // defines AP_Int8
AP_PARAMDEF(int16_t, Int16, AP_PARAM_INT16); // defines AP_Int16 AP_PARAMDEF(int16_t, Int16, AP_PARAM_INT16); // defines AP_Int16
AP_PARAMDEF(int32_t, Int32, AP_PARAM_INT32); // defines AP_Int32 AP_PARAMDEF(int32_t, Int32, AP_PARAM_INT32); // defines AP_Int32
// declare an array type
// _t is the base type
// _suffix is the suffix on the AP_* type name
// _size is the size of the array
// _pt is the enum ap_var_type type
#define AP_PARAMDEFA(_t, _suffix, _size, _pt) typedef AP_ParamA<_t, _size, _pt> AP_ ## _suffix;
AP_PARAMDEFA(float, Vector6f, 6, AP_PARAM_VECTOR6F);
// declare a non-scalar type // declare a non-scalar type
// this is used in AP_Math.h // this is used in AP_Math.h
// _t is the base type // _t is the base type