mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 18:38:28 -04:00
Move the AP_Var ctor out of the class definition; it's big enough that we don't want to inline it.
AP_Vars with either a name or an address are 'interesting' (the latter so we can save_all). Add the concept of address offsets to scopes. Now we have a container that we can put AP_Vars into that can be moved around in the EEPROM. This will make it easier for things like the PID library which need to support multiple instances getting their parameters from different parts of the ROM. Improve documentation. Suck it up and admit that we aren't going to do "identity"-based addressing for the EEPROM and just call the property "address". git-svn-id: https://arducopter.googlecode.com/svn/trunk@1417 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
158b3c91d3
commit
4545c3df80
@ -15,6 +15,22 @@ const AP_Float AP_GainUnity(1.0);
|
||||
const AP_Float AP_GainNegativeUnity(-1.0);
|
||||
const AP_Float AP_Zero(0);
|
||||
|
||||
// Constructor
|
||||
//
|
||||
AP_Var::AP_Var(AP_VarAddress address,
|
||||
const prog_char *name,
|
||||
const AP_VarScope *scope) :
|
||||
_address(address),
|
||||
_name(name),
|
||||
_scope(scope)
|
||||
{
|
||||
// if the variable is interesting, link it into the list
|
||||
if (_name || (_address != AP_VarNoAddress)) {
|
||||
_link = _variables;
|
||||
_variables = this;
|
||||
}
|
||||
}
|
||||
|
||||
// Destructor
|
||||
//
|
||||
// Removes named variables from the list.
|
||||
@ -23,8 +39,8 @@ AP_Var::~AP_Var(void)
|
||||
{
|
||||
AP_Var *p;
|
||||
|
||||
// only do this for variables that have names
|
||||
if (_name) {
|
||||
// only do this for variables that were considered interesting
|
||||
if (_name || (_address != AP_VarNoAddress)) {
|
||||
// if we are at the head of the list - for variables
|
||||
// recently constructed this is usually the case
|
||||
if (_variables == this) {
|
||||
@ -47,6 +63,88 @@ AP_Var::~AP_Var(void)
|
||||
}
|
||||
}
|
||||
|
||||
// Copy the variable name to the provided buffer.
|
||||
//
|
||||
void
|
||||
AP_Var::copy_name(char *buffer, size_t bufferSize) const
|
||||
{
|
||||
buffer[0] = '\0';
|
||||
if (_scope)
|
||||
_scope->copy_name(buffer, bufferSize);
|
||||
strlcat_P(buffer, _name, bufferSize);
|
||||
}
|
||||
|
||||
// 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::AP_VarAddress
|
||||
AP_VarScope::get_address(void) const
|
||||
{
|
||||
AP_Var::AP_VarAddress addr = _address;
|
||||
|
||||
if (_parent)
|
||||
addr += _parent->get_address();
|
||||
|
||||
return addr;
|
||||
}
|
||||
|
||||
// Compute the address in EEPROM where the variable is stored
|
||||
//
|
||||
AP_Var::AP_VarAddress
|
||||
AP_Var::_get_address(void) const
|
||||
{
|
||||
AP_VarAddress addr = _address;
|
||||
|
||||
// if we have an address at all
|
||||
if ((addr != AP_VarNoAddress) && _scope)
|
||||
addr += _scope->get_address();
|
||||
|
||||
return addr;
|
||||
}
|
||||
|
||||
// Save the variable to EEPROM, if supported
|
||||
//
|
||||
void
|
||||
AP_Var::save(void) const
|
||||
{
|
||||
AP_VarAddress addr = _get_address();
|
||||
|
||||
if (addr != AP_VarNoAddress) {
|
||||
uint8_t vbuf[AP_VarMaxSize];
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
// Load the variable from EEPROM, if supported
|
||||
//
|
||||
void
|
||||
AP_Var::load(void)
|
||||
{
|
||||
AP_VarAddress addr = _get_address();
|
||||
|
||||
if (addr != AP_VarNoAddress) {
|
||||
uint8_t vbuf[AP_VarMaxSize];
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// Lookup interface for variables.
|
||||
|
@ -20,27 +20,152 @@
|
||||
|
||||
#include "AP_MetaClass.h"
|
||||
|
||||
class AP_VarScope;
|
||||
|
||||
/// Base class for variables.
|
||||
///
|
||||
/// Provides naming and lookup services for variables.
|
||||
///
|
||||
class AP_Var : public AP_MetaClass
|
||||
{
|
||||
public:
|
||||
/// Storage address for variables that can be saved to EEPROM
|
||||
///
|
||||
/// If the variable is contained within a scope, then the address
|
||||
/// is relative to the scope.
|
||||
///
|
||||
/// @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.
|
||||
///
|
||||
typedef uint16_t AP_VarAddress;
|
||||
|
||||
/// An address value that indicates that a variable is not to be saved to EEPROM.
|
||||
/// This value is normally the default.
|
||||
///
|
||||
static const AP_VarAddress AP_VarNoAddress = !(AP_VarAddress)0;
|
||||
|
||||
/// 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.
|
||||
///
|
||||
static const size_t AP_VarMaxSize = 16;
|
||||
|
||||
/// Constructor
|
||||
///
|
||||
/// @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.
|
||||
/// The scope's name will be prepended to the variable name
|
||||
/// by ::copy_name.
|
||||
///
|
||||
AP_Var(AP_VarAddress address = AP_VarNoAddress,
|
||||
const prog_char *name = NULL,
|
||||
const AP_VarScope *scope = NULL);
|
||||
|
||||
/// Destructor
|
||||
///
|
||||
/// Note that the linked-list removal can be inefficient when named variables
|
||||
/// are destroyed in an order other than the reverse of the order in which
|
||||
/// they are created.
|
||||
///
|
||||
~AP_Var(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.
|
||||
///
|
||||
/// Note that if the combination of names is larger than the buffer, the
|
||||
/// result in the buffer will be truncated.
|
||||
///
|
||||
/// @param buffer The destination buffer
|
||||
/// @param bufferSize Total size of the destination buffer.
|
||||
///
|
||||
void copy_name(char *buffer, size_t bufferSize) const;
|
||||
|
||||
/// 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.
|
||||
///
|
||||
/// Note that variable index numbers are not constant, they depend on the
|
||||
/// the static construction order.
|
||||
///
|
||||
/// @param index enumerator for the variable to be returned
|
||||
///
|
||||
static AP_Var *lookup(int index);
|
||||
|
||||
/// Save the current value of the variable to EEPROM.
|
||||
///
|
||||
/// This interface works for any subclass that implements
|
||||
/// serialize.
|
||||
///
|
||||
void save(void) const;
|
||||
|
||||
/// Load the variable from EEPROM.
|
||||
///
|
||||
/// This interface works for any subclass that implements
|
||||
/// unserialize.
|
||||
///
|
||||
void load(void);
|
||||
|
||||
/// Save all variables to EEPROM
|
||||
///
|
||||
static void save_all(void);
|
||||
|
||||
/// Load all variables from EEPROM
|
||||
///
|
||||
static void load_all(void);
|
||||
|
||||
private:
|
||||
const AP_VarAddress _address;
|
||||
const prog_char *_name;
|
||||
const AP_VarScope * const _scope;
|
||||
AP_Var *_link;
|
||||
|
||||
/// Do the arithmetic required to compute the variable's address in EEPROM
|
||||
///
|
||||
/// @returns The address at which the variable is stored in EEPROM,
|
||||
/// or AP_VarNoAddress if it is not saved.
|
||||
///
|
||||
AP_VarAddress _get_address(void) const;
|
||||
|
||||
// static state used by ::lookup
|
||||
static AP_Var *_variables;
|
||||
static AP_Var *_lookupHint; /// pointer to the last variable that was looked up by ::lookup
|
||||
static int _lookupHintIndex; /// index of the last variable that was looked up by ::lookup
|
||||
};
|
||||
|
||||
/// Nestable scopes for variable names.
|
||||
///
|
||||
/// This provides a mechanism for scoping variable names, and
|
||||
/// may later be extended for other purposes.
|
||||
/// This provides a mechanism for scoping variable names and their
|
||||
/// EEPROM addresses.
|
||||
///
|
||||
/// 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 computes the address of a variable, it will take
|
||||
/// into account the address offsets of each of the variable's
|
||||
/// enclosing scopes.
|
||||
///
|
||||
class AP_VarScope
|
||||
{
|
||||
public:
|
||||
/// Constructor
|
||||
///
|
||||
/// @param scopeName The name of the scope.
|
||||
/// @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.
|
||||
///
|
||||
AP_VarScope(const prog_char *name,
|
||||
AP_Var::AP_VarAddress address = 0,
|
||||
AP_VarScope *parent = NULL) :
|
||||
_name(name),
|
||||
_parent(parent)
|
||||
_parent(parent),
|
||||
_address(address)
|
||||
{
|
||||
}
|
||||
|
||||
@ -59,143 +184,18 @@ public:
|
||||
strlcat_P(buffer, _name, bufferSize);
|
||||
}
|
||||
|
||||
/// Compute the address offset that this and any parent scope might apply
|
||||
/// to variables inside the scope.
|
||||
///
|
||||
/// This provides a way for variables to be grouped into collections whose
|
||||
/// EEPROM addresses can be more easily managed.
|
||||
///
|
||||
AP_Var::AP_VarAddress get_address(void) const;
|
||||
|
||||
private:
|
||||
const prog_char *_name; /// pointer to the scope name in program memory
|
||||
AP_VarScope *_parent; /// pointer to a parent scope, if one exists
|
||||
};
|
||||
|
||||
/// Base class for variables.
|
||||
///
|
||||
/// Provides naming and lookup services for variables.
|
||||
///
|
||||
class AP_Var : public AP_MetaClass
|
||||
{
|
||||
public:
|
||||
/// A unique identity for variables that can be saved to EEPROM
|
||||
///
|
||||
/// @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.
|
||||
///
|
||||
typedef uint16_t AP_VarIdentity;
|
||||
static const AP_VarIdentity AP_VarUnsaved = ~(AP_VarIdentity)0;
|
||||
|
||||
/// The largest variable that will be saved to EEPROM
|
||||
///
|
||||
static const size_t AP_VarMaxSize = 16;
|
||||
|
||||
/// Constructor
|
||||
///
|
||||
/// @param name An optional name by which the variable may be known.
|
||||
/// @param scope An optional scope that the variable may be a contained within.
|
||||
///
|
||||
AP_Var(AP_VarIdentity identity = AP_VarUnsaved,
|
||||
const prog_char *name = NULL,
|
||||
AP_VarScope *scope = NULL) :
|
||||
_identity(identity),
|
||||
_name(name),
|
||||
_scope(scope)
|
||||
{
|
||||
if (name) {
|
||||
_link = _variables;
|
||||
_variables = this;
|
||||
}
|
||||
}
|
||||
|
||||
/// Destructor
|
||||
///
|
||||
/// This is largely a safety net, as the linked-list removal can be inefficient
|
||||
/// when variables are destroyed in an order other than the reverse of the order
|
||||
/// in which they are created.
|
||||
///
|
||||
~AP_Var(void);
|
||||
|
||||
/// Copy the variable name, prefixed by any parent class names, to a buffer.
|
||||
///
|
||||
/// Note that if the combination of names is larger than the buffer, the
|
||||
/// result in the buffer will be truncated.
|
||||
///
|
||||
/// @param buffer The destination buffer
|
||||
/// @param bufferSize Total size of the destination buffer.
|
||||
///
|
||||
void copy_name(char *buffer, size_t bufferSize) const {
|
||||
buffer[0] = '\0';
|
||||
if (_scope)
|
||||
_scope->copy_name(buffer, bufferSize);
|
||||
strlcat_P(buffer, _name, bufferSize);
|
||||
}
|
||||
|
||||
/// Return a pointer to the n'th known variable
|
||||
///
|
||||
/// This interface is designed for the use of relatively low-rate clients;
|
||||
/// GCS interfaces in particular. It may implement an acceleration scheme
|
||||
/// for optimising the lookup of parameters.
|
||||
///
|
||||
/// Note that variable index numbers are not constant, they depend on the
|
||||
/// the static construction order.
|
||||
///
|
||||
/// @param index enumerator for the variable to be returned
|
||||
///
|
||||
static AP_Var *lookup(int index);
|
||||
|
||||
/// Save the current value of the variable to EEPROM.
|
||||
///
|
||||
/// This interface works for any subclass that implements
|
||||
/// serialize.
|
||||
///
|
||||
void save(void) {
|
||||
if (_identity != AP_VarUnsaved) {
|
||||
uint8_t vbuf[AP_VarMaxSize];
|
||||
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 *)_identity, size);
|
||||
}
|
||||
}
|
||||
|
||||
/// Load the variable from EEPROM.
|
||||
///
|
||||
/// This interface works for any subclass that implements
|
||||
/// unserialize.
|
||||
///
|
||||
void load(void) {
|
||||
if (_identity != AP_VarUnsaved) {
|
||||
uint8_t vbuf[AP_VarMaxSize];
|
||||
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 *)_identity, size);
|
||||
unserialize(vbuf, size);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Save all variables to EEPROM
|
||||
///
|
||||
static void save_all(void);
|
||||
|
||||
/// Load all variables from EEPROM
|
||||
///
|
||||
static void load_all(void);
|
||||
|
||||
private:
|
||||
const AP_VarIdentity _identity;
|
||||
const prog_char *_name;
|
||||
AP_VarScope *_scope;
|
||||
AP_Var *_link;
|
||||
|
||||
// static state used by ::lookup
|
||||
static AP_Var *_variables;
|
||||
static AP_Var *_lookupHint; /// pointer to the last variable that was looked up by ::lookup
|
||||
static int _lookupHintIndex; /// index of the last variable that was looked up by ::lookup
|
||||
AP_Var::AP_VarAddress _address; /// container base address, offsets contents
|
||||
};
|
||||
|
||||
|
||||
@ -225,17 +225,17 @@ public:
|
||||
/// @param varClass An optional class that the variable may be a member of.
|
||||
///
|
||||
AP_VarT<T>(T initialValue = 0,
|
||||
AP_VarIdentity identity = AP_VarUnsaved,
|
||||
AP_VarAddress address = AP_VarNoAddress,
|
||||
const prog_char *name = NULL,
|
||||
AP_VarScope *scope = NULL) :
|
||||
AP_Var(identity, name, scope),
|
||||
AP_Var(address, name, scope),
|
||||
_value(initialValue)
|
||||
{
|
||||
}
|
||||
|
||||
// serialize _value into the buffer, but only if it is big enough.
|
||||
///
|
||||
virtual size_t serialize(void *buf, size_t size) {
|
||||
virtual size_t serialize(void *buf, size_t size) const {
|
||||
if (size >= sizeof(T))
|
||||
*(T *)buf = _value;
|
||||
return sizeof(T);
|
||||
@ -251,7 +251,7 @@ public:
|
||||
|
||||
/// Value getter
|
||||
///
|
||||
T get(void) { return _value; }
|
||||
T get(void) const { return _value; }
|
||||
|
||||
/// Value setter
|
||||
///
|
||||
|
Loading…
Reference in New Issue
Block a user