diff --git a/libraries/AP_Common/AP_Param.cpp b/libraries/AP_Common/AP_Param.cpp index 95e81a076a..ef8c61485a 100644 --- a/libraries/AP_Common/AP_Param.cpp +++ b/libraries/AP_Common/AP_Param.cpp @@ -31,6 +31,7 @@ // some useful progmem macros #define PGM_UINT8(addr) pgm_read_byte((const prog_char *)addr) #define PGM_UINT16(addr) pgm_read_word((const uint16_t *)addr) +#define PGM_FLOAT(addr) pgm_read_float((const float *)addr) #define PGM_POINTER(addr) pgm_read_pointer((const void *)addr) // the 'GROUP_ID' of a element of a group is the 8 bit identifier used @@ -197,29 +198,32 @@ bool AP_Param::check_var_info(void) return false; } } - if (total_size > _eeprom_size) { - serialDebug("total_size %u exceeds _eeprom_size %u", - total_size, _eeprom_size); - 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; } // setup the _var_info[] table -bool AP_Param::setup(const AP_Param::Info *info, uint8_t num_vars, uint16_t eeprom_size) +bool AP_Param::setup(const struct AP_Param::Info *info, uint16_t eeprom_size) { struct EEPROM_header hdr; + uint8_t i; _eeprom_size = eeprom_size; _var_info = info; - _num_vars = num_vars; + + for (i=0; PGM_UINT8(&info[i].type) != AP_PARAM_NONE; i++) ; + _num_vars = i; if (!check_var_info()) { return false; } - serialDebug("setup %u vars", (unsigned)num_vars); + serialDebug("setup %u vars", (unsigned)_num_vars); // check the header eeprom_read_block(&hdr, 0, sizeof(hdr)); @@ -621,6 +625,17 @@ bool AP_Param::save(void) return false; } + // if the value is the default value then don't save + if (phdr.type <= AP_PARAM_FLOAT && + cast_to_float((enum ap_var_type)phdr.type) == PGM_FLOAT(&info->def_value)) { + 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)); @@ -655,6 +670,14 @@ bool AP_Param::load(void) // 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)), + PGM_FLOAT(&ginfo->def_value)); + } else { + set_value((enum ap_var_type)phdr.type, (void*)PGM_POINTER(&info->ptr), PGM_FLOAT(&info->def_value)); + } return false; } @@ -674,12 +697,69 @@ bool AP_Param::load(void) return true; } +// set a AP_Param variable to a specified value +void AP_Param::set_value(enum ap_var_type type, void *ptr, float def_value) +{ + switch (type) { + case AP_PARAM_INT8: + ((AP_Int8 *)ptr)->set(def_value); + break; + case AP_PARAM_INT16: + ((AP_Int16 *)ptr)->set(def_value); + break; + case AP_PARAM_INT32: + ((AP_Int32 *)ptr)->set(def_value); + break; + case AP_PARAM_FLOAT: + ((AP_Float *)ptr)->set(def_value); + break; + default: + break; + } +} + +// load default values for scalars in a group +void AP_Param::load_defaults_group(const struct GroupInfo *group_info, uintptr_t base) +{ + uint8_t type; + for (uint8_t i=0; + (type=PGM_UINT8(&group_info[i].type)) != AP_PARAM_NONE; + i++) { + if (type == AP_PARAM_GROUP) { + const struct GroupInfo *ginfo = (const struct GroupInfo *)PGM_POINTER(&group_info[i].group_info); + load_defaults_group(ginfo, base); + } else 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)); + } + } +} + + +// load default values for all scalars +void AP_Param::load_defaults(void) +{ + for (uint8_t i=0; i<_num_vars; i++) { + uint8_t 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); + uintptr_t base = PGM_POINTER(&_var_info[i].ptr); + load_defaults_group(group_info, base); + } else 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)); + } + } +} + + // Load all variables from EEPROM // bool AP_Param::load_all(void) { struct Param_header phdr; uint16_t ofs = sizeof(AP_Param::EEPROM_header); + while (ofs < _eeprom_size) { eeprom_read_block(&phdr, (void *)ofs, sizeof(phdr)); // note that this is an || not an && for robustness diff --git a/libraries/AP_Common/AP_Param.h b/libraries/AP_Common/AP_Param.h index 2d1dd397f4..585982fad2 100644 --- a/libraries/AP_Common/AP_Param.h +++ b/libraries/AP_Common/AP_Param.h @@ -31,14 +31,15 @@ #define AP_CLASSTYPE(class, element) (((const class *)1)->element.vtype) // declare a group var_info line -#define AP_GROUPINFO(name, idx, class, element) { AP_CLASSTYPE(class, element), idx, name, AP_VAROFFSET(class, element) } +#define AP_GROUPINFO(name, idx, class, element, def) { AP_CLASSTYPE(class, element), idx, name, AP_VAROFFSET(class, element), {def_value:def} } // declare a nested group entry in a group var_info #ifdef AP_NESTED_GROUPS_ENABLED -#define AP_NESTEDGROUPINFO(class, idx) { AP_PARAM_GROUP, idx, "", 0, class::var_info } +#define AP_NESTEDGROUPINFO(class, idx) { AP_PARAM_GROUP, idx, "", 0, { group_info: class::var_info } } #endif -#define AP_GROUPEND { AP_PARAM_NONE, 0xFF, "" } +#define AP_GROUPEND { AP_PARAM_NONE, 0xFF, "", 0, { group_info : NULL } } +#define AP_VAREND { AP_PARAM_NONE, "", 0, NULL, { group_info : NULL } } enum ap_var_type { AP_PARAM_NONE = 0, @@ -67,27 +68,42 @@ public: uint8_t idx; // identifier within the group const char name[AP_MAX_NAME_SIZE]; uintptr_t offset; // offset within the object - const struct GroupInfo *group_info; + union { + const struct GroupInfo *group_info; + const float def_value; + }; }; struct Info { uint8_t type; // AP_PARAM_* const char name[AP_MAX_NAME_SIZE]; uint8_t key; // k_param_* void *ptr; // pointer to the variable in memory - const struct GroupInfo *group_info; + union { + const struct GroupInfo *group_info; + const float def_value; + }; }; - // a token used for first()/next() state - typedef struct { - uint8_t key; - uint8_t group_element; - uint8_t idx; // offset into array types - } ParamToken; - // called once at startup to setup the _var_info[] table. This // will also check the EEPROM header and re-initialise it if the // wrong version is found - static bool setup(const struct Info *info, uint8_t num_vars, uint16_t eeprom_size); + static bool setup(const struct Info *info, uint16_t eeprom_size); + + // constructor to load default values and setup var_info table + AP_Param(const struct Info *info, uint16_t eeprom_size) { + setup(info, eeprom_size); + load_defaults(); + } + + // empty constructor for child classes + AP_Param() {} + + // a token used for first()/next() state + typedef struct { + uint32_t key:8; + uint32_t idx:6; // offset into array types + uint32_t group_element:18; + } ParamToken; // return true if AP_Param has been initialised via setup() static bool initialised(void); @@ -136,6 +152,15 @@ public: /// static bool load_all(void); + // set a AP_Param variable to a specified value + static void set_value(enum ap_var_type type, void *ptr, float def_value); + + // load default values for scalars in a group + static void load_defaults_group(const struct GroupInfo *group_info, uintptr_t base); + + // load default values for all scalars + static void load_defaults(void); + /// Erase all variables in EEPROM. /// static void erase_all(void); @@ -179,24 +204,21 @@ private: - key: the k_param enum value from Parameter.h in the sketch - group_element: This is zero for top level parameters. For - parameters stored within an object the top 4 bits - are the idx field of the GroupInfo structue (the index into - the first level of object indirection). The second - 4 bits are the idx field from the second level of - object indirection. This allows for two levels of - object to be stored in the eeprom + parameters stored within an object this is divided + into 3 lots of 6 bits, allowing for three levels + of object to be stored in the eeprom - type: the ap_var_type value for the variable */ struct Param_header { - uint8_t key; - uint8_t group_element; - uint8_t type; + uint32_t key:8; + uint32_t type:6; + uint32_t group_element:18; }; // number of bits in each level of nesting of groups - static const uint8_t _group_level_shift = 4; - static const uint8_t _group_bits = 8; + static const uint8_t _group_level_shift = 6; + static const uint8_t _group_bits = 18; static const uint8_t _sentinal_key = 0xFF; static const uint8_t _sentinal_type = 0xFF; @@ -241,7 +263,7 @@ private: // values filled into the EEPROM header static const uint8_t k_EEPROM_magic0 = 0x50; static const uint8_t k_EEPROM_magic1 = 0x41; ///< "AP" - static const uint8_t k_EEPROM_revision = 5; ///< current format revision + static const uint8_t k_EEPROM_revision = 6; ///< current format revision }; /// Template class for scalar variables. @@ -256,18 +278,6 @@ template class AP_ParamT : public AP_Param { public: - /// Constructor for scalar variable. - /// - /// Initialises a stand-alone variable with optional initial value. - /// - /// @param default_value Value the variable should have at startup. - /// - AP_ParamT (const T initial_value = 0) : - AP_Param(), - _value(initial_value) - { - } - static const ap_var_type vtype = PT; /// Value getter @@ -346,6 +356,7 @@ template class AP_ParamV : public AP_Param { public: + static const ap_var_type vtype = PT; /// Value getter @@ -406,6 +417,7 @@ template class AP_ParamA : public AP_Param { public: + static const ap_var_type vtype = PT; /// Array operator accesses members. diff --git a/libraries/AP_Common/tools/eedump_apparam.c b/libraries/AP_Common/tools/eedump_apparam.c index 6f4f7174a6..084cd156bf 100644 --- a/libraries/AP_Common/tools/eedump_apparam.c +++ b/libraries/AP_Common/tools/eedump_apparam.c @@ -18,7 +18,7 @@ struct EEPROM_header { static const uint16_t k_EEPROM_magic0 = 0x50; static const uint16_t k_EEPROM_magic1 = 0x41; -static const uint16_t k_EEPROM_revision = 5; +static const uint16_t k_EEPROM_revision = 6; enum ap_var_type { AP_PARAM_NONE = 0, @@ -37,9 +37,9 @@ static const char *type_names[8] = { }; struct Param_header { - uint8_t key; - uint8_t group_element; - uint8_t type; + uint32_t key:8; + uint32_t type:6; + uint32_t group_element:18; }; diff --git a/libraries/AP_Common/tools/eedump_apparam.pl b/libraries/AP_Common/tools/eedump_apparam.pl old mode 100644 new mode 100755 index d5e972a035..dbe14b21e7 --- a/libraries/AP_Common/tools/eedump_apparam.pl +++ b/libraries/AP_Common/tools/eedump_apparam.pl @@ -13,7 +13,7 @@ if (ord($buffer2) != 0x41 && ord($buffer) != 0x50) { exit; } read(IN,$buffer,1); -if (ord($buffer) != 5) { +if (ord($buffer) != 6) { print "bad version"; exit; } @@ -26,47 +26,53 @@ $a = 0; while (read(IN,$buffer,1)) { $pos = (tell(IN) - 1); - if (ord($buffer) == 0xff) { + my $key = ord($buffer); + + if ($key == 0xff) { printf("end sentinel at %u\n", $pos); last; } - read(IN,$buffer2,1); - read(IN,$buffer3,1); - - if (ord($buffer3) == 0) { #none - $size = 0; - $type = "NONE"; - } elsif (ord($buffer3) == 1) { #int8 - $size = 1; - $type = "INT8"; - } elsif (ord($buffer3) == 2) { #int16 - $size = 2; - $type = "INT16"; - } elsif (ord($buffer3) == 3) { #int32 - $size = 4; - $type = "INT32"; - } elsif (ord($buffer3) == 4) { #float - $size = 4; - $type = "FLOAT"; - } elsif (ord($buffer3) == 5) { #vector 3 - $size = 3*4; - $type = "VECTOR3F"; - } elsif (ord($buffer3) == 6) { #vector6 - $size = 6*4; - $type = "VECTOR6F"; - } elsif (ord($buffer3) == 7) { #matrix - $size = 3*3*4; - $type = "MATRIX6F"; - } elsif (ord($buffer3) == 8) { #group - $size = 0; - $type = "GROUP"; - } else { - print "Unknown type\n"; - $size = 0; - } + read(IN,$buffer2,1); + read(IN,$buffer3,1); + read(IN,$buffer4,1); - printf("%04x: type %u ($type) key %u group_element %u size %d\n ", $pos, ord($buffer3),ord($buffer),ord($buffer2), $size); + my $itype = ord($buffer2)&0x3F; + my $group_element = (ord($buffer2)>>6) | (ord($buffer3)<<8) | (ord($buffer4)<<16); + + if ($itype == 0) { #none + $size = 0; + $type = "NONE"; + } elsif ($itype == 1) { #int8 + $size = 1; + $type = "INT8"; + } elsif ($itype == 2) { #int16 + $size = 2; + $type = "INT16"; + } elsif ($itype == 3) { #int32 + $size = 4; + $type = "INT32"; + } elsif ($itype == 4) { #float + $size = 4; + $type = "FLOAT"; + } elsif ($itype == 5) { #vector 3 + $size = 3*4; + $type = "VECTOR3F"; + } elsif ($itype == 6) { #vector6 + $size = 6*4; + $type = "VECTOR6F"; + } elsif ($itype == 7) { #matrix + $size = 3*3*4; + $type = "MATRIX6F"; + } elsif ($itype == 8) { #group + $size = 0; + $type = "GROUP"; + } else { + print "Unknown type\n"; + $size = 0; + } + + printf("%04x: type %u ($type) key %u group_element %u size %d\n ", $pos, $itype, $key, $group_element, $size); for ($i = 0; $i < ($size); $i++) { read(IN,$buffer,1);