diff --git a/libraries/AP_Common/AP_MetaClass.h b/libraries/AP_Common/AP_MetaClass.h index f80ef79bf5..fa8632960f 100644 --- a/libraries/AP_Common/AP_MetaClass.h +++ b/libraries/AP_Common/AP_MetaClass.h @@ -24,10 +24,14 @@ #include #include // for RAMEND +/// /// Basic meta-class from which other AP_* classes can derive. /// /// Functions that form the public API to the metaclass are prefixed meta_. /// +/// Note that classes inheriting from AP_Meta_class *must* have a default +/// constructor and destructor in order for meta_cast to work. +/// class AP_Meta_class { public: diff --git a/libraries/AP_Common/AP_Var.cpp b/libraries/AP_Common/AP_Var.cpp index a4ea528cd5..01e456757b 100644 --- a/libraries/AP_Common/AP_Var.cpp +++ b/libraries/AP_Common/AP_Var.cpp @@ -22,16 +22,74 @@ AP_Float AP_Float_zero(0); AP_Var *AP_Var::_variables = NULL; AP_Var *AP_Var::_lookup_hint = NULL; int AP_Var::_lookup_hint_index = 0; +uint16_t AP_Var::_tail_sentinel; +bool AP_Var::_EEPROM_scanned; +AP_Var_group::AP_Var_group() +{ +} + +AP_Var_group::~AP_Var_group() +{ +} // Constructor // -AP_Var::AP_Var(Address address, const prog_char *name, const AP_Var_scope *scope, Flags flags) : - _address(address), _name(name), _scope(scope), _flags(flags) +AP_Var::AP_Var(Key key, const prog_char *name, AP_Var_group *group, Flags flags) : + _group(group), + _key(key | k_not_located), + _name(name), + _flags(flags) { - // link the variable into the list of known variables - _link = _variables; - _variables = this; + AP_Var *vp; + + // Insert the variable or group into the list of known variables. + // + // Variables belonging to a group are inserted into the list following the group, + // which may not itself yet be in the global list. Thus groups must be + // statically constructed (which guarantees _link will be zero due to the BSS + // being cleared). + // + + if (group) { + + // Sort the variable into the list of variables following the group itself. + vp = group; + + for (;;) { + // if there are no more entries, we insert at the end + if (vp->_link == NULL) + break; + + // if the next entry in the list is a group, insert before it + if (meta_type_equivalent(this, vp->_link)) + break; + + // if the next entry has a higher index, insert before it + if ((vp->_link->_key & k_key_mask) > key) + break; + vp = vp->_link; + } + + // insert into the group's list + _link = vp->_link; + vp->_link = this; + + } else { + // Insert directly at the head of the list. Take into account the possibility + // that what is being inserted is a group that already has variables sorted after it. + // + vp = this; + if (meta_cast(this) != NULL) { + // we are inserting a group, scan to the end of any list of pre-attached variables + while (vp->_link != NULL) + vp = vp->_link; + } + + // insert at the head of the global list + vp->_link = _variables; + _variables = this; + } // reset the lookup cache _lookup_hint_index = 0; @@ -43,116 +101,104 @@ AP_Var::AP_Var(Address address, const prog_char *name, const AP_Var_scope *scope // AP_Var::~AP_Var(void) { - // if we are at the head of the list - for variables - // recently constructed this is usually the case - if (_variables == this) { - // remove us from the head of the list - _variables = _link; + AP_Var **vp; - } else { - // traverse the list looking for the entry that points to us - AP_Var *p = _variables; + // Groups can only be destroyed when they have no members. + // + // If this is a group with one or more members, _link is not NULL + // and _link->group is this. + // + if ((_link != NULL) && (_link->_group == this)) + return; - while (p) { - // is it pointing at us? - if (p->_link == this) { - // make it point at what we point at, and done - p->_link = _link; - break; - } - // try the next one - p = p->_link; + // Walk the list and remove this when we find it. + // + vp = &_variables; + while (*vp != NULL) { + // pointer pointing at this? + if (*vp == this) { + *vp = _link; + break; } + + // address of next entry's link pointer + vp = &((*vp)->_link); } // reset the lookup cache _lookup_hint_index = 0; } -void AP_Var::set_default(void) -{ - // Default implementation of ::set_default does nothing -} - -// Copy the variable name to the provided buffer. +// Copy the variable's whole name to the supplied buffer. +// +// If the variable is a group member, prepend the group name. // void AP_Var::copy_name(char *buffer, size_t buffer_size) const { buffer[0] = '\0'; - if (_scope) { - _scope->copy_name(buffer, buffer_size); - } + if (_group) + _group->copy_name(buffer, buffer_size); strlcat_P(buffer, _name, buffer_size); } -// Compute any offsets that should be applied to a variable in this scope. -// -// Note that this depends on the default _address value for scopes being zero. -// -AP_Var::Address AP_Var_scope::get_address(void) const -{ - AP_Var::Address addr = _address; - - if (_parent) { - addr += _parent->get_address(); - } - - return addr; -} - -// Compute the address in EEPROM where the variable is stored -// -AP_Var::Address AP_Var::_get_address(void) const -{ - Address addr = _address; - - // if we have an address at all - if ((addr != k_no_address) && _scope) { - addr += _scope->get_address(); - } - - return addr; -} - // Save the variable to EEPROM, if supported // -void AP_Var::save(void) const +bool AP_Var::save(void) { - Address addr = _get_address(); + uint8_t vbuf[k_max_size]; + size_t size; - if (addr != k_no_address) { - uint8_t vbuf[k_max_size]; - size_t size; - - // serialize the variable into the buffer and work out how big it is - size = serialize(vbuf, sizeof(vbuf)); - - // if it fit in the buffer, save it to EEPROM - if (size <= sizeof(vbuf)) { - eeprom_write_block(vbuf, (void *)addr, size); - } + // if the variable is a group member, save the group + if (_group) { + return _group->save(); } + + // locate the variable in EEPROM, allocating space as required + if (!_EEPROM_locate(true)) { + return false; + } + + // serialize the variable into the buffer and work out how big it is + size = serialize(vbuf, sizeof(vbuf)); + + // if it fit in the buffer, save it to EEPROM + if (size <= sizeof(vbuf)) { + eeprom_write_block(vbuf, (void *)_key, size); + } + return true; } // Load the variable from EEPROM, if supported // -void AP_Var::load(void) +bool AP_Var::load(void) { - Address addr = _get_address(); + uint8_t vbuf[k_max_size]; + size_t size; - if (addr != k_no_address) { - uint8_t vbuf[k_max_size]; - size_t size; - - // ask the unserializer how big the variable is - size = unserialize(NULL, 0); - - // read the buffer from EEPROM - if (size <= sizeof(vbuf)) { - eeprom_read_block(vbuf, (void *)addr, size); - unserialize(vbuf, size); - } + // if the variable is a group member, load the group + if (_group) { + return _group->load(); } + + // locate the variable in EEPROM, but do not allocate space + if (!_EEPROM_locate(false)) { + return false; + } + + // ask the unserializer how big the variable is + // + // XXX should check size in EEPROM var header too... + // + size = unserialize(NULL, 0); + + // Read the buffer from EEPROM, now that _EEPROM_locate + // has converted _key into an EEPROM address. + // + if (size <= sizeof(vbuf)) { + eeprom_read_block(vbuf, (void *)_key, size); + unserialize(vbuf, size); + } + return true; } // @@ -160,25 +206,26 @@ void AP_Var::load(void) // AP_Var * -AP_Var::lookup(int index) +AP_Var::lookup_by_index(int index) { AP_Var *p; int i; // establish initial search state - if (_lookup_hint_index && // we have a cached hint - (index >= _lookup_hint_index)) { // the desired index is at or after the hint + // + if (_lookup_hint_index && // we have a cached hint (cannot use a hint index of zero) + (index >= _lookup_hint_index)) { // the desired index is at or after the hint - p = _lookup_hint; // start at the hint point - i = index - _lookup_hint_index; // count only the distance from the hint to the index + p = _lookup_hint; // start at the hint point + i = index - _lookup_hint_index; // count only the distance from the hint to the index } else { - p = _variables; // start at the beginning of the list - i = index; // count to the index + p = _variables; // start at the beginning of the list + i = index; // count to the index } // search - while (i-- && p) { // count until we hit the index or the end of the list + while (p && i--) { // count until we hit the index or the end of the list p = p->_link; } @@ -187,34 +234,211 @@ AP_Var::lookup(int index) _lookup_hint_index = index; _lookup_hint = p; } - - return (p); + return p; } -// Save all variables that have an identity. +// Save all variables that don't opt out. // -void AP_Var::save_all(void) +// +bool AP_Var::save_all(void) { + bool result = true; AP_Var *p = _variables; while (p) { - if (!p->has_flags(k_no_auto_load)) { - p->save(); + if (!p->has_flags(k_no_auto_load) && // not opted out + !(p->_group)) { // not saved with a group + + if (!p->save()) { + result = false; + } } p = p->_link; } + return result; } -// Load all variables that have an identity. +// Load all variables that don't opt out. // -void AP_Var::load_all(void) +bool AP_Var::load_all(void) { + bool result; AP_Var *p = _variables; while (p) { - if (!p->has_flags(k_no_auto_load)) { - p->load(); + if (!p->has_flags(k_no_auto_load) && // not opted out + !(p->_group)) { // not loaded with a group + + if (!p->load()) { + result = false; + } } p = p->_link; } + return result; } + +// Scan the list of variables for a matching key. +// +AP_Var * +AP_Var::_lookup_by_key(Key key) +{ + AP_Var *p; + Key nl_key; + Var_header var_header; + + nl_key = key | k_not_located; // key to expect in memory + + // scan the list of variables + p = _variables; + while (p) { + + // if the variable is a group member, it cannot be found by key search + if (p->_group) { + continue; + } + + // has this variable been located? + if (p->_key & k_not_located) { + // does the variable have the non-located form of the key? + if (p->_key == nl_key) + return p; // found it + } else { + // read the header from EEPROM and compare it with the search key + eeprom_read_block(&var_header, (void *)p->_key, sizeof(var_header)); + if (var_header.key == key) + return p; + } + + // try the next variable + p = p->_link; + } + return NULL; +} + +// Scan the EEPROM and assign addresses to all the variables that +// are known and found therein. +// +bool AP_Var::_EEPROM_scan(void) +{ + struct EEPROM_header ee_header; + struct Var_header var_header; + AP_Var *vp; + + // assume that the EEPROM is empty + _tail_sentinel = 0; + + // read the header and validate + eeprom_read_block(0, &ee_header, sizeof(ee_header)); + if ((ee_header.magic != k_EEPROM_magic) || + (ee_header.revision != k_EEPROM_revision)) + return false; + + // scan the EEPROM + // + // Avoid trying to read a header when there isn't enough space left. + // + _tail_sentinel = sizeof(ee_header); + while (_tail_sentinel < (k_EEPROM_size - sizeof(var_header) - 1)) { + + // read a variable header + eeprom_read_block(&var_header, (void *)_tail_sentinel, sizeof(var_header)); + + // if the header is for the sentinel, scanning is complete + if (var_header.key == k_tail_sentinel) + break; + + // if the variable plus the sentinel would extend past the end of EEPROM, we are done + if (k_EEPROM_size <= ( + _tail_sentinel + // current position + sizeof(ee_header) + // header for this variable + var_header.size + 1 + // data for this variable + sizeof(ee_header))) // header for sentinel + break; + + // look for a variable with this key + vp = _lookup_by_key(var_header.key); + if (vp) { + // adjust the variable's key to point to this entry + vp->_key = _tail_sentinel; + } + + // move to the next variable header + _tail_sentinel += sizeof(var_header) + var_header.size + 1; + } + _EEPROM_scanned = true; + return true; +} + +// Locate a variable in EEPROM, allocating space if required. +// +bool AP_Var::_EEPROM_locate(bool allocate) +{ + Var_header var_header; + size_t size; + + // Has the variable already been located? + if (!(_key & k_not_located)) { + return true; // it has + } + + // Does it have a no-key key? + if (_key == k_no_key) { + return false; // it does, and thus it has no location + } + + // If the EEPROM has not been scanned, try that now + if (!_EEPROM_scanned) { + _EEPROM_scan(); + } + + // If not located and not permitted to allocate, we have failed + if ((_key & k_not_located) && !allocate) { + return false; + } + + // Ask the serializer for the size of the thing we are allocating, and fail + // if it is too large or if it has no size + size = serialize(NULL, 0); + if ((0 == size) || (size > k_max_size)) + return false; + + // Make sure there will be space in the EEPROM for the variable, its + // header and the new tail sentinel + if ((_tail_sentinel + size + sizeof(Var_header) * 2) > k_EEPROM_size) + return false; + + // If there is no data in the EEPROM, write the header and move the + // sentinel + if (0 == _tail_sentinel) { + EEPROM_header ee_header; + + ee_header.magic = k_EEPROM_magic; + ee_header.revision = k_EEPROM_revision; + ee_header.spare = 0; + + eeprom_write_block(0, &ee_header, sizeof(ee_header)); + + _tail_sentinel = sizeof(ee_header); + } + + // Write a new sentinel first + var_header.key = k_tail_sentinel; + var_header.size = 0; + eeprom_write_block(&var_header, (void *)(_tail_sentinel + sizeof(Var_header) + size), sizeof(var_header)); + + // Write the header for the block we have just located + var_header.key = _key & k_key_mask; + var_header.size = size - 1; + eeprom_write_block(&var_header, (void *)_tail_sentinel, sizeof(Var_header)); + + // Save the located address for the variable + _key = _tail_sentinel + sizeof(Var_header); + + // Update to the new tail sentinel + _tail_sentinel += sizeof(Var_header) + size; + + // We have successfully allocated space and thus located the variable + return true; +} + diff --git a/libraries/AP_Common/AP_Var.h b/libraries/AP_Common/AP_Var.h index b6dbefa510..acb058ad97 100644 --- a/libraries/AP_Common/AP_Var.h +++ b/libraries/AP_Common/AP_Var.h @@ -27,7 +27,7 @@ #include "AP_MetaClass.h" -class AP_Var_scope; +class AP_Var_group; /// Base class for variables. /// @@ -36,50 +36,111 @@ class AP_Var_scope; class AP_Var : public AP_Meta_class { public: - /// Storage address for variables that can be saved to EEPROM + /// EEPROM header /// - /// If the variable is contained within a scope, then the address - /// is relative to the scope. + /// This structure is placed at the head of the EEPROM to indicate + /// that the ROM is formatted for AP_Var. /// - /// @todo This might be used as a token for mass serialisation, - /// but for now it's just the address of the variable's backing - /// store in EEPROM. + /// The EEPROM may thus be cheaply erased by overwriting just one + /// byte of the header magic. /// - typedef uint16_t Address; + struct EEPROM_header { + uint16_t magic; + uint8_t revision; + uint8_t spare; + }; - /// An address value that indicates that a variable is not to be saved to EEPROM. + static const uint16_t k_EEPROM_magic = 0x5041; ///< "AP" + static const uint16_t k_EEPROM_revision = 1; ///< current format revision + + /// Storage key for variables that are saved in EEPROM. + /// + /// The key is used to associate a known variable in memory + /// with storage for the variable in the EEPROM. When the + /// variable's storage is located in EEPROM, the key value is + /// replaced with an EEPROM address. + /// + /// When the variable is saved to EEPROM, a header is attached + /// which contains the key along with other details. + /// + typedef uint16_t Key; + + /// The header prepended to a variable stored in EEPROM + /// + struct Var_header { + /// The size of the variable, minus one. + /// This allows a variable to be anything from one to 32 bytes long. + /// + uint16_t size:5; + + /// The key assigned to the variable. + /// + uint16_t key:8; + + /// Spare bits, currently unused + /// + /// @todo One could be a parity bit? + /// + uint16_t spare:3; + }; + + /// An key that indicates that a variable is not to be saved to EEPROM. /// /// As the value has all bits set to 1, it's not a legal EEPROM address for any - /// EEPROM smaller than 64K. + /// EEPROM smaller than 64K (and it's too big to fit the Var_header::key field). /// /// This value is normally the default. /// - static const Address k_no_address = ~(Address)0; + static const Key k_no_key = 0xffff; + + /// A key that has this bit set is still a key; if it has been cleared then + /// the key's value is actually an address in EEPROM. + /// + static const Key k_not_located = (Key)1 << 15; + + /// Key assigned to the terminal entry in EEPROM. + /// + static const Key k_tail_sentinel = 0xff; + + /// A bitmask that removes any control bits from a key giving just the + /// value. + /// + static const Key k_key_mask = ~(k_not_located); /// The largest variable that will be saved to EEPROM. /// This affects the amount of stack space that is required by the ::save, ::load, - /// ::save_all and ::load_all functions. + /// ::save_all and ::load_all functions. It should match the maximum size that can + /// be encoded in the Var_header::size field. /// - static const size_t k_max_size = 16; + static const size_t k_max_size = 32; /// Optional flags affecting the behavior and usage of the variable. /// enum Flags { k_no_flags = 0, - k_no_auto_load = (1 << 0), ///< will not be loaded by ::load_all or saved by ::save_all - k_no_import = (1 << 1) ///< new values must not be imported from e.g. a GCS + + /// The variable will not be loaded by ::load_all or saved by ::save_all, but it + /// has a key and can be loaded/saved manually. + k_no_auto_load = (1 << 0), + + /// This flag is advisory; it indicates that the variable's value should not be + /// imported from outside, e.g. from a GCS. + k_no_import = (1 << 1) }; + AP_Var() {} + /// Constructor /// + /// @param key The storage key to be associated with this variable. /// @param name An optional name by which the variable may be known. /// This name may be looked up via the ::lookup function. - /// @param scope An optional scope that the variable may be a contained within. + /// @param group An optional group to which the variable may belong. /// The scope's name will be prepended to the variable name /// by ::copy_name. + /// @param flags Optional flags which control how the variable behaves. /// - AP_Var(Address address = k_no_address, const prog_char *name = NULL, const AP_Var_scope *scope = NULL, - Flags flags = k_no_flags); + AP_Var(Key key, const prog_char *name, AP_Var_group *group, Flags flags); /// Destructor /// @@ -91,10 +152,6 @@ public: /// ~AP_Var(void); - /// Set the variable to its default value - /// - virtual void set_default(void); - /// Copy the variable's name, prefixed by any parent class names, to a buffer. /// /// If the variable has no name, the buffer will contain an empty string. @@ -109,122 +166,184 @@ public: /// Return a pointer to the n'th known variable. /// - /// This function is used to iterate the set of variables that are considered - /// interesting; i.e. those that may be saved to EEPROM, or that have a name. + /// This function is used to iterate the set of variables, and is optimised + /// for the case where the index argument is increased sequentially from one + /// call to the next. /// - /// Note that variable index numbers are not constant, they depend on the - /// the static construction order. + /// Note that variable index numbers are not constant; they will vary + /// from build to build. /// - /// @param index enumerator for the variable to be returned + /// @param index Enumerator for the variable to be returned + /// @return Pointer to the index'th variable, or NULL if + /// the set of variables has been exhausted. /// - static AP_Var *lookup(int index); + static AP_Var *lookup_by_index(int index); /// Save the current value of the variable to EEPROM. /// /// This interface works for any subclass that implements - /// serialize. + /// AP_Meta_class::serialize. /// - void save(void) const; + /// Note that invoking this method on a variable that is a group + /// member will cause the entire group to be saved. + /// + /// @return True if the variable was saved successfully. + /// + bool save(void); /// Load the variable from EEPROM. /// /// This interface works for any subclass that implements - /// unserialize. + /// AP_Meta_class::unserialize. /// - void load(void); + /// If the variable has not previously been saved to EEPROM, this + /// routine will return failure. + /// + /// Note that invoking this method on a variable that is a group + /// member will cause the entire group to be loaded. + /// + /// @return True if the variable was loaded successfully. + /// + bool load(void); /// Save all variables to EEPROM /// - static void save_all(void); + /// This routine performs a best-efforts attempt to save all + /// of the variables to EEPROM. If some fail to save, the others + /// that succeed will still all be saved. + /// + /// @return False if any variable failed to save. + /// + static bool save_all(void); /// Load all variables from EEPROM /// - static void load_all(void); + /// This function performs a best-efforts attempt to load all + /// of the variables from EEPROM. If some fail to load, their + /// values will remain as they are. + /// + /// @return False if any variable failed to load. Note + /// That this may be caused by a variable not having + /// previously been saved. + /// + static bool load_all(void); /// Test for flags that may be set. /// /// @param flagval Flag or flags to be tested - /// @returns True if all of the bits in flagval are set in the flags. + /// @return True if all of the bits in flagval are set in the flags. /// bool has_flags(Flags flagval) const { return (_flags & flagval) == flagval; } private: - const Address _address; - const prog_char *_name; - const AP_Var_scope *const _scope; - AP_Var *_link; - uint8_t _flags; - - /// Do the arithmetic required to compute the variable's address in EEPROM - /// - /// @returns The address at which the variable is stored in EEPROM, - /// or k_no_address if it is not saved. - /// - Address _get_address(void) const; + AP_Var_group *_group; ///< Group that the variable may be a member of + Key _key; ///< storage key + const prog_char *_name; ///< name known to external agents (GCS, etc.) + AP_Var *_link; ///< linked list pointer to next variable + uint8_t _flags; ///< flag bits // static state used by ::lookup - static AP_Var *_variables; - static AP_Var *_lookup_hint; /// pointer to the last variable that was looked up by ::lookup - static int _lookup_hint_index; /// index of the last variable that was looked up by ::lookup + static AP_Var *_variables; ///< linked list of all variables + static AP_Var *_lookup_hint; ///< pointer to the last variable that was looked up by ::lookup + static int _lookup_hint_index; ///< index of the last variable that was looked up by ::lookup + + + // EEPROM space allocation and scanning + static uint16_t _tail_sentinel; ///< EEPROM address of the tail sentinel + static bool _EEPROM_scanned; ///< true once the EEPROM has been scanned and addresses assigned + + static const uint16_t k_EEPROM_size = 4096; ///< XXX avr-libc doesn't consistently export this + + + /// Return a pointer to the variable with a given key. + /// + /// This function is used to search the set of variables for + /// one with a given key. + /// + /// Note that this search will fail (XXX should it?) if the variable + /// assigned this key has been located in EEPROM. + /// + /// @param key The key to search for. + /// @return Pointer to the variable assigned the key, + /// or NULL if no variable owns up to it. + /// + static AP_Var *_lookup_by_key(Key key); + + /// Scan the EEPROM and assign addresses to any known variables + /// that have entries there. + /// + /// @return True if the EEPROM was scanned successfully. + /// + static bool _EEPROM_scan(void); + + /// Locate this variable in the EEPROM. + /// + /// @param allocate If true, and the variable does not already have + /// space allocated in EEPROM, allocate it. + /// @return True if the _key field is a valid EEPROM address with + /// space reserved for the variable to be saved. False + /// if the variable does not have a key, or space could not + /// be allocated and the variable does not already exist in + /// EEPROM. + /// + bool _EEPROM_locate(bool allocate); + }; -/// Nestable scopes for variable names. +/// Variable groups. /// -/// This provides a mechanism for scoping variable names and their -/// EEPROM addresses. +/// Grouped variables are treated as a single variable when loaded from +/// or saved to EEPROM. The size limits for variables apply to the entire +/// group; thus a group cannot be larger than the largest legal variable. /// -/// When AP_Var is asked for the name of a variable, it will -/// prepend the names of all enclosing scopes. This provides a way -/// of grouping variables and saving memory when many share a large -/// common prefix. +/// When AP_Var is asked for the name of a variable that is a member +/// of a group, it will prepend the name of the group; this helps save +/// memory. /// -/// When AP_var computes the address of a variable, it will take -/// into account the address offsets of each of the variable's -/// enclosing scopes. +/// Variables belonging to a group are always sorted into the global +/// variable list after the group. /// -class AP_Var_scope +class AP_Var_group : public AP_Var { public: + AP_Var_group(); + virtual ~AP_Var_group(); + /// Constructor /// - /// @param name The name of the scope. - /// @param address An EEPROM address offset to be added to the address assigned to - /// any variables within the scope. - /// @param parent Optional parent scope to nest within. + /// @param key Storage key for the group. + /// @param name An optional name prefix for members of the group. /// - AP_Var_scope(const prog_char *name, AP_Var::Address address = 0, AP_Var_scope *parent = NULL) : - _name(name), _parent(parent), _address(address) { + AP_Var_group(Key key, const prog_char *name = NULL) : + AP_Var(key, name, NULL, k_no_flags) + { } - /// Copy the scope name, prefixed by any parent scope names, to a buffer. + /// Serialize the group. /// - /// Note that if the combination of names is larger than the buffer, the - /// result in the buffer will be truncated. + /// Iteratively serializes the entire group into the supplied buffer. /// - /// @param buffer The destination buffer - /// @param bufferSize Total size of the destination buffer. + /// @param buf Buffer into which serialized data should be placed. + /// @param bufSize The size of the buffer provided. + /// @return The size of the serialized data, even if that data would + /// have overflowed the buffer. /// - void copy_name(char *buffer, size_t bufferSize) const { - if (_parent) { - _parent->copy_name(buffer, bufferSize); - } - strlcat_P(buffer, _name, bufferSize); - } + virtual size_t serialize(void *buf, size_t bufSize) const; - /// Compute the address offset that this and any parent scope might apply - /// to variables inside the scope. + /// Unserialize the group. /// - /// This provides a way for variables to be grouped into collections whose - /// EEPROM addresses can be more easily managed. + /// Iteratively unserializes the group from the supplied buffer. /// - AP_Var::Address get_address(void) const; + /// @param buf Buffer containing serialized data. + /// @param bufSize The size of the buffer. + /// @return The number of bytes from the buffer that would be consumed + /// unserializing the data. If the value is less than or equal + /// to bufSize, unserialization was successful. + /// + virtual size_t unserialize(void *buf, size_t bufSize); -private: - const prog_char *_name; /// pointer to the scope name in program memory - AP_Var_scope *_parent; /// pointer to a parent scope, if one exists - AP_Var::Address _address; /// container base address, offsets contents }; /// Template class for scalar variables. @@ -238,23 +357,45 @@ template class AP_VarT : public AP_Var { public: - /// Constructor + /// Constructor for non-grouped variable. /// - /// @note Constructors for AP_Var are specialised so that they can - /// pass the correct typeCode argument to the AP_Var ctor. + /// Initialises a stand-alone variable with optional initial value, storage key, name and flags. /// - /// @param initialValue Value the variable should have at startup. - /// @param identity A unique token used when saving the variable to EEPROM. - /// Note that this token must be unique, and should only be - /// changed for a variable if the EEPROM version is updated - /// to prevent the accidental unserialisation of old data - /// into the wrong variable. - /// @param name An optional name by which the variable may be known. - /// @param varClass An optional class that the variable may be a member of. + /// @param default_value Value the variable should have at startup. + /// @param key Storage key for the variable. If not set, or set to AP_Var::k_no_key + /// the variable cannot be loaded from or saved to EEPROM. + /// @param name An optional name by which the variable may be known. + /// @param flags Optional flags that may affect the behaviour of the variable. /// - AP_VarT (T default_value = 0, Address address = k_no_address, const prog_char *name = NULL, - AP_Var_scope *scope = NULL, Flags flags = k_no_flags) : - AP_Var(address, name, scope, flags), _value(default_value), _default_value(default_value) { + AP_VarT (T initial_value = 0, + Key key = k_no_key, + const prog_char *name = NULL, + Flags flags = k_no_flags) : + AP_Var(key, name, NULL, flags), + _value(initial_value) + { + } + + /// Constructor for grouped variable. + /// + /// Initialises a variable that is a member of a group with optional initial value, name and flags. + /// + /// @param group The group that this variable belongs to. + /// @param index The variable's index within the group. Index values must be unique + /// within the group, as they ensure that the group's layout in EEPROM + /// is consistent. + /// @param initial_value The value the variable takes at startup. + /// @param name An optional name by which the variable may be known. + /// @param flags Optional flags that may affect the behaviour of the variable. + /// + AP_VarT (AP_Var_group *group, + Key index, + T initial_value = 0, + const prog_char *name = NULL, + Flags flags = k_no_flags) : + AP_Var(index, name, group, flags), + _value(initial_value) + { } // serialize _value into the buffer, but only if it is big enough. @@ -275,11 +416,6 @@ public: return sizeof(T); } - /// Restore the variable to its default value - virtual void set_default(void) { - _value = _default_value; - } - /// Value getter /// T get(void) const { @@ -306,7 +442,8 @@ public: return v; } - /// Copy assignment from T + /// Copy assignment from T is equivalent to ::set. + /// AP_VarT& operator=(T v) { _value = v; return *this; @@ -314,8 +451,8 @@ public: protected: T _value; - T _default_value; }; + /// Convenience macro for defining instances of the AP_Var template /// #define AP_VARDEF(_t, _n) typedef AP_VarT<_t> AP_##_n; @@ -336,21 +473,32 @@ AP_VARDEF(int32_t, Int32); // defines AP_Int32 /// to achieve this. /// /// Note that any protocol transporting serialized data should be aware that the -/// encoding used is effectively Q5.10 (one sign bit, 5 integer bits, 10 fractional bits). +/// encoding used is effectively Q5.10 (one sign bit, 5 integer bits, 10 fractional bits), +/// giving an effective range of approximately +/-31.999 /// class AP_Float16 : public AP_Float { public: - /// Constructor mimics AP_Float::AP_Float() + /// Constructors mimic AP_Float::AP_Float() /// - AP_Float16(float default_value = 0, - Address address = k_no_address, + AP_Float16(float initial_value = 0, + Key key = k_no_key, const prog_char *name = NULL, - AP_Var_scope *scope = NULL, Flags flags = k_no_flags) : - AP_Float(default_value, address, name, scope, flags) { + AP_Float(initial_value, key, name, flags) + { } + AP_Float16(AP_Var_group *group, + Key index, + float initial_value = 0, + const prog_char *name = NULL, + Flags flags = k_no_flags) : + AP_Float(group, index, initial_value, name, flags) + { + } + + // Serialize _value as Q5.10. // virtual size_t serialize(void *buf, size_t size) const { diff --git a/libraries/AP_Common/examples/AP_Var/AP_Var.pde b/libraries/AP_Common/examples/AP_Var/AP_Var.pde index 78008386d6..076c3a63b8 100644 --- a/libraries/AP_Common/examples/AP_Var/AP_Var.pde +++ b/libraries/AP_Common/examples/AP_Var/AP_Var.pde @@ -112,6 +112,15 @@ setup(void) REQUIRE(AP_Float_negative_unity = -1.0); } + // AP_Var: initial value + { + TEST(var_initial_value); + + AP_Float f(12.345); + + REQUIRE(f == 12.345); + } + // AP_Var: set, get, assignment { TEST(var_set_get); @@ -126,19 +135,6 @@ setup(void) REQUIRE(f.get() == 10.0); } - // AP_Var: default value - { - TEST(var_default_value); - - AP_Float f(10.0); - - REQUIRE(f == 10.0); - f = 0; - REQUIRE(f == 0); - f.set_default(); - REQUIRE(f == 10.0); - } - // AP_Var: cast to type { TEST(var_cast_to_type); @@ -169,7 +165,7 @@ setup(void) { TEST(var_naming); - AP_Float f(0, AP_Var::k_no_address, PSTR("test")); + AP_Float f(0, AP_Var::k_no_key, PSTR("test")); char name_buffer[16]; f.copy_name(name_buffer, sizeof(name_buffer)); @@ -177,6 +173,7 @@ setup(void) } // AP_Var: serialize + // note that this presumes serialisation to the native in-memory format { TEST(var_serialize); @@ -202,12 +199,58 @@ setup(void) REQUIRE(f == 10); } + // AP_Var: enumeration + // note that this test presumes the singly-linked list implementation of the list + { + TEST(var_enumeration); + + AP_Var *v = AP_Var::lookup_by_index(0); + + // test basic enumeration + AP_Float f1(0, AP_Var::k_no_key, PSTR("test1")); + REQUIRE(AP_Var::lookup_by_index(0) == &f1); + REQUIRE(AP_Var::lookup_by_index(1) == v); + + // test that new entries arrive in order + { + AP_Float f2(0, AP_Var::k_no_key, PSTR("test2")); + REQUIRE(AP_Var::lookup_by_index(0) == &f2); + REQUIRE(AP_Var::lookup_by_index(1) == &f1); + REQUIRE(AP_Var::lookup_by_index(2) == v); + + { + AP_Float f3(0, AP_Var::k_no_key, PSTR("test3")); + REQUIRE(AP_Var::lookup_by_index(0) == &f3); + REQUIRE(AP_Var::lookup_by_index(1) == &f2); + REQUIRE(AP_Var::lookup_by_index(2) == &f1); + REQUIRE(AP_Var::lookup_by_index(3) == v); + } + } + + // test that destruction removes from the list + REQUIRE(AP_Var::lookup_by_index(0) == &f1); + REQUIRE(AP_Var::lookup_by_index(1) == v); + } + + // AP_Var: group names + { + TEST(group_names); + + AP_Var_group group(AP_Var::k_no_key, PSTR("group_")); + AP_Float f(&group, 1, 1.0, PSTR("test")); + char name_buffer[16]; + + f.copy_name(name_buffer, sizeof(name_buffer)); + REQUIRE(!strcmp(name_buffer, "group_test")); + } + +#if SAVE // AP_Var: load and save { TEST(var_load_save); - AP_Float f1(10, 4); - AP_Float f2(0, 4); + AP_Float f1(10, 4); + AP_Float f2(0, 4); f2.save(); f2 = 1.0; @@ -219,58 +262,13 @@ setup(void) REQUIRE(f2 == 10); } - // AP_Var: enumeration - // note that this test presumes the singly-linked list implementation of the list + // AP_Var: group load/save { - TEST(var_enumeration); + TEST(var_group_loadsave); - AP_Var *v = AP_Var::lookup(0); - - // test basic enumeration - AP_Float f1(0, AP_Var::k_no_address, PSTR("test1")); - REQUIRE(AP_Var::lookup(0) == &f1); - REQUIRE(AP_Var::lookup(1) == v); - - // test that new entries arrive in order - { - AP_Float f2(0, AP_Var::k_no_address, PSTR("test2")); - REQUIRE(AP_Var::lookup(0) == &f2); - REQUIRE(AP_Var::lookup(1) == &f1); - REQUIRE(AP_Var::lookup(2) == v); - - { - AP_Float f3(0, AP_Var::k_no_address, PSTR("test3")); - REQUIRE(AP_Var::lookup(0) == &f3); - REQUIRE(AP_Var::lookup(1) == &f2); - REQUIRE(AP_Var::lookup(2) == &f1); - REQUIRE(AP_Var::lookup(3) == v); - } - } - - // test that destruction removes from the list - REQUIRE(AP_Var::lookup(0) == &f1); - REQUIRE(AP_Var::lookup(1) == v); - } - - // AP_Var: scope names - { - TEST(var_scope_names); - - AP_Var_scope scope(PSTR("scope_")); - AP_Float f(1.0, AP_Var::k_no_address, PSTR("test"), &scope); - char name_buffer[16]; - - f.copy_name(name_buffer, sizeof(name_buffer)); - REQUIRE(!strcmp(name_buffer, "scope_test")); - } - - // AP_Var: scope address offsets - { - TEST(var_scope_addressing); - - AP_Var_scope scope(PSTR("scope"), 4); + AP_Var_group group(PSTR("group_"), 4); AP_Float f1(10.0, 8); - AP_Float f2(1.0, 4, PSTR("var"), &scope); + AP_Float f2(1.0, 4, PSTR("var"), &group); f1.save(); f1.load(); @@ -283,6 +281,7 @@ setup(void) f1.load(); REQUIRE(f1 == 1); } +#endif // AP_Var: derived types {