Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Param/AP_Param.cpp

3083 lines
94 KiB
C++
Raw Permalink Blame History

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
//
//
// total up and check overflow
// check size of group var_info
/// @file AP_Param.cpp
/// @brief The AP variable store.
#include "AP_Param.h"
#include <cmath>
#include <string.h>
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS.h>
#include <StorageManager/StorageManager.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_InternalError/AP_InternalError.h>
#include <AP_Filesystem/AP_Filesystem.h>
#include <stdio.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
#endif
extern const AP_HAL::HAL &hal;
uint16_t AP_Param::sentinal_offset;
// singleton instance
AP_Param *AP_Param::_singleton;
#define ENABLE_DEBUG 0
#if ENABLE_DEBUG
# define FATAL(fmt, args ...) AP_HAL::panic(fmt, ## args);
# define Debug(fmt, args ...) do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
# define FATAL(fmt, args ...) AP_HAL::panic("Bad parameter table");
# define Debug(fmt, args ...)
#endif
#if HAL_GCS_ENABLED
#define GCS_SEND_PARAM(name, type, v) gcs().send_parameter_value(name, type, v)
#else
#define GCS_SEND_PARAM(name, type, v)
#endif
// Note about AP_Vector3f handling.
// The code has special cases for AP_Vector3f to allow it to be viewed
// as both a single 3 element vector and as a set of 3 AP_Float
// variables. This is done to make it possible for MAVLink to see
// vectors as parameters, which allows users to save their compass
// offsets in MAVLink parameter files. The code involves quite a few
// special cases which could be generalised to any vector/matrix type
// if we end up needing this behaviour for other than AP_Vector3f
// static member variables for AP_Param.
//
// number of rows in the _var_info[] table
uint16_t AP_Param::_num_vars;
#if AP_PARAM_DYNAMIC_ENABLED
uint16_t AP_Param::_num_vars_base;
AP_Param::Info *AP_Param::_var_info_dynamic;
static const char *_empty_string = "";
uint8_t AP_Param::_dynamic_table_sizes[AP_PARAM_MAX_DYNAMIC];
#endif
// cached parameter count
uint16_t AP_Param::_parameter_count;
uint16_t AP_Param::_count_marker;
uint16_t AP_Param::_count_marker_done;
HAL_Semaphore AP_Param::_count_sem;
// storage and naming information about all types that can be saved
const AP_Param::Info *AP_Param::_var_info;
struct AP_Param::param_override *AP_Param::param_overrides;
uint16_t AP_Param::param_overrides_len;
uint16_t AP_Param::num_param_overrides;
uint16_t AP_Param::num_read_only;
ObjectBuffer_TS<AP_Param::param_save> AP_Param::save_queue{30};
bool AP_Param::registered_save_handler;
AP_Param::defaults_list *AP_Param::default_list;
// we need a dummy object for the parameter save callback
static AP_Param save_dummy;
#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
/*
this holds default parameters in the normal NAME=value form for a
parameter file. It can be manipulated by apj_tool.py to change the
defaults on a binary without recompiling
*/
const AP_Param::param_defaults_struct AP_Param::param_defaults_data = {
"PARMDEF",
{ 0x55, 0x37, 0xf4, 0xa0, 0x38, 0x5d, 0x48, 0x5b },
AP_PARAM_MAX_EMBEDDED_PARAM,
0
};
#endif
// storage object
StorageAccess AP_Param::_storage(StorageManager::StorageParam);
// backup storage object
StorageAccess AP_Param::_storage_bak(StorageManager::StorageParamBak);
// flags indicating frame type
uint16_t AP_Param::_frame_type_flags;
// write to EEPROM
void AP_Param::eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size)
{
_storage.write_block(ofs, ptr, size);
_storage_bak.write_block(ofs, ptr, size);
}
bool AP_Param::_hide_disabled_groups = true;
// write a sentinal value at the given offset
void AP_Param::write_sentinal(uint16_t ofs)
{
struct Param_header phdr;
phdr.type = _sentinal_type;
set_key(phdr, _sentinal_key);
phdr.group_element = _sentinal_group;
eeprom_write_check(&phdr, ofs, sizeof(phdr));
sentinal_offset = ofs;
}
// erase all EEPROM variables by re-writing the header and adding
// a sentinal
void AP_Param::erase_all(void)
{
struct EEPROM_header hdr;
// write the header
hdr.magic[0] = k_EEPROM_magic0;
hdr.magic[1] = k_EEPROM_magic1;
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));
}
/* the 'group_id' of a element of a group is the 18 bit identifier
used to distinguish between this element of the group and other
elements of the same group. It is calculated using a bit shift per
level of nesting, so the first level of nesting gets 6 bits the 2nd
level gets the next 6 bits, and the 3rd level gets the last 6
bits. This limits groups to having at most 64 elements.
*/
uint32_t AP_Param::group_id(const struct GroupInfo *grpinfo, uint32_t base, uint8_t i, uint8_t shift)
{
if (grpinfo[i].idx == 0 && shift != 0 && !(grpinfo[i].flags & AP_PARAM_FLAG_NO_SHIFT)) {
/*
this is a special case for a bug in the original design. An
idx of 0 shifted by n bits is still zero, which makes it
indistinguishable from a different parameter. This can lead
to parameter loops. We use index 63 for that case.
*/
return base + (63U<<shift);
}
return base + (grpinfo[i].idx<<shift);
}
/*
check if a frame type should be included. A frame is included if
either there are no frame type flags on a parameter or if at least
one of the flags has been set by set_frame_type_flags()
*/
bool AP_Param::check_frame_type(uint16_t flags)
{
if (flags & AP_PARAM_FLAG_HIDDEN) {
// hidden on all frames
return false;
}
uint16_t frame_flags = flags >> AP_PARAM_FRAME_TYPE_SHIFT;
if (frame_flags == 0) {
return true;
}
return (frame_flags & _frame_type_flags) != 0;
}
// validate a group info table
void AP_Param::check_group_info(const struct AP_Param::GroupInfo * group_info,
uint16_t * total_size,
uint8_t group_shift,
uint8_t prefix_length)
{
uint8_t type;
uint64_t used_mask = 0;
for (uint8_t i=0;
(type=group_info[i].type) != AP_PARAM_NONE;
i++) {
uint8_t idx = group_info[i].idx;
if (idx >= (1<<_group_level_shift)) {
FATAL("idx too large (%u) in %s", idx, group_info[i].name);
}
if (group_shift != 0 && idx == 0) {
// treat idx 0 as 63 for duplicates. See group_id()
idx = 63;
}
if (used_mask & (1ULL<<idx)) {
FATAL("Duplicate group idx %u for %s", idx, group_info[i].name);
}
used_mask |= (1ULL<<idx);
if (type == AP_PARAM_GROUP) {
// a nested group
if (group_shift + _group_level_shift >= _group_bits) {
FATAL("double group nesting in %s", group_info[i].name);
}
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
if (ginfo == nullptr) {
continue;
}
check_group_info(ginfo, total_size, group_shift + _group_level_shift, prefix_length + strlen(group_info[i].name));
continue;
}
uint8_t size = type_size((enum ap_var_type)type);
if (size == 0) {
FATAL("invalid type in %s", group_info[i].name);
}
if (prefix_length + strlen(group_info[i].name) > 16) {
FATAL("suffix is too long in %s", group_info[i].name);
}
(*total_size) += size + sizeof(struct Param_header);
}
}
// check for duplicate key values
bool AP_Param::duplicate_key(uint16_t vindex, uint16_t key)
{
for (uint16_t i=vindex+1; i<_num_vars; i++) {
uint16_t key2 = var_info(i).key;
if (key2 == key) {
// no duplicate keys allowed
return true;
}
}
return false;
}
/*
get group_info pointer for a group
*/
const struct AP_Param::GroupInfo *AP_Param::get_group_info(const struct GroupInfo &ginfo)
{
if (ginfo.flags & AP_PARAM_FLAG_INFO_POINTER) {
return *ginfo.group_info_ptr;
}
return ginfo.group_info;
}
/*
get group_info pointer for a group
*/
const struct AP_Param::GroupInfo *AP_Param::get_group_info(const struct Info &info)
{
if (info.flags & AP_PARAM_FLAG_INFO_POINTER) {
return *info.group_info_ptr;
}
return info.group_info;
}
// validate the _var_info[] table
void AP_Param::check_var_info(void)
{
uint16_t total_size = sizeof(struct EEPROM_header);
for (uint16_t i=0; i<_num_vars; i++) {
const auto &info = var_info(i);
uint8_t type = info.type;
uint16_t key = info.key;
if (type == AP_PARAM_GROUP) {
if (i == 0) {
FATAL("first element can't be a group, for first() call");
}
const struct GroupInfo *group_info = get_group_info(info);
if (group_info == nullptr) {
continue;
}
check_group_info(group_info, &total_size, 0, strlen(info.name));
} else {
uint8_t size = type_size((enum ap_var_type)type);
if (size == 0) {
// not a valid type - the top level list can't contain
// AP_PARAM_NONE
FATAL("AP_PARAM_NONE at top level");
}
total_size += size + sizeof(struct Param_header);
}
if (duplicate_key(i, key)) {
FATAL("duplicate key");
}
if (type != AP_PARAM_GROUP && (info.flags & AP_PARAM_FLAG_POINTER)) {
FATAL("only groups can be pointers");
}
}
// 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
}
// setup the _var_info[] table
bool AP_Param::setup(void)
{
struct EEPROM_header hdr {};
struct EEPROM_header hdr2 {};
// check the header
_storage.read_block(&hdr, 0, sizeof(hdr));
_storage_bak.read_block(&hdr2, 0, sizeof(hdr2));
if (hdr.magic[0] != k_EEPROM_magic0 ||
hdr.magic[1] != k_EEPROM_magic1 ||
hdr.revision != k_EEPROM_revision) {
if (hdr2.magic[0] == k_EEPROM_magic0 &&
hdr2.magic[1] == k_EEPROM_magic1 &&
hdr2.revision == k_EEPROM_revision &&
_storage.copy_area(_storage_bak)) {
// restored from backup
INTERNAL_ERROR(AP_InternalError::error_t::params_restored);
return true;
}
// header doesn't match. We can't recover any variables. Wipe
// the header and setup the sentinal directly after the header
Debug("bad header in setup - erasing");
erase_all();
}
// ensure that backup is in sync with primary
_storage_bak.copy_area(_storage);
return true;
}
// check if AP_Param has been initialised
bool AP_Param::initialised(void)
{
return _var_info != nullptr;
}
/*
adjust offset of a group element for nested groups and group pointers
The new_offset variable is relative to the vindex base. This makes
dealing with pointer groups tricky
*/
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) {
new_offset += group_info.offset;
return true;
}
if (group_info.flags & AP_PARAM_FLAG_POINTER) {
// group_info.offset refers to a pointer
ptrdiff_t base;
if (!get_base(var_info(vindex), base)) {
// the object is not allocated yet
return false;
}
void **p = (void **)(base + new_offset + group_info.offset);
if (*p == nullptr) {
// the object is not allocated yet
return false;
}
// calculate offset that is needed to take base object and adjust for this object
new_offset = ((ptrdiff_t)*p) - base;
}
return true;
}
/*
get the base pointer for a variable, accounting for AP_PARAM_FLAG_POINTER
*/
bool AP_Param::get_base(const struct Info &info, ptrdiff_t &base)
{
if (info.flags & AP_PARAM_FLAG_POINTER) {
base = *(ptrdiff_t *)info.ptr;
return (base != (ptrdiff_t)0);
}
base = (ptrdiff_t)info.ptr;
return true;
}
namespace AP {
AP_Param *param()
{
return AP_Param::get_singleton();
}
}
// find the info structure given a header and a group_info table
// 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,
uint16_t vindex,
const struct GroupInfo *group_info,
uint32_t group_base,
uint8_t group_shift,
ptrdiff_t group_offset)
{
uint8_t type;
for (uint8_t i=0;
(type=group_info[i].type) != AP_PARAM_NONE;
i++) {
if (type == AP_PARAM_GROUP) {
// a nested group
if (group_shift + _group_level_shift >= _group_bits) {
// too deeply nested - this should have been caught by
// setup() !
return nullptr;
}
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
if (ginfo == nullptr) {
continue;
}
ptrdiff_t new_offset = group_offset;
if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
continue;
}
const struct AP_Param::Info *ret = find_by_header_group(phdr, ptr, vindex, ginfo,
group_id(group_info, group_base, i, group_shift),
group_shift + _group_level_shift, new_offset);
if (ret != nullptr) {
return ret;
}
continue;
}
if (group_id(group_info, group_base, i, group_shift) == phdr.group_element && type == phdr.type) {
// found a group element
ptrdiff_t base;
if (!get_base(var_info(vindex), base)) {
continue;
}
*ptr = (void*)(base + group_info[i].offset + group_offset);
return &var_info(vindex);
}
}
return nullptr;
}
// 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++) {
const auto &info = var_info(i);
uint8_t type = info.type;
uint16_t key = info.key;
if (key != get_key(phdr)) {
// not the right key
continue;
}
if (type == AP_PARAM_GROUP) {
const struct GroupInfo *group_info = get_group_info(info);
if (group_info == nullptr) {
continue;
}
return find_by_header_group(phdr, ptr, i, group_info, 0, 0, 0);
}
if (type == phdr.type) {
// found it
ptrdiff_t base;
if (!get_base(info, base)) {
return nullptr;
}
*ptr = (void*)base;
return &info;
}
}
return nullptr;
}
// 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,
uint16_t vindex,
uint32_t group_base,
uint8_t group_shift,
ptrdiff_t group_offset,
uint32_t * group_element,
const struct GroupInfo * &group_ret,
struct GroupNesting &group_nesting,
uint8_t * idx) const
{
ptrdiff_t base;
if (!get_base(var_info(vindex), base)) {
return nullptr;
}
uint8_t type;
for (uint8_t i=0;
(type=group_info[i].type) != AP_PARAM_NONE;
i++) {
ptrdiff_t ofs = group_info[i].offset + group_offset;
if (type == AP_PARAM_GROUP) {
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
if (ginfo == nullptr) {
continue;
}
// a nested group
if (group_shift + _group_level_shift >= _group_bits) {
// too deeply nested - this should have been caught by
// setup() !
return nullptr;
}
const struct AP_Param::Info *info;
ptrdiff_t new_offset = group_offset;
if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
continue;
}
if (group_nesting.level >= group_nesting.numlevels) {
return nullptr;
}
group_nesting.group_ret[group_nesting.level++] = &group_info[i];
info = find_var_info_group(ginfo, vindex,
group_id(group_info, group_base, i, group_shift),
group_shift + _group_level_shift,
new_offset,
group_element,
group_ret,
group_nesting,
idx);
if (info != nullptr) {
return info;
}
group_nesting.level--;
} else if ((ptrdiff_t) this == base + ofs) {
*group_element = group_id(group_info, group_base, i, group_shift);
group_ret = &group_info[i];
*idx = 0;
return &var_info(vindex);
} else if (type == AP_PARAM_VECTOR3F &&
(base+ofs+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
base+ofs+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
// we are inside a Vector3f. We need to work out which
// element of the vector the current object refers to.
*idx = (((ptrdiff_t) this) - (base+ofs))/sizeof(float);
*group_element = group_id(group_info, group_base, i, group_shift);
group_ret = &group_info[i];
return &var_info(vindex);
}
}
return nullptr;
}
// find the info structure for a variable
const struct AP_Param::Info *AP_Param::find_var_info(uint32_t * group_element,
const struct GroupInfo * &group_ret,
struct GroupNesting &group_nesting,
uint8_t * idx) const
{
group_ret = nullptr;
for (uint16_t i=0; i<_num_vars; i++) {
const auto &info = var_info(i);
uint8_t type = info.type;
ptrdiff_t base;
if (!get_base(info, base)) {
continue;
}
if (type == AP_PARAM_GROUP) {
const struct GroupInfo *group_info = get_group_info(info);
if (group_info == nullptr) {
continue;
}
const struct AP_Param::Info *info2;
info2 = find_var_info_group(group_info, i, 0, 0, 0, group_element, group_ret, group_nesting, idx);
if (info2 != nullptr) {
return info2;
}
} else if (base == (ptrdiff_t) this) {
*group_element = 0;
*idx = 0;
return &info;
} else if (type == AP_PARAM_VECTOR3F &&
(base+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
base+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
// we are inside a Vector3f. Work out which element we are
// referring to.
*idx = (((ptrdiff_t) this) - base)/sizeof(float);
*group_element = 0;
return &info;
}
}
return nullptr;
}
// find the info structure for a variable
const struct AP_Param::Info *AP_Param::find_var_info_token(const ParamToken &token,
uint32_t * group_element,
const struct GroupInfo * &group_ret,
struct GroupNesting &group_nesting,
uint8_t * idx) const
{
uint16_t i = token.key;
const auto &info = var_info(i);
uint8_t type = info.type;
ptrdiff_t base;
if (!get_base(info, base)) {
return nullptr;
}
group_ret = nullptr;
if (type == AP_PARAM_GROUP) {
const struct GroupInfo *group_info = get_group_info(info);
if (group_info == nullptr) {
return nullptr;
}
const struct AP_Param::Info *info2;
info2 = find_var_info_group(group_info, i, 0, 0, 0, group_element, group_ret, group_nesting, idx);
if (info2 != nullptr) {
return info2;
}
} else if (base == (ptrdiff_t) this) {
*group_element = 0;
*idx = 0;
return &info;
} else if (type == AP_PARAM_VECTOR3F &&
(base+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
base+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
// we are inside a Vector3f. Work out which element we are
// referring to.
*idx = (((ptrdiff_t) this) - base)/sizeof(float);
*group_element = 0;
return &info;
}
return nullptr;
}
// return the storage size for a AP_PARAM_* type
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;
}
Debug("unknown type %d\n", type);
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 && on the key and group, 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) {
return true;
}
// also check for 0xFFFFFFFF and 0x00000000, which are the fill
// values for storage. These can appear if power off occurs while
// writing data
uint32_t v = *(uint32_t *)&phdr;
if (v == 0 || v == 0xFFFFFFFF) {
return true;
}
return false;
}
// 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
// if the sentinal isn't found either, the offset is set to 0xFFFF
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 < _storage.size()) {
_storage.read_block(&phdr, ofs, sizeof(phdr));
if (phdr.type == target->type &&
get_key(phdr) == get_key(*target) &&
phdr.group_element == target->group_element) {
// found it
*pofs = ofs;
return true;
}
if (is_sentinal(phdr)) {
// we've reached the sentinal
*pofs = ofs;
sentinal_offset = ofs;
return false;
}
ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
}
*pofs = 0xffff;
Debug("scan past end of eeprom");
return false;
}
/**
* add a _X, _Y, _Z suffix to the name of a Vector3f element
* @param buffer
* @param buffer_size
* @param idx Suffix: 0 --> _X; 1 --> _Y; 2 --> _Z; (other --> undefined)
*/
void AP_Param::add_vector3f_suffix(char *buffer, size_t buffer_size, uint8_t idx) const
{
const size_t len = strnlen(buffer, buffer_size);
if (len + 2 <= buffer_size) {
buffer[len] = '_';
buffer[len + 1] = static_cast<char>('X' + idx);
if (len + 3 <= buffer_size) {
buffer[len + 2] = 0;
}
}
}
// 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_token(const ParamToken &token, char *buffer, size_t buffer_size, bool force_scalar) const
{
uint32_t group_element;
const struct GroupInfo *ginfo;
struct GroupNesting group_nesting {};
uint8_t idx;
const struct AP_Param::Info *info = find_var_info_token(token, &group_element, ginfo, group_nesting, &idx);
if (info == nullptr) {
*buffer = 0;
Debug("no info found");
return;
}
copy_name_info(info, ginfo, group_nesting, idx, buffer, buffer_size, force_scalar);
}
void AP_Param::copy_name_info(const struct AP_Param::Info *info,
const struct GroupInfo *ginfo,
const struct GroupNesting &group_nesting,
uint8_t idx, char *buffer, size_t buffer_size, bool force_scalar) const
{
strncpy(buffer, info->name, buffer_size);
for (uint8_t i=0; i<group_nesting.level; i++) {
uint8_t len = strnlen(buffer, buffer_size);
if (len < buffer_size) {
strncpy(&buffer[len], group_nesting.group_ret[i]->name, buffer_size-len);
}
}
if (ginfo != nullptr) {
uint8_t len = strnlen(buffer, buffer_size);
if (len < buffer_size) {
strncpy(&buffer[len], ginfo->name, buffer_size-len);
}
if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == ginfo->type) {
// the caller wants a specific element in a Vector3f
add_vector3f_suffix(buffer, buffer_size, idx);
}
} else if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == info->type) {
add_vector3f_suffix(buffer, buffer_size, idx);
}
}
// Find a variable by name in a group
AP_Param *
AP_Param::find_group(const char *name, uint16_t vindex, ptrdiff_t group_offset,
const struct GroupInfo *group_info, enum ap_var_type *ptype)
{
uint8_t type;
for (uint8_t i=0;
(type=group_info[i].type) != AP_PARAM_NONE;
i++) {
if (type == AP_PARAM_GROUP) {
if (strncasecmp(name, group_info[i].name, strlen(group_info[i].name)) != 0) {
continue;
}
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
if (ginfo == nullptr) {
continue;
}
ptrdiff_t new_offset = group_offset;
if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
continue;
}
AP_Param *ap = find_group(name+strlen(group_info[i].name), vindex, new_offset, ginfo, ptype);
if (ap != nullptr) {
return ap;
}
} else if (strcasecmp(name, group_info[i].name) == 0) {
ptrdiff_t base;
if (!get_base(var_info(vindex), base)) {
continue;
}
*ptype = (enum ap_var_type)type;
return (AP_Param *)(base + group_info[i].offset + group_offset);
} else if (type == AP_PARAM_VECTOR3F) {
// special case for finding Vector3f elements
uint8_t suffix_len = strnlen(group_info[i].name, AP_MAX_NAME_SIZE);
if (strncmp(name, group_info[i].name, suffix_len) == 0 &&
name[suffix_len] == '_' &&
(name[suffix_len+1] == 'X' ||
name[suffix_len+1] == 'Y' ||
name[suffix_len+1] == 'Z')) {
ptrdiff_t base;
if (!get_base(var_info(vindex), base)) {
continue;
}
AP_Float *v = (AP_Float *)(base + group_info[i].offset + group_offset);
*ptype = AP_PARAM_FLOAT;
switch (name[suffix_len+1]) {
case 'X':
return (AP_Float *)&v[0];
case 'Y':
return (AP_Float *)&v[1];
case 'Z':
return (AP_Float *)&v[2];
}
}
}
}
return nullptr;
}
// Find a variable by name.
//
AP_Param *
AP_Param::find(const char *name, enum ap_var_type *ptype, uint16_t *flags)
{
for (uint16_t i=0; i<_num_vars; i++) {
const auto &info = var_info(i);
uint8_t type = info.type;
if (type == AP_PARAM_GROUP) {
uint8_t len = strnlen(info.name, AP_MAX_NAME_SIZE);
if (strncmp(name, info.name, len) != 0) {
continue;
}
const struct GroupInfo *group_info = get_group_info(info);
if (group_info == nullptr) {
continue;
}
AP_Param *ap = find_group(name + len, i, 0, group_info, ptype);
if (ap != nullptr) {
if (flags != nullptr) {
uint32_t group_element = 0;
const struct GroupInfo *ginfo;
struct GroupNesting group_nesting {};
uint8_t idx;
ap->find_var_info(&group_element, ginfo, group_nesting, &idx);
if (ginfo != nullptr) {
*flags = ginfo->flags;
}
}
return ap;
}
// we continue looking as we want to allow top level
// parameter to have the same prefix name as group
// parameters, for example CAM_P_G
} else if (strcasecmp(name, info.name) == 0) {
*ptype = (enum ap_var_type)type;
ptrdiff_t base;
if (!get_base(info, base)) {
return nullptr;
}
return (AP_Param *)base;
}
}
return nullptr;
}
// Find a variable by index. Note that this is quite slow.
//
AP_Param *
AP_Param::find_by_index(uint16_t idx, enum ap_var_type *ptype, ParamToken *token)
{
AP_Param *ap;
uint16_t count=0;
for (ap=AP_Param::first(token, ptype);
ap && count < idx;
ap=AP_Param::next_scalar(token, ptype)) {
count++;
}
return ap;
}
// by-name equivalent of find_by_index()
AP_Param* AP_Param::find_by_name(const char* name, enum ap_var_type *ptype, ParamToken *token)
{
AP_Param *ap;
uint16_t count = 0;
for (ap = AP_Param::first(token, ptype);
ap && *ptype != AP_PARAM_GROUP && *ptype != AP_PARAM_NONE;
ap = AP_Param::next_scalar(token, ptype)) {
int32_t ret = strncasecmp(name, var_info(token->key).name, AP_MAX_NAME_SIZE);
if (ret >= 0) {
char buf[AP_MAX_NAME_SIZE];
ap->copy_name_token(*token, buf, AP_MAX_NAME_SIZE);
if (strncasecmp(name, buf, AP_MAX_NAME_SIZE) == 0) {
break;
}
}
count++;
}
return ap;
}
/*
Find a variable by pointer, returning key. This is used for loading pointer variables
*/
bool AP_Param::find_key_by_pointer_group(const void *ptr, uint16_t vindex,
const struct GroupInfo *group_info,
ptrdiff_t offset, uint16_t &key)
{
for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
if (group_info[i].type != AP_PARAM_GROUP) {
continue;
}
ptrdiff_t base;
if (!get_base(var_info(vindex), base)) {
continue;
}
if (group_info[i].flags & AP_PARAM_FLAG_POINTER) {
if (ptr == *(void **)(base+group_info[i].offset+offset)) {
key = var_info(vindex).key;
return true;
}
} else if (ptr == (void *)(base+group_info[i].offset+offset)) {
key = var_info(vindex).key;
return true;
}
ptrdiff_t new_offset = offset;
if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
continue;
}
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
if (ginfo == nullptr) {
continue;
}
if (find_key_by_pointer_group(ptr, vindex, ginfo, new_offset, key)) {
return true;
}
}
return false;
}
/*
Find a variable by pointer, returning key. This is used for loading pointer variables
*/
bool AP_Param::find_key_by_pointer(const void *ptr, uint16_t &key)
{
for (uint16_t i=0; i<_num_vars; i++) {
const auto &info = var_info(i);
if (info.type != AP_PARAM_GROUP) {
continue;
}
if ((info.flags & AP_PARAM_FLAG_POINTER) &&
ptr == *(void **)info.ptr) {
key = info.key;
return true;
}
ptrdiff_t offset = 0;
const struct GroupInfo *ginfo = get_group_info(info);
if (ginfo == nullptr) {
continue;
}
if (find_key_by_pointer_group(ptr, i, ginfo, offset, key)) {
return true;
}
}
return false;
}
/*
Find key to top level group parameters by pointer
*/
bool AP_Param::find_top_level_key_by_pointer(const void *ptr, uint16_t &key)
{
for (uint16_t i=0; i<_num_vars; i++) {
const auto &info = var_info(i);
if (info.type != AP_PARAM_GROUP) {
continue;
}
if (ptr == (void **)info.ptr) {
key = info.key;
return true;
}
}
return false;
}
/*
fetch a parameter value based on the index within a group. This
is used to find the old value of a parameter that has been
removed from an object.
*/
bool AP_Param::get_param_by_index(void *obj_ptr, uint8_t idx, ap_var_type old_ptype, void *pvalue)
{
uint16_t key;
if (!find_top_level_key_by_pointer(obj_ptr, key)) {
return false;
}
const ConversionInfo type_info = {key, idx, old_ptype, nullptr };
return AP_Param::find_old_parameter(&type_info, (AP_Param *)pvalue);
}
// Find a object by name.
//
AP_Param *
AP_Param::find_object(const char *name)
{
for (uint16_t i=0; i<_num_vars; i++) {
const auto &info = var_info(i);
if (strcasecmp(name, info.name) == 0) {
ptrdiff_t base;
if (!get_base(info, base)) {
return nullptr;
}
return (AP_Param *)base;
}
}
return nullptr;
}
// notify GCS of current value of parameter
void AP_Param::notify() const {
uint32_t group_element = 0;
const struct GroupInfo *ginfo;
struct GroupNesting group_nesting {};
uint8_t idx;
const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
if (info == nullptr) {
// this is probably very bad
return;
}
char name[AP_MAX_NAME_SIZE+1];
copy_name_info(info, ginfo, group_nesting, idx, name, sizeof(name), true);
uint32_t param_header_type;
if (ginfo != nullptr) {
param_header_type = ginfo->type;
} else {
param_header_type = info->type;
}
send_parameter(name, (enum ap_var_type)param_header_type, idx);
}
/*
Save the variable to HAL storage, synchronous version
*/
void AP_Param::save_sync(bool force_save, bool send_to_gcs)
{
uint32_t group_element = 0;
const struct GroupInfo *ginfo;
struct GroupNesting group_nesting {};
uint8_t idx;
const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
const AP_Param *ap;
if (info == nullptr) {
// we don't have any info on how to store it
return;
}
struct Param_header phdr;
// create the header we will use to store the variable
if (ginfo != nullptr) {
phdr.type = ginfo->type;
if (ginfo->flags & AP_PARAM_FLAG_HIDDEN) {
send_to_gcs = false;
}
} else {
phdr.type = info->type;
if (info->flags & AP_PARAM_FLAG_HIDDEN) {
send_to_gcs = false;
}
}
set_key(phdr, info->key);
phdr.group_element = group_element;
ap = this;
if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {
// only vector3f can have non-zero idx for now
return;
}
if (idx != 0) {
ap = (const AP_Param *)((ptrdiff_t)ap) - (idx*sizeof(float));
}
if (phdr.type == AP_PARAM_INT8 && ginfo != nullptr && (ginfo->flags & AP_PARAM_FLAG_ENABLE)) {
// clear cached parameter count
invalidate_count();
}
char name[AP_MAX_NAME_SIZE+1];
copy_name_info(info, ginfo, group_nesting, idx, name, sizeof(name), true);
// scan EEPROM to find the right location
uint16_t ofs;
if (scan(&phdr, &ofs)) {
// found an existing copy of the variable
eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
if (send_to_gcs) {
send_parameter(name, (enum ap_var_type)phdr.type, idx);
}
return;
}
if (ofs == (uint16_t) ~0) {
return;
}
// if the value is the default value then don't save
if (phdr.type <= AP_PARAM_FLOAT) {
float v1 = cast_to_float((enum ap_var_type)phdr.type);
float v2;
if (ginfo != nullptr) {
v2 = get_default_value(this, *ginfo);
} else {
v2 = get_default_value(this, *info);
}
if (is_equal(v1,v2) && !force_save) {
if (send_to_gcs) {
GCS_SEND_PARAM(name, (enum ap_var_type)info->type, v2);
}
return;
}
if (!force_save &&
(phdr.type != AP_PARAM_INT32 &&
(fabsf(v1-v2) < 0.0001f*fabsf(v1)))) {
// for other than 32 bit integers, we accept values within
// 0.01 percent of the current value as being the same
if (send_to_gcs) {
GCS_SEND_PARAM(name, (enum ap_var_type)info->type, v2);
}
return;
}
}
if (ofs+type_size((enum ap_var_type)phdr.type)+2*sizeof(phdr) >= _storage.size()) {
// we are out of room for saving variables
DEV_PRINTF("EEPROM full\n");
return;
}
// 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));
eeprom_write_check(&phdr, ofs, sizeof(phdr));
if (send_to_gcs) {
send_parameter(name, (enum ap_var_type)phdr.type, idx);
}
}
/*
put variable into queue to be saved
*/
void AP_Param::save(bool force_save)
{
struct param_save p, p2;
p.param = this;
p.force_save = force_save;
if (save_queue.peek(p2) &&
p2.param == this &&
p2.force_save == force_save) {
// this one is already at the head of the list to be
// saved. This check is cheap and catches the case where we
// are flooding the save queue with one parameter (eg. mission
// creation, changing MIS_TOTAL)
return;
}
while (!save_queue.push(p)) {
// if we can't save to the queue
if (hal.util->get_soft_armed() && hal.scheduler->in_main_thread()) {
// if we are armed in main thread then don't sleep, instead we lose the
// parameter save
return;
}
// when we are disarmed then loop waiting for a slot to become
// available. This guarantees completion for large parameter
// set loads
hal.scheduler->expect_delay_ms(1);
hal.scheduler->delay_microseconds(500);
hal.scheduler->expect_delay_ms(0);
}
}
/*
background function for saving parameters. This runs on the IO thread
*/
void AP_Param::save_io_handler(void)
{
struct param_save p;
while (save_queue.pop(p)) {
p.param->save_sync(p.force_save, true);
}
if (hal.scheduler->is_system_initialized()) {
// pay the cost of parameter counting in the IO thread
count_parameters();
}
}
/*
wait for all parameters to save
*/
void AP_Param::flush(void)
{
uint16_t counter = 200; // 2 seconds max
while (counter-- && save_queue.available()) {
hal.scheduler->expect_delay_ms(10);
hal.scheduler->delay(10);
hal.scheduler->expect_delay_ms(0);
}
}
// Load the variable from EEPROM, if supported
//
bool AP_Param::load(void)
{
uint32_t group_element = 0;
const struct GroupInfo *ginfo;
struct GroupNesting group_nesting {};
uint8_t idx;
const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
if (info == nullptr) {
// 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
if (ginfo != nullptr) {
phdr.type = ginfo->type;
} else {
phdr.type = info->type;
}
set_key(phdr, info->key);
phdr.group_element = group_element;
// 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
ptrdiff_t base;
if (!get_base(*info, base)) {
return false;
}
if (ginfo != nullptr) {
// add in nested group offset
ptrdiff_t group_offset = 0;
for (uint8_t i=0; i<group_nesting.level; i++) {
group_offset += group_nesting.group_ret[i]->offset;
}
set_value((enum ap_var_type)phdr.type, (void*)(base + ginfo->offset + group_offset),
get_default_value(this, *ginfo));
} else {
set_value((enum ap_var_type)phdr.type, (void*)base,
get_default_value(this, *info));
}
return false;
}
if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {
// only vector3f can have non-zero idx for now
return false;
}
AP_Param *ap;
ap = this;
if (idx != 0) {
ap = (AP_Param *)((ptrdiff_t)ap) - (idx*sizeof(float));
}
// found it
_storage.read_block(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
return true;
}
bool AP_Param::configured_in_storage(void) const
{
uint32_t group_element = 0;
const struct GroupInfo *ginfo;
struct GroupNesting group_nesting {};
uint8_t idx;
const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
if (info == nullptr) {
// 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
if (ginfo != nullptr) {
phdr.type = ginfo->type;
} else {
phdr.type = info->type;
}
set_key(phdr, info->key);
phdr.group_element = group_element;
// scan EEPROM to find the right location
uint16_t ofs;
// only vector3f can have non-zero idx for now
return scan(&phdr, &ofs) && (phdr.type == AP_PARAM_VECTOR3F || idx == 0);
}
bool AP_Param::configured_in_defaults_file(bool &read_only) const
{
if (num_param_overrides == 0) {
return false;
}
uint32_t group_element = 0;
const struct GroupInfo *ginfo;
struct GroupNesting group_nesting {};
uint8_t idx;
const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
if (info == nullptr) {
// we don't have any info on how to load it
return false;
}
for (uint16_t i=0; i<num_param_overrides; i++) {
if (this == param_overrides[i].object_ptr) {
read_only = param_overrides[i].read_only;
return true;
}
}
return false;
}
bool AP_Param::configured(void) const
{
bool read_only;
return configured_in_defaults_file(read_only) || configured_in_storage();
}
bool AP_Param::is_read_only(void) const
{
if (num_read_only == 0) {
return false;
}
bool read_only;
if (configured_in_defaults_file(read_only)) {
return read_only;
}
return false;
}
// set a AP_Param variable to a specified value
void AP_Param::set_value(enum ap_var_type type, void *ptr, float value)
{
switch (type) {
case AP_PARAM_INT8:
((AP_Int8 *)ptr)->set(value);
break;
case AP_PARAM_INT16:
((AP_Int16 *)ptr)->set(value);
break;
case AP_PARAM_INT32:
((AP_Int32 *)ptr)->set(value);
break;
case AP_PARAM_FLOAT:
((AP_Float *)ptr)->set(value);
break;
default:
break;
}
}
// load default values for scalars in a group. This does not recurse
// into other objects. This is a static function that should be called
// in the objects constructor
void AP_Param::setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
{
ptrdiff_t base = (ptrdiff_t)object_pointer;
uint8_t type;
for (uint8_t i=0;
(type=group_info[i].type) != AP_PARAM_NONE;
i++) {
if (type <= AP_PARAM_FLOAT) {
void *ptr = (void *)(base + group_info[i].offset);
set_value((enum ap_var_type)type, ptr,
get_default_value((const AP_Param *)ptr, group_info[i]));
} else if (type == AP_PARAM_VECTOR3F) {
// Single default for all components
void *ptr = (void *)(base + group_info[i].offset);
const float default_val = get_default_value((const AP_Param *)ptr, group_info[i]);
((AP_Vector3f *)ptr)->set(Vector3f{default_val, default_val, default_val});
}
}
}
// set a value directly in an object. This should only be used by
// example code, not by mainline vehicle code
bool AP_Param::set_object_value(const void *object_pointer,
const struct GroupInfo *group_info,
const char *name, float value)
{
ptrdiff_t base = (ptrdiff_t)object_pointer;
uint8_t type;
bool found = false;
for (uint8_t i=0;
(type=group_info[i].type) != AP_PARAM_NONE;
i++) {
if (strcmp(name, group_info[i].name) == 0 && type <= AP_PARAM_FLOAT) {
void *ptr = (void *)(base + group_info[i].offset);
set_value((enum ap_var_type)type, ptr, value);
// return true here ?
found = true;
}
}
return found;
}
// load default values for all scalars in a sketch. This does not
// recurse into sub-objects
void AP_Param::setup_sketch_defaults(void)
{
setup();
for (uint16_t i=0; i<_num_vars; i++) {
const auto &info = var_info(i);
uint8_t type = info.type;
if (type <= AP_PARAM_FLOAT) {
ptrdiff_t base;
if (get_base(info, base)) {
set_value((enum ap_var_type)type, (void*)base,
get_default_value((const AP_Param *)base, info));
}
}
}
}
// Load all variables from EEPROM
//
bool AP_Param::load_all()
{
struct Param_header phdr;
uint16_t ofs = sizeof(AP_Param::EEPROM_header);
reload_defaults_file(false);
if (!registered_save_handler) {
registered_save_handler = true;
hal.scheduler->register_io_process(FUNCTOR_BIND((&save_dummy), &AP_Param::save_io_handler, void));
}
while (ofs < _storage.size()) {
_storage.read_block(&phdr, ofs, sizeof(phdr));
if (is_sentinal(phdr)) {
// we've reached the sentinal
sentinal_offset = ofs;
return true;
}
const struct AP_Param::Info *info;
void *ptr;
info = find_by_header(phdr, &ptr);
if (info != nullptr) {
_storage.read_block(ptr, 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
Debug("no sentinal in load_all");
return false;
}
/*
* reload from hal.util defaults file or embedded param region
* @last_pass: if this is the last pass on defaults - unknown parameters are
* ignored but if this is set a warning will be emitted
*/
void AP_Param::reload_defaults_file(bool last_pass)
{
#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
if (param_defaults_data.length != 0) {
load_embedded_param_defaults(last_pass);
return;
}
#endif
#if HAL_OS_POSIX_IO == 1
/*
if the HAL specifies a defaults parameter file then override
defaults using that file
*/
const char *default_file = hal.util->get_custom_defaults_file();
if (default_file) {
if (load_defaults_file(default_file, last_pass)) {
printf("Loaded defaults from %s\n", default_file);
} else {
AP_HAL::panic("Failed to load defaults from %s\n", default_file);
}
}
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
hal.util->set_cmdline_parameters();
#endif
}
/*
Load all variables from EEPROM for a particular object. This is
required for dynamically loaded objects
*/
void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct GroupInfo *group_info)
{
struct Param_header phdr;
uint16_t key;
if (!find_key_by_pointer(object_pointer, key)) {
DEV_PRINTF("ERROR: Unable to find param pointer\n");
return;
}
for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
if (group_info[i].type == AP_PARAM_GROUP) {
ptrdiff_t new_offset = 0;
if (!adjust_group_offset(key, group_info[i], new_offset)) {
continue;
}
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
if (ginfo != nullptr) {
load_object_from_eeprom((void *)(((ptrdiff_t)object_pointer)+new_offset), ginfo);
}
}
uint16_t ofs = sizeof(AP_Param::EEPROM_header);
while (ofs < _storage.size()) {
_storage.read_block(&phdr, ofs, sizeof(phdr));
// note that this is an || not an && for robustness
// against power off while adding a variable
if (is_sentinal(phdr)) {
// we've reached the sentinal
sentinal_offset = ofs;
break;
}
if (get_key(phdr) == key) {
const struct AP_Param::Info *info;
void *ptr;
info = find_by_header(phdr, &ptr);
if (info != nullptr) {
if ((ptrdiff_t)ptr == ((ptrdiff_t)object_pointer)+group_info[i].offset) {
_storage.read_block(ptr, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
break;
}
}
}
ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
}
}
// reset cached param counter as we may be loading a dynamic var_info
invalidate_count();
}
// return the first variable in _var_info
AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype, float *default_val)
{
token->key = 0;
token->group_element = 0;
token->idx = 0;
if (_num_vars == 0) {
return nullptr;
}
ptrdiff_t base;
if (!get_base(var_info(0), base)) {
// should be impossible, first var needs to be non-pointer
return nullptr;
}
if (ptype != nullptr) {
*ptype = (enum ap_var_type)var_info(0).type;
}
#if AP_PARAM_DEFAULTS_ENABLED
if (default_val != nullptr) {
*default_val = get_default_value((AP_Param *)base, var_info(0));
}
check_default((AP_Param *)base, default_val);
#endif
return (AP_Param *)base;
}
/// Returns the next variable in a group, recursing into groups
/// as needed
AP_Param *AP_Param::next_group(const uint16_t vindex, const struct GroupInfo *group_info,
bool *found_current,
const uint32_t group_base,
const uint8_t group_shift,
const ptrdiff_t group_offset,
ParamToken *token,
enum ap_var_type *ptype,
bool skip_disabled,
float *default_val)
{
enum ap_var_type type;
for (uint8_t i=0;
(type=(enum ap_var_type)group_info[i].type) != AP_PARAM_NONE;
i++) {
if (!check_frame_type(group_info[i].flags)) {
continue;
}
if (type == AP_PARAM_GROUP) {
// a nested group
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
if (ginfo == nullptr) {
continue;
}
AP_Param *ap;
ptrdiff_t new_offset = group_offset;
if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
continue;
}
ap = next_group(vindex, ginfo, found_current, group_id(group_info, group_base, i, group_shift),
group_shift + _group_level_shift, new_offset, token, ptype, skip_disabled, default_val);
if (ap != nullptr) {
return ap;
}
} else {
if (*found_current) {
// got a new one
token->key = vindex;
token->group_element = group_id(group_info, group_base, i, group_shift);
token->idx = 0;
if (ptype != nullptr) {
*ptype = type;
}
ptrdiff_t base;
if (!get_base(var_info(vindex), base)) {
continue;
}
AP_Param *ret = (AP_Param*)(base + group_info[i].offset + group_offset);
if (skip_disabled &&
_hide_disabled_groups &&
group_info[i].type == AP_PARAM_INT8 &&
(group_info[i].flags & AP_PARAM_FLAG_ENABLE) &&
((AP_Int8 *)ret)->get() == 0) {
token->last_disabled = 1;
}
#if AP_PARAM_DEFAULTS_ENABLED
if (default_val != nullptr) {
*default_val = get_default_value(ret, group_info[i]);
}
#endif
return ret;
}
if (group_id(group_info, group_base, i, group_shift) == token->group_element) {
*found_current = true;
if (token->last_disabled) {
token->last_disabled = 0;
return nullptr;
}
if (type == AP_PARAM_VECTOR3F && token->idx < 3) {
// return the next element of the vector as a
// float
token->idx++;
if (ptype != nullptr) {
*ptype = AP_PARAM_FLOAT;
}
ptrdiff_t base;
if (!get_base(var_info(vindex), base)) {
continue;
}
ptrdiff_t ofs = base + group_info[i].offset + group_offset;
ofs += sizeof(float)*(token->idx - 1u);
#if AP_PARAM_DEFAULTS_ENABLED
if (default_val != nullptr) {
*default_val = get_default_value((AP_Param *)ofs, group_info[i]);
}
#endif
return (AP_Param *)ofs;
}
}
}
}
return nullptr;
}
/// Returns the next variable in _var_info, recursing into groups
/// as needed
AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_disabled, float *default_val)
{
uint16_t i = token->key;
bool found_current = false;
if (i >= _num_vars) {
// illegal token
return nullptr;
}
enum ap_var_type type = (enum ap_var_type)var_info(i).type;
// allow Vector3f to be seen as 3 variables. First as a vector,
// then as 3 separate floats
if (type == AP_PARAM_VECTOR3F && token->idx < 3) {
token->idx++;
if (ptype != nullptr) {
*ptype = AP_PARAM_FLOAT;
}
AP_Param *ret = (AP_Param *)(((token->idx - 1u)*sizeof(float))+(ptrdiff_t)var_info(i).ptr);
#if AP_PARAM_DEFAULTS_ENABLED
if (default_val != nullptr) {
*default_val = get_default_value(ret, var_info(i));
}
#endif
return ret;
}
if (type != AP_PARAM_GROUP) {
i++;
found_current = true;
}
for (; i<_num_vars; i++) {
const auto &info = var_info(i);
if (!check_frame_type(info.flags)) {
continue;
}
type = (enum ap_var_type)info.type;
if (type == AP_PARAM_GROUP) {
const struct GroupInfo *group_info = get_group_info(info);
if (group_info == nullptr) {
continue;
}
AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, 0, token, ptype, skip_disabled, default_val);
if (ap != nullptr) {
return ap;
}
} else {
// found the next one
token->key = i;
token->group_element = 0;
token->idx = 0;
if (ptype != nullptr) {
*ptype = type;
}
#if AP_PARAM_DEFAULTS_ENABLED
if (default_val != nullptr) {
*default_val = get_default_value((AP_Param *)info.ptr, info);
}
#endif
return (AP_Param *)(info.ptr);
}
}
return nullptr;
}
/// Returns the next scalar in _var_info, recursing into groups
/// as needed
AP_Param *AP_Param::next_scalar(ParamToken *token, enum ap_var_type *ptype, float *default_val)
{
AP_Param *ap;
enum ap_var_type type;
while ((ap = next(token, &type, true, default_val)) != nullptr && type > AP_PARAM_FLOAT) ;
if (ap != nullptr) {
if (ptype != nullptr) {
*ptype = type;
}
}
#if AP_PARAM_DEFAULTS_ENABLED
check_default(ap, default_val);
#endif
return ap;
}
/// cast a variable to a float given its type
float AP_Param::cast_to_float(enum ap_var_type type) const
{
switch (type) {
case AP_PARAM_INT8:
return ((AP_Int8 *)this)->cast_to_float();
case AP_PARAM_INT16:
return ((AP_Int16 *)this)->cast_to_float();
case AP_PARAM_INT32:
return ((AP_Int32 *)this)->cast_to_float();
case AP_PARAM_FLOAT:
return ((AP_Float *)this)->cast_to_float();
default:
return NAN;
}
}
/*
find an old parameter and return it.
*/
bool AP_Param::find_old_parameter(const struct ConversionInfo *info, AP_Param *value)
{
// find the old value in EEPROM.
uint16_t pofs;
AP_Param::Param_header header;
header.type = info->type;
set_key(header, info->old_key);
header.group_element = info->old_group_element;
if (!scan(&header, &pofs)) {
// the old parameter isn't saved in the EEPROM.
return false;
}
// load the old value from EEPROM
_storage.read_block(value, pofs+sizeof(header), type_size((enum ap_var_type)header.type));
return true;
}
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
// convert one old vehicle parameter to new object parameter
void AP_Param::convert_old_parameter(const struct ConversionInfo *info, float scaler, uint8_t flags)
{
uint8_t old_value[type_size(info->type)];
AP_Param *ap = (AP_Param *)&old_value[0];
if (!find_old_parameter(info, ap)) {
// the old parameter isn't saved in the EEPROM. It was
// probably still set to the default value, which isn't stored
// no need to convert
return;
}
// find the new variable in the variable structures
enum ap_var_type ptype;
AP_Param *ap2;
ap2 = find(&info->new_name[0], &ptype);
if (ap2 == nullptr) {
DEV_PRINTF("Unknown conversion '%s'\n", info->new_name);
return;
}
// see if we can load it from EEPROM
if (!(flags & CONVERT_FLAG_FORCE) && ap2->configured_in_storage()) {
// the new parameter already has a value set by the user, or
// has already been converted
return;
}
// see if they are the same type and no scaling applied
if (ptype == info->type && is_equal(scaler, 1.0f) && flags == 0) {
// copy the value over only if the new parameter does not already
// have the old value (via a default).
if (memcmp(ap2, ap, sizeof(old_value)) != 0) {
memcpy(ap2, ap, sizeof(old_value));
// and save
ap2->save();
}
} else if (ptype <= AP_PARAM_FLOAT && info->type <= AP_PARAM_FLOAT) {
// perform scalar->scalar conversion
float v = ap->cast_to_float(info->type);
if (flags & CONVERT_FLAG_REVERSE) {
// convert a _REV parameter to a _REVERSED parameter
v = is_equal(v, -1.0f)?1:0;
}
if (!is_equal(v,ap2->cast_to_float(ptype))) {
// the value needs to change
set_value(ptype, ap2, v * scaler);
ap2->save();
}
} else {
// can't do vector<->scalar conversion, or different vector types
DEV_PRINTF("Bad conversion type '%s'\n", info->new_name);
}
}
#pragma GCC diagnostic pop
// convert old vehicle parameters to new object parametersv
void AP_Param::convert_old_parameters(const struct ConversionInfo *conversion_table, uint8_t table_size, uint8_t flags)
{
for (uint8_t i=0; i<table_size; i++) {
convert_old_parameter(&conversion_table[i], 1.0f, flags);
}
// we need to flush here to prevent a later set_default_by_name()
// causing a save to be done on a converted parameter
flush();
}
// move all parameters from a class to a new location
// is_top_level: Is true if the class had its own top level key, param_key. It is false if the class was a subgroup
void AP_Param::convert_class(uint16_t param_key, void *object_pointer,
const struct AP_Param::GroupInfo *group_info,
uint16_t old_index, uint16_t old_top_element, bool is_top_level)
{
const uint8_t group_shift = is_top_level ? 0 : 6;
for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
struct ConversionInfo info;
info.old_key = param_key;
info.type = (ap_var_type)group_info[i].type;
info.new_name = nullptr;
uint16_t idx = group_info[i].idx;
if (group_shift != 0 && idx == 0) {
// Note: Index 0 is treated as 63 for group bit shifting purposes. See group_id()
idx = 63;
}
info.old_group_element = (idx << group_shift) + old_index;
uint8_t old_value[type_size(info.type)];
AP_Param *ap = (AP_Param *)&old_value[0];
if (!AP_Param::find_old_parameter(&info, ap)) {
// the parameter wasn't set in the old eeprom
continue;
}
AP_Param *ap2 = (AP_Param *)(group_info[i].offset + (uint8_t *)object_pointer);
if (ap2->configured_in_storage()) {
// user has already set a value, or previous conversion was done
continue;
}
memcpy(ap2, ap, sizeof(old_value));
// and save
ap2->save();
}
// we need to flush here to prevent a later set_default_by_name()
// causing a save to be done on a converted parameter
flush();
}
/*
convert width of a parameter, allowing update to wider scalar values
without changing the parameter indexes
*/
bool AP_Param::convert_parameter_width(ap_var_type old_ptype)
{
if (configured_in_storage()) {
// already converted or set by the user
return false;
}
uint32_t group_element = 0;
const struct GroupInfo *ginfo;
struct GroupNesting group_nesting {};
uint8_t idx;
const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
if (info == nullptr) {
return false;
}
// remember the type
ap_var_type new_ptype;
if (ginfo != nullptr) {
new_ptype = (ap_var_type)ginfo->type;
} else {
new_ptype = (ap_var_type)info->type;
}
// create the header we will use to scan for the variable
struct Param_header phdr;
phdr.type = old_ptype;
set_key(phdr, info->key);
phdr.group_element = group_element;
// scan EEPROM to find the right location
uint16_t pofs;
if (!scan(&phdr, &pofs)) {
// it isn't in storage
return false;
}
// load the old value from EEPROM
uint8_t old_value[type_size(old_ptype)];
_storage.read_block(old_value, pofs+sizeof(phdr), sizeof(old_value));
AP_Param *old_ap = (AP_Param *)&old_value[0];
// going via float is safe as the only time we would be converting
// from AP_Int32 is when converting to float
float old_float_value = old_ap->cast_to_float(old_ptype);
set_value(new_ptype, this, old_float_value);
// force save as the new type
save(true);
return true;
}
/*
set a parameter to a float value
*/
void AP_Param::set_float(float value, enum ap_var_type var_type)
{
if (isnan(value) || isinf(value)) {
return;
}
// add a small amount before casting parameter values
// from float to integer to avoid truncating to the
// next lower integer value.
float rounding_addition = 0.01f;
// handle variables with standard type IDs
if (var_type == AP_PARAM_FLOAT) {
((AP_Float *)this)->set(value);
} else if (var_type == AP_PARAM_INT32) {
if (value < 0) rounding_addition = -rounding_addition;
float v = value+rounding_addition;
v = constrain_float(v, INT32_MIN, INT32_MAX);
((AP_Int32 *)this)->set(v);
} else if (var_type == AP_PARAM_INT16) {
if (value < 0) rounding_addition = -rounding_addition;
float v = value+rounding_addition;
v = constrain_float(v, INT16_MIN, INT16_MAX);
((AP_Int16 *)this)->set(v);
} else if (var_type == AP_PARAM_INT8) {
if (value < 0) rounding_addition = -rounding_addition;
float v = value+rounding_addition;
v = constrain_float(v, INT8_MIN, INT8_MAX);
((AP_Int8 *)this)->set(v);
}
}
/*
parse a parameter file line
*/
bool AP_Param::parse_param_line(char *line, char **vname, float &value, bool &read_only)
{
if (line[0] == '#') {
return false;
}
char *saveptr = nullptr;
/*
note that we need the \r\n as delimiters to prevent us getting
strings with line termination in the results
*/
char *pname = strtok_r(line, ", =\t\r\n", &saveptr);
if (pname == nullptr) {
return false;
}
if (strlen(pname) > AP_MAX_NAME_SIZE) {
return false;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// Workaround to prevent FORMAT_VERSION in param file resulting in invalid
// EEPROM. For details, see: https://github.com/ArduPilot/ardupilot/issues/15579
if (strcmp(pname, "FORMAT_VERSION") == 0) {
::printf("Warning: Ignoring FORMAT_VERSION in param file\n");
return false;
}
#endif
const char *value_s = strtok_r(nullptr, ", =\t\r\n", &saveptr);
if (value_s == nullptr) {
return false;
}
value = strtof(value_s, NULL);
*vname = pname;
const char *flags_s = strtok_r(nullptr, ", =\t\r\n", &saveptr);
if (flags_s && strcmp(flags_s, "@READONLY") == 0) {
read_only = true;
} else {
read_only = false;
}
return true;
}
#if HAVE_FILESYSTEM_SUPPORT
// increments num_defaults for each default found in filename
bool AP_Param::count_defaults_in_file(const char *filename, uint16_t &num_defaults)
{
// try opening the file both in the posix filesystem and using AP::FS
int file_apfs = AP::FS().open(filename, O_RDONLY, true);
if (file_apfs == -1) {
return false;
}
char line[100];
/*
work out how many parameter default structures to allocate
*/
while (AP::FS().fgets(line, sizeof(line)-1, file_apfs)) {
char *pname;
float value;
bool read_only;
if (!parse_param_line(line, &pname, value, read_only)) {
continue;
}
enum ap_var_type var_type;
if (!find(pname, &var_type)) {
continue;
}
num_defaults++;
}
AP::FS().close(file_apfs);
return true;
}
bool AP_Param::read_param_defaults_file(const char *filename, bool last_pass, uint16_t &idx)
{
// try opening the file both in the posix filesystem and using AP::FS
int file_apfs = AP::FS().open(filename, O_RDONLY, true);
if (file_apfs == -1) {
AP_HAL::panic("AP_Param: Failed to re-open defaults file");
return false;
}
char line[100];
while (AP::FS().fgets(line, sizeof(line)-1, file_apfs)) {
char *pname;
float value;
bool read_only;
if (!parse_param_line(line, &pname, value, read_only)) {
continue;
}
enum ap_var_type var_type;
AP_Param *vp = find(pname, &var_type);
if (!vp) {
if (last_pass) {
#if ENABLE_DEBUG
::printf("Ignored unknown param %s in defaults file %s\n",
pname, filename);
hal.console->printf(
"Ignored unknown param %s in defaults file %s\n",
pname, filename);
#endif
}
continue;
}
if (idx >= param_overrides_len) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
}
param_overrides[idx].object_ptr = vp;
param_overrides[idx].value = value;
param_overrides[idx].read_only = read_only;
if (read_only) {
num_read_only++;
}
idx++;
if (!vp->configured_in_storage()) {
vp->set_float(value, var_type);
}
}
AP::FS().close(file_apfs);
return true;
}
/*
load a default set of parameters from a file
*/
bool AP_Param::load_defaults_file(const char *filename, bool last_pass)
{
if (filename == nullptr) {
return false;
}
char *mutable_filename = strdup(filename);
if (mutable_filename == nullptr) {
AP_HAL::panic("AP_Param: Failed to allocate mutable string");
}
uint16_t num_defaults = 0;
char *saveptr = nullptr;
for (char *pname = strtok_r(mutable_filename, ",", &saveptr);
pname != nullptr;
pname = strtok_r(nullptr, ",", &saveptr)) {
if (!count_defaults_in_file(pname, num_defaults)) {
free(mutable_filename);
return false;
}
}
free(mutable_filename);
delete[] param_overrides;
param_overrides_len = 0;
num_param_overrides = 0;
num_read_only = 0;
param_overrides = new param_override[num_defaults];
if (param_overrides == nullptr) {
AP_HAL::panic("AP_Param: Failed to allocate overrides");
return false;
}
param_overrides_len = num_defaults;
if (num_defaults == 0) {
return true;
}
saveptr = nullptr;
mutable_filename = strdup(filename);
if (mutable_filename == nullptr) {
AP_HAL::panic("AP_Param: Failed to allocate mutable string");
}
uint16_t idx = 0;
for (char *pname = strtok_r(mutable_filename, ",", &saveptr);
pname != nullptr;
pname = strtok_r(nullptr, ",", &saveptr)) {
if (!read_param_defaults_file(pname, last_pass, idx)) {
free(mutable_filename);
return false;
}
}
free(mutable_filename);
num_param_overrides = num_defaults;
return true;
}
#endif // HAVE_FILESYSTEM_SUPPORT
#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
/*
count the number of embedded parameter defaults
*/
bool AP_Param::count_embedded_param_defaults(uint16_t &count)
{
const volatile char *ptr = param_defaults_data.data;
int32_t length = param_defaults_data.length;
count = 0;
while (length>0) {
char line[100];
char *pname;
float value;
bool read_only;
uint16_t i;
uint16_t n = length;
for (i=0;i<n;i++) {
if (ptr[i] == '\n') {
break;
}
}
uint16_t linelen = MIN(i,sizeof(line)-1);
memcpy(line, (void *)ptr, linelen);
line[linelen] = 0;
length -= i+1;
ptr += i+1;
if (line[0] == '#' || line[0] == 0) {
continue;
}
if (!parse_param_line(line, &pname, value, read_only)) {
continue;
}
enum ap_var_type var_type;
if (!find(pname, &var_type)) {
continue;
}
count++;
}
return true;
}
/*
* load a default set of parameters from a embedded parameter region
* @last_pass: if this is the last pass on defaults - unknown parameters are
* ignored but if this is set a warning will be emitted
*/
void AP_Param::load_embedded_param_defaults(bool last_pass)
{
delete[] param_overrides;
param_overrides = nullptr;
param_overrides_len = 0;
num_param_overrides = 0;
num_read_only = 0;
uint16_t num_defaults = 0;
if (!count_embedded_param_defaults(num_defaults)) {
return;
}
param_overrides = new param_override[num_defaults];
if (param_overrides == nullptr) {
AP_HAL::panic("AP_Param: Failed to allocate overrides");
return;
}
param_overrides_len = num_defaults;
const volatile char *ptr = param_defaults_data.data;
int32_t length = param_defaults_data.length;
uint16_t idx = 0;
while (idx < num_defaults && length > 0) {
char line[100];
char *pname;
float value;
bool read_only;
uint16_t i;
uint16_t n = length;
for (i=0;i<n;i++) {
if (ptr[i] == '\n') {
break;
}
}
uint16_t linelen = MIN(i,sizeof(line)-1);
memcpy(line, (void *)ptr, linelen);
line[linelen] = 0;
length -= i+1;
ptr += i+1;
if (line[0] == '#' || line[0] == 0) {
continue;
}
if (!parse_param_line(line, &pname, value, read_only)) {
continue;
}
enum ap_var_type var_type;
AP_Param *vp = find(pname, &var_type);
if (!vp) {
if (last_pass) {
#if ENABLE_DEBUG
::printf("Ignored unknown param %s from embedded region (offset=%u)\n",
pname, unsigned(ptr - param_defaults_data.data));
hal.console->printf(
"Ignored unknown param %s from embedded region (offset=%u)\n",
pname, unsigned(ptr - param_defaults_data.data));
#endif
}
continue;
}
param_overrides[idx].object_ptr = vp;
param_overrides[idx].value = value;
param_overrides[idx].read_only = read_only;
if (read_only) {
num_read_only++;
}
idx++;
if (!vp->configured_in_storage()) {
vp->set_float(value, var_type);
}
}
num_param_overrides = num_defaults;
}
#endif // AP_PARAM_MAX_EMBEDDED_PARAM > 0
/*
find a default value given a pointer to a default value in flash
*/
float AP_Param::get_default_value(const AP_Param *vp, const struct GroupInfo &info)
{
for (uint16_t i=0; i<num_param_overrides; i++) {
if (vp == param_overrides[i].object_ptr) {
return param_overrides[i].value;
}
}
if ((info.flags & AP_PARAM_FLAG_DEFAULT_POINTER) != 0) {
return *((float*)((ptrdiff_t)vp - info.def_value_offset));
}
return info.def_value;
}
float AP_Param::get_default_value(const AP_Param *vp, const struct Info &info)
{
for (uint16_t i=0; i<num_param_overrides; i++) {
if (vp == param_overrides[i].object_ptr) {
return param_overrides[i].value;
}
}
if ((info.flags & AP_PARAM_FLAG_DEFAULT_POINTER) != 0) {
return *((float*)((ptrdiff_t)vp - info.def_value_offset));
}
return info.def_value;
}
void AP_Param::send_parameter(const char *name, enum ap_var_type var_type, uint8_t idx) const
{
if (idx != 0 && var_type == AP_PARAM_VECTOR3F) {
var_type = AP_PARAM_FLOAT;
}
if (var_type > AP_PARAM_VECTOR3F) {
// invalid
return;
}
if (var_type != AP_PARAM_VECTOR3F) {
// nice and simple for scalar types
GCS_SEND_PARAM(name, var_type, cast_to_float(var_type));
return;
}
// for vectors we need to send 3 messages. Note that we also come here for the case
// of a set of the first element of a AP_Vector3f. This happens as the ap->save() call can't
// distinguish between a vector and scalar save. It means that setting first element of a vector
// via MAVLink results in sending all 3 elements to the GCS
#if HAL_GCS_ENABLED
const Vector3f &v = ((AP_Vector3f *)this)->get();
char name2[AP_MAX_NAME_SIZE+1];
strncpy(name2, name, AP_MAX_NAME_SIZE);
name2[AP_MAX_NAME_SIZE] = 0;
char &name_axis = name2[strlen(name)-1];
name_axis = 'X';
GCS_SEND_PARAM(name2, AP_PARAM_FLOAT, v.x);
name_axis = 'Y';
GCS_SEND_PARAM(name2, AP_PARAM_FLOAT, v.y);
name_axis = 'Z';
GCS_SEND_PARAM(name2, AP_PARAM_FLOAT, v.z);
#endif // HAL_GCS_ENABLED
}
/*
return count of all scalar parameters.
Note that this function may be called from the IO thread, so needs
to be thread safe
*/
uint16_t AP_Param::count_parameters(void)
{
// if we haven't cached the parameter count yet...
WITH_SEMAPHORE(_count_sem);
if (_parameter_count != 0 &&
_count_marker == _count_marker_done) {
return _parameter_count;
}
/*
cope with another thread invalidating the count while we are
counting
*/
uint8_t limit = 4;
while ((_parameter_count == 0 ||
_count_marker != _count_marker_done) &&
limit--) {
AP_Param *vp;
AP_Param::ParamToken token {};
uint16_t count = 0;
uint16_t marker = _count_marker;
for (vp = AP_Param::first(&token, nullptr);
vp != nullptr;
vp = AP_Param::next_scalar(&token, nullptr)) {
count++;
}
_parameter_count = count;
_count_marker_done = marker;
}
return _parameter_count;
}
/*
invalidate parameter count cache
*/
void AP_Param::invalidate_count(void)
{
// we don't take the semaphore here as we don't want to block. The
// not-equal test is strong enough to ensure we get the right
// answer
_count_marker++;
}
/*
set a default value by name
*/
bool AP_Param::set_default_by_name(const char *name, float value)
{
enum ap_var_type vtype;
AP_Param *vp = find(name, &vtype);
if (vp == nullptr) {
return false;
}
switch (vtype) {
case AP_PARAM_INT8:
((AP_Int8 *)vp)->set_default(value);
return true;
case AP_PARAM_INT16:
((AP_Int16 *)vp)->set_default(value);
return true;
case AP_PARAM_INT32:
((AP_Int32 *)vp)->set_default(value);
return true;
case AP_PARAM_FLOAT:
((AP_Float *)vp)->set_default(value);
return true;
default:
break;
}
// not a supported type
return false;
}
/*
set parameter defaults from a defaults_struct table
sends GCS message and panics (in SITL only) if parameter is not found
*/
void AP_Param::set_defaults_from_table(const struct defaults_table_struct *table, uint8_t count)
{
for (uint8_t i=0; i<count; i++) {
if (!AP_Param::set_default_by_name(table[i].name, table[i].value)) {
AP_BoardConfig::config_error("param deflt fail:%s", table[i].name);
}
}
}
/*
set a value by name
*/
bool AP_Param::set_by_name(const char *name, float value)
{
enum ap_var_type vtype;
AP_Param *vp = find(name, &vtype);
if (vp == nullptr) {
return false;
}
switch (vtype) {
case AP_PARAM_INT8:
((AP_Int8 *)vp)->set(value);
return true;
case AP_PARAM_INT16:
((AP_Int16 *)vp)->set(value);
return true;
case AP_PARAM_INT32:
((AP_Int32 *)vp)->set(value);
return true;
case AP_PARAM_FLOAT:
((AP_Float *)vp)->set(value);
return true;
default:
break;
}
// not a supported type
return false;
}
/*
get a value by name
*/
bool AP_Param::get(const char *name, float &value)
{
enum ap_var_type vtype;
AP_Param *vp = find(name, &vtype);
if (vp == nullptr) {
return false;
}
switch (vtype) {
case AP_PARAM_INT8:
value = ((AP_Int8 *)vp)->get();
break;
case AP_PARAM_INT16:
value = ((AP_Int16 *)vp)->get();
break;
case AP_PARAM_INT32:
value = ((AP_Int32 *)vp)->get();
break;
case AP_PARAM_FLOAT:
value = ((AP_Float *)vp)->get();
break;
default:
// not a supported type
return false;
}
return true;
}
/*
set and save a value by name
*/
bool AP_Param::set_and_save_by_name(const char *name, float value)
{
enum ap_var_type vtype;
AP_Param *vp = find(name, &vtype);
if (vp == nullptr) {
return false;
}
switch (vtype) {
case AP_PARAM_INT8:
((AP_Int8 *)vp)->set_and_save(value);
return true;
case AP_PARAM_INT16:
((AP_Int16 *)vp)->set_and_save(value);
return true;
case AP_PARAM_INT32:
((AP_Int32 *)vp)->set_and_save(value);
return true;
case AP_PARAM_FLOAT:
((AP_Float *)vp)->set_and_save(value);
return true;
default:
break;
}
// not a supported type
return false;
}
/*
set and save a value by name
*/
bool AP_Param::set_and_save_by_name_ifchanged(const char *name, float value)
{
enum ap_var_type vtype;
AP_Param *vp = find(name, &vtype);
if (vp == nullptr) {
return false;
}
switch (vtype) {
case AP_PARAM_INT8:
((AP_Int8 *)vp)->set_and_save_ifchanged(value);
return true;
case AP_PARAM_INT16:
((AP_Int16 *)vp)->set_and_save_ifchanged(value);
return true;
case AP_PARAM_INT32:
((AP_Int32 *)vp)->set_and_save_ifchanged(value);
return true;
case AP_PARAM_FLOAT:
((AP_Float *)vp)->set_and_save_ifchanged(value);
return true;
default:
break;
}
// not a supported type
return false;
}
#if AP_PARAM_DEFAULTS_ENABLED
void AP_Param::check_default(AP_Param *ap, float *default_value)
{
if (default_value == nullptr || ap == nullptr) {
return;
}
if (default_list != nullptr) {
for (defaults_list *item = default_list; item; item = item->next) {
if (item->ap == ap) {
*default_value = item->val;
return;
}
}
}
}
void AP_Param::add_default(AP_Param *ap, float v)
{
// Embedded defaults trump runtime, don't allow override
for (uint16_t i=0; i<num_param_overrides; i++) {
if (ap == param_overrides[i].object_ptr) {
return;
}
}
if (default_list != nullptr) {
// check is param is already in list
for (defaults_list *item = default_list; item; item = item->next) {
// update existing entry
if (item->ap == ap) {
item->val = v;
return;
}
}
}
// add to list
defaults_list *new_item = new defaults_list;
if (new_item == nullptr) {
return;
}
new_item->ap = ap;
new_item->val = v;
new_item->next = default_list;
default_list = new_item;
}
#endif // AP_PARAM_DEFAULTS_ENABLED
#if AP_PARAM_KEY_DUMP
/*
do not remove this show_all() code, it is essential for debugging
and creating conversion tables
*/
// print the value of all variables
void AP_Param::show(const AP_Param *ap, const char *s,
enum ap_var_type type, AP_HAL::BetterStream *port)
{
switch (type) {
case AP_PARAM_INT8:
::printf("%s: %d\n", s, (int)((AP_Int8 *)ap)->get());
break;
case AP_PARAM_INT16:
::printf("%s: %d\n", s, (int)((AP_Int16 *)ap)->get());
break;
case AP_PARAM_INT32:
::printf("%s: %ld\n", s, (long)((AP_Int32 *)ap)->get());
break;
case AP_PARAM_FLOAT:
::printf("%s: %f\n", s, (double)((AP_Float *)ap)->get());
break;
default:
break;
}
}
// print the value of all variables
void AP_Param::show(const AP_Param *ap, const ParamToken &token,
enum ap_var_type type, AP_HAL::BetterStream *port)
{
char s[AP_MAX_NAME_SIZE+1];
ap->copy_name_token(token, s, sizeof(s), true);
s[AP_MAX_NAME_SIZE] = 0;
show(ap, s, type, port);
}
// print the value of all variables
void AP_Param::show_all(AP_HAL::BetterStream *port, bool showKeyValues)
{
ParamToken token;
AP_Param *ap;
enum ap_var_type type;
float default_value = nanf("0x4152"); // from logger quiet_nanf
for (ap=AP_Param::first(&token, &type, &default_value);
ap;
ap=AP_Param::next_scalar(&token, &type, &default_value)) {
if (showKeyValues) {
::printf("Key %u: Index %u: GroupElement %u : Default %f :", (unsigned)var_info(token.key).key, (unsigned)token.idx, (unsigned)token.group_element, default_value);
default_value = nanf("0x4152");
}
show(ap, token, type, port);
hal.scheduler->delay(1);
}
#if AP_PARAM_DEFAULTS_ENABLED
uint16_t list_len = 0;
if (default_list != nullptr) {
for (defaults_list *item = default_list; item; item = item->next) {
list_len++;
}
}
::printf("Defaults list containts %i params (%li bytes)\n", list_len, list_len*sizeof(defaults_list));
#endif
}
#endif // AP_PARAM_KEY_DUMP
#if AP_PARAM_DYNAMIC_ENABLED
/*
allow for dynamically added parameter tables from scripts
The layout we create is as follows:
- a top level Info with the given prefix, using one of the 10 possible slots in _var_info_dynamic
- a dynamically allocated GroupInfo table, never freed, of size (num_params+2)
- the GroupInfo table has an initial AP_Int32 hidden entry with a 32 bit CRC of the prefix
- the last GroupInfo is a footer
*/
bool AP_Param::add_table(uint8_t _key, const char *prefix, uint8_t num_params)
{
// check if the key already exists. We only check base params to allow
// for scripting reload without a conflict
uint16_t key = uint16_t(_key) + AP_PARAM_DYNAMIC_KEY_BASE;
for (uint16_t i=0; i<_num_vars_base; i++) {
if (var_info(i).key == key) {
return false;
}
}
if (num_params > 63) {
return false;
}
// we use a crc of the prefix to ensure the table key isn't re-used
const int32_t crc = int32_t(crc32_small(0, (const uint8_t *)prefix, strlen(prefix)));
int32_t current_crc;
if (load_int32(key, 0, current_crc) && current_crc != crc) {
// crc mismatch, we have a conflict with an existing use of this key
return false;
}
// create the dynamic table if needed. This is never freed
if (_var_info_dynamic == nullptr) {
_var_info_dynamic = (Info *)calloc(AP_PARAM_MAX_DYNAMIC, sizeof(struct Info));
if (_var_info_dynamic == nullptr) {
return false;
}
for (uint8_t i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
auto &info = _var_info_dynamic[i];
info.type = AP_PARAM_NONE;
info.name = _empty_string;
info.key = 0xFFFF;
info.ptr = nullptr;
info.group_info = nullptr;
info.flags = 0;
}
// make tables available
_num_vars += AP_PARAM_MAX_DYNAMIC;
}
// find existing key (allows for script reload)
uint8_t i;
for (i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
auto &info = _var_info_dynamic[i];
if (info.type != AP_PARAM_NONE && info.key == key) {
if (_dynamic_table_sizes[i] != 0 &&
num_params > _dynamic_table_sizes[i]) {
// can't expand the table at runtime
return false;
}
if (strcmp(prefix, info.name) != 0) {
// prefix has changed, reject as two scripts running
// with the same key
return false;
}
break;
}
}
if (i == AP_PARAM_MAX_DYNAMIC) {
// find an unused slot
for (i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
auto &info = _var_info_dynamic[i];
if (info.type == AP_PARAM_NONE ) {
break;
}
}
}
if (i == AP_PARAM_MAX_DYNAMIC) {
// no empty slots
return false;
}
auto &info = _var_info_dynamic[i];
// create memory for the array of floats if needed
// first float is used for the crc
if (info.ptr == nullptr) {
info.ptr = calloc(num_params+1, sizeof(float));
if (info.ptr == nullptr) {
return false;
}
}
// allocate the name
if (info.name == _empty_string) {
info.name = strdup(prefix);
if (info.name == nullptr) {
free(const_cast<void*>(info.ptr));
info.ptr = nullptr;
info.name = _empty_string;
return false;
}
}
// if it doesn't exist then create the table
if (info.group_info == nullptr) {
info.group_info = (GroupInfo *)calloc(num_params+2, sizeof(GroupInfo));
if (info.group_info == nullptr) {
free(const_cast<void*>(info.ptr));
free(const_cast<char*>(info.name));
info.ptr = nullptr;
info.name = _empty_string;
return false;
}
// fill in footer for all entries
for (uint8_t gi=1; gi<num_params+2; gi++) {
auto &ginfo = const_cast<GroupInfo*>(info.group_info)[gi];
ginfo.name = _empty_string;
ginfo.idx = 0xff;
}
// hidden first parameter containing AP_Int32 crc
auto &hinfo = const_cast<GroupInfo*>(info.group_info)[0];
hinfo.flags = AP_PARAM_FLAG_HIDDEN;
hinfo.name = _empty_string;
hinfo.idx = 0;
hinfo.offset = 0;
hinfo.type = AP_PARAM_INT32;
// fill in default value with the CRC. Relies on sizeof crc == sizeof float
memcpy((uint8_t *)&hinfo.def_value, (const uint8_t *)&crc, sizeof(crc));
}
// remember the table size
if (_dynamic_table_sizes[i] == 0) {
_dynamic_table_sizes[i] = num_params;
}
// make the group active
info.key = key;
info.type = AP_PARAM_GROUP;
invalidate_count();
// save the CRC
AP_Int32 *crc_param = const_cast<AP_Int32 *>((AP_Int32 *)info.ptr);
crc_param->set(crc);
crc_param->save(true);
return true;
}
/*
Load an AP_Int32 variable from EEPROM using top level key and group element. Used to confirm
a key in add_table()
*/
bool AP_Param::load_int32(uint16_t key, uint32_t group_element, int32_t &value)
{
struct Param_header phdr;
phdr.type = AP_PARAM_INT32;
set_key(phdr, key);
phdr.group_element = group_element;
// scan EEPROM to find the right location
uint16_t ofs;
if (!scan(&phdr, &ofs)) {
return false;
}
// found it
_storage.read_block(&value, ofs+sizeof(phdr), type_size(AP_PARAM_INT32));
return true;
}
/*
add a parameter to a dynamic table
*/
bool AP_Param::add_param(uint8_t _key, uint8_t param_num, const char *pname, float default_value)
{
if (_var_info_dynamic == nullptr) {
// No dynamic tables available
return false;
}
// check for valid values
if (param_num == 0 || param_num > 63 || strlen(pname) > 16) {
return false;
}
uint16_t key = uint16_t(_key) + AP_PARAM_DYNAMIC_KEY_BASE;
// find the info
uint8_t i;
for (i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
auto &info = _var_info_dynamic[i];
if (info.key == key) {
break;
}
}
if (i == AP_PARAM_MAX_DYNAMIC) {
// not found
return false;
}
if (param_num > _dynamic_table_sizes[i]) {
return false;
}
auto &info = _var_info_dynamic[i];
if (info.ptr == nullptr) {
return false;
}
// check CRC
auto &hinfo = const_cast<GroupInfo*>(info.group_info)[0];
const int32_t crc = *(const int32_t *)(&hinfo.def_value);
int32_t current_crc;
if (load_int32(key, 0, current_crc) && current_crc != crc) {
// crc mismatch, we have a conflict with an existing use of this key
return false;
}
// fill in idx of any gaps, leaving them hidden, this allows
// scripts to remove parameters
for (uint8_t j=1; j<param_num; j++) {
auto &g = const_cast<GroupInfo*>(info.group_info)[j];
if (g.idx == 0xff) {
g.idx = j;
g.flags = AP_PARAM_FLAG_HIDDEN;
g.offset = j*sizeof(float);
g.type = AP_PARAM_FLOAT;
}
}
auto &ginfo = const_cast<GroupInfo*>(info.group_info)[param_num];
if (ginfo.name == _empty_string) {
// we don't allow name change while running
ginfo.name = strdup(pname);
if (ginfo.name == nullptr) {
ginfo.name = _empty_string;
return false;
}
}
ginfo.offset = param_num*sizeof(float);
ginfo.idx = param_num;
float *def_value = const_cast<float *>(&ginfo.def_value);
*def_value = default_value;
ginfo.type = AP_PARAM_FLOAT;
invalidate_count();
// load from storage if available
AP_Float *pvalues = const_cast<AP_Float *>((const AP_Float *)info.ptr);
AP_Float &p = pvalues[param_num];
p.set_default(default_value);
p.load();
return true;
}
#endif
Reference in New Issue View Git Blame Copy Permalink