2013-08-29 02:34:34 -03:00
|
|
|
/*
|
|
|
|
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/>.
|
|
|
|
*/
|
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
//
|
|
|
|
//
|
|
|
|
|
2012-02-12 03:22:57 -04:00
|
|
|
// total up and check overflow
|
|
|
|
// check size of group var_info
|
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
/// @file AP_Param.cpp
|
|
|
|
/// @brief The AP variable store.
|
2015-12-23 13:25:02 -04:00
|
|
|
#include "AP_Param.h"
|
2012-02-11 07:51:35 -04:00
|
|
|
|
2016-03-31 18:43:36 -03:00
|
|
|
#include <cmath>
|
2015-12-23 13:25:02 -04:00
|
|
|
#include <string.h>
|
2012-02-11 07:51:35 -04:00
|
|
|
|
2015-08-11 03:28:45 -03:00
|
|
|
#include <AP_Common/AP_Common.h>
|
2015-10-26 09:01:52 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2015-08-11 03:28:45 -03:00
|
|
|
#include <AP_Math/AP_Math.h>
|
2015-12-23 13:25:02 -04:00
|
|
|
#include <GCS_MAVLink/GCS.h>
|
2015-08-16 14:53:24 -03:00
|
|
|
#include <StorageManager/StorageManager.h>
|
2016-06-30 01:16:13 -03:00
|
|
|
#include <stdio.h>
|
2012-02-11 07:51:35 -04:00
|
|
|
|
2012-09-18 15:08:38 -03:00
|
|
|
extern const AP_HAL::HAL &hal;
|
|
|
|
|
2016-01-02 05:23:24 -04:00
|
|
|
#define ENABLE_DEBUG 1
|
2012-02-11 07:51:35 -04:00
|
|
|
|
2014-11-09 17:09:08 -04:00
|
|
|
#if ENABLE_DEBUG
|
2017-01-13 09:17:47 -04:00
|
|
|
# define Debug(fmt, args ...) do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
|
2012-02-11 07:51:35 -04:00
|
|
|
#else
|
2014-11-09 17:09:08 -04:00
|
|
|
# define Debug(fmt, args ...)
|
2012-02-11 07:51:35 -04:00
|
|
|
#endif
|
|
|
|
|
2012-02-24 20:37:15 -04:00
|
|
|
// 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
|
2012-02-12 18:54:46 -04:00
|
|
|
|
2012-02-24 20:37:15 -04:00
|
|
|
|
|
|
|
// static member variables for AP_Param.
|
2012-02-11 07:51:35 -04:00
|
|
|
//
|
|
|
|
|
|
|
|
// number of rows in the _var_info[] table
|
2015-12-30 17:27:22 -04:00
|
|
|
uint16_t AP_Param::_num_vars;
|
2012-02-11 07:51:35 -04:00
|
|
|
|
2016-01-05 23:41:07 -04:00
|
|
|
// cached parameter count
|
|
|
|
uint16_t AP_Param::_parameter_count;
|
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// storage and naming information about all types that can be saved
|
|
|
|
const AP_Param::Info *AP_Param::_var_info;
|
|
|
|
|
2016-10-30 02:24:21 -03:00
|
|
|
struct AP_Param::param_override *AP_Param::param_overrides = nullptr;
|
2015-04-01 22:24:45 -03:00
|
|
|
uint16_t AP_Param::num_param_overrides = 0;
|
|
|
|
|
2017-11-09 02:45:58 -04:00
|
|
|
/*
|
|
|
|
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
|
|
|
|
};
|
|
|
|
|
2014-08-13 01:43:19 -03:00
|
|
|
// storage object
|
|
|
|
StorageAccess AP_Param::_storage(StorageManager::StorageParam);
|
|
|
|
|
2017-02-07 19:45:20 -04:00
|
|
|
// flags indicating frame type
|
|
|
|
uint16_t AP_Param::_frame_type_flags;
|
2014-08-13 01:43:19 -03:00
|
|
|
|
2013-06-04 01:02:49 -03:00
|
|
|
// write to EEPROM
|
2012-02-11 07:51:35 -04:00
|
|
|
void AP_Param::eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size)
|
|
|
|
{
|
2014-08-13 01:43:19 -03:00
|
|
|
_storage.write_block(ofs, ptr, size);
|
2012-02-11 07:51:35 -04:00
|
|
|
}
|
|
|
|
|
2016-10-25 18:32:10 -03:00
|
|
|
bool AP_Param::_hide_disabled_groups = true;
|
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// write a sentinal value at the given offset
|
|
|
|
void AP_Param::write_sentinal(uint16_t ofs)
|
|
|
|
{
|
|
|
|
struct Param_header phdr;
|
2012-02-12 18:54:46 -04:00
|
|
|
phdr.type = _sentinal_type;
|
2015-12-30 17:27:22 -04:00
|
|
|
set_key(phdr, _sentinal_key);
|
2012-02-12 18:54:46 -04:00
|
|
|
phdr.group_element = _sentinal_group;
|
2012-02-11 07:51:35 -04:00
|
|
|
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;
|
|
|
|
|
|
|
|
// write the header
|
2012-02-12 19:12:02 -04:00
|
|
|
hdr.magic[0] = k_EEPROM_magic0;
|
|
|
|
hdr.magic[1] = k_EEPROM_magic1;
|
2012-02-11 07:51:35 -04:00
|
|
|
hdr.revision = k_EEPROM_revision;
|
2012-02-12 19:12:02 -04:00
|
|
|
hdr.spare = 0;
|
2012-02-11 07:51:35 -04:00
|
|
|
eeprom_write_check(&hdr, 0, sizeof(hdr));
|
|
|
|
|
|
|
|
// add a sentinal directly after the header
|
|
|
|
write_sentinal(sizeof(struct EEPROM_header));
|
|
|
|
}
|
|
|
|
|
2016-02-15 03:37:40 -04:00
|
|
|
/* 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.
|
|
|
|
*/
|
2016-10-22 04:10:11 -03:00
|
|
|
uint32_t AP_Param::group_id(const struct GroupInfo *grpinfo, uint32_t base, uint8_t i, uint8_t shift)
|
2016-02-15 03:37:40 -04:00
|
|
|
{
|
2016-03-17 22:33:16 -03:00
|
|
|
if (grpinfo[i].idx == 0 && shift != 0 && !(grpinfo[i].flags & AP_PARAM_NO_SHIFT)) {
|
2016-02-15 03:37:40 -04:00
|
|
|
/*
|
|
|
|
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);
|
|
|
|
}
|
2017-02-07 19:45:20 -04:00
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
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)
|
|
|
|
{
|
|
|
|
uint16_t frame_flags = flags >> AP_PARAM_FRAME_TYPE_SHIFT;
|
|
|
|
if (frame_flags == 0) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return (frame_flags & _frame_type_flags) != 0;
|
|
|
|
}
|
|
|
|
|
2012-02-12 03:22:57 -04:00
|
|
|
// validate a group info table
|
2012-08-17 03:18:11 -03:00
|
|
|
bool AP_Param::check_group_info(const struct AP_Param::GroupInfo * group_info,
|
|
|
|
uint16_t * total_size,
|
2014-11-09 17:09:08 -04:00
|
|
|
uint8_t group_shift,
|
|
|
|
uint8_t prefix_length)
|
2012-02-12 03:22:57 -04:00
|
|
|
{
|
|
|
|
uint8_t type;
|
2016-01-02 05:28:24 -04:00
|
|
|
uint64_t used_mask = 0;
|
2015-12-27 16:22:32 -04:00
|
|
|
for (uint8_t i=0;
|
|
|
|
(type=group_info[i].type) != AP_PARAM_NONE;
|
|
|
|
i++) {
|
2016-01-02 05:28:24 -04:00
|
|
|
uint8_t idx = group_info[i].idx;
|
|
|
|
if (idx >= (1<<_group_level_shift)) {
|
|
|
|
Debug("idx too large (%u) in %s", idx, group_info[i].name);
|
|
|
|
return false;
|
|
|
|
}
|
2016-02-15 03:37:40 -04:00
|
|
|
if (group_shift != 0 && idx == 0) {
|
|
|
|
// great idx 0 as 63 for duplicates. See group_id()
|
|
|
|
idx = 63;
|
|
|
|
}
|
2017-07-15 03:46:43 -03:00
|
|
|
if (used_mask & (1ULL<<idx)) {
|
|
|
|
Debug("Duplicate group idx %u for %s", idx, group_info[i].name);
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
used_mask |= (1ULL<<idx);
|
2012-02-12 03:22:57 -04:00
|
|
|
if (type == AP_PARAM_GROUP) {
|
|
|
|
// a nested group
|
|
|
|
if (group_shift + _group_level_shift >= _group_bits) {
|
2015-10-29 13:07:08 -03:00
|
|
|
Debug("double group nesting in %s", group_info[i].name);
|
2012-02-12 03:22:57 -04:00
|
|
|
return false;
|
|
|
|
}
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
|
|
|
|
if (ginfo == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
if (!check_group_info(ginfo, total_size, group_shift + _group_level_shift, prefix_length + strlen(group_info[i].name))) {
|
2012-02-12 03:22:57 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
uint8_t size = type_size((enum ap_var_type)type);
|
|
|
|
if (size == 0) {
|
2015-10-29 13:07:08 -03:00
|
|
|
Debug("invalid type in %s", group_info[i].name);
|
2014-11-09 17:09:08 -04:00
|
|
|
return false;
|
|
|
|
}
|
2015-10-25 13:23:25 -03:00
|
|
|
if (prefix_length + strlen(group_info[i].name) > 16) {
|
2015-10-29 13:07:08 -03:00
|
|
|
Debug("suffix is too long in %s", group_info[i].name);
|
2012-02-12 03:22:57 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
(*total_size) += size + sizeof(struct Param_header);
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2012-03-02 00:45:18 -04:00
|
|
|
// check for duplicate key values
|
2015-12-30 17:27:22 -04:00
|
|
|
bool AP_Param::duplicate_key(uint16_t vindex, uint16_t key)
|
2012-03-02 00:45:18 -04:00
|
|
|
{
|
2015-12-30 17:27:22 -04:00
|
|
|
for (uint16_t i=vindex+1; i<_num_vars; i++) {
|
|
|
|
uint16_t key2 = _var_info[i].key;
|
2012-03-02 00:45:18 -04:00
|
|
|
if (key2 == key) {
|
|
|
|
// no duplicate keys allowed
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2017-02-27 02:39:12 -04:00
|
|
|
/*
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2012-02-12 03:22:57 -04:00
|
|
|
// validate the _var_info[] table
|
|
|
|
bool AP_Param::check_var_info(void)
|
|
|
|
{
|
|
|
|
uint16_t total_size = sizeof(struct EEPROM_header);
|
|
|
|
|
2015-12-30 17:27:22 -04:00
|
|
|
for (uint16_t i=0; i<_num_vars; i++) {
|
2015-12-23 13:25:02 -04:00
|
|
|
uint8_t type = _var_info[i].type;
|
2015-12-30 17:27:22 -04:00
|
|
|
uint16_t key = _var_info[i].key;
|
2012-02-12 03:22:57 -04:00
|
|
|
if (type == AP_PARAM_GROUP) {
|
|
|
|
if (i == 0) {
|
|
|
|
// first element can't be a group, for first() call
|
|
|
|
return false;
|
|
|
|
}
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *group_info = get_group_info(_var_info[i]);
|
|
|
|
if (group_info == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
if (!check_group_info(group_info, &total_size, 0, strlen(_var_info[i].name))) {
|
2012-02-12 03:22:57 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
uint8_t size = type_size((enum ap_var_type)type);
|
|
|
|
if (size == 0) {
|
2012-02-12 16:43:09 -04:00
|
|
|
// not a valid type - the top level list can't contain
|
2012-02-12 18:54:46 -04:00
|
|
|
// AP_PARAM_NONE
|
2012-02-12 03:22:57 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
total_size += size + sizeof(struct Param_header);
|
|
|
|
}
|
2012-03-02 00:45:18 -04:00
|
|
|
if (duplicate_key(i, key)) {
|
|
|
|
return false;
|
|
|
|
}
|
2016-04-01 02:16:03 -03:00
|
|
|
if (type != AP_PARAM_GROUP && (_var_info[i].flags & AP_PARAM_FLAG_POINTER)) {
|
|
|
|
// only groups can be pointers
|
|
|
|
return false;
|
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
2012-08-06 22:00:43 -03:00
|
|
|
|
|
|
|
// 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
|
|
|
|
|
2012-02-12 03:22:57 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// setup the _var_info[] table
|
2012-12-13 18:58:23 -04:00
|
|
|
bool AP_Param::setup(void)
|
2012-02-11 07:51:35 -04:00
|
|
|
{
|
|
|
|
struct EEPROM_header hdr;
|
2012-02-12 03:22:57 -04:00
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// check the header
|
2014-08-13 01:43:19 -03:00
|
|
|
_storage.read_block(&hdr, 0, sizeof(hdr));
|
2012-02-12 19:12:02 -04:00
|
|
|
if (hdr.magic[0] != k_EEPROM_magic0 ||
|
|
|
|
hdr.magic[1] != k_EEPROM_magic1 ||
|
2012-02-11 07:51:35 -04:00
|
|
|
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
|
2014-11-09 17:09:08 -04:00
|
|
|
Debug("bad header in setup - erasing");
|
2012-02-11 07:51:35 -04:00
|
|
|
erase_all();
|
|
|
|
}
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2012-02-17 00:12:48 -04:00
|
|
|
// check if AP_Param has been initialised
|
|
|
|
bool AP_Param::initialised(void)
|
|
|
|
{
|
2016-10-30 02:24:21 -03:00
|
|
|
return _var_info != nullptr;
|
2012-02-17 00:12:48 -04:00
|
|
|
}
|
|
|
|
|
2015-12-29 06:21:54 -04:00
|
|
|
/*
|
|
|
|
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
|
|
|
|
*/
|
2015-12-30 17:27:22 -04:00
|
|
|
bool AP_Param::adjust_group_offset(uint16_t vindex, const struct GroupInfo &group_info, ptrdiff_t &new_offset)
|
2015-12-29 06:21:54 -04:00
|
|
|
{
|
|
|
|
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
|
2016-04-01 02:16:03 -03:00
|
|
|
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);
|
2015-12-29 06:21:54 -04:00
|
|
|
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
|
2016-04-01 02:16:03 -03:00
|
|
|
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);
|
2015-12-29 06:21:54 -04:00
|
|
|
}
|
2016-04-01 02:16:03 -03:00
|
|
|
base = (ptrdiff_t)info.ptr;
|
2015-12-29 06:21:54 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2016-04-01 02:16:03 -03:00
|
|
|
|
2012-02-12 03:22:57 -04:00
|
|
|
// 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,
|
2015-12-30 17:27:22 -04:00
|
|
|
uint16_t vindex,
|
2012-02-12 03:22:57 -04:00
|
|
|
const struct GroupInfo *group_info,
|
2016-10-22 04:10:11 -03:00
|
|
|
uint32_t group_base,
|
2015-12-27 16:22:32 -04:00
|
|
|
uint8_t group_shift,
|
2015-12-29 06:21:54 -04:00
|
|
|
ptrdiff_t group_offset)
|
2012-02-12 03:22:57 -04:00
|
|
|
{
|
|
|
|
uint8_t type;
|
2015-12-27 16:22:32 -04:00
|
|
|
for (uint8_t i=0;
|
|
|
|
(type=group_info[i].type) != AP_PARAM_NONE;
|
|
|
|
i++) {
|
2012-02-12 03:22:57 -04:00
|
|
|
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() !
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
|
|
|
|
if (ginfo == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
2015-12-29 06:21:54 -04:00
|
|
|
ptrdiff_t new_offset = group_offset;
|
|
|
|
|
|
|
|
if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
|
|
|
|
continue;
|
2015-12-27 16:22:32 -04:00
|
|
|
}
|
2015-12-29 06:21:54 -04:00
|
|
|
|
2012-02-12 03:22:57 -04:00
|
|
|
const struct AP_Param::Info *ret = find_by_header_group(phdr, ptr, vindex, ginfo,
|
2016-02-15 03:37:40 -04:00
|
|
|
group_id(group_info, group_base, i, group_shift),
|
2015-12-27 16:22:32 -04:00
|
|
|
group_shift + _group_level_shift, new_offset);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ret != nullptr) {
|
2012-02-12 03:22:57 -04:00
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
continue;
|
|
|
|
}
|
2016-02-15 03:37:40 -04:00
|
|
|
if (group_id(group_info, group_base, i, group_shift) == phdr.group_element && type == phdr.type) {
|
2012-02-12 03:22:57 -04:00
|
|
|
// found a group element
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[vindex], base)) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
*ptr = (void*)(base + group_info[i].offset + group_offset);
|
2012-02-12 03:22:57 -04:00
|
|
|
return &_var_info[vindex];
|
|
|
|
}
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// 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
|
2015-12-30 17:27:22 -04:00
|
|
|
for (uint16_t i=0; i<_num_vars; i++) {
|
2015-12-23 13:25:02 -04:00
|
|
|
uint8_t type = _var_info[i].type;
|
2015-12-30 17:27:22 -04:00
|
|
|
uint16_t key = _var_info[i].key;
|
|
|
|
if (key != get_key(phdr)) {
|
2012-02-11 07:51:35 -04:00
|
|
|
// not the right key
|
|
|
|
continue;
|
|
|
|
}
|
2015-05-27 02:29:40 -03:00
|
|
|
if (type == AP_PARAM_GROUP) {
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *group_info = get_group_info(_var_info[i]);
|
|
|
|
if (group_info == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
2015-12-27 16:22:32 -04:00
|
|
|
return find_by_header_group(phdr, ptr, i, group_info, 0, 0, 0);
|
2015-05-27 02:29:40 -03:00
|
|
|
}
|
|
|
|
if (type == phdr.type) {
|
|
|
|
// found it
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[i], base)) {
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2016-04-01 02:16:03 -03:00
|
|
|
}
|
|
|
|
*ptr = (void*)base;
|
2012-02-11 07:51:35 -04:00
|
|
|
return &_var_info[i];
|
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// find the info structure for a variable in a group
|
2012-08-17 03:18:11 -03:00
|
|
|
const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInfo * group_info,
|
2015-12-30 17:27:22 -04:00
|
|
|
uint16_t vindex,
|
2016-02-15 03:37:40 -04:00
|
|
|
uint32_t group_base,
|
2012-08-17 03:18:11 -03:00
|
|
|
uint8_t group_shift,
|
2015-12-29 06:21:54 -04:00
|
|
|
ptrdiff_t group_offset,
|
2012-11-05 07:08:43 -04:00
|
|
|
uint32_t * group_element,
|
2015-12-27 16:22:32 -04:00
|
|
|
const struct GroupInfo * &group_ret,
|
2016-02-15 03:37:40 -04:00
|
|
|
struct GroupNesting &group_nesting,
|
2013-04-19 04:46:40 -03:00
|
|
|
uint8_t * idx) const
|
2012-02-12 03:22:57 -04:00
|
|
|
{
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[vindex], base)) {
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2016-04-01 02:16:03 -03:00
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
uint8_t type;
|
2015-12-27 16:22:32 -04:00
|
|
|
for (uint8_t i=0;
|
|
|
|
(type=group_info[i].type) != AP_PARAM_NONE;
|
|
|
|
i++) {
|
2015-12-29 06:21:54 -04:00
|
|
|
ptrdiff_t ofs = group_info[i].offset + group_offset;
|
2012-02-12 03:22:57 -04:00
|
|
|
if (type == AP_PARAM_GROUP) {
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
|
|
|
|
if (ginfo == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
// a nested group
|
|
|
|
if (group_shift + _group_level_shift >= _group_bits) {
|
|
|
|
// too deeply nested - this should have been caught by
|
|
|
|
// setup() !
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
|
|
|
const struct AP_Param::Info *info;
|
2015-12-29 06:21:54 -04:00
|
|
|
ptrdiff_t new_offset = group_offset;
|
|
|
|
if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
|
|
|
|
continue;
|
2015-12-27 16:22:32 -04:00
|
|
|
}
|
2016-02-15 03:37:40 -04:00
|
|
|
if (group_nesting.level >= group_nesting.numlevels) {
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2016-02-15 03:37:40 -04:00
|
|
|
}
|
|
|
|
group_nesting.group_ret[group_nesting.level++] = &group_info[i];
|
2012-02-12 03:22:57 -04:00
|
|
|
info = find_var_info_group(ginfo, vindex,
|
2016-02-15 03:37:40 -04:00
|
|
|
group_id(group_info, group_base, i, group_shift),
|
2012-02-12 03:22:57 -04:00
|
|
|
group_shift + _group_level_shift,
|
2015-12-27 16:22:32 -04:00
|
|
|
new_offset,
|
2012-02-12 03:22:57 -04:00
|
|
|
group_element,
|
2012-02-23 23:19:00 -04:00
|
|
|
group_ret,
|
2016-02-15 03:37:40 -04:00
|
|
|
group_nesting,
|
2012-02-23 23:19:00 -04:00
|
|
|
idx);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (info != nullptr) {
|
2012-02-12 03:22:57 -04:00
|
|
|
return info;
|
2012-02-11 07:51:35 -04:00
|
|
|
}
|
2016-02-15 03:37:40 -04:00
|
|
|
group_nesting.level--;
|
2015-12-29 06:21:54 -04:00
|
|
|
} else if ((ptrdiff_t) this == base + ofs) {
|
2016-02-15 03:37:40 -04:00
|
|
|
*group_element = group_id(group_info, group_base, i, group_shift);
|
2015-12-27 16:22:32 -04:00
|
|
|
group_ret = &group_info[i];
|
2012-02-23 23:19:00 -04:00
|
|
|
*idx = 0;
|
|
|
|
return &_var_info[vindex];
|
|
|
|
} else if (type == AP_PARAM_VECTOR3F &&
|
2015-12-29 06:21:54 -04:00
|
|
|
(base+ofs+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
|
|
|
|
base+ofs+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
|
2012-02-24 20:37:15 -04:00
|
|
|
// we are inside a Vector3f. We need to work out which
|
|
|
|
// element of the vector the current object refers to.
|
2015-12-29 06:21:54 -04:00
|
|
|
*idx = (((ptrdiff_t) this) - (base+ofs))/sizeof(float);
|
2016-02-15 03:37:40 -04:00
|
|
|
*group_element = group_id(group_info, group_base, i, group_shift);
|
2015-12-27 16:22:32 -04:00
|
|
|
group_ret = &group_info[i];
|
2012-02-12 03:22:57 -04:00
|
|
|
return &_var_info[vindex];
|
2012-02-11 07:51:35 -04:00
|
|
|
}
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-11 07:51:35 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// find the info structure for a variable
|
2012-11-05 07:08:43 -04:00
|
|
|
const struct AP_Param::Info *AP_Param::find_var_info(uint32_t * group_element,
|
2015-12-27 16:22:32 -04:00
|
|
|
const struct GroupInfo * &group_ret,
|
2016-02-15 03:37:40 -04:00
|
|
|
struct GroupNesting &group_nesting,
|
2015-10-06 19:08:59 -03:00
|
|
|
uint8_t * idx) const
|
2012-02-11 07:51:35 -04:00
|
|
|
{
|
2016-10-30 02:24:21 -03:00
|
|
|
group_ret = nullptr;
|
2015-12-27 16:22:32 -04:00
|
|
|
|
2015-12-30 17:27:22 -04:00
|
|
|
for (uint16_t i=0; i<_num_vars; i++) {
|
2015-12-23 13:25:02 -04:00
|
|
|
uint8_t type = _var_info[i].type;
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[i], base)) {
|
|
|
|
continue;
|
|
|
|
}
|
2012-02-11 07:51:35 -04:00
|
|
|
if (type == AP_PARAM_GROUP) {
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *group_info = get_group_info(_var_info[i]);
|
|
|
|
if (group_info == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
const struct AP_Param::Info *info;
|
2016-02-15 03:37:40 -04:00
|
|
|
info = find_var_info_group(group_info, i, 0, 0, 0, group_element, group_ret, group_nesting, idx);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (info != nullptr) {
|
2012-02-12 03:22:57 -04:00
|
|
|
return info;
|
2012-02-11 07:51:35 -04:00
|
|
|
}
|
2015-12-29 06:21:54 -04:00
|
|
|
} else if (base == (ptrdiff_t) this) {
|
2012-02-23 23:19:00 -04:00
|
|
|
*group_element = 0;
|
|
|
|
*idx = 0;
|
|
|
|
return &_var_info[i];
|
|
|
|
} else if (type == AP_PARAM_VECTOR3F &&
|
2015-12-29 06:21:54 -04:00
|
|
|
(base+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
|
|
|
|
base+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
|
2012-02-24 20:37:15 -04:00
|
|
|
// we are inside a Vector3f. Work out which element we are
|
|
|
|
// referring to.
|
2015-12-29 06:21:54 -04:00
|
|
|
*idx = (((ptrdiff_t) this) - base)/sizeof(float);
|
2012-02-11 07:51:35 -04:00
|
|
|
*group_element = 0;
|
|
|
|
return &_var_info[i];
|
|
|
|
}
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-11 07:51:35 -04:00
|
|
|
}
|
|
|
|
|
2013-01-08 00:28:16 -04:00
|
|
|
|
|
|
|
// find the info structure for a variable
|
2013-04-19 04:46:40 -03:00
|
|
|
const struct AP_Param::Info *AP_Param::find_var_info_token(const ParamToken &token,
|
2013-01-08 00:28:16 -04:00
|
|
|
uint32_t * group_element,
|
2015-12-27 16:22:32 -04:00
|
|
|
const struct GroupInfo * &group_ret,
|
2016-02-15 03:37:40 -04:00
|
|
|
struct GroupNesting &group_nesting,
|
2013-04-19 04:46:40 -03:00
|
|
|
uint8_t * idx) const
|
2013-01-08 00:28:16 -04:00
|
|
|
{
|
2015-12-30 17:27:22 -04:00
|
|
|
uint16_t i = token.key;
|
2015-12-23 13:25:02 -04:00
|
|
|
uint8_t type = _var_info[i].type;
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[i], base)) {
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2016-04-01 02:16:03 -03:00
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
group_ret = nullptr;
|
2015-12-27 16:22:32 -04:00
|
|
|
|
2013-01-08 00:28:16 -04:00
|
|
|
if (type == AP_PARAM_GROUP) {
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *group_info = get_group_info(_var_info[i]);
|
|
|
|
if (group_info == nullptr) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
2013-01-08 00:28:16 -04:00
|
|
|
const struct AP_Param::Info *info;
|
2016-02-15 03:37:40 -04:00
|
|
|
info = find_var_info_group(group_info, i, 0, 0, 0, group_element, group_ret, group_nesting, idx);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (info != nullptr) {
|
2013-01-08 00:28:16 -04:00
|
|
|
return info;
|
|
|
|
}
|
2015-12-29 06:21:54 -04:00
|
|
|
} else if (base == (ptrdiff_t) this) {
|
2013-01-08 00:28:16 -04:00
|
|
|
*group_element = 0;
|
|
|
|
*idx = 0;
|
|
|
|
return &_var_info[i];
|
|
|
|
} else if (type == AP_PARAM_VECTOR3F &&
|
2015-12-29 06:21:54 -04:00
|
|
|
(base+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
|
|
|
|
base+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
|
2013-01-08 00:28:16 -04:00
|
|
|
// we are inside a Vector3f. Work out which element we are
|
|
|
|
// referring to.
|
2015-12-29 06:21:54 -04:00
|
|
|
*idx = (((ptrdiff_t) this) - base)/sizeof(float);
|
2013-01-08 00:28:16 -04:00
|
|
|
*group_element = 0;
|
|
|
|
return &_var_info[i];
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2013-01-08 00:28:16 -04:00
|
|
|
}
|
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// return the storage size for a AP_PARAM_* type
|
2013-01-01 21:51:25 -04:00
|
|
|
uint8_t AP_Param::type_size(enum ap_var_type type)
|
2012-02-11 07:51:35 -04:00
|
|
|
{
|
|
|
|
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;
|
|
|
|
}
|
2014-11-09 17:09:08 -04:00
|
|
|
Debug("unknown type %u\n", type);
|
2012-02-11 07:51:35 -04:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2015-12-30 17:27:22 -04:00
|
|
|
/*
|
|
|
|
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 &&, 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 ||
|
|
|
|
phdr.group_element == _sentinal_group) {
|
|
|
|
return true;
|
|
|
|
}
|
2017-08-17 04:23:36 -03:00
|
|
|
// 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;
|
|
|
|
}
|
2015-12-30 17:27:22 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// 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
|
2013-05-24 11:30:22 -03:00
|
|
|
// if not found return the offset of the sentinal
|
2013-07-09 04:53:04 -03:00
|
|
|
// if the sentinal isn't found either, the offset is set to 0xFFFF
|
2012-02-11 07:51:35 -04:00
|
|
|
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);
|
2014-08-13 01:43:19 -03:00
|
|
|
while (ofs < _storage.size()) {
|
|
|
|
_storage.read_block(&phdr, ofs, sizeof(phdr));
|
2012-02-11 07:51:35 -04:00
|
|
|
if (phdr.type == target->type &&
|
2015-12-30 17:27:22 -04:00
|
|
|
get_key(phdr) == get_key(*target) &&
|
2012-02-11 07:51:35 -04:00
|
|
|
phdr.group_element == target->group_element) {
|
|
|
|
// found it
|
|
|
|
*pofs = ofs;
|
|
|
|
return true;
|
|
|
|
}
|
2015-12-30 17:27:22 -04:00
|
|
|
if (is_sentinal(phdr)) {
|
2012-02-11 07:51:35 -04:00
|
|
|
// we've reached the sentinal
|
|
|
|
*pofs = ofs;
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
|
|
|
|
}
|
2013-07-09 04:53:04 -03:00
|
|
|
*pofs = 0xffff;
|
2014-11-09 17:09:08 -04:00
|
|
|
Debug("scan past end of eeprom");
|
2012-02-11 07:51:35 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2013-05-24 11:30:22 -03:00
|
|
|
/**
|
|
|
|
* 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)
|
|
|
|
*/
|
2013-04-19 04:46:40 -03:00
|
|
|
void AP_Param::add_vector3f_suffix(char *buffer, size_t buffer_size, uint8_t idx) const
|
2012-02-23 23:19:00 -04:00
|
|
|
{
|
2013-05-24 11:30:22 -03:00
|
|
|
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;
|
|
|
|
}
|
2012-11-20 07:27:11 -04:00
|
|
|
}
|
2012-02-23 23:19:00 -04:00
|
|
|
}
|
|
|
|
|
2013-01-08 00:28:16 -04:00
|
|
|
// Copy the variable's whole name to the supplied buffer.
|
|
|
|
//
|
|
|
|
// If the variable is a group member, prepend the group name.
|
|
|
|
//
|
2013-04-19 04:46:40 -03:00
|
|
|
void AP_Param::copy_name_token(const ParamToken &token, char *buffer, size_t buffer_size, bool force_scalar) const
|
2013-01-08 00:28:16 -04:00
|
|
|
{
|
|
|
|
uint32_t group_element;
|
|
|
|
const struct GroupInfo *ginfo;
|
2016-02-15 03:37:40 -04:00
|
|
|
struct GroupNesting group_nesting {};
|
2013-01-08 00:28:16 -04:00
|
|
|
uint8_t idx;
|
2016-02-15 03:37:40 -04:00
|
|
|
const struct AP_Param::Info *info = find_var_info_token(token, &group_element, ginfo, group_nesting, &idx);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (info == nullptr) {
|
2013-01-08 00:28:16 -04:00
|
|
|
*buffer = 0;
|
2014-11-09 17:09:08 -04:00
|
|
|
Debug("no info found");
|
2013-01-08 00:28:16 -04:00
|
|
|
return;
|
|
|
|
}
|
2016-02-15 03:37:40 -04:00
|
|
|
copy_name_info(info, ginfo, group_nesting, idx, buffer, buffer_size, force_scalar);
|
2015-10-06 19:08:59 -03:00
|
|
|
}
|
|
|
|
|
2015-12-27 16:22:32 -04:00
|
|
|
void AP_Param::copy_name_info(const struct AP_Param::Info *info,
|
|
|
|
const struct GroupInfo *ginfo,
|
2016-02-15 03:37:40 -04:00
|
|
|
const struct GroupNesting &group_nesting,
|
2015-12-27 16:22:32 -04:00
|
|
|
uint8_t idx, char *buffer, size_t buffer_size, bool force_scalar) const
|
2015-10-06 19:08:59 -03:00
|
|
|
{
|
2015-10-25 13:23:25 -03:00
|
|
|
strncpy(buffer, info->name, buffer_size);
|
2016-02-15 03:37:40 -04:00
|
|
|
for (uint8_t i=0; i<group_nesting.level; i++) {
|
2015-12-27 16:22:32 -04:00
|
|
|
uint8_t len = strnlen(buffer, buffer_size);
|
|
|
|
if (len < buffer_size) {
|
2016-02-15 03:37:40 -04:00
|
|
|
strncpy(&buffer[len], group_nesting.group_ret[i]->name, buffer_size-len);
|
2015-12-27 16:22:32 -04:00
|
|
|
}
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ginfo != nullptr) {
|
2013-01-08 00:28:16 -04:00
|
|
|
uint8_t len = strnlen(buffer, buffer_size);
|
|
|
|
if (len < buffer_size) {
|
2015-10-25 13:23:25 -03:00
|
|
|
strncpy(&buffer[len], ginfo->name, buffer_size-len);
|
2013-01-08 00:28:16 -04:00
|
|
|
}
|
2015-12-23 13:25:02 -04:00
|
|
|
if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == ginfo->type) {
|
2013-01-08 00:28:16 -04:00
|
|
|
// the caller wants a specific element in a Vector3f
|
|
|
|
add_vector3f_suffix(buffer, buffer_size, idx);
|
2012-02-23 23:19:00 -04:00
|
|
|
}
|
2015-12-23 13:25:02 -04:00
|
|
|
} else if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == info->type) {
|
2012-08-17 03:18:11 -03:00
|
|
|
add_vector3f_suffix(buffer, buffer_size, idx);
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// Find a variable by name in a group
|
|
|
|
AP_Param *
|
2015-12-30 17:27:22 -04:00
|
|
|
AP_Param::find_group(const char *name, uint16_t vindex, ptrdiff_t group_offset,
|
2015-12-29 06:21:54 -04:00
|
|
|
const struct GroupInfo *group_info, enum ap_var_type *ptype)
|
2012-02-12 03:22:57 -04:00
|
|
|
{
|
|
|
|
uint8_t type;
|
2015-12-27 16:22:32 -04:00
|
|
|
for (uint8_t i=0;
|
|
|
|
(type=group_info[i].type) != AP_PARAM_NONE;
|
|
|
|
i++) {
|
2012-02-12 03:22:57 -04:00
|
|
|
if (type == AP_PARAM_GROUP) {
|
2015-12-27 16:22:32 -04:00
|
|
|
if (strncasecmp(name, group_info[i].name, strlen(group_info[i].name)) != 0) {
|
|
|
|
continue;
|
|
|
|
}
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
|
|
|
|
if (ginfo == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
2015-12-29 06:21:54 -04:00
|
|
|
ptrdiff_t new_offset = group_offset;
|
|
|
|
|
|
|
|
if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
|
|
|
|
continue;
|
2015-12-27 16:22:32 -04:00
|
|
|
}
|
2015-12-29 06:21:54 -04:00
|
|
|
|
2015-12-27 16:22:32 -04:00
|
|
|
AP_Param *ap = find_group(name+strlen(group_info[i].name), vindex, new_offset, ginfo, ptype);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ap != nullptr) {
|
2012-02-12 03:22:57 -04:00
|
|
|
return ap;
|
|
|
|
}
|
2015-12-27 16:22:32 -04:00
|
|
|
} else if (strcasecmp(name, group_info[i].name) == 0) {
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[vindex], base)) {
|
|
|
|
continue;
|
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
*ptype = (enum ap_var_type)type;
|
2016-04-01 02:16:03 -03:00
|
|
|
return (AP_Param *)(base + group_info[i].offset + group_offset);
|
2012-02-23 23:19:00 -04:00
|
|
|
} else if (type == AP_PARAM_VECTOR3F) {
|
|
|
|
// special case for finding Vector3f elements
|
2015-10-25 13:23:25 -03:00
|
|
|
uint8_t suffix_len = strnlen(group_info[i].name, AP_MAX_NAME_SIZE);
|
2015-10-25 13:23:25 -03:00
|
|
|
if (strncmp(name, group_info[i].name, suffix_len) == 0 &&
|
2012-02-23 23:19:00 -04:00
|
|
|
name[suffix_len] == '_' &&
|
2012-11-20 07:27:11 -04:00
|
|
|
(name[suffix_len+1] == 'X' ||
|
|
|
|
name[suffix_len+1] == 'Y' ||
|
|
|
|
name[suffix_len+1] == 'Z')) {
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[vindex], base)) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
AP_Float *v = (AP_Float *)(base + group_info[i].offset + group_offset);
|
2012-02-23 23:19:00 -04:00
|
|
|
*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];
|
|
|
|
}
|
|
|
|
}
|
2012-02-11 07:51:35 -04:00
|
|
|
}
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-11 07:51:35 -04:00
|
|
|
}
|
|
|
|
|
2012-02-12 03:22:57 -04:00
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// Find a variable by name.
|
|
|
|
//
|
|
|
|
AP_Param *
|
2012-02-12 03:22:57 -04:00
|
|
|
AP_Param::find(const char *name, enum ap_var_type *ptype)
|
2012-02-11 07:51:35 -04:00
|
|
|
{
|
2015-12-30 17:27:22 -04:00
|
|
|
for (uint16_t i=0; i<_num_vars; i++) {
|
2015-12-23 13:25:02 -04:00
|
|
|
uint8_t type = _var_info[i].type;
|
2012-02-11 07:51:35 -04:00
|
|
|
if (type == AP_PARAM_GROUP) {
|
2015-10-25 13:23:25 -03:00
|
|
|
uint8_t len = strnlen(_var_info[i].name, AP_MAX_NAME_SIZE);
|
2015-10-25 13:23:25 -03:00
|
|
|
if (strncmp(name, _var_info[i].name, len) != 0) {
|
2012-02-11 07:51:35 -04:00
|
|
|
continue;
|
|
|
|
}
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *group_info = get_group_info(_var_info[i]);
|
|
|
|
if (group_info == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
2015-12-27 16:22:32 -04:00
|
|
|
AP_Param *ap = find_group(name + len, i, 0, group_info, ptype);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ap != nullptr) {
|
2012-02-26 19:51:57 -04:00
|
|
|
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
|
2015-10-25 13:22:16 -03:00
|
|
|
} else if (strcasecmp(name, _var_info[i].name) == 0) {
|
2012-02-12 03:22:57 -04:00
|
|
|
*ptype = (enum ap_var_type)type;
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[i], base)) {
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2016-04-01 02:16:03 -03:00
|
|
|
}
|
|
|
|
return (AP_Param *)base;
|
2012-02-11 07:51:35 -04:00
|
|
|
}
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-11 07:51:35 -04:00
|
|
|
}
|
|
|
|
|
2012-09-19 18:42:35 -03:00
|
|
|
// Find a variable by index. Note that this is quite slow.
|
|
|
|
//
|
|
|
|
AP_Param *
|
2013-01-08 18:45:02 -04:00
|
|
|
AP_Param::find_by_index(uint16_t idx, enum ap_var_type *ptype, ParamToken *token)
|
2012-09-19 18:42:35 -03:00
|
|
|
{
|
|
|
|
AP_Param *ap;
|
|
|
|
uint16_t count=0;
|
2013-01-08 18:45:02 -04:00
|
|
|
for (ap=AP_Param::first(token, ptype);
|
2012-09-19 18:42:35 -03:00
|
|
|
ap && count < idx;
|
2013-01-08 18:45:02 -04:00
|
|
|
ap=AP_Param::next_scalar(token, ptype)) {
|
2012-09-19 18:42:35 -03:00
|
|
|
count++;
|
|
|
|
}
|
|
|
|
return ap;
|
|
|
|
}
|
|
|
|
|
2015-12-29 06:21:54 -04:00
|
|
|
|
|
|
|
/*
|
2016-04-01 02:16:03 -03:00
|
|
|
Find a variable by pointer, returning key. This is used for loading pointer variables
|
2015-12-29 06:21:54 -04:00
|
|
|
*/
|
2016-04-01 02:16:03 -03:00
|
|
|
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)
|
2015-12-29 06:21:54 -04:00
|
|
|
{
|
2016-04-01 02:16:03 -03:00
|
|
|
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;
|
|
|
|
}
|
2017-02-27 02:39:12 -04:00
|
|
|
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)) {
|
2016-04-01 02:16:03 -03:00
|
|
|
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++) {
|
|
|
|
if (_var_info[i].type != AP_PARAM_GROUP) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
if ((_var_info[i].flags & AP_PARAM_FLAG_POINTER) &&
|
|
|
|
ptr == *(void **)_var_info[i].ptr) {
|
|
|
|
key = _var_info[i].key;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
ptrdiff_t offset = 0;
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *ginfo = get_group_info(_var_info[i]);
|
|
|
|
if (ginfo == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
if (find_key_by_pointer_group(ptr, i, ginfo, offset, key)) {
|
2016-04-01 02:16:03 -03:00
|
|
|
return true;
|
|
|
|
}
|
2015-12-29 06:21:54 -04:00
|
|
|
}
|
2016-04-01 02:16:03 -03:00
|
|
|
return false;
|
2015-12-29 06:21:54 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2012-12-13 18:58:23 -04:00
|
|
|
// Find a object by name.
|
|
|
|
//
|
|
|
|
AP_Param *
|
|
|
|
AP_Param::find_object(const char *name)
|
|
|
|
{
|
2015-12-30 17:27:22 -04:00
|
|
|
for (uint16_t i=0; i<_num_vars; i++) {
|
2015-10-25 13:22:16 -03:00
|
|
|
if (strcasecmp(name, _var_info[i].name) == 0) {
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[i], base)) {
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2016-04-01 02:16:03 -03:00
|
|
|
}
|
|
|
|
return (AP_Param *)base;
|
2012-12-13 18:58:23 -04:00
|
|
|
}
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-12-13 18:58:23 -04:00
|
|
|
}
|
|
|
|
|
2015-10-06 19:08:59 -03:00
|
|
|
// notify GCS of current value of parameter
|
|
|
|
void AP_Param::notify() const {
|
|
|
|
uint32_t group_element = 0;
|
|
|
|
const struct GroupInfo *ginfo;
|
2016-02-15 03:37:40 -04:00
|
|
|
struct GroupNesting group_nesting {};
|
2015-10-06 19:08:59 -03:00
|
|
|
uint8_t idx;
|
|
|
|
|
2016-02-15 03:37:40 -04:00
|
|
|
const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (info == nullptr) {
|
2015-10-06 19:08:59 -03:00
|
|
|
// this is probably very bad
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
char name[AP_MAX_NAME_SIZE+1];
|
2016-02-15 03:37:40 -04:00
|
|
|
copy_name_info(info, ginfo, group_nesting, idx, name, sizeof(name), true);
|
2015-10-06 19:08:59 -03:00
|
|
|
|
|
|
|
uint32_t param_header_type;
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ginfo != nullptr) {
|
2015-12-23 13:25:02 -04:00
|
|
|
param_header_type = ginfo->type;
|
2015-10-06 19:08:59 -03:00
|
|
|
} else {
|
2015-12-23 13:25:02 -04:00
|
|
|
param_header_type = info->type;
|
2015-10-06 19:08:59 -03:00
|
|
|
}
|
|
|
|
|
2016-03-16 03:03:30 -03:00
|
|
|
send_parameter(name, (enum ap_var_type)param_header_type, idx);
|
2015-10-06 19:08:59 -03:00
|
|
|
}
|
|
|
|
|
2012-12-13 18:58:23 -04:00
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// Save the variable to EEPROM, if supported
|
|
|
|
//
|
2013-06-02 01:48:36 -03:00
|
|
|
bool AP_Param::save(bool force_save)
|
2012-02-11 07:51:35 -04:00
|
|
|
{
|
2012-11-05 07:08:43 -04:00
|
|
|
uint32_t group_element = 0;
|
2012-02-12 03:22:57 -04:00
|
|
|
const struct GroupInfo *ginfo;
|
2016-02-15 03:37:40 -04:00
|
|
|
struct GroupNesting group_nesting {};
|
2012-02-23 23:19:00 -04:00
|
|
|
uint8_t idx;
|
2016-02-15 03:37:40 -04:00
|
|
|
const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
|
2012-02-23 23:19:00 -04:00
|
|
|
const AP_Param *ap;
|
2012-02-11 07:51:35 -04:00
|
|
|
|
2016-10-30 02:24:21 -03:00
|
|
|
if (info == nullptr) {
|
2012-02-11 07:51:35 -04:00
|
|
|
// 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
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ginfo != nullptr) {
|
2015-12-23 13:25:02 -04:00
|
|
|
phdr.type = ginfo->type;
|
2012-02-12 03:22:57 -04:00
|
|
|
} else {
|
2015-12-23 13:25:02 -04:00
|
|
|
phdr.type = info->type;
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
2015-12-30 17:27:22 -04:00
|
|
|
set_key(phdr, info->key);
|
2012-02-12 18:54:46 -04:00
|
|
|
phdr.group_element = group_element;
|
2012-02-11 07:51:35 -04:00
|
|
|
|
2012-02-23 23:19:00 -04:00
|
|
|
ap = this;
|
|
|
|
if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {
|
|
|
|
// only vector3f can have non-zero idx for now
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (idx != 0) {
|
2015-12-29 06:21:54 -04:00
|
|
|
ap = (const AP_Param *)((ptrdiff_t)ap) - (idx*sizeof(float));
|
2012-02-23 23:19:00 -04:00
|
|
|
}
|
|
|
|
|
2016-01-05 23:41:07 -04:00
|
|
|
if (phdr.type == AP_PARAM_INT8 && ginfo != nullptr && (ginfo->flags & AP_PARAM_FLAG_ENABLE)) {
|
|
|
|
// clear cached parameter count
|
|
|
|
_parameter_count = 0;
|
|
|
|
}
|
|
|
|
|
2015-10-06 06:54:24 -03:00
|
|
|
char name[AP_MAX_NAME_SIZE+1];
|
2016-02-15 03:37:40 -04:00
|
|
|
copy_name_info(info, ginfo, group_nesting, idx, name, sizeof(name), true);
|
2015-10-06 06:54:24 -03:00
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// scan EEPROM to find the right location
|
|
|
|
uint16_t ofs;
|
|
|
|
if (scan(&phdr, &ofs)) {
|
|
|
|
// found an existing copy of the variable
|
2012-02-23 23:19:00 -04:00
|
|
|
eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
|
2016-03-16 03:03:30 -03:00
|
|
|
send_parameter(name, (enum ap_var_type)phdr.type, idx);
|
2012-02-11 07:51:35 -04:00
|
|
|
return true;
|
|
|
|
}
|
2012-08-17 03:18:11 -03:00
|
|
|
if (ofs == (uint16_t) ~0) {
|
2012-02-11 07:51:35 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2012-08-06 22:00:43 -03:00
|
|
|
// if the value is the default value then don't save
|
2012-08-08 00:13:30 -03:00
|
|
|
if (phdr.type <= AP_PARAM_FLOAT) {
|
|
|
|
float v1 = cast_to_float((enum ap_var_type)phdr.type);
|
|
|
|
float v2;
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ginfo != nullptr) {
|
2017-01-22 06:42:54 -04:00
|
|
|
v2 = get_default_value(this, &ginfo->def_value);
|
2012-08-08 00:13:30 -03:00
|
|
|
} else {
|
2017-01-22 06:42:54 -04:00
|
|
|
v2 = get_default_value(this, &info->def_value);
|
2012-08-08 00:13:30 -03:00
|
|
|
}
|
2015-05-04 23:35:47 -03:00
|
|
|
if (is_equal(v1,v2) && !force_save) {
|
2015-10-06 06:54:24 -03:00
|
|
|
GCS_MAVLINK::send_parameter_value_all(name, (enum ap_var_type)info->type, v2);
|
2012-08-08 00:13:30 -03:00
|
|
|
return true;
|
|
|
|
}
|
2016-05-24 03:55:45 -03:00
|
|
|
if (!force_save &&
|
|
|
|
(phdr.type != AP_PARAM_INT32 &&
|
|
|
|
(fabsf(v1-v2) < 0.0001f*fabsf(v1)))) {
|
2012-08-08 00:13:30 -03:00
|
|
|
// for other than 32 bit integers, we accept values within
|
|
|
|
// 0.01 percent of the current value as being the same
|
2015-10-06 06:54:24 -03:00
|
|
|
GCS_MAVLINK::send_parameter_value_all(name, (enum ap_var_type)info->type, v2);
|
2012-08-17 03:18:11 -03:00
|
|
|
return true;
|
2012-08-08 00:13:30 -03:00
|
|
|
}
|
2012-08-06 22:00:43 -03:00
|
|
|
}
|
|
|
|
|
2014-08-13 01:43:19 -03:00
|
|
|
if (ofs+type_size((enum ap_var_type)phdr.type)+2*sizeof(phdr) >= _storage.size()) {
|
2012-08-06 22:00:43 -03:00
|
|
|
// we are out of room for saving variables
|
2017-01-21 01:00:43 -04:00
|
|
|
hal.console->printf("EEPROM full\n");
|
2012-08-06 22:00:43 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// write a new sentinal, then the data, then the header
|
|
|
|
write_sentinal(ofs + sizeof(phdr) + type_size((enum ap_var_type)phdr.type));
|
2012-02-23 23:19:00 -04:00
|
|
|
eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
|
2012-02-11 07:51:35 -04:00
|
|
|
eeprom_write_check(&phdr, ofs, sizeof(phdr));
|
2015-11-08 23:27:39 -04:00
|
|
|
|
2016-03-16 03:03:30 -03:00
|
|
|
send_parameter(name, (enum ap_var_type)phdr.type, idx);
|
2012-02-11 07:51:35 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Load the variable from EEPROM, if supported
|
|
|
|
//
|
|
|
|
bool AP_Param::load(void)
|
|
|
|
{
|
2012-11-05 07:08:43 -04:00
|
|
|
uint32_t group_element = 0;
|
2012-02-12 03:22:57 -04:00
|
|
|
const struct GroupInfo *ginfo;
|
2016-02-15 03:37:40 -04:00
|
|
|
struct GroupNesting group_nesting {};
|
2012-02-23 23:19:00 -04:00
|
|
|
uint8_t idx;
|
2016-02-15 03:37:40 -04:00
|
|
|
const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (info == nullptr) {
|
2012-02-11 07:51:35 -04:00
|
|
|
// 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
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ginfo != nullptr) {
|
2015-12-23 13:25:02 -04:00
|
|
|
phdr.type = ginfo->type;
|
2012-02-12 03:22:57 -04:00
|
|
|
} else {
|
2015-12-23 13:25:02 -04:00
|
|
|
phdr.type = info->type;
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
2015-12-30 17:27:22 -04:00
|
|
|
set_key(phdr, info->key);
|
2012-02-12 18:54:46 -04:00
|
|
|
phdr.group_element = group_element;
|
2012-02-11 07:51:35 -04:00
|
|
|
|
|
|
|
// scan EEPROM to find the right location
|
|
|
|
uint16_t ofs;
|
|
|
|
if (!scan(&phdr, &ofs)) {
|
2012-08-06 22:00:43 -03:00
|
|
|
// if the value isn't stored in EEPROM then set the default value
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(*info, base)) {
|
|
|
|
return false;
|
|
|
|
}
|
2016-03-08 00:29:48 -04:00
|
|
|
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ginfo != nullptr) {
|
2016-03-08 00:29:48 -04:00
|
|
|
// 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),
|
2017-01-22 06:42:54 -04:00
|
|
|
get_default_value(this, &ginfo->def_value));
|
2012-08-06 22:00:43 -03:00
|
|
|
} else {
|
2016-04-01 02:16:03 -03:00
|
|
|
set_value((enum ap_var_type)phdr.type, (void*)base,
|
2017-01-22 06:42:54 -04:00
|
|
|
get_default_value(this, &info->def_value));
|
2012-08-06 22:00:43 -03:00
|
|
|
}
|
2012-02-11 07:51:35 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2012-02-27 18:42:34 -04:00
|
|
|
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) {
|
2015-12-29 06:21:54 -04:00
|
|
|
ap = (AP_Param *)((ptrdiff_t)ap) - (idx*sizeof(float));
|
2012-02-27 18:42:34 -04:00
|
|
|
}
|
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// found it
|
2014-08-13 01:43:19 -03:00
|
|
|
_storage.read_block(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
|
2012-02-11 07:51:35 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2017-01-19 22:29:17 -04:00
|
|
|
bool AP_Param::configured_in_storage(void) const
|
2015-10-21 23:57:05 -03:00
|
|
|
{
|
|
|
|
uint32_t group_element = 0;
|
|
|
|
const struct GroupInfo *ginfo;
|
2016-02-15 03:37:40 -04:00
|
|
|
struct GroupNesting group_nesting {};
|
2015-10-21 23:57:05 -03:00
|
|
|
uint8_t idx;
|
2016-02-15 03:37:40 -04:00
|
|
|
const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (info == nullptr) {
|
2015-10-21 23:57:05 -03:00
|
|
|
// 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
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ginfo != nullptr) {
|
2015-12-23 13:25:02 -04:00
|
|
|
phdr.type = ginfo->type;
|
2015-10-21 23:57:05 -03:00
|
|
|
} else {
|
2015-12-23 13:25:02 -04:00
|
|
|
phdr.type = info->type;
|
2015-10-21 23:57:05 -03:00
|
|
|
}
|
2015-12-30 17:27:22 -04:00
|
|
|
set_key(phdr, info->key);
|
2015-10-21 23:57:05 -03:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
|
2017-01-19 22:29:17 -04:00
|
|
|
bool AP_Param::configured_in_defaults_file(void) const
|
2015-10-21 23:57:05 -03:00
|
|
|
{
|
|
|
|
uint32_t group_element = 0;
|
|
|
|
const struct GroupInfo *ginfo;
|
2016-02-15 03:37:40 -04:00
|
|
|
struct GroupNesting group_nesting {};
|
2015-10-21 23:57:05 -03:00
|
|
|
uint8_t idx;
|
2016-02-15 03:37:40 -04:00
|
|
|
const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (info == nullptr) {
|
2015-10-21 23:57:05 -03:00
|
|
|
// we don't have any info on how to load it
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
for (uint16_t i=0; i<num_param_overrides; i++) {
|
2017-01-22 06:42:54 -04:00
|
|
|
if (this == param_overrides[i].object_ptr) {
|
2015-10-21 23:57:05 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2012-08-17 03:18:11 -03:00
|
|
|
// set a AP_Param variable to a specified value
|
2013-05-30 08:45:25 -03:00
|
|
|
void AP_Param::set_value(enum ap_var_type type, void *ptr, float value)
|
2012-08-06 22:00:43 -03:00
|
|
|
{
|
|
|
|
switch (type) {
|
|
|
|
case AP_PARAM_INT8:
|
2013-05-30 08:45:25 -03:00
|
|
|
((AP_Int8 *)ptr)->set(value);
|
2012-08-06 22:00:43 -03:00
|
|
|
break;
|
|
|
|
case AP_PARAM_INT16:
|
2013-05-30 08:45:25 -03:00
|
|
|
((AP_Int16 *)ptr)->set(value);
|
2012-08-06 22:00:43 -03:00
|
|
|
break;
|
|
|
|
case AP_PARAM_INT32:
|
2013-05-30 08:45:25 -03:00
|
|
|
((AP_Int32 *)ptr)->set(value);
|
2012-08-06 22:00:43 -03:00
|
|
|
break;
|
|
|
|
case AP_PARAM_FLOAT:
|
2013-05-30 08:45:25 -03:00
|
|
|
((AP_Float *)ptr)->set(value);
|
2012-08-06 22:00:43 -03:00
|
|
|
break;
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2012-12-12 17:40:42 -04:00
|
|
|
// 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)
|
2012-08-06 22:00:43 -03:00
|
|
|
{
|
2015-12-29 06:21:54 -04:00
|
|
|
ptrdiff_t base = (ptrdiff_t)object_pointer;
|
2012-08-06 22:00:43 -03:00
|
|
|
uint8_t type;
|
2015-12-27 16:22:32 -04:00
|
|
|
for (uint8_t i=0;
|
|
|
|
(type=group_info[i].type) != AP_PARAM_NONE;
|
|
|
|
i++) {
|
2012-12-12 17:40:42 -04:00
|
|
|
if (type <= AP_PARAM_FLOAT) {
|
2015-12-23 13:25:02 -04:00
|
|
|
void *ptr = (void *)(base + group_info[i].offset);
|
2017-01-22 06:42:54 -04:00
|
|
|
set_value((enum ap_var_type)type, ptr,
|
|
|
|
get_default_value((const AP_Param *)ptr, &group_info[i].def_value));
|
2012-08-06 22:00:43 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-07-20 20:37:41 -03:00
|
|
|
// set a value directly in an object. This should only be used by
|
|
|
|
// example code, not by mainline vehicle code
|
2016-10-04 05:07:21 -03:00
|
|
|
bool AP_Param::set_object_value(const void *object_pointer,
|
|
|
|
const struct GroupInfo *group_info,
|
2014-07-20 20:37:41 -03:00
|
|
|
const char *name, float value)
|
|
|
|
{
|
2015-12-29 06:21:54 -04:00
|
|
|
ptrdiff_t base = (ptrdiff_t)object_pointer;
|
2014-07-20 20:37:41 -03:00
|
|
|
uint8_t type;
|
2016-10-04 05:07:21 -03:00
|
|
|
bool found = false;
|
2015-12-27 16:22:32 -04:00
|
|
|
for (uint8_t i=0;
|
|
|
|
(type=group_info[i].type) != AP_PARAM_NONE;
|
|
|
|
i++) {
|
2014-07-20 20:37:41 -03:00
|
|
|
if (strcmp(name, group_info[i].name) == 0 && type <= AP_PARAM_FLOAT) {
|
2015-12-23 13:25:02 -04:00
|
|
|
void *ptr = (void *)(base + group_info[i].offset);
|
2014-07-20 20:37:41 -03:00
|
|
|
set_value((enum ap_var_type)type, ptr, value);
|
2016-10-04 05:07:21 -03:00
|
|
|
// return true here ?
|
|
|
|
found = true;
|
2014-07-20 20:37:41 -03:00
|
|
|
}
|
|
|
|
}
|
2016-10-04 05:07:21 -03:00
|
|
|
return found;
|
2014-07-20 20:37:41 -03:00
|
|
|
}
|
|
|
|
|
2012-08-06 22:00:43 -03:00
|
|
|
|
2012-12-12 17:40:42 -04:00
|
|
|
// load default values for all scalars in a sketch. This does not
|
|
|
|
// recurse into sub-objects
|
2012-12-13 18:58:23 -04:00
|
|
|
void AP_Param::setup_sketch_defaults(void)
|
2012-08-06 22:00:43 -03:00
|
|
|
{
|
2012-12-13 18:58:23 -04:00
|
|
|
setup();
|
2015-12-30 17:27:22 -04:00
|
|
|
for (uint16_t i=0; i<_num_vars; i++) {
|
2015-12-23 13:25:02 -04:00
|
|
|
uint8_t type = _var_info[i].type;
|
2012-12-12 17:40:42 -04:00
|
|
|
if (type <= AP_PARAM_FLOAT) {
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (get_base(_var_info[i], base)) {
|
2017-01-22 06:42:54 -04:00
|
|
|
set_value((enum ap_var_type)type, (void*)base,
|
|
|
|
get_default_value((const AP_Param *)base, &_var_info[i].def_value));
|
2016-04-01 02:16:03 -03:00
|
|
|
}
|
2012-08-06 22:00:43 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2012-02-11 07:51:35 -04:00
|
|
|
// Load all variables from EEPROM
|
|
|
|
//
|
2017-01-09 05:52:36 -04:00
|
|
|
bool AP_Param::load_all(bool check_defaults_file)
|
2012-02-11 07:51:35 -04:00
|
|
|
{
|
|
|
|
struct Param_header phdr;
|
|
|
|
uint16_t ofs = sizeof(AP_Param::EEPROM_header);
|
2012-08-06 22:00:43 -03:00
|
|
|
|
2017-01-09 05:52:36 -04:00
|
|
|
reload_defaults_file(check_defaults_file);
|
2015-04-01 22:24:45 -03:00
|
|
|
|
2014-08-13 01:43:19 -03:00
|
|
|
while (ofs < _storage.size()) {
|
|
|
|
_storage.read_block(&phdr, ofs, sizeof(phdr));
|
2012-02-12 18:54:46 -04:00
|
|
|
// note that this is an || not an && for robustness
|
|
|
|
// against power off while adding a variable
|
2015-12-30 17:27:22 -04:00
|
|
|
if (is_sentinal(phdr)) {
|
2012-02-11 07:51:35 -04:00
|
|
|
// we've reached the sentinal
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
const struct AP_Param::Info *info;
|
|
|
|
void *ptr;
|
|
|
|
|
|
|
|
info = find_by_header(phdr, &ptr);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (info != nullptr) {
|
2014-08-13 01:43:19 -03:00
|
|
|
_storage.read_block(ptr, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
|
2012-02-11 07:51:35 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
|
|
|
|
}
|
|
|
|
|
|
|
|
// we didn't find the sentinal
|
2014-11-09 17:09:08 -04:00
|
|
|
Debug("no sentinal in load_all");
|
2012-02-11 07:51:35 -04:00
|
|
|
return false;
|
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
|
2017-01-09 05:52:36 -04:00
|
|
|
/*
|
|
|
|
reload from hal.util defaults file
|
|
|
|
*/
|
|
|
|
void AP_Param::reload_defaults_file(bool panic_on_error)
|
|
|
|
{
|
2017-11-09 02:45:58 -04:00
|
|
|
if (param_defaults_data.length != 0) {
|
2018-01-06 01:56:06 -04:00
|
|
|
load_embedded_param_defaults(false);
|
2017-11-09 02:45:58 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2017-01-09 05:52:36 -04:00
|
|
|
#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, panic_on_error)) {
|
|
|
|
printf("Loaded defaults from %s\n", default_file);
|
|
|
|
} else {
|
|
|
|
printf("Failed to load defaults from %s\n", default_file);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
|
2015-12-29 06:21:54 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
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;
|
2016-04-01 02:16:03 -03:00
|
|
|
uint16_t key;
|
2015-12-29 06:21:54 -04:00
|
|
|
|
2017-02-27 02:39:12 -04:00
|
|
|
// reset cached param counter as we may be loading a dynamic var_info
|
|
|
|
_parameter_count = 0;
|
|
|
|
|
2016-04-01 02:16:03 -03:00
|
|
|
if (!find_key_by_pointer(object_pointer, key)) {
|
2015-12-29 06:21:54 -04:00
|
|
|
hal.console->printf("ERROR: Unable to find param pointer\n");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2016-04-01 02:16:03 -03:00
|
|
|
for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
|
2015-12-29 06:21:54 -04:00
|
|
|
if (group_info[i].type == AP_PARAM_GROUP) {
|
|
|
|
ptrdiff_t new_offset = 0;
|
2016-04-01 02:16:03 -03:00
|
|
|
if (!adjust_group_offset(key, group_info[i], new_offset)) {
|
2015-12-29 06:21:54 -04:00
|
|
|
continue;
|
|
|
|
}
|
2017-02-27 02:39:12 -04:00
|
|
|
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);
|
|
|
|
}
|
2015-12-29 06:21:54 -04:00
|
|
|
}
|
|
|
|
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
|
2015-12-30 17:27:22 -04:00
|
|
|
if (is_sentinal(phdr)) {
|
2015-12-29 06:21:54 -04:00
|
|
|
// we've reached the sentinal
|
|
|
|
break;
|
|
|
|
}
|
2016-04-01 02:16:03 -03:00
|
|
|
if (get_key(phdr) == key) {
|
2015-12-29 06:21:54 -04:00
|
|
|
const struct AP_Param::Info *info;
|
|
|
|
void *ptr;
|
|
|
|
|
|
|
|
info = find_by_header(phdr, &ptr);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (info != nullptr) {
|
2015-12-29 06:21:54 -04:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2012-02-12 03:22:57 -04:00
|
|
|
// return the first variable in _var_info
|
2012-02-19 01:57:49 -04:00
|
|
|
AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype)
|
2012-02-12 03:22:57 -04:00
|
|
|
{
|
2012-02-19 01:57:49 -04:00
|
|
|
token->key = 0;
|
|
|
|
token->group_element = 0;
|
2012-02-23 23:19:00 -04:00
|
|
|
token->idx = 0;
|
2012-02-12 03:22:57 -04:00
|
|
|
if (_num_vars == 0) {
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ptype != nullptr) {
|
2015-12-27 16:22:32 -04:00
|
|
|
*ptype = (enum ap_var_type)_var_info[0].type;
|
2012-02-12 04:18:24 -04:00
|
|
|
}
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[0], base)) {
|
|
|
|
// should be impossible, first var needs to be non-pointer
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2016-04-01 02:16:03 -03:00
|
|
|
}
|
|
|
|
return (AP_Param *)base;
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
/// Returns the next variable in a group, recursing into groups
|
|
|
|
/// as needed
|
2015-12-30 17:27:22 -04:00
|
|
|
AP_Param *AP_Param::next_group(uint16_t vindex, const struct GroupInfo *group_info,
|
2012-02-12 03:22:57 -04:00
|
|
|
bool *found_current,
|
2016-10-22 04:10:11 -03:00
|
|
|
uint32_t group_base,
|
2012-02-12 03:22:57 -04:00
|
|
|
uint8_t group_shift,
|
2015-12-29 06:21:54 -04:00
|
|
|
ptrdiff_t group_offset,
|
2012-02-19 01:57:49 -04:00
|
|
|
ParamToken *token,
|
2012-02-12 03:22:57 -04:00
|
|
|
enum ap_var_type *ptype)
|
|
|
|
{
|
2012-02-23 23:19:00 -04:00
|
|
|
enum ap_var_type type;
|
2012-02-12 03:22:57 -04:00
|
|
|
for (uint8_t i=0;
|
2015-12-27 16:22:32 -04:00
|
|
|
(type=(enum ap_var_type)group_info[i].type) != AP_PARAM_NONE;
|
2012-02-12 03:22:57 -04:00
|
|
|
i++) {
|
2017-02-07 19:45:20 -04:00
|
|
|
if (!check_frame_type(group_info[i].flags)) {
|
|
|
|
continue;
|
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
if (type == AP_PARAM_GROUP) {
|
|
|
|
// a nested group
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *ginfo = get_group_info(group_info[i]);
|
|
|
|
if (ginfo == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
AP_Param *ap;
|
2015-12-29 06:21:54 -04:00
|
|
|
ptrdiff_t new_offset = group_offset;
|
|
|
|
|
|
|
|
if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
|
|
|
|
continue;
|
2015-12-27 16:22:32 -04:00
|
|
|
}
|
2015-12-29 06:21:54 -04:00
|
|
|
|
2016-02-15 03:37:40 -04:00
|
|
|
ap = next_group(vindex, ginfo, found_current, group_id(group_info, group_base, i, group_shift),
|
2015-12-27 16:22:32 -04:00
|
|
|
group_shift + _group_level_shift, new_offset, token, ptype);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ap != nullptr) {
|
2012-02-12 03:22:57 -04:00
|
|
|
return ap;
|
|
|
|
}
|
2015-12-27 16:22:32 -04:00
|
|
|
} else {
|
2012-02-12 03:22:57 -04:00
|
|
|
if (*found_current) {
|
|
|
|
// got a new one
|
2012-02-19 01:57:49 -04:00
|
|
|
token->key = vindex;
|
2016-02-15 03:37:40 -04:00
|
|
|
token->group_element = group_id(group_info, group_base, i, group_shift);
|
2012-02-23 23:19:00 -04:00
|
|
|
token->idx = 0;
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ptype != nullptr) {
|
2012-02-23 23:19:00 -04:00
|
|
|
*ptype = type;
|
2012-02-12 04:18:24 -04:00
|
|
|
}
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[vindex], base)) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
return (AP_Param*)(base + group_info[i].offset + group_offset);
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
2016-02-15 03:37:40 -04:00
|
|
|
if (group_id(group_info, group_base, i, group_shift) == token->group_element) {
|
2012-02-12 03:22:57 -04:00
|
|
|
*found_current = true;
|
2012-02-23 23:19:00 -04:00
|
|
|
if (type == AP_PARAM_VECTOR3F && token->idx < 3) {
|
|
|
|
// return the next element of the vector as a
|
|
|
|
// float
|
|
|
|
token->idx++;
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ptype != nullptr) {
|
2012-02-23 23:19:00 -04:00
|
|
|
*ptype = AP_PARAM_FLOAT;
|
|
|
|
}
|
2016-04-01 02:16:03 -03:00
|
|
|
ptrdiff_t base;
|
|
|
|
if (!get_base(_var_info[vindex], base)) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
ptrdiff_t ofs = base + group_info[i].offset + group_offset;
|
2013-05-24 11:39:32 -03:00
|
|
|
ofs += sizeof(float)*(token->idx - 1u);
|
2012-02-23 23:19:00 -04:00
|
|
|
return (AP_Param *)ofs;
|
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
/// Returns the next variable in _var_info, recursing into groups
|
|
|
|
/// as needed
|
2012-02-19 01:57:49 -04:00
|
|
|
AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype)
|
2012-02-12 03:22:57 -04:00
|
|
|
{
|
2015-12-30 17:27:22 -04:00
|
|
|
uint16_t i = token->key;
|
2012-02-12 03:22:57 -04:00
|
|
|
bool found_current = false;
|
|
|
|
if (i >= _num_vars) {
|
|
|
|
// illegal token
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
2015-12-23 13:25:02 -04:00
|
|
|
enum ap_var_type type = (enum ap_var_type)_var_info[i].type;
|
2012-02-23 23:19:00 -04:00
|
|
|
|
|
|
|
// 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++;
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ptype != nullptr) {
|
2012-02-23 23:19:00 -04:00
|
|
|
*ptype = AP_PARAM_FLOAT;
|
|
|
|
}
|
2015-12-29 06:21:54 -04:00
|
|
|
return (AP_Param *)(((token->idx - 1u)*sizeof(float))+(ptrdiff_t)_var_info[i].ptr);
|
2012-02-23 23:19:00 -04:00
|
|
|
}
|
|
|
|
|
2012-02-12 03:22:57 -04:00
|
|
|
if (type != AP_PARAM_GROUP) {
|
|
|
|
i++;
|
|
|
|
found_current = true;
|
|
|
|
}
|
|
|
|
for (; i<_num_vars; i++) {
|
2017-02-07 19:45:20 -04:00
|
|
|
if (!check_frame_type(_var_info[i].flags)) {
|
|
|
|
continue;
|
|
|
|
}
|
2015-12-23 13:25:02 -04:00
|
|
|
type = (enum ap_var_type)_var_info[i].type;
|
2012-02-12 03:22:57 -04:00
|
|
|
if (type == AP_PARAM_GROUP) {
|
2017-02-27 02:39:12 -04:00
|
|
|
const struct GroupInfo *group_info = get_group_info(_var_info[i]);
|
|
|
|
if (group_info == nullptr) {
|
|
|
|
continue;
|
|
|
|
}
|
2015-12-27 16:22:32 -04:00
|
|
|
AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, 0, token, ptype);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ap != nullptr) {
|
2012-02-12 03:22:57 -04:00
|
|
|
return ap;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// found the next one
|
2012-02-19 01:57:49 -04:00
|
|
|
token->key = i;
|
|
|
|
token->group_element = 0;
|
2012-02-23 23:19:00 -04:00
|
|
|
token->idx = 0;
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ptype != nullptr) {
|
2012-02-23 23:19:00 -04:00
|
|
|
*ptype = type;
|
2012-02-12 04:18:24 -04:00
|
|
|
}
|
2015-12-27 16:22:32 -04:00
|
|
|
return (AP_Param *)(_var_info[i].ptr);
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
return nullptr;
|
2012-02-12 03:22:57 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
/// Returns the next scalar in _var_info, recursing into groups
|
|
|
|
/// as needed
|
2012-02-19 01:57:49 -04:00
|
|
|
AP_Param *AP_Param::next_scalar(ParamToken *token, enum ap_var_type *ptype)
|
2012-02-12 03:22:57 -04:00
|
|
|
{
|
|
|
|
AP_Param *ap;
|
2012-02-12 04:18:24 -04:00
|
|
|
enum ap_var_type type;
|
2016-10-30 02:24:21 -03:00
|
|
|
while ((ap = next(token, &type)) != nullptr && type > AP_PARAM_FLOAT) ;
|
2015-12-29 07:43:25 -04:00
|
|
|
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ap != nullptr && type == AP_PARAM_INT8) {
|
2015-12-29 07:43:25 -04:00
|
|
|
/*
|
|
|
|
check if this is an enable variable. To do that we need to
|
|
|
|
find the info structures for the variable
|
|
|
|
*/
|
|
|
|
uint32_t group_element;
|
|
|
|
const struct GroupInfo *ginfo;
|
2016-02-15 03:37:40 -04:00
|
|
|
struct GroupNesting group_nesting {};
|
2015-12-29 07:43:25 -04:00
|
|
|
uint8_t idx;
|
2016-02-15 03:37:40 -04:00
|
|
|
const struct AP_Param::Info *info = ap->find_var_info_token(*token, &group_element,
|
|
|
|
ginfo, group_nesting, &idx);
|
2015-12-29 07:43:25 -04:00
|
|
|
if (info && ginfo &&
|
|
|
|
(ginfo->flags & AP_PARAM_FLAG_ENABLE) &&
|
2017-11-28 19:56:25 -04:00
|
|
|
!(ginfo->flags & AP_PARAM_FLAG_IGNORE_ENABLE) &&
|
2016-10-25 18:32:10 -03:00
|
|
|
((AP_Int8 *)ap)->get() == 0 &&
|
|
|
|
_hide_disabled_groups) {
|
2015-12-29 07:43:25 -04:00
|
|
|
/*
|
|
|
|
this is a disabled parameter tree, include this
|
|
|
|
parameter but not others below it. We need to keep
|
|
|
|
looking until we go past the parameters in this object
|
|
|
|
*/
|
|
|
|
ParamToken token2 = *token;
|
|
|
|
enum ap_var_type type2;
|
|
|
|
AP_Param *ap2;
|
2016-10-30 02:24:21 -03:00
|
|
|
while ((ap2 = next(&token2, &type2)) != nullptr) {
|
2015-12-29 07:43:25 -04:00
|
|
|
if (token2.key != token->key) {
|
|
|
|
break;
|
|
|
|
}
|
2016-02-15 03:37:40 -04:00
|
|
|
if (group_nesting.level != 0 && (token->group_element & 0x3F) != (token2.group_element & 0x3F)) {
|
2015-12-29 07:43:25 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
// update the returned token so the next() call goes from this point
|
|
|
|
*token = token2;
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ap != nullptr && ptype != nullptr) {
|
2012-02-12 04:18:24 -04:00
|
|
|
*ptype = type;
|
|
|
|
}
|
2012-02-12 03:22:57 -04:00
|
|
|
return ap;
|
|
|
|
}
|
2012-02-12 04:18:58 -04:00
|
|
|
|
|
|
|
|
|
|
|
/// cast a variable to a float given its type
|
2013-04-19 04:46:40 -03:00
|
|
|
float AP_Param::cast_to_float(enum ap_var_type type) const
|
2012-02-12 04:18:58 -04:00
|
|
|
{
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
2012-02-12 19:16:55 -04:00
|
|
|
|
2016-12-30 19:49:43 -04:00
|
|
|
/*
|
|
|
|
find an old parameter and return it.
|
|
|
|
*/
|
|
|
|
bool AP_Param::find_old_parameter(const struct ConversionInfo *info, AP_Param *value)
|
2013-05-30 08:45:25 -03:00
|
|
|
{
|
|
|
|
// find the old value in EEPROM.
|
|
|
|
uint16_t pofs;
|
|
|
|
AP_Param::Param_header header;
|
2015-12-23 13:25:02 -04:00
|
|
|
header.type = info->type;
|
2015-12-30 17:27:22 -04:00
|
|
|
set_key(header, info->old_key);
|
2015-12-23 13:25:02 -04:00
|
|
|
header.group_element = info->old_group_element;
|
2013-05-30 08:45:25 -03:00
|
|
|
if (!scan(&header, &pofs)) {
|
2016-12-30 19:49:43 -04:00
|
|
|
// 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
|
2017-01-10 02:07:53 -04:00
|
|
|
void AP_Param::convert_old_parameter(const struct ConversionInfo *info, float scaler, uint8_t flags)
|
2016-12-30 19:49:43 -04:00
|
|
|
{
|
|
|
|
uint8_t old_value[type_size(info->type)];
|
|
|
|
AP_Param *ap = (AP_Param *)&old_value[0];
|
|
|
|
|
|
|
|
if (!find_old_parameter(info, ap)) {
|
2013-05-30 08:45:25 -03:00
|
|
|
// 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;
|
2015-10-25 16:01:02 -03:00
|
|
|
ap2 = find(&info->new_name[0], &ptype);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (ap2 == nullptr) {
|
2015-10-25 17:10:41 -03:00
|
|
|
hal.console->printf("Unknown conversion '%s'\n", info->new_name);
|
2013-05-30 08:45:25 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// see if we can load it from EEPROM
|
2017-01-10 02:07:53 -04:00
|
|
|
if (!(flags & CONVERT_FLAG_FORCE) && ap2->configured_in_storage()) {
|
2013-05-30 08:45:25 -03:00
|
|
|
// the new parameter already has a value set by the user, or
|
|
|
|
// has already been converted
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2016-02-16 22:29:47 -04:00
|
|
|
// see if they are the same type and no scaling applied
|
2017-01-10 02:07:53 -04:00
|
|
|
if (ptype == info->type && is_equal(scaler, 1.0f) && flags == 0) {
|
2013-05-30 08:45:25 -03:00
|
|
|
// 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();
|
|
|
|
}
|
2016-12-30 19:49:43 -04:00
|
|
|
} else if (ptype <= AP_PARAM_FLOAT && info->type <= AP_PARAM_FLOAT) {
|
2013-05-30 08:45:25 -03:00
|
|
|
// perform scalar->scalar conversion
|
2016-12-30 19:49:43 -04:00
|
|
|
float v = ap->cast_to_float(info->type);
|
2017-01-10 02:07:53 -04:00
|
|
|
if (flags & CONVERT_FLAG_REVERSE) {
|
|
|
|
// convert a _REV parameter to a _REVERSED parameter
|
|
|
|
v = is_equal(v, -1.0f)?1:0;
|
|
|
|
}
|
2015-05-04 23:35:47 -03:00
|
|
|
if (!is_equal(v,ap2->cast_to_float(ptype))) {
|
2013-05-30 08:45:25 -03:00
|
|
|
// the value needs to change
|
2016-02-16 22:29:47 -04:00
|
|
|
set_value(ptype, ap2, v * scaler);
|
2013-05-30 08:45:25 -03:00
|
|
|
ap2->save();
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// can't do vector<->scalar conversion, or different vector types
|
2015-10-25 17:10:41 -03:00
|
|
|
hal.console->printf("Bad conversion type '%s'\n", info->new_name);
|
2013-05-30 08:45:25 -03:00
|
|
|
}
|
|
|
|
}
|
2015-01-01 03:55:11 -04:00
|
|
|
#pragma GCC diagnostic pop
|
2013-05-30 08:45:25 -03:00
|
|
|
|
|
|
|
|
|
|
|
// convert old vehicle parameters to new object parametersv
|
2017-01-10 02:07:53 -04:00
|
|
|
void AP_Param::convert_old_parameters(const struct ConversionInfo *conversion_table, uint8_t table_size, uint8_t flags)
|
2013-05-30 08:45:25 -03:00
|
|
|
{
|
|
|
|
for (uint8_t i=0; i<table_size; i++) {
|
2017-01-10 02:07:53 -04:00
|
|
|
convert_old_parameter(&conversion_table[i], 1.0f, flags);
|
2013-05-30 08:45:25 -03:00
|
|
|
}
|
|
|
|
}
|
2015-04-01 19:54:13 -03:00
|
|
|
|
2017-01-10 02:48:36 -04:00
|
|
|
/*
|
|
|
|
move old class variables for a class that was sub-classed to one that isn't
|
|
|
|
For example, used when the AP_MotorsTri class changed from having its own parameter table
|
|
|
|
plus a subgroup for AP_MotorsMulticopter to just inheriting the AP_MotorsMulticopter var_info
|
|
|
|
|
|
|
|
This does not handle nesting beyond the single level shift
|
|
|
|
*/
|
|
|
|
void AP_Param::convert_parent_class(uint8_t param_key, void *object_pointer,
|
|
|
|
const struct AP_Param::GroupInfo *group_info)
|
|
|
|
{
|
|
|
|
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;
|
|
|
|
info.old_group_element = uint16_t(group_info[i].idx)<<6;
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
|
|
|
uint8_t *new_value = group_info[i].offset + (uint8_t *)object_pointer;
|
|
|
|
memcpy(new_value, old_value, sizeof(old_value));
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-12-30 19:49:43 -04:00
|
|
|
|
2015-04-01 19:54:13 -03:00
|
|
|
/*
|
2015-08-10 04:25:29 -03:00
|
|
|
set a parameter to a float value
|
2015-04-01 19:54:13 -03:00
|
|
|
*/
|
2015-08-10 04:25:29 -03:00
|
|
|
void AP_Param::set_float(float value, enum ap_var_type var_type)
|
2015-04-01 19:54:13 -03:00
|
|
|
{
|
|
|
|
if (isnan(value) || isinf(value)) {
|
2015-08-10 04:25:29 -03:00
|
|
|
return;
|
2015-04-01 19:54:13 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// add a small amount before casting parameter values
|
|
|
|
// from float to integer to avoid truncating to the
|
|
|
|
// next lower integer value.
|
2015-04-24 00:57:36 -03:00
|
|
|
float rounding_addition = 0.01f;
|
2015-04-01 19:54:13 -03:00
|
|
|
|
|
|
|
// handle variables with standard type IDs
|
|
|
|
if (var_type == AP_PARAM_FLOAT) {
|
2015-08-10 04:25:29 -03:00
|
|
|
((AP_Float *)this)->set(value);
|
2015-04-01 19:54:13 -03:00
|
|
|
} else if (var_type == AP_PARAM_INT32) {
|
|
|
|
if (value < 0) rounding_addition = -rounding_addition;
|
|
|
|
float v = value+rounding_addition;
|
|
|
|
v = constrain_float(v, -2147483648.0, 2147483647.0);
|
2015-08-10 04:25:29 -03:00
|
|
|
((AP_Int32 *)this)->set(v);
|
2015-04-01 19:54:13 -03:00
|
|
|
} else if (var_type == AP_PARAM_INT16) {
|
|
|
|
if (value < 0) rounding_addition = -rounding_addition;
|
|
|
|
float v = value+rounding_addition;
|
|
|
|
v = constrain_float(v, -32768, 32767);
|
2015-08-10 04:25:29 -03:00
|
|
|
((AP_Int16 *)this)->set(v);
|
2015-04-01 19:54:13 -03:00
|
|
|
} else if (var_type == AP_PARAM_INT8) {
|
|
|
|
if (value < 0) rounding_addition = -rounding_addition;
|
|
|
|
float v = value+rounding_addition;
|
|
|
|
v = constrain_float(v, -128, 127);
|
2015-08-10 04:25:29 -03:00
|
|
|
((AP_Int8 *)this)->set(v);
|
2015-04-01 19:54:13 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-04-01 22:24:45 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
parse a parameter file line
|
|
|
|
*/
|
|
|
|
bool AP_Param::parse_param_line(char *line, char **vname, float &value)
|
|
|
|
{
|
|
|
|
if (line[0] == '#') {
|
|
|
|
return false;
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
char *saveptr = nullptr;
|
2015-04-01 22:24:45 -03:00
|
|
|
char *pname = strtok_r(line, ", =\t", &saveptr);
|
2016-10-30 02:24:21 -03:00
|
|
|
if (pname == nullptr) {
|
2015-04-01 22:24:45 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (strlen(pname) > AP_MAX_NAME_SIZE) {
|
|
|
|
return false;
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
const char *value_s = strtok_r(nullptr, ", =\t", &saveptr);
|
|
|
|
if (value_s == nullptr) {
|
2015-04-01 22:24:45 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
value = atof(value_s);
|
|
|
|
*vname = pname;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2017-11-09 02:45:58 -04:00
|
|
|
|
|
|
|
#if HAL_OS_POSIX_IO == 1
|
|
|
|
#include <stdio.h>
|
|
|
|
|
2016-11-07 07:12:46 -04:00
|
|
|
// increments num_defaults for each default found in filename
|
|
|
|
bool AP_Param::count_defaults_in_file(const char *filename, uint16_t &num_defaults, bool panic_on_error)
|
2015-04-01 22:24:45 -03:00
|
|
|
{
|
|
|
|
FILE *f = fopen(filename, "r");
|
2016-10-30 02:24:21 -03:00
|
|
|
if (f == nullptr) {
|
2015-04-01 22:24:45 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
char line[100];
|
|
|
|
|
|
|
|
/*
|
|
|
|
work out how many parameter default structures to allocate
|
|
|
|
*/
|
|
|
|
while (fgets(line, sizeof(line)-1, f)) {
|
|
|
|
char *pname;
|
|
|
|
float value;
|
|
|
|
if (!parse_param_line(line, &pname, value)) {
|
|
|
|
continue;
|
|
|
|
}
|
2017-01-22 06:42:54 -04:00
|
|
|
enum ap_var_type var_type;
|
|
|
|
if (!find(pname, &var_type)) {
|
2017-01-09 05:52:36 -04:00
|
|
|
if (panic_on_error) {
|
|
|
|
fclose(f);
|
|
|
|
::printf("invalid param %s in defaults file\n", pname);
|
|
|
|
AP_HAL::panic("AP_Param: Invalid param in defaults file");
|
|
|
|
return false;
|
|
|
|
} else {
|
|
|
|
continue;
|
|
|
|
}
|
2015-04-01 22:24:45 -03:00
|
|
|
}
|
|
|
|
num_defaults++;
|
|
|
|
}
|
|
|
|
fclose(f);
|
|
|
|
|
2016-11-07 06:29:30 -04:00
|
|
|
return true;
|
|
|
|
}
|
2015-04-01 22:24:45 -03:00
|
|
|
|
2016-11-07 06:29:30 -04:00
|
|
|
bool AP_Param::read_param_defaults_file(const char *filename)
|
|
|
|
{
|
|
|
|
FILE *f = fopen(filename, "r");
|
2016-10-30 02:24:21 -03:00
|
|
|
if (f == nullptr) {
|
2015-10-20 18:37:00 -03:00
|
|
|
AP_HAL::panic("AP_Param: Failed to re-open defaults file");
|
2015-04-01 22:24:45 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
uint16_t idx = 0;
|
2016-11-07 06:29:30 -04:00
|
|
|
char line[100];
|
2015-04-01 22:24:45 -03:00
|
|
|
while (fgets(line, sizeof(line)-1, f)) {
|
|
|
|
char *pname;
|
|
|
|
float value;
|
|
|
|
if (!parse_param_line(line, &pname, value)) {
|
|
|
|
continue;
|
|
|
|
}
|
2017-01-22 06:42:54 -04:00
|
|
|
enum ap_var_type var_type;
|
|
|
|
AP_Param *vp = find(pname, &var_type);
|
|
|
|
if (!vp) {
|
2017-01-09 05:52:36 -04:00
|
|
|
continue;
|
2015-04-01 22:24:45 -03:00
|
|
|
}
|
2017-01-22 06:42:54 -04:00
|
|
|
param_overrides[idx].object_ptr = vp;
|
2015-04-01 22:24:45 -03:00
|
|
|
param_overrides[idx].value = value;
|
|
|
|
idx++;
|
2017-01-09 23:38:45 -04:00
|
|
|
if (!vp->configured_in_storage()) {
|
|
|
|
vp->set_float(value, var_type);
|
|
|
|
}
|
2015-04-01 22:24:45 -03:00
|
|
|
}
|
|
|
|
fclose(f);
|
2016-11-07 06:29:30 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
load a default set of parameters from a file
|
|
|
|
*/
|
|
|
|
bool AP_Param::load_defaults_file(const char *filename, bool panic_on_error)
|
|
|
|
{
|
|
|
|
if (filename == nullptr) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2016-11-07 07:12:46 -04:00
|
|
|
char *mutable_filename = strdup(filename);
|
|
|
|
if (mutable_filename == nullptr) {
|
|
|
|
AP_HAL::panic("AP_Param: Failed to allocate mutable string");
|
|
|
|
}
|
|
|
|
|
2016-11-07 06:29:30 -04:00
|
|
|
uint16_t num_defaults = 0;
|
2016-11-07 07:12:46 -04:00
|
|
|
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, panic_on_error)) {
|
|
|
|
free(mutable_filename);
|
|
|
|
return false;
|
|
|
|
}
|
2016-11-07 06:29:30 -04:00
|
|
|
}
|
2016-11-07 07:12:46 -04:00
|
|
|
free(mutable_filename);
|
2016-11-07 06:29:30 -04:00
|
|
|
|
|
|
|
if (param_overrides != nullptr) {
|
|
|
|
free(param_overrides);
|
|
|
|
}
|
|
|
|
num_param_overrides = 0;
|
|
|
|
|
|
|
|
param_overrides = new param_override[num_defaults];
|
|
|
|
if (param_overrides == nullptr) {
|
|
|
|
AP_HAL::panic("AP_Param: Failed to allocate overrides");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2016-11-07 07:12:46 -04:00
|
|
|
saveptr = nullptr;
|
|
|
|
mutable_filename = strdup(filename);
|
|
|
|
if (mutable_filename == nullptr) {
|
|
|
|
AP_HAL::panic("AP_Param: Failed to allocate mutable string");
|
|
|
|
}
|
|
|
|
for (char *pname = strtok_r(mutable_filename, ",", &saveptr);
|
|
|
|
pname != nullptr;
|
|
|
|
pname = strtok_r(nullptr, ",", &saveptr)) {
|
|
|
|
if (!read_param_defaults_file(pname)) {
|
|
|
|
free(mutable_filename);
|
|
|
|
return false;
|
|
|
|
}
|
2016-11-07 06:29:30 -04:00
|
|
|
}
|
2016-11-07 07:12:46 -04:00
|
|
|
free(mutable_filename);
|
2015-04-01 22:24:45 -03:00
|
|
|
|
|
|
|
num_param_overrides = num_defaults;
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif // HAL_OS_POSIX_IO
|
|
|
|
|
2017-11-09 02:45:58 -04:00
|
|
|
/*
|
|
|
|
count the number of embedded parameter defaults
|
|
|
|
*/
|
|
|
|
bool AP_Param::count_embedded_param_defaults(uint16_t &count, bool panic_on_error)
|
|
|
|
{
|
|
|
|
const volatile char *ptr = param_defaults_data.data;
|
|
|
|
uint16_t length = param_defaults_data.length;
|
|
|
|
count = 0;
|
|
|
|
|
|
|
|
while (length) {
|
|
|
|
char line[100];
|
|
|
|
char *pname;
|
|
|
|
float value;
|
|
|
|
uint16_t i;
|
|
|
|
uint16_t n = MIN(length, sizeof(line)-1);
|
|
|
|
for (i=0;i<n;i++) {
|
|
|
|
if (ptr[i] == '\n') {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (i == n) {
|
|
|
|
// no newline
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
memcpy(line, (void *)ptr, i);
|
|
|
|
line[i] = 0;
|
|
|
|
|
|
|
|
length -= i+1;
|
|
|
|
ptr += i+1;
|
|
|
|
|
|
|
|
if (line[0] == '#' || line[0] == 0) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!parse_param_line(line, &pname, value)) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
enum ap_var_type var_type;
|
|
|
|
if (!find(pname, &var_type)) {
|
|
|
|
if (panic_on_error) {
|
|
|
|
::printf("invalid param %s in defaults file\n", pname);
|
|
|
|
AP_HAL::panic("AP_Param: Invalid param in embedded defaults");
|
|
|
|
} else {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
count++;
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
load a default set of parameters from a embedded parameter region
|
|
|
|
*/
|
|
|
|
void AP_Param::load_embedded_param_defaults(bool panic_on_error)
|
|
|
|
{
|
|
|
|
if (param_overrides != nullptr) {
|
|
|
|
free(param_overrides);
|
|
|
|
param_overrides = nullptr;
|
|
|
|
}
|
|
|
|
|
|
|
|
num_param_overrides = 0;
|
|
|
|
uint16_t num_defaults = 0;
|
|
|
|
if (!count_embedded_param_defaults(num_defaults, panic_on_error)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
param_overrides = new param_override[num_defaults];
|
|
|
|
if (param_overrides == nullptr) {
|
|
|
|
AP_HAL::panic("AP_Param: Failed to allocate overrides");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
const volatile char *ptr = param_defaults_data.data;
|
|
|
|
uint16_t length = param_defaults_data.length;
|
|
|
|
uint16_t idx = 0;
|
|
|
|
|
|
|
|
while (length) {
|
|
|
|
char line[100];
|
|
|
|
char *pname;
|
|
|
|
float value;
|
|
|
|
uint16_t i;
|
|
|
|
uint16_t n = MIN(length, sizeof(line)-1);
|
|
|
|
for (i=0;i<n;i++) {
|
|
|
|
if (ptr[i] == '\n') {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (i == n) {
|
|
|
|
// no newline
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
memcpy(line, (void*)ptr, i);
|
|
|
|
line[i] = 0;
|
|
|
|
|
|
|
|
length -= i+1;
|
|
|
|
ptr += i+1;
|
|
|
|
|
|
|
|
if (line[0] == '#' || line[0] == 0) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!parse_param_line(line, &pname, value)) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
enum ap_var_type var_type;
|
|
|
|
AP_Param *vp = find(pname, &var_type);
|
|
|
|
if (!vp) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
param_overrides[idx].object_ptr = vp;
|
|
|
|
param_overrides[idx].value = value;
|
|
|
|
idx++;
|
|
|
|
if (!vp->configured_in_storage()) {
|
|
|
|
vp->set_float(value, var_type);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
num_param_overrides = num_defaults;
|
|
|
|
}
|
|
|
|
|
2015-04-01 22:24:45 -03:00
|
|
|
/*
|
|
|
|
find a default value given a pointer to a default value in flash
|
|
|
|
*/
|
2017-01-22 06:42:54 -04:00
|
|
|
float AP_Param::get_default_value(const AP_Param *vp, const float *def_value_ptr)
|
2015-04-01 22:24:45 -03:00
|
|
|
{
|
|
|
|
for (uint16_t i=0; i<num_param_overrides; i++) {
|
2017-01-22 06:42:54 -04:00
|
|
|
if (vp == param_overrides[i].object_ptr) {
|
2015-04-01 22:24:45 -03:00
|
|
|
return param_overrides[i].value;
|
|
|
|
}
|
|
|
|
}
|
2015-12-23 13:25:02 -04:00
|
|
|
return *def_value_ptr;
|
2015-04-01 22:24:45 -03:00
|
|
|
}
|
|
|
|
|
2015-11-08 23:27:39 -04:00
|
|
|
|
2016-03-16 03:03:30 -03:00
|
|
|
void AP_Param::send_parameter(const char *name, enum ap_var_type var_type, uint8_t idx) const
|
2015-11-08 23:27:39 -04:00
|
|
|
{
|
2016-03-16 03:03:30 -03:00
|
|
|
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) {
|
2015-11-08 23:27:39 -04:00
|
|
|
// nice and simple for scalar types
|
2016-03-16 03:03:30 -03:00
|
|
|
GCS_MAVLINK::send_parameter_value_all(name, var_type, cast_to_float(var_type));
|
2015-11-08 23:27:39 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2016-03-16 03:03:30 -03:00
|
|
|
// 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
|
2016-02-12 13:40:00 -04:00
|
|
|
const Vector3f &v = ((AP_Vector3f *)this)->get();
|
2016-03-16 03:03:30 -03:00
|
|
|
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_MAVLINK::send_parameter_value_all(name2, AP_PARAM_FLOAT, v.x);
|
2015-11-08 23:27:39 -04:00
|
|
|
name_axis = 'Y';
|
2016-03-16 03:03:30 -03:00
|
|
|
GCS_MAVLINK::send_parameter_value_all(name2, AP_PARAM_FLOAT, v.y);
|
2015-11-08 23:27:39 -04:00
|
|
|
name_axis = 'Z';
|
2016-03-16 03:03:30 -03:00
|
|
|
GCS_MAVLINK::send_parameter_value_all(name2, AP_PARAM_FLOAT, v.z);
|
2015-11-08 23:27:39 -04:00
|
|
|
}
|
2016-01-05 23:41:07 -04:00
|
|
|
|
|
|
|
/*
|
2017-04-29 01:05:26 -03:00
|
|
|
return count of all scalar parameters.
|
|
|
|
Note that this function may be called from the IO thread, so needs
|
|
|
|
to be thread safe
|
2016-01-05 23:41:07 -04:00
|
|
|
*/
|
|
|
|
uint16_t AP_Param::count_parameters(void)
|
|
|
|
{
|
|
|
|
// if we haven't cached the parameter count yet...
|
2017-04-29 01:05:26 -03:00
|
|
|
uint16_t ret = _parameter_count;
|
|
|
|
if (0 == ret) {
|
2016-01-05 23:41:07 -04:00
|
|
|
AP_Param *vp;
|
|
|
|
AP_Param::ParamToken token;
|
|
|
|
|
2018-03-31 11:40:41 -03:00
|
|
|
for (vp = AP_Param::first(&token, nullptr);
|
|
|
|
vp != nullptr;
|
|
|
|
vp = AP_Param::next_scalar(&token, nullptr)) {
|
2017-04-29 01:05:26 -03:00
|
|
|
ret++;
|
2018-03-31 11:40:41 -03:00
|
|
|
}
|
2017-04-29 01:05:26 -03:00
|
|
|
_parameter_count = ret;
|
2016-01-05 23:41:07 -04:00
|
|
|
}
|
2017-04-29 01:05:26 -03:00
|
|
|
return ret;
|
2016-01-05 23:41:07 -04:00
|
|
|
}
|
|
|
|
|
2016-04-01 02:39:51 -03:00
|
|
|
/*
|
|
|
|
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;
|
|
|
|
}
|
2017-10-20 15:10:09 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
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;
|
|
|
|
}
|
2018-01-24 00:42:09 -04:00
|
|
|
|
|
|
|
#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:
|
|
|
|
port->printf("%s: %d\n", s, (int)((AP_Int8 *)ap)->get());
|
|
|
|
break;
|
|
|
|
case AP_PARAM_INT16:
|
|
|
|
port->printf("%s: %d\n", s, (int)((AP_Int16 *)ap)->get());
|
|
|
|
break;
|
|
|
|
case AP_PARAM_INT32:
|
|
|
|
port->printf("%s: %ld\n", s, (long)((AP_Int32 *)ap)->get());
|
|
|
|
break;
|
|
|
|
case AP_PARAM_FLOAT:
|
|
|
|
port->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;
|
|
|
|
|
|
|
|
for (ap=AP_Param::first(&token, &type);
|
|
|
|
ap;
|
|
|
|
ap=AP_Param::next_scalar(&token, &type)) {
|
|
|
|
if (showKeyValues) {
|
|
|
|
port->printf("Key %i: Index %i: GroupElement %i : ", token.key, token.idx, token.group_element);
|
|
|
|
}
|
|
|
|
show(ap, token, type, port);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif // AP_PARAM_KEY_DUMP
|
|
|
|
|