diff --git a/libraries/AP_Common/AP_Param.cpp b/libraries/AP_Common/AP_Param.cpp new file mode 100644 index 0000000000..242be99f1b --- /dev/null +++ b/libraries/AP_Common/AP_Param.cpp @@ -0,0 +1,366 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// +// This is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License as published by the +// Free Software Foundation; either version 2.1 of the License, or (at +// your option) any later version. +// + +/// @file AP_Param.cpp +/// @brief The AP variable store. + + +#include + +#include +#include + +// #define ENABLE_FASTSERIAL_DEBUG + +#ifdef ENABLE_FASTSERIAL_DEBUG +# include +# define serialDebug(fmt, args...) if (FastSerial::getInitialized(0)) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(0); } while(0) +#else +# define serialDebug(fmt, args...) +#endif + +// Static member variables for AP_Param. +// + +// number of rows in the _var_info[] table +uint16_t AP_Param::_num_vars; + +// storage and naming information about all types that can be saved +const AP_Param::Info *AP_Param::_var_info; + +// write to EEPROM, checking each byte to avoid writing +// bytes that are already correct +void AP_Param::eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size) +{ + const uint8_t *b = (const uint8_t *)ptr; + while (size--) { + uint8_t v = eeprom_read_byte((const uint8_t *)ofs); + if (v != *b) { + eeprom_write_byte((uint8_t *)ofs, *b); + } + b++; + ofs++; + } +} + +// write a sentinal value at the given offset +void AP_Param::write_sentinal(uint16_t ofs) +{ + struct Param_header phdr; + phdr.type = AP_PARAM_NONE; + phdr.key = 0; + phdr.group_element = 0; + eeprom_write_check(&phdr, ofs, sizeof(phdr)); +} + +// erase all EEPROM variables by re-writing the header and adding +// a sentinal +void AP_Param::erase_all(void) +{ + struct EEPROM_header hdr; + + serialDebug("erase_all"); + + // write the header + hdr.magic = k_EEPROM_magic; + hdr.revision = k_EEPROM_revision; + hdr.spare = 0; + eeprom_write_check(&hdr, 0, sizeof(hdr)); + + // add a sentinal directly after the header + write_sentinal(sizeof(struct EEPROM_header)); +} + +// setup the _var_info[] table +bool AP_Param::setup(const AP_Param::Info *info, uint16_t num_vars) +{ + struct EEPROM_header hdr; + + _var_info = info; + _num_vars = num_vars; + + serialDebug("setup %u vars", (unsigned)num_vars); + + // check the header + eeprom_read_block(&hdr, 0, sizeof(hdr)); + if (hdr.magic != k_EEPROM_magic || + hdr.revision != k_EEPROM_revision) { + // header doesn't match. We can't recover any variables. Wipe + // the header and setup the sentinal directly after the header + serialDebug("bad header in setup - erasing"); + erase_all(); + } + + return true; +} + +// 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 (uint16_t i=0; i<_num_vars; i++) { + uint8_t type = pgm_read_byte(&_var_info[i].type); + uint16_t key = pgm_read_word(&_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_read_pointer(&_var_info[i].ptr); + return &_var_info[i]; + } + + // for groups we need to check each group element + const struct GroupInfo *group_info = (const struct GroupInfo *)pgm_read_pointer(&_var_info[i].group_info); + for (uint8_t j=0; + pgm_read_byte(&group_info[j].type) != AP_PARAM_NONE; + j++) { + if (j == phdr.group_element) { + // found a group element + *ptr = (void*)(pgm_read_pointer(&_var_info[i].ptr) + pgm_read_word(&group_info[j].offset)); + return &_var_info[i]; + } + } + } + serialDebug("failed to find type=%u key=%u\n", + (unsigned)phdr.type, + (unsigned)phdr.key); + return NULL; +} + +// find the info structure for a variable +const struct AP_Param::Info *AP_Param::find_var_info(uint8_t *group_element) +{ + for (uint16_t i=0; i<_num_vars; i++) { + uint8_t type = pgm_read_byte(&_var_info[i].type); + uintptr_t base = pgm_read_pointer(&_var_info[i].ptr); + if (type == AP_PARAM_GROUP) { + const struct GroupInfo *group_info = (const struct GroupInfo *)pgm_read_pointer(&_var_info[i].group_info); + for (uint8_t j=0; + (type=pgm_read_byte(&group_info[j].type)) != AP_PARAM_NONE ; + j++) { + if ((uintptr_t)this == base + pgm_read_pointer(&group_info[j].offset)) { + if (group_element != NULL) { + *group_element = j; + } + return &_var_info[i]; + } + } + } else if (base == (uintptr_t)this) { + *group_element = 0; + return &_var_info[i]; + } + } + return NULL; +} + +// return the storage size for a AP_PARAM_* type +const uint8_t AP_Param::type_size(enum ap_var_type type) +{ + switch (type) { + case AP_PARAM_NONE: + case AP_PARAM_GROUP: + return 0; + case AP_PARAM_INT8: + return 1; + case AP_PARAM_INT16: + return 2; + case AP_PARAM_INT32: + return 4; + case AP_PARAM_FLOAT: + return 4; + case AP_PARAM_VECTOR3F: + return 3*4; + case AP_PARAM_MATRIX3F: + return 3*3*4; + } + serialDebug("unknown type %u\n", type); + return 0; +} + +// scan the EEPROM looking for a given variable by header content +// return true if found, along with the offset in the EEPROM where +// the variable is stored +// if not found return the offset of the sentinal, or +bool AP_Param::scan(const AP_Param::Param_header *target, uint16_t *pofs) +{ + struct Param_header phdr; + uint16_t ofs = sizeof(AP_Param::EEPROM_header); + while (ofs < k_EEPROM_size) { + eeprom_read_block(&phdr, (const void *)ofs, sizeof(phdr)); + if (phdr.type == target->type && + phdr.key == target->key && + phdr.group_element == target->group_element) { + // found it + *pofs = ofs; + return true; + } + if (phdr.type == AP_PARAM_NONE && + phdr.key == 0) { + // we've reached the sentinal + *pofs = ofs; + serialDebug("failed to scan type=%u key=%u\n", + (unsigned)target->type, + (unsigned)target->key); + return false; + } + ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr); + } + *pofs = ~0; + serialDebug("scan past end of eeprom"); + return false; +} + +// 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(char *buffer, size_t buffer_size) +{ + uint8_t group_element; + const struct AP_Param::Info *info = find_var_info(&group_element); + if (info == NULL) { + *buffer = 0; + serialDebug("no info found"); + return; + } + strncpy_P(buffer, info->name, buffer_size); + if (pgm_read_byte(&info->type) == AP_PARAM_GROUP) { + uint8_t len = strnlen(buffer, buffer_size); + if (len < buffer_size) { + const struct GroupInfo *group_info = (const struct GroupInfo *)pgm_read_pointer(&info->group_info); + strncpy_P(&buffer[len], group_info->name, buffer_size-len); + } + } +} + +// Find a variable by name. +// +AP_Param * +AP_Param::find(const char *name) +{ + for (uint16_t i=0; i<_num_vars; i++) { + uint8_t type = pgm_read_byte(&_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_read_pointer(&_var_info[i].group_info); + for (uint8_t j=0; + (type=pgm_read_byte(&group_info[j].type)) != AP_PARAM_NONE ; + j++) { + if (strcasecmp_P(name+len, group_info[j].name) == 0) { + uintptr_t p = pgm_read_pointer(&_var_info[i].ptr); + return (AP_Param *)(p + pgm_read_pointer(&group_info[j].offset)); + } + } + } else if (strcasecmp_P(name, _var_info[i].name) == 0) { + return (AP_Param *)pgm_read_pointer(&_var_info[i].ptr); + } + } + return NULL; +} + +// Save the variable to EEPROM, if supported +// +bool AP_Param::save(void) +{ + uint8_t group_element; + const struct AP_Param::Info *info = find_var_info(&group_element); + + 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 + phdr.type = pgm_read_byte(&info->type); + phdr.key = pgm_read_word(&info->key); + phdr.group_element = group_element; + + // scan EEPROM to find the right location + uint16_t ofs; + if (scan(&phdr, &ofs)) { + // found an existing copy of the variable + eeprom_write_check(this, 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(this, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type)); + eeprom_write_check(&phdr, ofs, sizeof(phdr)); + return true; +} + +// Load the variable from EEPROM, if supported +// +bool AP_Param::load(void) +{ + uint8_t group_element; + const struct AP_Param::Info *info = find_var_info(&group_element); + 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 + phdr.type = pgm_read_byte(&info->type); + phdr.key = pgm_read_word(&info->key); + phdr.group_element = group_element; + + // scan EEPROM to find the right location + uint16_t ofs; + if (!scan(&phdr, &ofs)) { + return false; + } + + // found it + eeprom_read_block(this, (void*)(ofs+sizeof(phdr)), type_size((enum ap_var_type)phdr.type)); + return true; +} + +// 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 < k_EEPROM_size) { + eeprom_read_block(&phdr, (const void *)ofs, sizeof(phdr)); + if (phdr.type == AP_PARAM_NONE && + phdr.key == 0) { + // we've reached the sentinal + return true; + } + + const struct AP_Param::Info *info; + void *ptr; + + info = find_by_header(phdr, &ptr); + if (info != NULL) { + eeprom_read_block(ptr, (void*)(ofs+sizeof(phdr)), type_size((enum ap_var_type)phdr.type)); + } + + ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr); + } + + // we didn't find the sentinal + serialDebug("no sentinal in load_all"); + return false; +} diff --git a/libraries/AP_Common/AP_Param.h b/libraries/AP_Common/AP_Param.h new file mode 100644 index 0000000000..2ea0182ccf --- /dev/null +++ b/libraries/AP_Common/AP_Param.h @@ -0,0 +1,374 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// +// This is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License as published by the +// Free Software Foundation; either version 2.1 of the License, or (at +// your option) any later version. +// + +/// @file AP_Param.h +/// @brief A system for managing and storing variables that are of +/// general interest to the system. + +#ifndef AP_PARAM_H +#define AP_PARAM_H +#include +#include +#include + +#include +#include +#include + +#define AP_MAX_NAME_SIZE 15 + +// a varient of offsetof() to work around C++ restrictions. +// this can only be used when the offset of a variable in a object +// is constant and known at compile time +#define VAROFFSET(type, element) (((uintptr_t)(&((const type *)1)->element))-1) + +enum ap_var_type { + AP_PARAM_NONE = 0, + AP_PARAM_INT8, + AP_PARAM_INT16, + AP_PARAM_INT32, + AP_PARAM_FLOAT, + AP_PARAM_VECTOR3F, + AP_PARAM_VECTOR6F, + AP_PARAM_MATRIX3F, + AP_PARAM_GROUP +}; + +/// Base class for variables. +/// +/// Provides naming and lookup services for variables. +/// +class AP_Param +{ +public: + /// EEPROM header + /// + /// This structure is placed at the head of the EEPROM to indicate + /// that the ROM is formatted for AP_Param. + /// + struct EEPROM_header { + uint16_t magic; + uint8_t revision; + uint8_t spare; + }; + + /// This header is prepended to a variable stored in EEPROM. + struct Param_header { + uint16_t type:4; + uint16_t key:9; + uint16_t group_element:3; + }; + + // the Info and GroupInfo structures are passed by the main + // program in setup() to give information on how variables are + // named and their location in memory + struct GroupInfo { + uint8_t type; // AP_PARAM_* + const char name[AP_MAX_NAME_SIZE]; + uintptr_t offset; // offset within the object + }; + struct Info { + uint8_t type; // AP_PARAM_* + const char name[AP_MAX_NAME_SIZE]; + uint16_t key; // k_param_* + void *ptr; // pointer to the variable in memory + const struct GroupInfo *group_info; + }; + + // every AP_Param type has a vtype which tells its type. This is + // used to make the initialisation of var_info[] less error prone + static const ap_var_type vtype = AP_PARAM_NONE; + + // 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, uint16_t num_vars); + + /// Copy the variable's name, prefixed by any containing group name, 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); + + /// Find a variable by name. + /// + /// If the variable has no name, it cannot be found by this interface. + /// + /// @param name The full name of the variable to be found. + /// @return A pointer to the variable, or NULL if + /// it does not exist. + /// + static AP_Param *find(const char *name); + + /// Save the current value of the variable to EEPROM. + /// + /// @return True if the variable was saved successfully. + /// + bool save(void); + + /// Load the variable from EEPROM. + /// + /// @return True if the variable was loaded successfully. + /// + bool load(void); + + /// Load all variables from EEPROM + /// + /// 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 + /// + static bool load_all(void); + + /// Erase all variables in EEPROM. + /// + static void erase_all(void); + + +private: + const struct Info *find_var_info(uint8_t *group_element); + static const struct Info *find_by_header(struct Param_header phdr, void **ptr); + static void write_sentinal(uint16_t ofs); + bool scan(const struct Param_header *phdr, uint16_t *pofs); + static const uint8_t type_size(enum ap_var_type type); + static void eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size); + + static uint16_t _num_vars; + static const struct Info *_var_info; + + static const uint16_t k_EEPROM_size = 4096; ///< XXX avr-libc doesn't consistently export this + + // values filled into the EEPROM header + static const uint16_t k_EEPROM_magic = 0x5041; ///< "AP" + static const uint16_t k_EEPROM_revision = 3; ///< current format revision +}; + +/// Template class for scalar variables. +/// +/// Objects of this type have a value, and can be treated in many ways as though they +/// were the value. +/// +/// @tparam T The scalar type of the variable +/// @tparam PT The AP_PARAM_* type +/// +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 + /// + T get(void) const { + return _value; + } + + /// Value setter + /// + void set(T v) { + _value = v; + } + + /// Combined set and save + /// + bool set_and_save(T v) { + set(v); + return save(); + } + + /// Conversion to T returns a reference to the value. + /// + /// This allows the class to be used in many situations where the value would be legal. + /// + operator T &() { + return _value; + } + + /// Copy assignment from self does nothing. + /// + AP_ParamT& operator=(AP_ParamT& v) { + return v; + } + + /// Copy assignment from T is equivalent to ::set. + /// + AP_ParamT& operator=(T v) { + _value = v; + return *this; + } + + /// AP_ParamT types can implement AP_Param::cast_to_float + /// + float cast_to_float(void) { + return (float)_value; + } + +protected: + T _value; +}; + + +/// Template class for non-scalar variables. +/// +/// Objects of this type have a value, and can be treated in many ways as though they +/// were the value. +/// +/// @tparam T The scalar type of the variable +/// @tparam PT AP_PARAM_* type +/// +template +class AP_ParamV : public AP_Param +{ +public: + static const ap_var_type vtype = PT; + + /// Value getter + /// + T get(void) const { + return _value; + } + + /// Value setter + /// + void set(T v) { + _value = v; + } + + /// Combined set and save + /// + bool set_and_save(T v) { + set(v); + return save(); + } + + /// Conversion to T returns a reference to the value. + /// + /// This allows the class to be used in many situations where the value would be legal. + /// + operator T &() { + return _value; + } + + /// Copy assignment from self does nothing. + /// + AP_ParamT& operator=(AP_ParamT& v) { + return v; + } + + /// Copy assignment from T is equivalent to ::set. + /// + AP_ParamT& operator=(T v) { + _value = v; + return *this; + } + +protected: + T _value; +}; + + +/// Template class for array variables. +/// +/// Objects created using this template behave like arrays of the type T, +/// but are stored like single variables. +/// +/// @tparam T The scalar type of the variable +/// @tparam N number of elements +/// @tparam PT the AP_PARAM_* type +/// +template +class AP_ParamA : public AP_Param +{ +public: + static const ap_var_type vtype = PT; + + /// Array operator accesses members. + /// + /// @note It would be nice to range-check i here, but then what would we return? + /// + T &operator [](uint8_t i) { + return _value[i]; + } + + /// Value getter + /// + /// @note Returns zero for index values out of range. + /// + T get(uint8_t i) const { + if (i < N) { + return _value[i]; + } else { + return (T)0; + } + } + + /// Value setter + /// + /// @note Attempts to set an index out of range are discarded. + /// + void set(uint8_t i, T v) { + if (i < N) { + _value[i] = v; + } + } + + /// Copy assignment from self does nothing. + /// + AP_ParamA& operator=(AP_ParamA& v) { + return v; + } + +protected: + T _value[N]; +}; + + + +/// Convenience macro for defining instances of the AP_ParamT template. +/// +#define AP_PARAMDEF(_t, _n, _pt) typedef AP_ParamT<_t, _pt> AP_##_n; +AP_PARAMDEF(float, Float, AP_PARAM_FLOAT); // defines AP_Float +AP_PARAMDEF(int8_t, Int8, AP_PARAM_INT8); // defines AP_Int8 +AP_PARAMDEF(int16_t, Int16, AP_PARAM_INT16); // defines AP_Int16 +AP_PARAMDEF(int32_t, Int32, AP_PARAM_INT32); // defines AP_Int32 + +#define AP_PARAMDEFV(_t, _n, _pt) typedef AP_ParamV<_t, _pt> AP_##_n; +AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F); +AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F); + +#define AP_PARAMDEFA(_t, _n, _size, _pt) typedef AP_ParamA<_t, _size, _pt> AP_##_n; +AP_PARAMDEFA(float, Vector6f, 6, AP_PARAM_VECTOR6F); + +/// Rely on built in casting for other variable types +/// to minimize template creation and save memory +#define AP_Uint8 AP_Int8 +#define AP_Uint16 AP_Int16 +#define AP_Uint32 AP_Int32 +#define AP_Bool AP_Int8 + +#endif // AP_PARAM_H