Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : //
17 : //
18 :
19 : // total up and check overflow
20 : // check size of group var_info
21 :
22 : /// @file AP_Param.cpp
23 : /// @brief The AP variable store.
24 : #include "AP_Param.h"
25 :
26 : #include <cmath>
27 : #include <string.h>
28 :
29 : #include <AP_Common/AP_Common.h>
30 : #include <AP_HAL/AP_HAL.h>
31 : #include <AP_Math/AP_Math.h>
32 : #include <GCS_MAVLink/GCS.h>
33 : #include <StorageManager/StorageManager.h>
34 : #include <AP_BoardConfig/AP_BoardConfig.h>
35 : #include <AP_InternalError/AP_InternalError.h>
36 : #include <AP_Filesystem/AP_Filesystem.h>
37 : #include <stdio.h>
38 : #include <AP_ROMFS/AP_ROMFS.h>
39 :
40 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
41 : #include <SITL/SITL.h>
42 : #endif
43 :
44 : #include "AP_Param_config.h"
45 :
46 : extern const AP_HAL::HAL &hal;
47 :
48 : uint16_t AP_Param::sentinel_offset;
49 :
50 : // singleton instance
51 : AP_Param *AP_Param::_singleton;
52 :
53 : #ifndef AP_PARAM_STORAGE_BAK_ENABLED
54 : // we only have a storage region for backup storage if we have at
55 : // least 32768 bytes or storage. We also don't enable when using flash
56 : // storage as this can lead to loss of storage when updating to a
57 : // larger storage size
58 : #define AP_PARAM_STORAGE_BAK_ENABLED (HAL_STORAGE_SIZE>=32768) && !defined(STORAGE_FLASH_PAGE)
59 : #endif
60 :
61 :
62 : #define ENABLE_DEBUG 0
63 :
64 : #if ENABLE_DEBUG
65 : # define FATAL(fmt, args ...) AP_HAL::panic(fmt, ## args);
66 : # define Debug(fmt, args ...) do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
67 : #else
68 : # define FATAL(fmt, args ...) AP_HAL::panic("Bad parameter table");
69 : # define Debug(fmt, args ...)
70 : #endif
71 :
72 : #if HAL_GCS_ENABLED
73 : #define GCS_SEND_PARAM(name, type, v) gcs().send_parameter_value(name, type, v)
74 : #else
75 : #define GCS_SEND_PARAM(name, type, v)
76 : #endif
77 :
78 : // Note about AP_Vector3f handling.
79 : // The code has special cases for AP_Vector3f to allow it to be viewed
80 : // as both a single 3 element vector and as a set of 3 AP_Float
81 : // variables. This is done to make it possible for MAVLink to see
82 : // vectors as parameters, which allows users to save their compass
83 : // offsets in MAVLink parameter files. The code involves quite a few
84 : // special cases which could be generalised to any vector/matrix type
85 : // if we end up needing this behaviour for other than AP_Vector3f
86 :
87 :
88 : // static member variables for AP_Param.
89 : //
90 :
91 : // number of rows in the _var_info[] table
92 : uint16_t AP_Param::_num_vars;
93 :
94 : #if AP_PARAM_DYNAMIC_ENABLED
95 : uint16_t AP_Param::_num_vars_base;
96 : AP_Param::Info *AP_Param::_var_info_dynamic;
97 : static const char *_empty_string = "";
98 : uint8_t AP_Param::_dynamic_table_sizes[AP_PARAM_MAX_DYNAMIC];
99 : #endif
100 :
101 : // cached parameter count
102 : uint16_t AP_Param::_parameter_count;
103 : uint16_t AP_Param::_count_marker;
104 : uint16_t AP_Param::_count_marker_done;
105 : HAL_Semaphore AP_Param::_count_sem;
106 :
107 : // storage and naming information about all types that can be saved
108 : const AP_Param::Info *AP_Param::_var_info;
109 :
110 : struct AP_Param::param_override *AP_Param::param_overrides;
111 : uint16_t AP_Param::param_overrides_len;
112 : uint16_t AP_Param::num_param_overrides;
113 : uint16_t AP_Param::num_read_only;
114 :
115 : // goes true if we run out of param space
116 : bool AP_Param::eeprom_full;
117 :
118 : ObjectBuffer_TS<AP_Param::param_save> AP_Param::save_queue{30};
119 : bool AP_Param::registered_save_handler;
120 :
121 : bool AP_Param::done_all_default_params;
122 :
123 : AP_Param::defaults_list *AP_Param::default_list;
124 :
125 : // we need a dummy object for the parameter save callback
126 : static AP_Param save_dummy;
127 :
128 : #if AP_PARAM_MAX_EMBEDDED_PARAM > 0
129 : /*
130 : this holds default parameters in the normal NAME=value form for a
131 : parameter file. It can be manipulated by apj_tool.py to change the
132 : defaults on a binary without recompiling
133 : */
134 : const AP_Param::param_defaults_struct AP_Param::param_defaults_data = {
135 : "PARMDEF",
136 : { 0x55, 0x37, 0xf4, 0xa0, 0x38, 0x5d, 0x48, 0x5b },
137 : AP_PARAM_MAX_EMBEDDED_PARAM,
138 : 0
139 : };
140 : #endif
141 :
142 : // storage object
143 : StorageAccess AP_Param::_storage(StorageManager::StorageParam);
144 :
145 : #if AP_PARAM_STORAGE_BAK_ENABLED
146 : // backup storage object
147 : StorageAccess AP_Param::_storage_bak(StorageManager::StorageParamBak);
148 : #endif
149 :
150 : // flags indicating frame type
151 : uint16_t AP_Param::_frame_type_flags;
152 :
153 : // write to EEPROM
154 99650 : void AP_Param::eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size)
155 : {
156 99650 : _storage.write_block(ofs, ptr, size);
157 : #if AP_PARAM_STORAGE_BAK_ENABLED
158 : _storage_bak.write_block(ofs, ptr, size);
159 : #endif
160 99650 : }
161 :
162 : bool AP_Param::_hide_disabled_groups = true;
163 :
164 : // write a sentinel value at the given offset
165 14602 : void AP_Param::write_sentinel(uint16_t ofs)
166 : {
167 : struct Param_header phdr;
168 14602 : phdr.type = _sentinel_type;
169 14602 : set_key(phdr, _sentinel_key);
170 14602 : phdr.group_element = _sentinel_group;
171 14602 : eeprom_write_check(&phdr, ofs, sizeof(phdr));
172 14602 : sentinel_offset = ofs;
173 14602 : }
174 :
175 : // erase all EEPROM variables by re-writing the header and adding
176 : // a sentinel
177 600 : void AP_Param::erase_all(void)
178 : {
179 : struct EEPROM_header hdr;
180 :
181 : // write the header
182 600 : hdr.magic[0] = k_EEPROM_magic0;
183 600 : hdr.magic[1] = k_EEPROM_magic1;
184 600 : hdr.revision = k_EEPROM_revision;
185 600 : hdr.spare = 0;
186 600 : eeprom_write_check(&hdr, 0, sizeof(hdr));
187 :
188 : // add a sentinel directly after the header
189 600 : write_sentinel(sizeof(struct EEPROM_header));
190 600 : }
191 :
192 : /* the 'group_id' of a element of a group is the 18 bit identifier
193 : used to distinguish between this element of the group and other
194 : elements of the same group. It is calculated using a bit shift per
195 : level of nesting, so the first level of nesting gets 6 bits the 2nd
196 : level gets the next 6 bits, and the 3rd level gets the last 6
197 : bits. This limits groups to having at most 64 elements.
198 : */
199 2768329221 : uint32_t AP_Param::group_id(const struct GroupInfo *grpinfo, uint32_t base, uint8_t i, uint8_t shift)
200 : {
201 2768329221 : if (grpinfo[i].idx == 0 && shift != 0 && !(grpinfo[i].flags & AP_PARAM_FLAG_NO_SHIFT)) {
202 : /*
203 : this is a special case for a bug in the original design. An
204 : idx of 0 shifted by n bits is still zero, which makes it
205 : indistinguishable from a different parameter. This can lead
206 : to parameter loops. We use index 63 for that case.
207 : */
208 20761005 : return base + (63U<<shift);
209 : }
210 2747568216 : return base + (grpinfo[i].idx<<shift);
211 : }
212 :
213 :
214 : /*
215 : check if a frame type should be included. A frame is included if
216 : either there are no frame type flags on a parameter or if at least
217 : one of the flags has been set by set_frame_type_flags()
218 : */
219 2627271543 : bool AP_Param::check_frame_type(uint16_t flags)
220 : {
221 2627271543 : if (flags & AP_PARAM_FLAG_HIDDEN) {
222 : // hidden on all frames
223 1503302 : return false;
224 : }
225 2625768241 : uint16_t frame_flags = flags >> AP_PARAM_FRAME_TYPE_SHIFT;
226 2625768241 : if (frame_flags == 0) {
227 2607195732 : return true;
228 : }
229 18572509 : return (frame_flags & _frame_type_flags) != 0;
230 : }
231 :
232 : // validate a group info table
233 1134045 : void AP_Param::check_group_info(const struct AP_Param::GroupInfo * group_info,
234 : uint16_t * total_size,
235 : uint8_t group_shift,
236 : uint8_t prefix_length)
237 : {
238 : uint8_t type;
239 1134045 : uint64_t used_mask = 0;
240 9041991 : for (uint8_t i=0;
241 9041991 : (type=group_info[i].type) != AP_PARAM_NONE;
242 : i++) {
243 7907946 : uint8_t idx = group_info[i].idx;
244 7907946 : if (idx >= (1<<_group_level_shift)) {
245 0 : FATAL("idx too large (%u) in %s", idx, group_info[i].name);
246 : }
247 7907946 : if (group_shift != 0 && idx == 0) {
248 : // treat idx 0 as 63 for duplicates. See group_id()
249 57893 : idx = 63;
250 : }
251 7907946 : if (used_mask & (1ULL<<idx)) {
252 0 : FATAL("Duplicate group idx %u for %s", idx, group_info[i].name);
253 : }
254 7907946 : used_mask |= (1ULL<<idx);
255 7907946 : if (type == AP_PARAM_GROUP) {
256 : // a nested group
257 1158670 : if (group_shift + _group_level_shift >= _group_bits) {
258 0 : FATAL("double group nesting in %s", group_info[i].name);
259 : }
260 1158670 : const struct GroupInfo *ginfo = get_group_info(group_info[i]);
261 1158670 : if (ginfo == nullptr) {
262 110665 : continue;
263 : }
264 1048005 : check_group_info(ginfo, total_size, group_shift + _group_level_shift, prefix_length + strlen(group_info[i].name));
265 1048005 : continue;
266 1048005 : }
267 6749276 : uint8_t size = type_size((enum ap_var_type)type);
268 6749276 : if (size == 0) {
269 0 : FATAL("invalid type in %s", group_info[i].name);
270 : }
271 6749276 : uint8_t param_name_length = prefix_length + strlen(group_info[i].name);
272 6749276 : if (type == AP_PARAM_VECTOR3F) {
273 : // need room for _X/_Y/_Z
274 274157 : param_name_length += 2;
275 : }
276 6749276 : if (param_name_length > 16) {
277 0 : FATAL("suffix is too long in %s (%u > 16)", group_info[i].name, param_name_length);
278 : }
279 6749276 : (*total_size) += size + sizeof(struct Param_header);
280 : }
281 1134045 : }
282 :
283 : // check for duplicate key values
284 201816 : bool AP_Param::duplicate_key(uint16_t vindex, uint16_t key)
285 : {
286 10892011 : for (uint16_t i=vindex+1; i<_num_vars; i++) {
287 10690195 : uint16_t key2 = var_info(i).key;
288 10690195 : if (key2 == key) {
289 : // no duplicate keys allowed
290 0 : return true;
291 : }
292 : }
293 201816 : return false;
294 : }
295 :
296 : /*
297 : get group_info pointer for a group
298 : */
299 540116536 : const struct AP_Param::GroupInfo *AP_Param::get_group_info(const struct GroupInfo &ginfo)
300 : {
301 540116536 : if (ginfo.flags & AP_PARAM_FLAG_INFO_POINTER) {
302 41767198 : return *ginfo.group_info_ptr;
303 : }
304 498349338 : return ginfo.group_info;
305 : }
306 :
307 : /*
308 : get group_info pointer for a group
309 : */
310 44833089 : const struct AP_Param::GroupInfo *AP_Param::get_group_info(const struct Info &info)
311 : {
312 44833089 : if (info.flags & AP_PARAM_FLAG_INFO_POINTER) {
313 1023197 : return *info.group_info_ptr;
314 : }
315 43809892 : return info.group_info;
316 : }
317 :
318 : // validate the _var_info[] table
319 2094 : void AP_Param::check_var_info(void)
320 : {
321 2094 : uint16_t total_size = sizeof(struct EEPROM_header);
322 :
323 205912 : for (uint16_t i=0; i<_num_vars; i++) {
324 203818 : const auto &info = var_info(i);
325 203818 : uint8_t type = info.type;
326 203818 : uint16_t key = info.key;
327 203818 : if (type == AP_PARAM_GROUP) {
328 88042 : if (i == 0) {
329 0 : FATAL("first element can't be a group, for first() call");
330 : }
331 88042 : const struct GroupInfo *group_info = get_group_info(info);
332 88042 : if (group_info == nullptr) {
333 2002 : continue;
334 : }
335 86040 : check_group_info(group_info, &total_size, 0, strlen(info.name));
336 : } else {
337 115776 : uint8_t size = type_size((enum ap_var_type)type);
338 115776 : if (size == 0) {
339 : // not a valid type - the top level list can't contain
340 : // AP_PARAM_NONE
341 0 : FATAL("AP_PARAM_NONE at top level");
342 : }
343 115776 : total_size += size + sizeof(struct Param_header);
344 : }
345 201816 : if (duplicate_key(i, key)) {
346 0 : FATAL("duplicate key");
347 : }
348 201816 : if (type != AP_PARAM_GROUP && (info.flags & AP_PARAM_FLAG_POINTER)) {
349 0 : FATAL("only groups can be pointers");
350 : }
351 : }
352 :
353 : // we no longer check if total_size is larger than _eeprom_size,
354 : // as we allow for more variables than could fit, relying on not
355 : // saving default values
356 2094 : }
357 :
358 :
359 : // setup the _var_info[] table
360 2094 : bool AP_Param::setup(void)
361 : {
362 2094 : struct EEPROM_header hdr {};
363 :
364 : // check the header
365 2094 : _storage.read_block(&hdr, 0, sizeof(hdr));
366 :
367 : #if AP_PARAM_STORAGE_BAK_ENABLED
368 : struct EEPROM_header hdr2 {};
369 : _storage_bak.read_block(&hdr2, 0, sizeof(hdr2));
370 : #endif
371 :
372 2094 : if (hdr.magic[0] != k_EEPROM_magic0 ||
373 2093 : hdr.magic[1] != k_EEPROM_magic1 ||
374 2093 : hdr.revision != k_EEPROM_revision) {
375 : #if AP_PARAM_STORAGE_BAK_ENABLED
376 : if (hdr2.magic[0] == k_EEPROM_magic0 &&
377 : hdr2.magic[1] == k_EEPROM_magic1 &&
378 : hdr2.revision == k_EEPROM_revision &&
379 : _storage.copy_area(_storage_bak)) {
380 : // restored from backup
381 : INTERNAL_ERROR(AP_InternalError::error_t::params_restored);
382 : return true;
383 : }
384 : #endif // AP_PARAM_STORAGE_BAK_ENABLED
385 : // header doesn't match. We can't recover any variables. Wipe
386 : // the header and setup the sentinel directly after the header
387 : Debug("bad header in setup - erasing");
388 1 : erase_all();
389 : }
390 :
391 : #if AP_PARAM_STORAGE_BAK_ENABLED
392 : // ensure that backup is in sync with primary
393 : _storage_bak.copy_area(_storage);
394 : #endif
395 :
396 2094 : return true;
397 : }
398 :
399 : // check if AP_Param has been initialised
400 0 : bool AP_Param::initialised(void)
401 : {
402 0 : return _var_info != nullptr;
403 : }
404 :
405 : /*
406 : adjust offset of a group element for nested groups and group pointers
407 :
408 : The new_offset variable is relative to the vindex base. This makes
409 : dealing with pointer groups tricky
410 : */
411 500121945 : bool AP_Param::adjust_group_offset(uint16_t vindex, const struct GroupInfo &group_info, ptrdiff_t &new_offset)
412 : {
413 500121945 : if (group_info.flags & AP_PARAM_FLAG_NESTED_OFFSET) {
414 462280680 : new_offset += group_info.offset;
415 462280680 : return true;
416 : }
417 37841265 : if (group_info.flags & AP_PARAM_FLAG_POINTER) {
418 : // group_info.offset refers to a pointer
419 : ptrdiff_t base;
420 36158632 : if (!get_base(var_info(vindex), base)) {
421 : // the object is not allocated yet
422 24266705 : return false;
423 : }
424 36158788 : void **p = (void **)(base + new_offset + group_info.offset);
425 36158788 : if (*p == nullptr) {
426 : // the object is not allocated yet
427 24266705 : return false;
428 : }
429 : // calculate offset that is needed to take base object and adjust for this object
430 11892083 : new_offset = ((ptrdiff_t)*p) - base;
431 : }
432 13574716 : return true;
433 : }
434 :
435 : /*
436 : get the base pointer for a variable, accounting for AP_PARAM_FLAG_POINTER
437 : */
438 355569801 : bool AP_Param::get_base(const struct Info &info, ptrdiff_t &base)
439 : {
440 355569801 : if (info.flags & AP_PARAM_FLAG_POINTER) {
441 7836287 : base = *(ptrdiff_t *)info.ptr;
442 7836287 : return (base != (ptrdiff_t)0);
443 : }
444 347733514 : base = (ptrdiff_t)info.ptr;
445 347733514 : return true;
446 : }
447 :
448 : namespace AP {
449 :
450 0 : AP_Param *param()
451 : {
452 0 : return AP_Param::get_singleton();
453 : }
454 :
455 : }
456 :
457 : // find the info structure given a header and a group_info table
458 : // return the Info structure and a pointer to the variables storage
459 3212541 : const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header phdr, void **ptr,
460 : uint16_t vindex,
461 : const struct GroupInfo *group_info,
462 : uint32_t group_base,
463 : uint8_t group_shift,
464 : ptrdiff_t group_offset)
465 : {
466 : uint8_t type;
467 27100503 : for (uint8_t i=0;
468 27100503 : (type=group_info[i].type) != AP_PARAM_NONE;
469 : i++) {
470 25047170 : if (type == AP_PARAM_GROUP) {
471 : // a nested group
472 4027755 : if (group_shift + _group_level_shift >= _group_bits) {
473 : // too deeply nested - this should have been caught by
474 : // setup() !
475 533248 : return nullptr;
476 : }
477 4027755 : const struct GroupInfo *ginfo = get_group_info(group_info[i]);
478 4027755 : if (ginfo == nullptr) {
479 3494507 : continue;
480 : }
481 2561323 : ptrdiff_t new_offset = group_offset;
482 :
483 2561323 : if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
484 71860 : continue;
485 : }
486 :
487 2489463 : const struct AP_Param::Info *ret = find_by_header_group(phdr, ptr, vindex, ginfo,
488 : group_id(group_info, group_base, i, group_shift),
489 2489463 : group_shift + _group_level_shift, new_offset);
490 2489463 : if (ret != nullptr) {
491 533248 : return ret;
492 : }
493 1956215 : continue;
494 1956215 : }
495 21019415 : if (group_id(group_info, group_base, i, group_shift) == phdr.group_element && type == phdr.type) {
496 : // found a group element
497 : ptrdiff_t base;
498 628070 : if (!get_base(var_info(vindex), base)) {
499 2110 : continue;
500 : }
501 625960 : *ptr = (void*)(base + group_info[i].offset + group_offset);
502 625960 : return &var_info(vindex);
503 : }
504 : }
505 2053333 : return nullptr;
506 : }
507 :
508 : // find the info structure given a header
509 : // return the Info structure and a pointer to the variables storage
510 731125 : const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr, void **ptr)
511 : {
512 : // loop over all named variables
513 70878016 : for (uint16_t i=0; i<_num_vars; i++) {
514 70874820 : const auto &info = var_info(i);
515 70874820 : uint8_t type = info.type;
516 70874820 : uint16_t key = info.key;
517 70874820 : if (key != get_key(phdr)) {
518 : // not the right key
519 70144328 : continue;
520 : }
521 730492 : if (type == AP_PARAM_GROUP) {
522 725641 : const struct GroupInfo *group_info = get_group_info(info);
523 725641 : if (group_info == nullptr) {
524 2563 : continue;
525 : }
526 723078 : return find_by_header_group(phdr, ptr, i, group_info, 0, 0, 0);
527 : }
528 4851 : if (type == phdr.type) {
529 : // found it
530 : ptrdiff_t base;
531 4851 : if (!get_base(info, base)) {
532 0 : return nullptr;
533 : }
534 4851 : *ptr = (void*)base;
535 4851 : return &info;
536 : }
537 : }
538 3196 : return nullptr;
539 : }
540 :
541 : // find the info structure for a variable in a group
542 202509629 : const struct AP_Param::Info *AP_Param::find_var_info_group(const struct GroupInfo * group_info,
543 : uint16_t vindex,
544 : uint32_t group_base,
545 : uint8_t group_shift,
546 : ptrdiff_t group_offset,
547 : uint32_t * group_element,
548 : const struct GroupInfo * &group_ret,
549 : struct GroupNesting &group_nesting,
550 : uint8_t * idx) const
551 : {
552 : ptrdiff_t base;
553 202509629 : if (!get_base(var_info(vindex), base)) {
554 0 : return nullptr;
555 : }
556 : uint8_t type;
557 1852218075 : for (uint8_t i=0;
558 1852218075 : (type=group_info[i].type) != AP_PARAM_NONE;
559 : i++) {
560 1659963169 : ptrdiff_t ofs = group_info[i].offset + group_offset;
561 1659963169 : if (type == AP_PARAM_GROUP) {
562 203125909 : const struct GroupInfo *ginfo = get_group_info(group_info[i]);
563 203125049 : if (ginfo == nullptr) {
564 24971761 : continue;
565 : }
566 : // a nested group
567 185820503 : if (group_shift + _group_level_shift >= _group_bits) {
568 : // too deeply nested - this should have been caught by
569 : // setup() !
570 4847778 : return nullptr;
571 : }
572 : const struct AP_Param::Info *info;
573 185820503 : ptrdiff_t new_offset = group_offset;
574 185820503 : if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
575 7667215 : continue;
576 : }
577 178153311 : if (group_nesting.level >= group_nesting.numlevels) {
578 0 : return nullptr;
579 : }
580 178153311 : group_nesting.group_ret[group_nesting.level++] = &group_info[i];
581 178154096 : info = find_var_info_group(ginfo, vindex,
582 : group_id(group_info, group_base, i, group_shift),
583 178153311 : group_shift + _group_level_shift,
584 : new_offset,
585 : group_element,
586 : group_ret,
587 : group_nesting,
588 : idx);
589 178156536 : if (info != nullptr) {
590 4847778 : return info;
591 : }
592 173308758 : group_nesting.level--;
593 1456837260 : } else if ((ptrdiff_t) this == base + ofs) {
594 4947235 : *group_element = group_id(group_info, group_base, i, group_shift);
595 4947234 : group_ret = &group_info[i];
596 4947234 : *idx = 0;
597 4947234 : return &var_info(vindex);
598 1451890025 : } else if (type == AP_PARAM_VECTOR3F &&
599 109292839 : (base+ofs+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
600 109078381 : base+ofs+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
601 : // we are inside a Vector3f. We need to work out which
602 : // element of the vector the current object refers to.
603 463561 : *idx = (((ptrdiff_t) this) - (base+ofs))/sizeof(float);
604 463561 : *group_element = group_id(group_info, group_base, i, group_shift);
605 428552 : group_ret = &group_info[i];
606 428552 : return &var_info(vindex);
607 : }
608 : }
609 192254906 : return nullptr;
610 : }
611 :
612 : // find the info structure for a variable
613 1383430 : const struct AP_Param::Info *AP_Param::find_var_info(uint32_t * group_element,
614 : const struct GroupInfo * &group_ret,
615 : struct GroupNesting &group_nesting,
616 : uint8_t * idx) const
617 : {
618 1383430 : group_ret = nullptr;
619 :
620 87957028 : for (uint16_t i=0; i<_num_vars; i++) {
621 87874224 : const auto &info = var_info(i);
622 87874223 : uint8_t type = info.type;
623 : ptrdiff_t base;
624 87874223 : if (!get_base(info, base)) {
625 709138 : continue;
626 : }
627 87165120 : if (type == AP_PARAM_GROUP) {
628 20183826 : const struct GroupInfo *group_info = get_group_info(info);
629 20183820 : if (group_info == nullptr) {
630 0 : continue;
631 : }
632 : const struct AP_Param::Info *info2;
633 20183820 : info2 = find_var_info_group(group_info, i, 0, 0, 0, group_element, group_ret, group_nesting, idx);
634 20183816 : if (info2 != nullptr) {
635 1300650 : return info2;
636 : }
637 66981294 : } else if (base == (ptrdiff_t) this) {
638 98805 : *group_element = 0;
639 98805 : *idx = 0;
640 98805 : return &info;
641 66882489 : } else if (type == AP_PARAM_VECTOR3F &&
642 26 : (base+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
643 26 : base+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
644 : // we are inside a Vector3f. Work out which element we are
645 : // referring to.
646 0 : *idx = (((ptrdiff_t) this) - base)/sizeof(float);
647 0 : *group_element = 0;
648 0 : return &info;
649 : }
650 : }
651 82804 : return nullptr;
652 : }
653 :
654 :
655 : // find the info structure for a variable
656 4341176 : const struct AP_Param::Info *AP_Param::find_var_info_token(const ParamToken &token,
657 : uint32_t * group_element,
658 : const struct GroupInfo * &group_ret,
659 : struct GroupNesting &group_nesting,
660 : uint8_t * idx) const
661 : {
662 4341176 : uint16_t i = token.key;
663 4341176 : const auto &info = var_info(i);
664 4341176 : uint8_t type = info.type;
665 : ptrdiff_t base;
666 4341176 : if (!get_base(info, base)) {
667 0 : return nullptr;
668 : }
669 4341176 : group_ret = nullptr;
670 :
671 4341176 : if (type == AP_PARAM_GROUP) {
672 4173943 : const struct GroupInfo *group_info = get_group_info(info);
673 4173943 : if (group_info == nullptr) {
674 0 : return nullptr;
675 : }
676 : const struct AP_Param::Info *info2;
677 4173943 : info2 = find_var_info_group(group_info, i, 0, 0, 0, group_element, group_ret, group_nesting, idx);
678 4173943 : if (info2 != nullptr) {
679 4173943 : return info2;
680 : }
681 167233 : } else if (base == (ptrdiff_t) this) {
682 167233 : *group_element = 0;
683 167233 : *idx = 0;
684 167233 : return &info;
685 0 : } else if (type == AP_PARAM_VECTOR3F &&
686 0 : (base+(ptrdiff_t)sizeof(float) == (ptrdiff_t) this ||
687 0 : base+2*(ptrdiff_t)sizeof(float) == (ptrdiff_t) this)) {
688 : // we are inside a Vector3f. Work out which element we are
689 : // referring to.
690 0 : *idx = (((ptrdiff_t) this) - base)/sizeof(float);
691 0 : *group_element = 0;
692 0 : return &info;
693 : }
694 0 : return nullptr;
695 : }
696 :
697 : // return the storage size for a AP_PARAM_* type.
698 : // NOTE: if you add a type here, also add it to AP_Param_value_storage
699 : // (below) so the value buffers used during conversion stay big enough.
700 77835457 : uint8_t AP_Param::type_size(enum ap_var_type type)
701 : {
702 77835457 : switch (type) {
703 0 : case AP_PARAM_NONE:
704 : case AP_PARAM_GROUP:
705 0 : return 0;
706 23838705 : case AP_PARAM_INT8:
707 23838705 : return 1;
708 15193583 : case AP_PARAM_INT16:
709 15193583 : return 2;
710 18221516 : case AP_PARAM_INT32:
711 18221516 : return 4;
712 16318824 : case AP_PARAM_FLOAT:
713 16318824 : return 4;
714 4262829 : case AP_PARAM_VECTOR3F:
715 4262829 : return 3*4;
716 : }
717 : Debug("unknown type %d\n", type);
718 0 : return 0;
719 : }
720 :
721 : // a union of every storable parameter type; an object of this type is
722 : // large enough and suitably aligned to hold the value of any single
723 : // parameter without assuming which type is largest. Used as a value
724 : // buffer during parameter conversion. Keep the members in sync with
725 : // type_size().
726 : union AP_Param_value_storage {
727 : // Vector3f has a non-trivial constructor, so the union's default
728 : // constructor would otherwise be deleted; the value is always filled
729 : // in (read from EEPROM) before use.
730 327384 : AP_Param_value_storage() {}
731 : int8_t i8;
732 : int16_t i16;
733 : int32_t i32;
734 : float f;
735 : Vector3f v3f;
736 : };
737 :
738 : /*
739 : extract 9 bit key from Param_header
740 : */
741 179718445 : uint16_t AP_Param::get_key(const Param_header &phdr)
742 : {
743 179718445 : return ((uint16_t)phdr.key_high)<<8 | phdr.key_low;
744 : }
745 :
746 : /*
747 : set 9 bit key in Param_header
748 : */
749 1500241 : void AP_Param::set_key(Param_header &phdr, uint16_t key)
750 : {
751 1500241 : phdr.key_low = key & 0xFF;
752 1500241 : phdr.key_high = key >> 8;
753 1500241 : }
754 :
755 : /*
756 : return true if a header is the end of eeprom sentinel
757 : */
758 71624344 : bool AP_Param::is_sentinel(const Param_header &phdr)
759 : {
760 : // note that this is an ||, not an && on the key and group, as
761 : // this makes us more robust to power off while adding a variable
762 : // to EEPROM
763 141995932 : if (phdr.type == _sentinel_type ||
764 70371588 : get_key(phdr) == _sentinel_key) {
765 1252756 : return true;
766 : }
767 : // also check for 0xFFFFFFFF and 0x00000000, which are the fill
768 : // values for storage. These can appear if power off occurs while
769 : // writing data
770 70371588 : uint32_t v = *(uint32_t *)&phdr;
771 70371588 : if (v == 0 || v == 0xFFFFFFFF) {
772 0 : return true;
773 : }
774 70371588 : return false;
775 : }
776 :
777 : // scan the EEPROM looking for a given variable by header content
778 : // return true if found, along with the offset in the EEPROM where
779 : // the variable is stored
780 : // if not found return the offset of the sentinel
781 : // if the sentinel isn't found either, the offset is set to 0xFFFF
782 1485639 : bool AP_Param::scan(const AP_Param::Param_header *target, uint16_t *pofs)
783 : {
784 : struct Param_header phdr;
785 1485639 : uint16_t ofs = sizeof(AP_Param::EEPROM_header);
786 58022441 : while (ofs < _storage.size()) {
787 58017881 : _storage.read_block(&phdr, ofs, sizeof(phdr));
788 12371309 : if (phdr.type == target->type &&
789 70389190 : get_key(phdr) == get_key(*target) &&
790 1719798 : phdr.group_element == target->group_element) {
791 : // found it
792 491074 : *pofs = ofs;
793 491074 : return true;
794 : }
795 57526807 : if (is_sentinel(phdr)) {
796 : // we've reached the sentinel
797 990005 : *pofs = ofs;
798 990005 : sentinel_offset = ofs;
799 990005 : return false;
800 : }
801 56536802 : ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
802 : }
803 4560 : *pofs = 0xffff;
804 : Debug("scan past end of eeprom");
805 4560 : return false;
806 : }
807 :
808 : /**
809 : * add a _X, _Y, _Z suffix to the name of a Vector3f element
810 : * @param buffer
811 : * @param buffer_size
812 : * @param idx Suffix: 0 --> _X; 1 --> _Y; 2 --> _Z; (other --> undefined)
813 : */
814 562635 : void AP_Param::add_vector3f_suffix(char *buffer, size_t buffer_size, uint8_t idx) const
815 : {
816 562635 : const size_t len = strnlen(buffer, buffer_size);
817 562635 : if (len + 2 <= buffer_size) {
818 562635 : buffer[len] = '_';
819 562635 : buffer[len + 1] = static_cast<char>('X' + idx);
820 562635 : if (len + 3 <= buffer_size) {
821 506616 : buffer[len + 2] = 0;
822 : }
823 : }
824 562635 : }
825 :
826 : // Copy the variable's whole name to the supplied buffer.
827 : //
828 : // If the variable is a group member, prepend the group name.
829 : //
830 4341176 : void AP_Param::copy_name_token(const ParamToken &token, char *buffer, size_t buffer_size, bool force_scalar) const
831 : {
832 : uint32_t group_element;
833 : const struct GroupInfo *ginfo;
834 4341176 : struct GroupNesting group_nesting {};
835 : uint8_t idx;
836 4341176 : const struct AP_Param::Info *info = find_var_info_token(token, &group_element, ginfo, group_nesting, &idx);
837 4341176 : if (info == nullptr) {
838 0 : *buffer = 0;
839 : Debug("no info found");
840 0 : return;
841 : }
842 4341176 : copy_name_info(info, ginfo, group_nesting, idx, buffer, buffer_size, force_scalar);
843 : }
844 :
845 4419553 : void AP_Param::copy_name_info(const struct AP_Param::Info *info,
846 : const struct GroupInfo *ginfo,
847 : const struct GroupNesting &group_nesting,
848 : uint8_t idx, char *buffer, size_t buffer_size, bool force_scalar) const
849 : {
850 4419553 : strncpy(buffer, info->name, buffer_size);
851 8577016 : for (uint8_t i=0; i<group_nesting.level; i++) {
852 4157463 : uint8_t len = strnlen(buffer, buffer_size);
853 4157463 : if (len < buffer_size) {
854 4157463 : strncpy(&buffer[len], group_nesting.group_ret[i]->name, buffer_size-len);
855 : }
856 : }
857 4419553 : if (ginfo != nullptr) {
858 4250231 : uint8_t len = strnlen(buffer, buffer_size);
859 4250231 : if (len < buffer_size) {
860 4250231 : strncpy(&buffer[len], ginfo->name, buffer_size-len);
861 : }
862 4250231 : if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == ginfo->type) {
863 : // the caller wants a specific element in a Vector3f
864 562633 : add_vector3f_suffix(buffer, buffer_size, idx);
865 : }
866 169322 : } else if ((force_scalar || idx != 0) && AP_PARAM_VECTOR3F == info->type) {
867 2 : add_vector3f_suffix(buffer, buffer_size, idx);
868 : }
869 4419553 : }
870 :
871 : // Find a variable by name in a group
872 : AP_Param *
873 1352660 : AP_Param::find_group(const char *name, uint16_t vindex, ptrdiff_t group_offset,
874 : const struct GroupInfo *group_info, enum ap_var_type *ptype)
875 : {
876 : uint8_t type;
877 14756423 : for (uint8_t i=0;
878 14756423 : (type=group_info[i].type) != AP_PARAM_NONE;
879 : i++) {
880 14477589 : if (type == AP_PARAM_GROUP) {
881 5368042 : if (strncasecmp(name, group_info[i].name, strlen(group_info[i].name)) != 0) {
882 4720379 : continue;
883 : }
884 649651 : const struct GroupInfo *ginfo = get_group_info(group_info[i]);
885 649651 : if (ginfo == nullptr) {
886 1984 : continue;
887 : }
888 647667 : ptrdiff_t new_offset = group_offset;
889 :
890 647667 : if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
891 4 : continue;
892 : }
893 :
894 647663 : AP_Param *ap = find_group(name+strlen(group_info[i].name), vindex, new_offset, ginfo, ptype);
895 647663 : if (ap != nullptr) {
896 507927 : return ap;
897 : }
898 9109547 : } else if (strcasecmp(name, group_info[i].name) == 0) {
899 : ptrdiff_t base;
900 398370 : if (!get_base(var_info(vindex), base)) {
901 5424 : continue;
902 : }
903 392946 : *ptype = (enum ap_var_type)type;
904 392946 : return (AP_Param *)(base + group_info[i].offset + group_offset);
905 8711177 : } else if (type == AP_PARAM_VECTOR3F) {
906 : // special case for finding Vector3f elements
907 1395845 : uint8_t suffix_len = strnlen(group_info[i].name, AP_MAX_NAME_SIZE);
908 1395845 : if (strncmp(name, group_info[i].name, suffix_len) == 0 &&
909 206189 : name[suffix_len] == '_' &&
910 172953 : (name[suffix_len+1] == 'X' ||
911 115123 : name[suffix_len+1] == 'Y' ||
912 57302 : name[suffix_len+1] == 'Z')) {
913 : ptrdiff_t base;
914 172953 : if (!get_base(var_info(vindex), base)) {
915 0 : continue;
916 : }
917 172953 : AP_Float *v = (AP_Float *)(base + group_info[i].offset + group_offset);
918 172953 : *ptype = AP_PARAM_FLOAT;
919 172953 : switch (name[suffix_len+1]) {
920 57830 : case 'X':
921 172953 : return (AP_Float *)&v[0];
922 57821 : case 'Y':
923 57821 : return (AP_Float *)&v[1];
924 57302 : case 'Z':
925 57302 : return (AP_Float *)&v[2];
926 : }
927 : }
928 : }
929 : }
930 278834 : return nullptr;
931 : }
932 :
933 :
934 : // Find a variable by name.
935 : //
936 : AP_Param *
937 701320 : AP_Param::find(const char *name, enum ap_var_type *ptype, uint16_t *flags)
938 : {
939 53184530 : for (uint16_t i=0; i<_num_vars; i++) {
940 53144686 : const auto &info = var_info(i);
941 53144686 : uint8_t type = info.type;
942 53144686 : if (type == AP_PARAM_GROUP) {
943 15474937 : uint8_t len = strnlen(info.name, AP_MAX_NAME_SIZE);
944 15474937 : if (strncmp(name, info.name, len) != 0) {
945 14738032 : continue;
946 : }
947 736905 : const struct GroupInfo *group_info = get_group_info(info);
948 736905 : if (group_info == nullptr) {
949 31908 : continue;
950 : }
951 704997 : AP_Param *ap = find_group(name + len, i, 0, group_info, ptype);
952 704997 : if (ap != nullptr) {
953 565899 : if (flags != nullptr) {
954 9541 : uint32_t group_element = 0;
955 : const struct GroupInfo *ginfo;
956 9541 : struct GroupNesting group_nesting {};
957 : uint8_t idx;
958 9541 : ap->find_var_info(&group_element, ginfo, group_nesting, &idx);
959 9541 : if (ginfo != nullptr) {
960 9541 : *flags = ginfo->flags;
961 : } else {
962 0 : *flags = 0;
963 : }
964 : }
965 565899 : return ap;
966 : }
967 : // we continue looking as we want to allow top level
968 : // parameter to have the same prefix name as group
969 : // parameters, for example CAM_P_G
970 37669749 : } else if (strcasecmp(name, info.name) == 0) {
971 95577 : *ptype = (enum ap_var_type)type;
972 : ptrdiff_t base;
973 95577 : if (!get_base(info, base)) {
974 0 : return nullptr;
975 : }
976 95577 : if (flags != nullptr) {
977 586 : *flags = 0;
978 : }
979 95577 : return (AP_Param *)base;
980 : }
981 : }
982 39844 : return nullptr;
983 : }
984 :
985 : // Find a variable by index. Note that this is quite slow.
986 : //
987 : AP_Param *
988 1 : AP_Param::find_by_index(uint16_t idx, enum ap_var_type *ptype, ParamToken *token)
989 : {
990 : AP_Param *ap;
991 1 : uint16_t count=0;
992 1 : for (ap=AP_Param::first(token, ptype);
993 1289 : ap && count < idx;
994 1288 : ap=AP_Param::next_scalar(token, ptype)) {
995 1288 : count++;
996 : }
997 1 : return ap;
998 : }
999 :
1000 : // by-name equivalent of find_by_index()
1001 12 : AP_Param* AP_Param::find_by_name(const char* name, enum ap_var_type *ptype, ParamToken *token)
1002 : {
1003 : AP_Param *ap;
1004 12 : for (ap = AP_Param::first(token, ptype);
1005 42 : ap && *ptype != AP_PARAM_GROUP && *ptype != AP_PARAM_NONE;
1006 30 : ap = AP_Param::next_scalar(token, ptype)) {
1007 42 : const auto nlen = strlen(var_info(token->key).name);
1008 : /*
1009 : the name must either match the token name (if a non-group top level param)
1010 : or match up to the length (if a group).
1011 : This check avoids us traversing down into most groups, saving a lot of calls to copy_name_token()
1012 : */
1013 42 : int32_t ret = strncasecmp(name, var_info(token->key).name, nlen);
1014 42 : if (ret == 0) {
1015 : char buf[AP_MAX_NAME_SIZE];
1016 18 : ap->copy_name_token(*token, buf, AP_MAX_NAME_SIZE);
1017 18 : if (strncasecmp(name, buf, AP_MAX_NAME_SIZE) == 0) {
1018 12 : break;
1019 : }
1020 : }
1021 : }
1022 12 : return ap;
1023 : }
1024 :
1025 : /*
1026 : Find a variable by pointer, returning key. This is used for loading pointer variables
1027 : */
1028 5351021 : bool AP_Param::find_key_by_pointer_group(const void *ptr, uint16_t vindex,
1029 : const struct GroupInfo *group_info,
1030 : ptrdiff_t offset, uint16_t &key)
1031 : {
1032 45219820 : for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
1033 39899106 : if (group_info[i].type != AP_PARAM_GROUP) {
1034 35176837 : continue;
1035 : }
1036 : ptrdiff_t base;
1037 5261207 : if (!get_base(var_info(vindex), base)) {
1038 39581 : continue;
1039 : }
1040 5221626 : if (group_info[i].flags & AP_PARAM_FLAG_POINTER) {
1041 575372 : if (ptr == *(void **)(base+group_info[i].offset+offset)) {
1042 9458 : key = var_info(vindex).key;
1043 30307 : return true;
1044 : }
1045 4646254 : } else if (ptr == (void *)(base+group_info[i].offset+offset)) {
1046 15622 : key = var_info(vindex).key;
1047 15622 : return true;
1048 : }
1049 5196546 : ptrdiff_t new_offset = offset;
1050 5196546 : if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
1051 498621 : continue;
1052 : }
1053 4697925 : const struct GroupInfo *ginfo = get_group_info(group_info[i]);
1054 4697925 : if (ginfo == nullptr) {
1055 736 : continue;
1056 : }
1057 4697189 : if (find_key_by_pointer_group(ptr, vindex, ginfo, new_offset, key)) {
1058 5227 : return true;
1059 : }
1060 : }
1061 5320714 : return false;
1062 : }
1063 :
1064 :
1065 : /*
1066 : Find a variable by pointer, returning key. This is used for loading pointer variables
1067 : */
1068 33573 : bool AP_Param::find_key_by_pointer(const void *ptr, uint16_t &key)
1069 : {
1070 2289387 : for (uint16_t i=0; i<_num_vars; i++) {
1071 2289387 : const auto &info = var_info(i);
1072 2289387 : if (info.type != AP_PARAM_GROUP) {
1073 1618547 : continue;
1074 : }
1075 670840 : if ((info.flags & AP_PARAM_FLAG_POINTER) &&
1076 115888 : ptr == *(void **)info.ptr) {
1077 8493 : key = info.key;
1078 8493 : return true;
1079 : }
1080 662347 : ptrdiff_t offset = 0;
1081 662347 : const struct GroupInfo *ginfo = get_group_info(info);
1082 662347 : if (ginfo == nullptr) {
1083 8515 : continue;
1084 : }
1085 653832 : if (find_key_by_pointer_group(ptr, i, ginfo, offset, key)) {
1086 25080 : return true;
1087 : }
1088 : }
1089 0 : return false;
1090 : }
1091 :
1092 : /*
1093 : Find key to top level group parameters by pointer
1094 : */
1095 18215 : bool AP_Param::find_top_level_key_by_pointer(const void *ptr, uint16_t &key)
1096 : {
1097 1179445 : for (uint16_t i=0; i<_num_vars; i++) {
1098 1179405 : const auto &info = var_info(i);
1099 1179405 : if (info.type != AP_PARAM_GROUP) {
1100 987179 : continue;
1101 : }
1102 192226 : if (ptr == (void **)info.ptr) {
1103 18175 : key = info.key;
1104 18175 : return true;
1105 : }
1106 : }
1107 40 : return false;
1108 : }
1109 :
1110 : /*
1111 : fetch a parameter value based on the index within a group. This
1112 : is used to find the old value of a parameter that has been
1113 : removed from an object.
1114 : */
1115 13480 : bool AP_Param::get_param_by_index(void *obj_ptr, uint32_t idx, ap_var_type old_ptype, void *pvalue)
1116 : {
1117 : uint16_t key;
1118 13480 : if (!find_top_level_key_by_pointer(obj_ptr, key)) {
1119 40 : return false;
1120 : }
1121 13440 : const ConversionInfo type_info = {key, idx, old_ptype, nullptr };
1122 13440 : return AP_Param::find_old_parameter(&type_info, (AP_Param *)pvalue);
1123 : }
1124 :
1125 :
1126 : // notify GCS of current value of parameter
1127 7710 : void AP_Param::notify() const {
1128 7710 : uint32_t group_element = 0;
1129 : const struct GroupInfo *ginfo;
1130 7710 : struct GroupNesting group_nesting {};
1131 : uint8_t idx;
1132 :
1133 7710 : const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
1134 7710 : if (info == nullptr) {
1135 : // this is probably very bad
1136 0 : return;
1137 : }
1138 :
1139 : char name[AP_MAX_NAME_SIZE+1];
1140 7710 : copy_name_info(info, ginfo, group_nesting, idx, name, sizeof(name), true);
1141 :
1142 : uint32_t param_header_type;
1143 7710 : if (ginfo != nullptr) {
1144 7710 : param_header_type = ginfo->type;
1145 : } else {
1146 0 : param_header_type = info->type;
1147 : }
1148 :
1149 7710 : send_parameter(name, (enum ap_var_type)param_header_type, idx);
1150 : }
1151 :
1152 :
1153 : /*
1154 : Save the variable to HAL storage, synchronous version
1155 : */
1156 70770 : void AP_Param::save_sync(bool force_save, bool send_to_gcs)
1157 : {
1158 70770 : uint32_t group_element = 0;
1159 : const struct GroupInfo *ginfo;
1160 70770 : struct GroupNesting group_nesting {};
1161 : uint8_t idx;
1162 70770 : const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
1163 : const AP_Param *ap;
1164 :
1165 70770 : if (info == nullptr) {
1166 : // we don't have any info on how to store it
1167 56768 : return;
1168 : }
1169 :
1170 : struct Param_header phdr;
1171 :
1172 : // create the header we will use to store the variable
1173 70667 : if (ginfo != nullptr) {
1174 68578 : phdr.type = ginfo->type;
1175 68578 : if (ginfo->flags & AP_PARAM_FLAG_HIDDEN) {
1176 1009 : send_to_gcs = false;
1177 : }
1178 : } else {
1179 2089 : phdr.type = info->type;
1180 2089 : if (info->flags & AP_PARAM_FLAG_HIDDEN) {
1181 0 : send_to_gcs = false;
1182 : }
1183 : }
1184 70667 : set_key(phdr, info->key);
1185 70667 : phdr.group_element = group_element;
1186 :
1187 70667 : ap = this;
1188 70667 : if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {
1189 : // only vector3f can have non-zero idx for now
1190 0 : return;
1191 : }
1192 70667 : if (idx != 0) {
1193 832 : ap = (const AP_Param *)((ptrdiff_t)ap) - (idx*sizeof(float));
1194 : }
1195 :
1196 70667 : if (phdr.type == AP_PARAM_INT8 && ginfo != nullptr && (ginfo->flags & AP_PARAM_FLAG_ENABLE)) {
1197 : // clear cached parameter count
1198 2253 : invalidate_count();
1199 : }
1200 :
1201 : char name[AP_MAX_NAME_SIZE+1];
1202 70667 : copy_name_info(info, ginfo, group_nesting, idx, name, sizeof(name), true);
1203 :
1204 : // scan EEPROM to find the right location
1205 : uint16_t ofs;
1206 70667 : if (scan(&phdr, &ofs)) {
1207 : // found an existing copy of the variable
1208 56444 : eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
1209 56444 : if (send_to_gcs) {
1210 56395 : send_parameter(name, (enum ap_var_type)phdr.type, idx);
1211 : }
1212 56444 : return;
1213 : }
1214 14223 : if (ofs == (uint16_t) ~0) {
1215 0 : eeprom_full = true;
1216 0 : DEV_PRINTF("EEPROM full\n");
1217 0 : return;
1218 : }
1219 :
1220 : // if the value is the default value then don't save
1221 14223 : if (phdr.type <= AP_PARAM_FLOAT) {
1222 13446 : float v1 = cast_to_float((enum ap_var_type)phdr.type);
1223 : float v2;
1224 13446 : if (ginfo != nullptr) {
1225 12835 : v2 = get_default_value(this, *ginfo);
1226 : } else {
1227 611 : v2 = get_default_value(this, *info);
1228 : }
1229 13446 : if (is_equal(v1,v2) && !force_save) {
1230 221 : if (send_to_gcs) {
1231 221 : GCS_SEND_PARAM(name, (enum ap_var_type)info->type, v2);
1232 : }
1233 221 : return;
1234 : }
1235 13225 : if (!force_save &&
1236 6156 : (phdr.type != AP_PARAM_INT32 &&
1237 3336 : (fabsf(v1-v2) < 0.0001f*fabsf(v1)))) {
1238 : // for other than 32 bit integers, we accept values within
1239 : // 0.01 percent of the current value as being the same
1240 0 : if (send_to_gcs) {
1241 0 : GCS_SEND_PARAM(name, (enum ap_var_type)info->type, v2);
1242 : }
1243 0 : return;
1244 : }
1245 : }
1246 :
1247 14002 : if (ofs+type_size((enum ap_var_type)phdr.type)+2*sizeof(phdr) >= _storage.size()) {
1248 : // we are out of room for saving variables
1249 0 : eeprom_full = true;
1250 0 : DEV_PRINTF("EEPROM full\n");
1251 0 : return;
1252 : }
1253 :
1254 : // write a new sentinel, then the data, then the header
1255 14002 : write_sentinel(ofs + sizeof(phdr) + type_size((enum ap_var_type)phdr.type));
1256 14002 : eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
1257 14002 : eeprom_write_check(&phdr, ofs, sizeof(phdr));
1258 :
1259 14002 : if (send_to_gcs) {
1260 13033 : send_parameter(name, (enum ap_var_type)phdr.type, idx);
1261 : }
1262 : }
1263 :
1264 : /*
1265 : put variable into queue to be saved
1266 : */
1267 73125 : void AP_Param::save(bool force_save)
1268 : {
1269 : struct param_save p, p2;
1270 73125 : p.param = this;
1271 73125 : p.force_save = force_save;
1272 73125 : if (save_queue.peek(p2) &&
1273 75483 : p2.param == this &&
1274 2358 : p2.force_save == force_save) {
1275 : // this one is already at the head of the list to be
1276 : // saved. This check is cheap and catches the case where we
1277 : // are flooding the save queue with one parameter (eg. mission
1278 : // creation, changing MIS_TOTAL)
1279 2354 : return;
1280 : }
1281 70774 : while (!save_queue.push(p)) {
1282 : // if we can't save to the queue
1283 3 : if (hal.util->get_soft_armed() && hal.scheduler->in_main_thread()) {
1284 : // if we are armed in main thread then don't sleep, instead we lose the
1285 : // parameter save
1286 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1287 0 : return;
1288 : }
1289 : // when we are disarmed then loop waiting for a slot to become
1290 : // available. This guarantees completion for large parameter
1291 : // set loads
1292 3 : hal.scheduler->expect_delay_ms(1);
1293 3 : hal.scheduler->delay_microseconds(500);
1294 3 : hal.scheduler->expect_delay_ms(0);
1295 : }
1296 : }
1297 :
1298 : /*
1299 : background function for saving parameters. This runs on the IO thread
1300 : */
1301 243357862 : void AP_Param::save_io_handler(void)
1302 : {
1303 : struct param_save p;
1304 243428623 : while (save_queue.pop(p)) {
1305 70761 : p.param->save_sync(p.force_save, true);
1306 : }
1307 243357862 : if (hal.scheduler->is_system_initialized()) {
1308 : // pay the cost of parameter counting in the IO thread
1309 236970580 : count_parameters();
1310 : }
1311 243357862 : }
1312 :
1313 : /*
1314 : wait for all parameters to save
1315 : */
1316 37315 : void AP_Param::flush(void)
1317 : {
1318 37315 : uint16_t counter = 200; // 2 seconds max
1319 39462 : while (counter-- && save_queue.available()) {
1320 2147 : hal.scheduler->expect_delay_ms(10);
1321 2147 : hal.scheduler->delay(10);
1322 2147 : hal.scheduler->expect_delay_ms(0);
1323 : }
1324 37315 : }
1325 :
1326 : // Load the variable from EEPROM, if supported
1327 : //
1328 441155 : bool AP_Param::load(void)
1329 : {
1330 441155 : uint32_t group_element = 0;
1331 : const struct GroupInfo *ginfo;
1332 441155 : struct GroupNesting group_nesting {};
1333 : uint8_t idx;
1334 441155 : const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
1335 441155 : if (info == nullptr) {
1336 : // we don't have any info on how to load it
1337 0 : return false;
1338 : }
1339 :
1340 : struct Param_header phdr;
1341 :
1342 : // create the header we will use to match the variable
1343 441155 : if (ginfo != nullptr) {
1344 408472 : phdr.type = ginfo->type;
1345 : } else {
1346 32683 : phdr.type = info->type;
1347 : }
1348 441155 : set_key(phdr, info->key);
1349 441155 : phdr.group_element = group_element;
1350 :
1351 : // scan EEPROM to find the right location
1352 : uint16_t ofs;
1353 441155 : if (!scan(&phdr, &ofs)) {
1354 : // if the value isn't stored in EEPROM then set the default value
1355 : ptrdiff_t base;
1356 32301 : if (!get_base(*info, base)) {
1357 0 : return false;
1358 : }
1359 :
1360 32301 : if (ginfo != nullptr) {
1361 : // add in nested group offset
1362 1636 : ptrdiff_t group_offset = 0;
1363 1636 : for (uint8_t i=0; i<group_nesting.level; i++) {
1364 0 : group_offset += group_nesting.group_ret[i]->offset;
1365 : }
1366 1636 : set_value((enum ap_var_type)phdr.type, (void*)(base + ginfo->offset + group_offset),
1367 : get_default_value(this, *ginfo));
1368 : } else {
1369 30665 : set_value((enum ap_var_type)phdr.type, (void*)base,
1370 : get_default_value(this, *info));
1371 : }
1372 32301 : return false;
1373 : }
1374 :
1375 408854 : if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {
1376 : // only vector3f can have non-zero idx for now
1377 0 : return false;
1378 : }
1379 :
1380 408854 : AP_Param *ap = this;
1381 408854 : if (idx != 0) {
1382 0 : ap = (AP_Param *)((ptrdiff_t)ap) - (idx*sizeof(float));
1383 : }
1384 :
1385 : // found it
1386 408854 : _storage.read_block(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
1387 408854 : return true;
1388 : }
1389 :
1390 611521 : bool AP_Param::configured_in_storage(void) const
1391 : {
1392 611521 : uint32_t group_element = 0;
1393 : const struct GroupInfo *ginfo;
1394 611521 : struct GroupNesting group_nesting {};
1395 : uint8_t idx;
1396 611521 : const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
1397 611521 : if (info == nullptr) {
1398 : // we don't have any info on how to load it
1399 76360 : return false;
1400 : }
1401 :
1402 : struct Param_header phdr;
1403 :
1404 : // create the header we will use to match the variable
1405 535161 : if (ginfo != nullptr) {
1406 478784 : phdr.type = ginfo->type;
1407 : } else {
1408 56377 : phdr.type = info->type;
1409 : }
1410 535161 : set_key(phdr, info->key);
1411 535161 : phdr.group_element = group_element;
1412 :
1413 : // scan EEPROM to find the right location
1414 : uint16_t ofs;
1415 :
1416 : // only vector3f can have non-zero idx for now
1417 535161 : return scan(&phdr, &ofs) && (phdr.type == AP_PARAM_VECTOR3F || idx == 0);
1418 : }
1419 :
1420 220053 : bool AP_Param::configured_in_defaults_file(bool &read_only) const
1421 : {
1422 220053 : if (num_param_overrides == 0) {
1423 79523 : return false;
1424 : }
1425 140530 : uint32_t group_element = 0;
1426 : const struct GroupInfo *ginfo;
1427 140530 : struct GroupNesting group_nesting {};
1428 : uint8_t idx;
1429 140530 : const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
1430 140530 : if (info == nullptr) {
1431 : // we don't have any info on how to load it
1432 34 : return false;
1433 : }
1434 :
1435 10488342 : for (uint16_t i=0; i<num_param_overrides; i++) {
1436 10353109 : if (this == param_overrides[i].object_ptr) {
1437 5263 : read_only = param_overrides[i].read_only;
1438 5263 : return true;
1439 : }
1440 : }
1441 :
1442 135233 : return false;
1443 : }
1444 :
1445 220048 : bool AP_Param::configured(void) const
1446 : {
1447 : bool read_only;
1448 220048 : return configured_in_defaults_file(read_only) || configured_in_storage();
1449 : }
1450 :
1451 10126 : bool AP_Param::is_read_only(void) const
1452 : {
1453 10126 : if (num_read_only == 0) {
1454 10121 : return false;
1455 : }
1456 : bool read_only;
1457 5 : if (configured_in_defaults_file(read_only)) {
1458 5 : return read_only;
1459 : }
1460 0 : return false;
1461 : }
1462 :
1463 : // returns true if this parameter should be settable via the
1464 : // MAVLink interface:
1465 10125 : bool AP_Param::allow_set_via_mavlink(uint16_t flags) const
1466 : {
1467 10125 : if (is_read_only()) {
1468 2 : return false;
1469 : }
1470 :
1471 10123 : if (flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) {
1472 : // the user can set BRD_OPTIONS to enable set of internal
1473 : // parameters, for developer testing or unusual use cases
1474 0 : if (!AP_BoardConfig::allow_set_internal_parameters()) {
1475 0 : return false;
1476 : }
1477 : }
1478 :
1479 : #if HAL_GCS_ENABLED
1480 : // check the MAVLink library is OK with the concept:
1481 10123 : if (!gcs().get_allow_param_set()) {
1482 6 : return false;
1483 : }
1484 : #endif // HAL_GCS_ENABLED
1485 :
1486 10117 : return true;
1487 : }
1488 :
1489 : // set a AP_Param variable to a specified value
1490 6749769 : void AP_Param::set_value(enum ap_var_type type, void *ptr, float value)
1491 : {
1492 6749769 : switch (type) {
1493 3389687 : case AP_PARAM_INT8:
1494 3389687 : ((AP_Int8 *)ptr)->set(value);
1495 3389687 : break;
1496 1165540 : case AP_PARAM_INT16:
1497 1165540 : ((AP_Int16 *)ptr)->set(value);
1498 1165540 : break;
1499 436623 : case AP_PARAM_INT32:
1500 436623 : ((AP_Int32 *)ptr)->set(value);
1501 436623 : break;
1502 1757919 : case AP_PARAM_FLOAT:
1503 1757919 : ((AP_Float *)ptr)->set(value);
1504 1757919 : break;
1505 0 : default:
1506 0 : break;
1507 : }
1508 6749769 : }
1509 :
1510 : // load default values for scalars in a group. This does not recurse
1511 : // into other objects. This is a static function that should be called
1512 : // in the objects constructor
1513 1126086 : void AP_Param::setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
1514 : {
1515 1126086 : ptrdiff_t base = (ptrdiff_t)object_pointer;
1516 : uint8_t type;
1517 9069327 : for (uint8_t i=0;
1518 9069327 : (type=group_info[i].type) != AP_PARAM_NONE;
1519 : i++) {
1520 7943241 : if (type <= AP_PARAM_FLOAT) {
1521 6601692 : void *ptr = (void *)(base + group_info[i].offset);
1522 6601692 : set_value((enum ap_var_type)type, ptr,
1523 6601692 : get_default_value((const AP_Param *)ptr, group_info[i]));
1524 1341549 : } else if (type == AP_PARAM_VECTOR3F) {
1525 : // Single default for all components
1526 203509 : void *ptr = (void *)(base + group_info[i].offset);
1527 203509 : const float default_val = get_default_value((const AP_Param *)ptr, group_info[i]);
1528 203509 : ((AP_Vector3f *)ptr)->set(Vector3f{default_val, default_val, default_val});
1529 : }
1530 : }
1531 1126086 : }
1532 :
1533 : // set a value directly in an object. This should only be used by
1534 : // example code, not by mainline vehicle code
1535 0 : bool AP_Param::set_object_value(const void *object_pointer,
1536 : const struct GroupInfo *group_info,
1537 : const char *name, float value)
1538 : {
1539 0 : ptrdiff_t base = (ptrdiff_t)object_pointer;
1540 : uint8_t type;
1541 0 : bool found = false;
1542 0 : for (uint8_t i=0;
1543 0 : (type=group_info[i].type) != AP_PARAM_NONE;
1544 : i++) {
1545 0 : if (strcmp(name, group_info[i].name) == 0 && type <= AP_PARAM_FLOAT) {
1546 0 : void *ptr = (void *)(base + group_info[i].offset);
1547 0 : set_value((enum ap_var_type)type, ptr, value);
1548 : // return true here ?
1549 0 : found = true;
1550 : }
1551 : }
1552 0 : return found;
1553 : }
1554 :
1555 :
1556 : // load default values for all scalars in a sketch. This does not
1557 : // recurse into sub-objects
1558 2094 : void AP_Param::setup_sketch_defaults(void)
1559 : {
1560 2094 : setup();
1561 205912 : for (uint16_t i=0; i<_num_vars; i++) {
1562 203818 : const auto &info = var_info(i);
1563 203818 : uint8_t type = info.type;
1564 203818 : if (type <= AP_PARAM_FLOAT) {
1565 : ptrdiff_t base;
1566 115776 : if (get_base(info, base)) {
1567 115776 : set_value((enum ap_var_type)type, (void*)base,
1568 : get_default_value((const AP_Param *)base, info));
1569 : }
1570 : }
1571 : }
1572 2094 : }
1573 :
1574 :
1575 : // Load all variables from EEPROM
1576 : //
1577 2104 : bool AP_Param::load_all()
1578 : {
1579 : struct Param_header phdr;
1580 2104 : uint16_t ofs = sizeof(AP_Param::EEPROM_header);
1581 :
1582 2104 : reload_defaults_file(false);
1583 :
1584 2104 : if (!registered_save_handler) {
1585 2096 : registered_save_handler = true;
1586 2096 : hal.scheduler->register_io_process(FUNCTOR_BIND((&save_dummy), &AP_Param::save_io_handler, void));
1587 : }
1588 :
1589 107471 : while (ofs < _storage.size()) {
1590 107471 : _storage.read_block(&phdr, ofs, sizeof(phdr));
1591 107471 : if (is_sentinel(phdr)) {
1592 : // we've reached the sentinel
1593 2104 : sentinel_offset = ofs;
1594 2104 : return true;
1595 : }
1596 :
1597 : const struct AP_Param::Info *info;
1598 : void *ptr;
1599 :
1600 105367 : info = find_by_header(phdr, &ptr);
1601 105367 : if (info != nullptr) {
1602 87946 : _storage.read_block(ptr, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
1603 : }
1604 :
1605 105367 : ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
1606 : }
1607 :
1608 : // we didn't find the sentinel
1609 : Debug("no sentinel in load_all");
1610 0 : return false;
1611 : }
1612 :
1613 : /*
1614 : * reload from hal.util defaults file or embedded param region
1615 : * @last_pass: if this is the last pass on defaults - unknown parameters are
1616 : * ignored but if this is set a warning will be emitted
1617 : */
1618 3868 : void AP_Param::reload_defaults_file(bool last_pass)
1619 : {
1620 : #if AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED
1621 :
1622 : /*
1623 : if the HAL specifies a defaults parameter file then override
1624 : defaults using that file
1625 : */
1626 3868 : const char *default_file = hal.util->get_custom_defaults_file();
1627 3868 : if (default_file) {
1628 : #if AP_FILESYSTEM_FILE_READING_ENABLED
1629 3858 : load_defaults_file_from_filesystem(default_file, last_pass);
1630 : #elif defined(HAL_HAVE_AP_ROMFS_EMBEDDED_H)
1631 : load_defaults_file_from_romfs(default_file, last_pass);
1632 : #endif
1633 : }
1634 :
1635 : #endif // AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED
1636 :
1637 : #if AP_PARAM_MAX_EMBEDDED_PARAM > 0
1638 : if (param_defaults_data.length != 0) {
1639 : load_embedded_param_defaults(last_pass);
1640 : }
1641 : #endif
1642 :
1643 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
1644 3863 : hal.util->set_cmdline_parameters();
1645 : #endif
1646 3868 : }
1647 :
1648 : #if AP_FILESYSTEM_FILE_READING_ENABLED
1649 3858 : void AP_Param::load_defaults_file_from_filesystem(const char *default_file, bool last_pass)
1650 : {
1651 3858 : if (load_defaults_file(default_file, last_pass)) {
1652 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1653 3858 : printf("Loaded defaults from %s\n", default_file);
1654 : #endif
1655 : } else {
1656 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1657 0 : AP_HAL::panic("Failed to load defaults from %s", default_file);
1658 : #else
1659 0 : printf("Failed to load defaults from %s\n", default_file);
1660 : #endif
1661 : }
1662 3858 : }
1663 : #endif // AP_FILESYSTEM_FILE_READING_ENABLED
1664 :
1665 : #if defined(HAL_HAVE_AP_ROMFS_EMBEDDED_H)
1666 0 : void AP_Param::load_defaults_file_from_romfs(const char *default_file, bool last_pass)
1667 : {
1668 0 : const char *prefix = "@ROMFS/";
1669 0 : if (strncmp(default_file, prefix, strlen(prefix)) != 0) {
1670 : // does not start with ROMFS, do not attempt to retrieve from it
1671 0 : return;
1672 : }
1673 :
1674 : // filename without the prefix:
1675 0 : const char *trimmed_filename = &default_file[strlen(prefix)];
1676 :
1677 : uint32_t string_length;
1678 0 : const uint8_t *text = AP_ROMFS::find_decompress(trimmed_filename, string_length);
1679 0 : if (text == nullptr) {
1680 0 : return;
1681 : }
1682 :
1683 0 : load_param_defaults((const char*)text, string_length, last_pass);
1684 :
1685 0 : AP_ROMFS::free(text);
1686 :
1687 : }
1688 : #endif // HAL_HAVE_AP_ROMFS_EMBEDDED_H
1689 :
1690 : /*
1691 : Load all variables from EEPROM for a particular object. This is
1692 : required for dynamically loaded objects
1693 : */
1694 30311 : void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct GroupInfo *group_info)
1695 : {
1696 : struct Param_header phdr;
1697 : uint16_t key;
1698 :
1699 30311 : if (!find_key_by_pointer(object_pointer, key)) {
1700 0 : DEV_PRINTF("ERROR: Unable to find param pointer\n");
1701 0 : return;
1702 : }
1703 :
1704 308645 : for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
1705 278334 : if (group_info[i].type == AP_PARAM_GROUP) {
1706 14847 : ptrdiff_t new_offset = 0;
1707 14847 : if (!adjust_group_offset(key, group_info[i], new_offset)) {
1708 0 : continue;
1709 : }
1710 14847 : const struct GroupInfo *ginfo = get_group_info(group_info[i]);
1711 14847 : if (ginfo != nullptr) {
1712 14847 : load_object_from_eeprom((void *)(((ptrdiff_t)object_pointer)+new_offset), ginfo);
1713 : }
1714 : }
1715 278334 : uint16_t ofs = sizeof(AP_Param::EEPROM_header);
1716 13990066 : while (ofs < _storage.size()) {
1717 13990066 : _storage.read_block(&phdr, ofs, sizeof(phdr));
1718 : // note that this is an || not an && for robustness
1719 : // against power off while adding a variable
1720 13990066 : if (is_sentinel(phdr)) {
1721 : // we've reached the sentinel
1722 260647 : sentinel_offset = ofs;
1723 260647 : break;
1724 : }
1725 13729419 : if (get_key(phdr) == key) {
1726 : const struct AP_Param::Info *info;
1727 : void *ptr;
1728 :
1729 625758 : info = find_by_header(phdr, &ptr);
1730 625758 : if (info != nullptr) {
1731 542865 : if ((ptrdiff_t)ptr == ((ptrdiff_t)object_pointer)+group_info[i].offset) {
1732 17687 : _storage.read_block(ptr, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
1733 17687 : break;
1734 : }
1735 : }
1736 : }
1737 13711732 : ofs += type_size((enum ap_var_type)phdr.type) + sizeof(phdr);
1738 : }
1739 : }
1740 :
1741 30311 : if (!done_all_default_params) {
1742 : /*
1743 : the new subtree may need defaults from defaults.parm
1744 : */
1745 71 : reload_defaults_file(false);
1746 : }
1747 :
1748 : // reset cached param counter as we may be loading a dynamic var_info
1749 30311 : invalidate_count();
1750 : }
1751 :
1752 :
1753 : // return the first variable in _var_info
1754 12571 : AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype, float *default_val)
1755 : {
1756 12571 : token->key = 0;
1757 12571 : token->group_element = 0;
1758 12571 : token->idx = 0;
1759 12571 : if (_num_vars == 0) {
1760 0 : return nullptr;
1761 : }
1762 : ptrdiff_t base;
1763 12571 : if (!get_base(var_info(0), base)) {
1764 : // should be impossible, first var needs to be non-pointer
1765 0 : return nullptr;
1766 : }
1767 12571 : if (ptype != nullptr) {
1768 3092 : *ptype = (enum ap_var_type)var_info(0).type;
1769 : }
1770 : #if AP_PARAM_DEFAULTS_ENABLED
1771 12571 : if (default_val != nullptr) {
1772 2081 : *default_val = get_default_value((AP_Param *)base, var_info(0));
1773 : }
1774 12571 : check_default((AP_Param *)base, default_val);
1775 : #endif
1776 12571 : return (AP_Param *)base;
1777 : }
1778 :
1779 : /// Returns the next variable in a group, recursing into groups
1780 : /// as needed
1781 308215143 : AP_Param *AP_Param::next_group(const uint16_t vindex, const struct GroupInfo *group_info,
1782 : bool *found_current,
1783 : const uint32_t group_base,
1784 : const uint8_t group_shift,
1785 : const ptrdiff_t group_offset,
1786 : ParamToken *token,
1787 : enum ap_var_type *ptype,
1788 : bool skip_disabled,
1789 : float *default_val)
1790 : {
1791 : enum ap_var_type type;
1792 2880252681 : for (uint8_t i=0;
1793 2880252681 : (type=(enum ap_var_type)group_info[i].type) != AP_PARAM_NONE;
1794 : i++) {
1795 2608324328 : if (!check_frame_type(group_info[i].flags)) {
1796 9525781 : continue;
1797 : }
1798 2598798547 : if (type == AP_PARAM_GROUP) {
1799 : // a nested group
1800 326541767 : const struct GroupInfo *ginfo = get_group_info(group_info[i]);
1801 326541767 : if (ginfo == nullptr) {
1802 36587425 : continue;
1803 : }
1804 : AP_Param *ap;
1805 305983456 : ptrdiff_t new_offset = group_offset;
1806 :
1807 305983456 : if (!adjust_group_offset(vindex, group_info[i], new_offset)) {
1808 16029114 : continue;
1809 : }
1810 :
1811 289954342 : ap = next_group(vindex, ginfo, found_current, group_id(group_info, group_base, i, group_shift),
1812 289954342 : group_shift + _group_level_shift, new_offset, token, ptype, skip_disabled, default_val);
1813 289954342 : if (ap != nullptr) {
1814 17298743 : return ap;
1815 : }
1816 : } else {
1817 2272256780 : if (*found_current) {
1818 : // got a new one
1819 15701763 : token->key = vindex;
1820 15701763 : token->group_element = group_id(group_info, group_base, i, group_shift);
1821 15701763 : token->idx = 0;
1822 15701763 : if (ptype != nullptr) {
1823 15701763 : *ptype = type;
1824 : }
1825 : ptrdiff_t base;
1826 15701763 : if (!get_base(var_info(vindex), base)) {
1827 259999 : continue;
1828 : }
1829 :
1830 15441764 : AP_Param *ret = (AP_Param*)(base + group_info[i].offset + group_offset);
1831 :
1832 15441764 : if (skip_disabled &&
1833 15319864 : _hide_disabled_groups &&
1834 15319864 : group_info[i].type == AP_PARAM_INT8 &&
1835 32228108 : (group_info[i].flags & AP_PARAM_FLAG_ENABLE) &&
1836 1344580 : ((AP_Int8 *)ret)->get() == 0) {
1837 1253469 : token->last_disabled = 1;
1838 : }
1839 : #if AP_PARAM_DEFAULTS_ENABLED
1840 15441764 : if (default_val != nullptr) {
1841 2543411 : *default_val = get_default_value(ret, group_info[i]);
1842 : }
1843 : #endif
1844 15441764 : return ret;
1845 : }
1846 2256555017 : if (group_id(group_info, group_base, i, group_shift) == token->group_element) {
1847 17734575 : *found_current = true;
1848 17734575 : if (token->last_disabled) {
1849 1253469 : token->last_disabled = 0;
1850 1253469 : return nullptr;
1851 : }
1852 16481106 : if (type == AP_PARAM_VECTOR3F && token->idx < 3) {
1853 : // return the next element of the vector as a
1854 : // float
1855 2292814 : token->idx++;
1856 2292814 : if (ptype != nullptr) {
1857 2292814 : *ptype = AP_PARAM_FLOAT;
1858 : }
1859 : ptrdiff_t base;
1860 2292814 : if (!get_base(var_info(vindex), base)) {
1861 0 : continue;
1862 : }
1863 2292814 : ptrdiff_t ofs = base + group_info[i].offset + group_offset;
1864 2292814 : ofs += sizeof(float)*(token->idx - 1u);
1865 : #if AP_PARAM_DEFAULTS_ENABLED
1866 2292814 : if (default_val != nullptr) {
1867 372165 : *default_val = get_default_value((AP_Param *)ofs, group_info[i]);
1868 : }
1869 : #endif
1870 2292814 : return (AP_Param *)ofs;
1871 : }
1872 : }
1873 : }
1874 : }
1875 271928353 : return nullptr;
1876 : }
1877 :
1878 : /// Returns the next variable in _var_info, recursing into groups
1879 : /// as needed
1880 18431031 : AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype, bool skip_disabled, float *default_val)
1881 : {
1882 18431031 : uint16_t i = token->key;
1883 18431031 : bool found_current = false;
1884 18431031 : if (i >= _num_vars) {
1885 : // illegal token
1886 0 : return nullptr;
1887 : }
1888 18431031 : enum ap_var_type type = (enum ap_var_type)var_info(i).type;
1889 :
1890 : // allow Vector3f to be seen as 3 variables. First as a vector,
1891 : // then as 3 separate floats
1892 18431031 : if (type == AP_PARAM_VECTOR3F && token->idx < 3) {
1893 0 : token->idx++;
1894 0 : if (ptype != nullptr) {
1895 0 : *ptype = AP_PARAM_FLOAT;
1896 : }
1897 0 : AP_Param *ret = (AP_Param *)(((token->idx - 1u)*sizeof(float))+(ptrdiff_t)var_info(i).ptr);
1898 : #if AP_PARAM_DEFAULTS_ENABLED
1899 0 : if (default_val != nullptr) {
1900 0 : *default_val = get_default_value(ret, var_info(i));
1901 : }
1902 : #endif
1903 0 : return ret;
1904 : }
1905 :
1906 18431031 : if (type != AP_PARAM_GROUP) {
1907 696456 : i++;
1908 696456 : found_current = true;
1909 : }
1910 18959514 : for (; i<_num_vars; i++) {
1911 18947215 : const auto &info = var_info(i);
1912 18947215 : if (!check_frame_type(info.flags)) {
1913 0 : continue;
1914 : }
1915 18947215 : type = (enum ap_var_type)info.type;
1916 18947215 : if (type == AP_PARAM_GROUP) {
1917 18263061 : const struct GroupInfo *group_info = get_group_info(info);
1918 18263061 : if (group_info == nullptr) {
1919 2260 : continue;
1920 : }
1921 18260801 : AP_Param *ap = next_group(i, group_info, &found_current, 0, 0, 0, token, ptype, skip_disabled, default_val);
1922 18260801 : if (ap != nullptr) {
1923 17734578 : return ap;
1924 : }
1925 : } else {
1926 : // found the next one
1927 684154 : token->key = i;
1928 684154 : token->group_element = 0;
1929 684154 : token->idx = 0;
1930 684154 : if (ptype != nullptr) {
1931 684154 : *ptype = type;
1932 : }
1933 : #if AP_PARAM_DEFAULTS_ENABLED
1934 684154 : if (default_val != nullptr) {
1935 111199 : *default_val = get_default_value((AP_Param *)info.ptr, info);
1936 : }
1937 : #endif
1938 684154 : return (AP_Param *)(info.ptr);
1939 : }
1940 : }
1941 12299 : return nullptr;
1942 : }
1943 :
1944 : /// Returns the next scalar in _var_info, recursing into groups
1945 : /// as needed
1946 17666759 : AP_Param *AP_Param::next_scalar(ParamToken *token, enum ap_var_type *ptype, float *default_val)
1947 : {
1948 : AP_Param *ap;
1949 : enum ap_var_type type;
1950 18431031 : while ((ap = next(token, &type, true, default_val)) != nullptr && type > AP_PARAM_FLOAT) ;
1951 :
1952 17666759 : if (ap != nullptr) {
1953 17654267 : if (ptype != nullptr) {
1954 4331898 : *ptype = type;
1955 : }
1956 : }
1957 : #if AP_PARAM_DEFAULTS_ENABLED
1958 17666759 : check_default(ap, default_val);
1959 : #endif
1960 17666759 : return ap;
1961 : }
1962 :
1963 :
1964 : /// cast a variable to a float given its type
1965 4470649 : float AP_Param::cast_to_float(enum ap_var_type type) const
1966 : {
1967 4470649 : switch (type) {
1968 1419625 : case AP_PARAM_INT8:
1969 1419625 : return ((AP_Int8 *)this)->cast_to_float();
1970 889783 : case AP_PARAM_INT16:
1971 889783 : return ((AP_Int16 *)this)->cast_to_float();
1972 347650 : case AP_PARAM_INT32:
1973 347650 : return ((AP_Int32 *)this)->cast_to_float();
1974 1813591 : case AP_PARAM_FLOAT:
1975 1813591 : return ((AP_Float *)this)->cast_to_float();
1976 0 : default:
1977 0 : return NAN;
1978 : }
1979 : }
1980 :
1981 : /*
1982 : find an old parameter and return it.
1983 : */
1984 342146 : bool AP_Param::find_old_parameter(const struct ConversionInfo *info, AP_Param *value)
1985 : {
1986 : // find the old value in EEPROM.
1987 : uint16_t pofs;
1988 : AP_Param::Param_header header;
1989 342146 : header.type = info->type;
1990 342146 : set_key(header, info->old_key);
1991 342146 : header.group_element = info->old_group_element;
1992 342146 : if (!scan(&header, &pofs)) {
1993 : // the old parameter isn't saved in the EEPROM.
1994 342140 : return false;
1995 : }
1996 :
1997 : // load the old value from EEPROM
1998 6 : _storage.read_block(value, pofs+sizeof(header), type_size((enum ap_var_type)header.type));
1999 6 : return true;
2000 : }
2001 :
2002 : #pragma GCC diagnostic push
2003 : #pragma GCC diagnostic ignored "-Wformat"
2004 : // convert one old vehicle parameter to new object parameter
2005 127076 : void AP_Param::convert_old_parameter(const struct ConversionInfo *info, float scaler, uint8_t flags)
2006 : {
2007 : // a buffer large enough (and aligned) for any parameter value, which
2008 : // also avoids a VLA here
2009 127076 : AP_Param_value_storage old_value;
2010 127076 : AP_Param *ap = (AP_Param *)&old_value;
2011 :
2012 127076 : if (!find_old_parameter(info, ap)) {
2013 : // the old parameter isn't saved in the EEPROM. It was
2014 : // probably still set to the default value, which isn't stored
2015 : // no need to convert
2016 127072 : return;
2017 : }
2018 :
2019 : // find the new variable in the variable structures
2020 : enum ap_var_type ptype;
2021 4 : AP_Param *ap2 = find(&info->new_name[0], &ptype);
2022 4 : if (ap2 == nullptr) {
2023 0 : DEV_PRINTF("Unknown conversion '%s'\n", info->new_name);
2024 0 : return;
2025 : }
2026 :
2027 : // see if we can load it from EEPROM
2028 4 : if (!(flags & CONVERT_FLAG_FORCE) && ap2->configured_in_storage()) {
2029 : // the new parameter already has a value set by the user, or
2030 : // has already been converted
2031 0 : return;
2032 : }
2033 :
2034 : // see if they are the same type and no scaling applied
2035 4 : if (ptype == info->type && is_equal(scaler, 1.0f) && flags == 0) {
2036 : // copy the value over only if the new parameter does not already
2037 : // have the old value (via a default). Size the copy by the
2038 : // parameter type; old_value is large enough for any type, so
2039 : // sizeof() it would overrun the destination parameter.
2040 4 : const uint8_t value_size = type_size(info->type);
2041 4 : if (memcmp(ap2, ap, value_size) != 0) {
2042 4 : memcpy(ap2, ap, value_size);
2043 : // and save
2044 4 : ap2->save();
2045 : }
2046 0 : } else if (ptype <= AP_PARAM_FLOAT && info->type <= AP_PARAM_FLOAT) {
2047 : // perform scalar->scalar conversion
2048 0 : float v = ap->cast_to_float(info->type);
2049 0 : if (flags & CONVERT_FLAG_REVERSE) {
2050 : // convert a _REV parameter to a _REVERSED parameter
2051 0 : v = is_equal(v, -1.0f)?1:0;
2052 : }
2053 0 : if (!is_equal(v,ap2->cast_to_float(ptype))) {
2054 : // the value needs to change
2055 0 : set_value(ptype, ap2, v * scaler);
2056 0 : ap2->save();
2057 : }
2058 0 : } else {
2059 : // can't do vector<->scalar conversion, or different vector types
2060 0 : DEV_PRINTF("Bad conversion type '%s'\n", info->new_name);
2061 : }
2062 : }
2063 : #pragma GCC diagnostic pop
2064 :
2065 :
2066 : // convert old vehicle parameters to new object parameters
2067 4721 : void AP_Param::convert_old_parameters(const struct ConversionInfo *conversion_table, uint8_t table_size, uint8_t flags)
2068 : {
2069 4721 : convert_old_parameters_scaled(conversion_table, table_size, 1.0f, flags);
2070 4721 : }
2071 :
2072 : // convert old vehicle parameters to new object parameters with scaling - assumes all parameters will have the same scaling factor
2073 13584 : void AP_Param::convert_old_parameters_scaled(const struct ConversionInfo *conversion_table, uint8_t table_size, float scaler, uint8_t flags)
2074 : {
2075 93223 : for (uint8_t i=0; i<table_size; i++) {
2076 79639 : convert_old_parameter(&conversion_table[i], scaler, flags);
2077 : }
2078 : // we need to flush here to prevent a later set_default_by_name()
2079 : // causing a save to be done on a converted parameter
2080 13584 : flush();
2081 13584 : }
2082 :
2083 : // move all parameters from a class to a new location
2084 : // is_top_level: Is true if the class had its own top level key, param_key. It is false if the class was a subgroup
2085 22092 : void AP_Param::convert_class(uint16_t param_key, void *object_pointer,
2086 : const struct AP_Param::GroupInfo *group_info,
2087 : uint16_t old_index, bool is_top_level, bool recurse_sub_groups)
2088 : {
2089 22092 : const uint8_t group_shift = is_top_level ? 0 : 6;
2090 :
2091 226522 : for (uint8_t i=0; group_info[i].type != AP_PARAM_NONE; i++) {
2092 : struct ConversionInfo info;
2093 204430 : info.old_key = param_key;
2094 204430 : info.type = (ap_var_type)group_info[i].type;
2095 204430 : info.new_name = nullptr;
2096 :
2097 204430 : uint16_t idx = group_info[i].idx;
2098 204430 : if (group_shift != 0 && idx == 0) {
2099 : // Note: Index 0 is treated as 63 for group bit shifting purposes. See group_id()
2100 5408 : idx = 63;
2101 : }
2102 :
2103 204430 : if (info.type == AP_PARAM_GROUP) {
2104 : // Convert subgroups if enabled
2105 4122 : if (recurse_sub_groups) {
2106 : // Only recurse once
2107 4122 : convert_class(param_key, (uint8_t *)object_pointer + group_info[i].offset, get_group_info(group_info[i]), idx, false, false);
2108 : }
2109 204428 : continue;
2110 : }
2111 :
2112 200308 : info.old_group_element = (idx << group_shift) + old_index;
2113 :
2114 : // a buffer large enough (and aligned) for any parameter value,
2115 : // which also avoids a VLA here
2116 200308 : AP_Param_value_storage old_value;
2117 200308 : AP_Param *ap = (AP_Param *)&old_value;
2118 :
2119 200308 : if (!AP_Param::find_old_parameter(&info, ap)) {
2120 : // the parameter wasn't set in the old eeprom
2121 200306 : continue;
2122 : }
2123 :
2124 2 : AP_Param *ap2 = (AP_Param *)(group_info[i].offset + (uint8_t *)object_pointer);
2125 2 : if (ap2->configured_in_storage()) {
2126 : // user has already set a value, or previous conversion was done
2127 0 : continue;
2128 : }
2129 : // size the copy by the parameter type; old_value is large enough
2130 : // for any type, so sizeof() it would overrun the destination
2131 2 : memcpy(ap2, ap, type_size(info.type));
2132 : // and save
2133 2 : ap2->save();
2134 : }
2135 :
2136 : // we need to flush here to prevent a later set_default_by_name()
2137 : // causing a save to be done on a converted parameter
2138 22092 : flush();
2139 22092 : }
2140 :
2141 : // convert an object which was stored in a vehicle's G2 into a new
2142 : // object in AP_Vehicle.cpp:
2143 2061 : void AP_Param::convert_g2_objects(const void *g2, const G2ObjectConversion g2_conversions[], uint8_t num_conversions)
2144 : {
2145 : // Find G2's Top Level Key
2146 : ConversionInfo info;
2147 2061 : if (!find_top_level_key_by_pointer(g2, info.old_key)) {
2148 0 : return;
2149 : }
2150 9530 : for (uint8_t i=0; i<num_conversions; i++) {
2151 7469 : const auto &c { g2_conversions[i] };
2152 7469 : convert_class(info.old_key, c.object_pointer, c.var_info, c.old_index, false);
2153 : }
2154 : }
2155 :
2156 2094 : void AP_Param::convert_toplevel_objects(const TopLevelObjectConversion conversions[], uint8_t num_conversions)
2157 : {
2158 4188 : for (uint8_t i=0; i<num_conversions; i++) {
2159 2094 : const auto &c { conversions[i] };
2160 2094 : convert_class(c.old_index, c.object_pointer, c.var_info, 0, true);
2161 : }
2162 2094 : }
2163 :
2164 : /*
2165 : convert width of a parameter, allowing update to wider scalar values
2166 : without changing the parameter indexes
2167 : */
2168 102911 : bool AP_Param::_convert_parameter_width(ap_var_type old_ptype, float scale_factor, bool bitmask)
2169 : {
2170 102911 : if (configured_in_storage()) {
2171 : // already converted or set by the user
2172 708 : return false;
2173 : }
2174 :
2175 102203 : uint32_t group_element = 0;
2176 : const struct GroupInfo *ginfo;
2177 102203 : struct GroupNesting group_nesting {};
2178 : uint8_t idx;
2179 102203 : const struct AP_Param::Info *info = find_var_info(&group_element, ginfo, group_nesting, &idx);
2180 :
2181 102203 : if (info == nullptr) {
2182 6282 : return false;
2183 : }
2184 :
2185 : // remember the type
2186 : ap_var_type new_ptype;
2187 95921 : if (ginfo != nullptr) {
2188 88354 : new_ptype = (ap_var_type)ginfo->type;
2189 : } else {
2190 7567 : new_ptype = (ap_var_type)info->type;
2191 : }
2192 :
2193 : // create the header we will use to scan for the variable
2194 : struct Param_header phdr;
2195 95921 : phdr.type = old_ptype;
2196 95921 : set_key(phdr, info->key);
2197 95921 : phdr.group_element = group_element;
2198 :
2199 : // scan EEPROM to find the right location
2200 : uint16_t pofs;
2201 95921 : if (!scan(&phdr, &pofs)) {
2202 : // it isn't in storage
2203 95921 : return false;
2204 : }
2205 :
2206 : // load the old value from EEPROM. a buffer large enough (and aligned)
2207 : // for any parameter value also avoids a VLA here
2208 0 : AP_Param_value_storage old_value;
2209 0 : _storage.read_block(&old_value, pofs+sizeof(phdr), type_size(old_ptype));
2210 :
2211 0 : AP_Param *old_ap = (AP_Param *)&old_value;
2212 :
2213 0 : if (!bitmask) {
2214 : // Numeric conversion
2215 : // going via float is safe as the only time we would be converting
2216 : // from AP_Int32 is when converting to float
2217 0 : float old_float_value = old_ap->cast_to_float(old_ptype);
2218 0 : set_value(new_ptype, this, old_float_value*scale_factor);
2219 :
2220 : } else {
2221 : // Bitmask conversion, go via uint32
2222 : // int8 -1 should convert to int16 255
2223 : uint32_t mask;
2224 0 : switch (old_ptype) {
2225 0 : case AP_PARAM_INT8:
2226 0 : mask = (uint8_t)(*(AP_Int8*)old_ap);
2227 0 : break;
2228 0 : case AP_PARAM_INT16:
2229 0 : mask = (uint16_t)(*(AP_Int16*)old_ap);
2230 0 : break;
2231 0 : case AP_PARAM_INT32:
2232 0 : mask = (uint32_t)(*(AP_Int32*)old_ap);
2233 0 : break;
2234 0 : default:
2235 0 : return false;
2236 : }
2237 :
2238 0 : switch (new_ptype) {
2239 0 : case AP_PARAM_INT8:
2240 0 : ((AP_Int8 *)this)->set(mask);
2241 0 : break;
2242 0 : case AP_PARAM_INT16:
2243 0 : ((AP_Int16 *)this)->set(mask);
2244 0 : break;
2245 0 : case AP_PARAM_INT32:
2246 0 : ((AP_Int32 *)this)->set(mask);
2247 0 : break;
2248 0 : default:
2249 0 : return false;
2250 : }
2251 : }
2252 :
2253 :
2254 : // force save as the new type
2255 0 : save(true);
2256 :
2257 0 : return true;
2258 : }
2259 :
2260 : /*
2261 : set a parameter to a float value
2262 : */
2263 295683 : void AP_Param::set_float(float value, enum ap_var_type var_type)
2264 : {
2265 295683 : if (isnan(value) || isinf(value)) {
2266 0 : return;
2267 : }
2268 :
2269 : // add a small amount before casting parameter values
2270 : // from float to integer to avoid truncating to the
2271 : // next lower integer value.
2272 295683 : float rounding_addition = 0.01f;
2273 :
2274 : // handle variables with standard type IDs
2275 295683 : if (var_type == AP_PARAM_FLOAT) {
2276 136092 : ((AP_Float *)this)->set(value);
2277 159591 : } else if (var_type == AP_PARAM_INT32) {
2278 2249 : if (value < 0) rounding_addition = -rounding_addition;
2279 2249 : float v = value+rounding_addition;
2280 2249 : v = constrain_float(v, INT32_MIN, INT32_MAX);
2281 2249 : ((AP_Int32 *)this)->set(v);
2282 157342 : } else if (var_type == AP_PARAM_INT16) {
2283 97772 : if (value < 0) rounding_addition = -rounding_addition;
2284 97772 : float v = value+rounding_addition;
2285 97772 : v = constrain_float(v, INT16_MIN, INT16_MAX);
2286 97772 : ((AP_Int16 *)this)->set(v);
2287 59570 : } else if (var_type == AP_PARAM_INT8) {
2288 59570 : if (value < 0) rounding_addition = -rounding_addition;
2289 59570 : float v = value+rounding_addition;
2290 59570 : v = constrain_float(v, INT8_MIN, INT8_MAX);
2291 59570 : ((AP_Int8 *)this)->set(v);
2292 : }
2293 : }
2294 :
2295 :
2296 : /*
2297 : parse a parameter file line
2298 : */
2299 704062 : bool AP_Param::parse_param_line(char *line, char **vname, float &value, bool &read_only)
2300 : {
2301 704062 : if (line[0] == '#') {
2302 68966 : return false;
2303 : }
2304 635096 : char *saveptr = nullptr;
2305 : /*
2306 : note that we need the \r\n as delimiters to prevent us getting
2307 : strings with line termination in the results
2308 : */
2309 635096 : char *pname = strtok_r(line, ", =\t\r\n", &saveptr);
2310 635096 : if (pname == nullptr) {
2311 8144 : return false;
2312 : }
2313 626952 : if (strlen(pname) > AP_MAX_NAME_SIZE) {
2314 0 : return false;
2315 : }
2316 :
2317 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2318 : // Workaround to prevent FORMAT_VERSION in param file resulting in invalid
2319 : // EEPROM. For details, see: https://github.com/ArduPilot/ardupilot/issues/15579
2320 626952 : if (strcmp(pname, "FORMAT_VERSION") == 0) {
2321 0 : ::printf("Warning: Ignoring FORMAT_VERSION in param file\n");
2322 0 : return false;
2323 : }
2324 : #endif
2325 :
2326 626952 : const char *value_s = strtok_r(nullptr, ", =\t\r\n", &saveptr);
2327 626952 : if (value_s == nullptr) {
2328 8 : return false;
2329 : }
2330 626944 : value = strtof(value_s, NULL);
2331 626944 : *vname = pname;
2332 :
2333 626944 : const char *flags_s = strtok_r(nullptr, ", =\t\r\n", &saveptr);
2334 626944 : if (flags_s && strcmp(flags_s, "@READONLY") == 0) {
2335 4 : read_only = true;
2336 : } else {
2337 626940 : read_only = false;
2338 : }
2339 :
2340 626944 : return true;
2341 : }
2342 :
2343 :
2344 : #if AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED || AP_PARAM_DYNAMIC_ENABLED
2345 :
2346 : // increments num_defaults for each default found in filename
2347 10104 : bool AP_Param::count_defaults_in_file(const char *filename, uint16_t &num_defaults)
2348 : {
2349 : // try opening the file both in the posix filesystem and using AP::FS
2350 10104 : int file_apfs = AP::FS().open(filename, O_RDONLY, true);
2351 10104 : if (file_apfs == -1) {
2352 0 : return false;
2353 : }
2354 : char line[100];
2355 :
2356 : /*
2357 : work out how many parameter default structures to allocate
2358 : */
2359 362135 : while (AP::FS().fgets(line, sizeof(line), file_apfs)) {
2360 : char *pname;
2361 : float value;
2362 : bool read_only;
2363 352031 : if (!parse_param_line(line, &pname, value, read_only)) {
2364 58217 : continue;
2365 : }
2366 : enum ap_var_type var_type;
2367 313472 : if (!find(pname, &var_type)) {
2368 19658 : continue;
2369 : }
2370 293814 : num_defaults++;
2371 : }
2372 10104 : AP::FS().close(file_apfs);
2373 :
2374 10104 : return true;
2375 : }
2376 :
2377 10104 : bool AP_Param::read_param_defaults_file(const char *filename, bool last_pass, uint16_t &idx)
2378 : {
2379 : // try opening the file both in the posix filesystem and using AP::FS
2380 10104 : int file_apfs = AP::FS().open(filename, O_RDONLY, true);
2381 10104 : if (file_apfs == -1) {
2382 0 : AP_HAL::panic("AP_Param: Failed to re-open defaults file");
2383 : return false;
2384 : }
2385 :
2386 10104 : bool done_all = true;
2387 : char line[100];
2388 362135 : while (AP::FS().fgets(line, sizeof(line), file_apfs)) {
2389 : char *pname;
2390 : float value;
2391 : bool read_only;
2392 352031 : if (!parse_param_line(line, &pname, value, read_only)) {
2393 58217 : continue;
2394 : }
2395 : enum ap_var_type var_type;
2396 313472 : AP_Param *vp = find(pname, &var_type);
2397 313472 : if (!vp) {
2398 : if (last_pass) {
2399 : #if ENABLE_DEBUG
2400 : ::printf("Ignored unknown param %s in defaults file %s\n",
2401 : pname, filename);
2402 : hal.console->printf(
2403 : "Ignored unknown param %s in defaults file %s\n",
2404 : pname, filename);
2405 : #endif
2406 : }
2407 19658 : done_all = false;
2408 19658 : continue;
2409 : }
2410 293814 : if (idx >= param_overrides_len) {
2411 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
2412 0 : break;
2413 : }
2414 293814 : param_overrides[idx].object_ptr = vp;
2415 293814 : param_overrides[idx].value = value;
2416 293814 : param_overrides[idx].read_only = read_only;
2417 293814 : if (read_only) {
2418 2 : num_read_only++;
2419 : }
2420 293814 : idx++;
2421 293814 : if (!vp->configured_in_storage()) {
2422 285566 : vp->set_float(value, var_type);
2423 : }
2424 : }
2425 10104 : AP::FS().close(file_apfs);
2426 :
2427 10104 : done_all_default_params = done_all;
2428 :
2429 10104 : return true;
2430 : }
2431 :
2432 : /*
2433 : load a default set of parameters from a file
2434 : */
2435 3858 : bool AP_Param::load_defaults_file(const char *filename, bool last_pass)
2436 : {
2437 3858 : if (filename == nullptr) {
2438 0 : return false;
2439 : }
2440 :
2441 3858 : char *mutable_filename = strdup(filename);
2442 3858 : if (mutable_filename == nullptr) {
2443 0 : AP_HAL::panic("AP_Param: Failed to allocate mutable string");
2444 : }
2445 :
2446 3858 : uint16_t num_defaults = 0;
2447 3858 : char *saveptr = nullptr;
2448 3858 : for (char *pname = strtok_r(mutable_filename, ",", &saveptr);
2449 13962 : pname != nullptr;
2450 10104 : pname = strtok_r(nullptr, ",", &saveptr)) {
2451 10104 : if (!count_defaults_in_file(pname, num_defaults)) {
2452 0 : free(mutable_filename);
2453 0 : return false;
2454 : }
2455 : }
2456 3858 : free(mutable_filename);
2457 :
2458 3858 : delete[] param_overrides;
2459 3858 : param_overrides_len = 0;
2460 3858 : num_param_overrides = 0;
2461 3858 : num_read_only = 0;
2462 :
2463 3858 : param_overrides = NEW_NOTHROW param_override[num_defaults];
2464 3858 : if (param_overrides == nullptr) {
2465 0 : AP_HAL::panic("AP_Param: Failed to allocate overrides");
2466 : return false;
2467 : }
2468 3858 : param_overrides_len = num_defaults;
2469 :
2470 3858 : if (num_defaults == 0) {
2471 0 : return true;
2472 : }
2473 :
2474 3858 : saveptr = nullptr;
2475 3858 : mutable_filename = strdup(filename);
2476 3858 : if (mutable_filename == nullptr) {
2477 0 : AP_HAL::panic("AP_Param: Failed to allocate mutable string");
2478 : }
2479 3858 : uint16_t idx = 0;
2480 3858 : for (char *pname = strtok_r(mutable_filename, ",", &saveptr);
2481 13962 : pname != nullptr;
2482 10104 : pname = strtok_r(nullptr, ",", &saveptr)) {
2483 10104 : if (!read_param_defaults_file(pname, last_pass, idx)) {
2484 0 : free(mutable_filename);
2485 0 : return false;
2486 : }
2487 : }
2488 3858 : free(mutable_filename);
2489 :
2490 3858 : num_param_overrides = num_defaults;
2491 :
2492 : #if AP_PARAM_DEFAULTS_ENABLED
2493 3858 : purge_defaults_list_overrides();
2494 : #endif
2495 :
2496 3858 : return true;
2497 : }
2498 : #endif // AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED
2499 :
2500 : #if AP_PARAM_MAX_EMBEDDED_PARAM > 0 || defined(HAL_HAVE_AP_ROMFS_EMBEDDED_H)
2501 : /*
2502 : count the number of parameter defaults present in supplied string
2503 : */
2504 0 : bool AP_Param::count_param_defaults(const volatile char *ptr, int32_t length, uint16_t &count)
2505 : {
2506 0 : count = 0;
2507 :
2508 0 : while (length>0) {
2509 : char line[100];
2510 : char *pname;
2511 : float value;
2512 : bool read_only;
2513 : uint16_t i;
2514 0 : uint16_t n = length;
2515 0 : for (i=0;i<n;i++) {
2516 0 : if (ptr[i] == '\n') {
2517 0 : break;
2518 : }
2519 : }
2520 :
2521 0 : uint16_t linelen = MIN(i,sizeof(line)-1);
2522 0 : memcpy(line, (void *)ptr, linelen);
2523 0 : line[linelen] = 0;
2524 :
2525 0 : length -= i+1;
2526 0 : ptr += i+1;
2527 :
2528 0 : if (line[0] == '#' || line[0] == 0) {
2529 0 : continue;
2530 : }
2531 :
2532 0 : if (!parse_param_line(line, &pname, value, read_only)) {
2533 0 : continue;
2534 : }
2535 :
2536 : enum ap_var_type var_type;
2537 0 : if (!find(pname, &var_type)) {
2538 0 : continue;
2539 : }
2540 :
2541 0 : count++;
2542 : }
2543 0 : return true;
2544 : }
2545 :
2546 : /*
2547 : * load parameter defaults from supplied string
2548 : */
2549 0 : void AP_Param::load_param_defaults(const volatile char *ptr, int32_t length, bool last_pass)
2550 : {
2551 0 : delete[] param_overrides;
2552 0 : param_overrides = nullptr;
2553 0 : param_overrides_len = 0;
2554 0 : num_param_overrides = 0;
2555 0 : num_read_only = 0;
2556 :
2557 0 : uint16_t num_defaults = 0;
2558 0 : if (!count_param_defaults(ptr, length, num_defaults)) {
2559 0 : return;
2560 : }
2561 :
2562 0 : param_overrides = NEW_NOTHROW param_override[num_defaults];
2563 0 : if (param_overrides == nullptr) {
2564 0 : AP_HAL::panic("AP_Param: Failed to allocate overrides");
2565 : return;
2566 : }
2567 :
2568 0 : param_overrides_len = num_defaults;
2569 :
2570 0 : uint16_t idx = 0;
2571 :
2572 0 : while (idx < num_defaults && length > 0) {
2573 : char line[100];
2574 : char *pname;
2575 : float value;
2576 : bool read_only;
2577 : uint16_t i;
2578 0 : uint16_t n = length;
2579 0 : for (i=0;i<n;i++) {
2580 0 : if (ptr[i] == '\n') {
2581 0 : break;
2582 : }
2583 : }
2584 :
2585 0 : uint16_t linelen = MIN(i,sizeof(line)-1);
2586 0 : memcpy(line, (void *)ptr, linelen);
2587 0 : line[linelen] = 0;
2588 :
2589 0 : length -= i+1;
2590 0 : ptr += i+1;
2591 :
2592 0 : if (line[0] == '#' || line[0] == 0) {
2593 0 : continue;
2594 : }
2595 :
2596 0 : if (!parse_param_line(line, &pname, value, read_only)) {
2597 0 : continue;
2598 : }
2599 : enum ap_var_type var_type;
2600 0 : AP_Param *vp = find(pname, &var_type);
2601 0 : if (!vp) {
2602 : if (last_pass) {
2603 : #if ENABLE_DEBUG && (AP_PARAM_MAX_EMBEDDED_PARAM > 0)
2604 : ::printf("Ignored unknown param %s from embedded region (offset=%u)\n",
2605 : pname, unsigned(ptr - param_defaults_data.data));
2606 : hal.console->printf(
2607 : "Ignored unknown param %s from embedded region (offset=%u)\n",
2608 : pname, unsigned(ptr - param_defaults_data.data));
2609 : #endif
2610 : }
2611 0 : continue;
2612 : }
2613 0 : param_overrides[idx].object_ptr = vp;
2614 0 : param_overrides[idx].value = value;
2615 0 : param_overrides[idx].read_only = read_only;
2616 0 : if (read_only) {
2617 0 : num_read_only++;
2618 : }
2619 0 : idx++;
2620 0 : if (!vp->configured_in_storage()) {
2621 0 : vp->set_float(value, var_type);
2622 : }
2623 : }
2624 0 : num_param_overrides = num_defaults;
2625 :
2626 : #if AP_PARAM_DEFAULTS_ENABLED
2627 0 : purge_defaults_list_overrides();
2628 : #endif
2629 : }
2630 : #endif // AP_PARAM_MAX_EMBEDDED_PARAM > 0 || defined(HAL_HAVE_AP_ROMFS_EMBEDDED_H)
2631 :
2632 :
2633 : #if AP_PARAM_MAX_EMBEDDED_PARAM > 0
2634 : /*
2635 : * load a default set of parameters from a embedded parameter region
2636 : * @last_pass: if this is the last pass on defaults - unknown parameters are
2637 : * ignored but if this is set a warning will be emitted
2638 : */
2639 : void AP_Param::load_embedded_param_defaults(bool last_pass)
2640 : {
2641 : load_param_defaults(param_defaults_data.data, param_defaults_data.length, last_pass);
2642 : }
2643 : #endif // AP_PARAM_MAX_EMBEDDED_PARAM > 0
2644 :
2645 :
2646 : /*
2647 : find a default value given a pointer to a default value in flash
2648 : */
2649 9735248 : float AP_Param::get_default_value(const AP_Param *vp, const struct GroupInfo &info)
2650 : {
2651 245060004 : for (uint16_t i=0; i<num_param_overrides; i++) {
2652 235463042 : if (vp == param_overrides[i].object_ptr) {
2653 138286 : return param_overrides[i].value;
2654 : }
2655 : }
2656 9596962 : if ((info.flags & AP_PARAM_FLAG_DEFAULT_POINTER) != 0) {
2657 1965651 : return *((float*)((ptrdiff_t)vp - info.def_value_offset));
2658 : }
2659 7631311 : return info.def_value;
2660 : }
2661 :
2662 260332 : float AP_Param::get_default_value(const AP_Param *vp, const struct Info &info)
2663 : {
2664 9218979 : for (uint16_t i=0; i<num_param_overrides; i++) {
2665 9001522 : if (vp == param_overrides[i].object_ptr) {
2666 42875 : return param_overrides[i].value;
2667 : }
2668 : }
2669 217457 : if ((info.flags & AP_PARAM_FLAG_DEFAULT_POINTER) != 0) {
2670 0 : return *((float*)((ptrdiff_t)vp - info.def_value_offset));
2671 : }
2672 217457 : return info.def_value;
2673 : }
2674 :
2675 :
2676 77138 : void AP_Param::send_parameter(const char *name, enum ap_var_type var_type, uint8_t idx) const
2677 : {
2678 77138 : if (idx != 0 && var_type == AP_PARAM_VECTOR3F) {
2679 832 : var_type = AP_PARAM_FLOAT;
2680 : }
2681 77138 : if (var_type > AP_PARAM_VECTOR3F) {
2682 : // invalid
2683 71972 : return;
2684 : }
2685 77138 : if (var_type != AP_PARAM_VECTOR3F) {
2686 : // nice and simple for scalar types
2687 71972 : GCS_SEND_PARAM(name, var_type, cast_to_float(var_type));
2688 71972 : return;
2689 : }
2690 :
2691 : // for vectors we need to send 3 messages. Note that we also come here for the case
2692 : // of a set of the first element of a AP_Vector3f. This happens as the ap->save() call can't
2693 : // distinguish between a vector and scalar save. It means that setting first element of a vector
2694 : // via MAVLink results in sending all 3 elements to the GCS
2695 : #if HAL_GCS_ENABLED
2696 5166 : const Vector3f &v = ((AP_Vector3f *)this)->get();
2697 : char name2[AP_MAX_NAME_SIZE+1];
2698 5166 : strncpy(name2, name, AP_MAX_NAME_SIZE);
2699 5166 : name2[AP_MAX_NAME_SIZE] = 0;
2700 5166 : char &name_axis = name2[strlen(name)-1];
2701 :
2702 5166 : name_axis = 'X';
2703 5166 : GCS_SEND_PARAM(name2, AP_PARAM_FLOAT, v.x);
2704 5166 : name_axis = 'Y';
2705 5166 : GCS_SEND_PARAM(name2, AP_PARAM_FLOAT, v.y);
2706 5166 : name_axis = 'Z';
2707 5166 : GCS_SEND_PARAM(name2, AP_PARAM_FLOAT, v.z);
2708 : #endif // HAL_GCS_ENABLED
2709 : }
2710 :
2711 : /*
2712 : return count of all scalar parameters.
2713 : Note that this function may be called from the IO thread, so needs
2714 : to be thread safe
2715 : */
2716 470852707 : uint16_t AP_Param::count_parameters(void)
2717 : {
2718 : // if we haven't cached the parameter count yet...
2719 470852707 : WITH_SEMAPHORE(_count_sem);
2720 470852707 : if (_parameter_count != 0 &&
2721 470850613 : _count_marker == _count_marker_done) {
2722 470843289 : return _parameter_count;
2723 : }
2724 : /*
2725 : cope with another thread invalidating the count while we are
2726 : counting
2727 : */
2728 9418 : uint8_t limit = 4;
2729 9418 : while ((_parameter_count == 0 ||
2730 28376 : _count_marker != _count_marker_done) &&
2731 9479 : limit--) {
2732 : AP_Param *vp;
2733 9479 : AP_Param::ParamToken token {};
2734 9479 : uint16_t count = 0;
2735 9479 : uint16_t marker = _count_marker;
2736 :
2737 9479 : for (vp = AP_Param::first(&token, nullptr);
2738 13341327 : vp != nullptr;
2739 13331848 : vp = AP_Param::next_scalar(&token, nullptr)) {
2740 13331848 : count++;
2741 : }
2742 9479 : _parameter_count = count;
2743 9479 : _count_marker_done = marker;
2744 : }
2745 9418 : return _parameter_count;
2746 470852707 : }
2747 :
2748 : /*
2749 : invalidate parameter count cache
2750 : */
2751 41882 : void AP_Param::invalidate_count(void)
2752 : {
2753 : // we don't take the semaphore here as we don't want to block. The
2754 : // not-equal test is strong enough to ensure we get the right
2755 : // answer
2756 41882 : _count_marker++;
2757 41882 : }
2758 :
2759 : /*
2760 : set a default value by name
2761 : */
2762 29581 : bool AP_Param::set_default_by_name(const char *name, float value)
2763 : {
2764 : enum ap_var_type vtype;
2765 29581 : AP_Param *vp = find(name, &vtype);
2766 29581 : if (vp == nullptr) {
2767 0 : return false;
2768 : }
2769 29581 : switch (vtype) {
2770 1079 : case AP_PARAM_INT8:
2771 1079 : ((AP_Int8 *)vp)->set_default(value);
2772 1079 : return true;
2773 1835 : case AP_PARAM_INT16:
2774 1835 : ((AP_Int16 *)vp)->set_default(value);
2775 1835 : return true;
2776 274 : case AP_PARAM_INT32:
2777 274 : ((AP_Int32 *)vp)->set_default(value);
2778 274 : return true;
2779 26393 : case AP_PARAM_FLOAT:
2780 26393 : ((AP_Float *)vp)->set_default(value);
2781 26393 : return true;
2782 0 : default:
2783 0 : break;
2784 : }
2785 : // not a supported type
2786 0 : return false;
2787 : }
2788 :
2789 : /*
2790 : set parameter defaults from a defaults_struct table
2791 : sends GCS message and panics (in SITL only) if parameter is not found
2792 : */
2793 368 : void AP_Param::set_defaults_from_table(const struct defaults_table_struct *table, uint8_t count)
2794 : {
2795 7616 : for (uint8_t i=0; i<count; i++) {
2796 7248 : if (!AP_Param::set_default_by_name(table[i].name, table[i].value)) {
2797 0 : AP_BoardConfig::config_error("param deflt fail:%s", table[i].name);
2798 : }
2799 : }
2800 368 : }
2801 :
2802 : /*
2803 : set a value by name
2804 : */
2805 83 : bool AP_Param::set_by_name(const char *name, float value)
2806 : {
2807 : enum ap_var_type vtype;
2808 83 : AP_Param *vp = find(name, &vtype);
2809 83 : if (vp == nullptr) {
2810 0 : return false;
2811 : }
2812 83 : switch (vtype) {
2813 0 : case AP_PARAM_INT8:
2814 0 : ((AP_Int8 *)vp)->set(value);
2815 0 : return true;
2816 0 : case AP_PARAM_INT16:
2817 0 : ((AP_Int16 *)vp)->set(value);
2818 0 : return true;
2819 0 : case AP_PARAM_INT32:
2820 0 : ((AP_Int32 *)vp)->set(value);
2821 0 : return true;
2822 83 : case AP_PARAM_FLOAT:
2823 83 : ((AP_Float *)vp)->set(value);
2824 83 : return true;
2825 0 : default:
2826 0 : break;
2827 : }
2828 : // not a supported type
2829 0 : return false;
2830 : }
2831 :
2832 : /*
2833 : get a value by name
2834 : */
2835 4700 : bool AP_Param::get(const char *name, float &value)
2836 : {
2837 : enum ap_var_type vtype;
2838 4700 : AP_Param *vp = find(name, &vtype);
2839 4700 : if (vp == nullptr) {
2840 0 : return false;
2841 : }
2842 4700 : switch (vtype) {
2843 2140 : case AP_PARAM_INT8:
2844 2140 : value = ((AP_Int8 *)vp)->get();
2845 2140 : break;
2846 1407 : case AP_PARAM_INT16:
2847 1407 : value = ((AP_Int16 *)vp)->get();
2848 1407 : break;
2849 :
2850 226 : case AP_PARAM_INT32:
2851 226 : value = ((AP_Int32 *)vp)->get();
2852 226 : break;
2853 :
2854 927 : case AP_PARAM_FLOAT:
2855 927 : value = ((AP_Float *)vp)->get();
2856 927 : break;
2857 :
2858 0 : default:
2859 : // not a supported type
2860 0 : return false;
2861 : }
2862 4700 : return true;
2863 : }
2864 :
2865 : /*
2866 : set and save a value by name
2867 : */
2868 58 : bool AP_Param::set_and_save_by_name(const char *name, float value)
2869 : {
2870 : enum ap_var_type vtype;
2871 58 : AP_Param *vp = find(name, &vtype);
2872 58 : if (vp == nullptr) {
2873 0 : return false;
2874 : }
2875 58 : switch (vtype) {
2876 16 : case AP_PARAM_INT8:
2877 16 : ((AP_Int8 *)vp)->set_and_save(value);
2878 16 : return true;
2879 28 : case AP_PARAM_INT16:
2880 28 : ((AP_Int16 *)vp)->set_and_save(value);
2881 28 : return true;
2882 2 : case AP_PARAM_INT32:
2883 2 : ((AP_Int32 *)vp)->set_and_save(value);
2884 2 : return true;
2885 12 : case AP_PARAM_FLOAT:
2886 12 : ((AP_Float *)vp)->set_and_save(value);
2887 12 : return true;
2888 0 : default:
2889 0 : break;
2890 : }
2891 : // not a supported type
2892 0 : return false;
2893 : }
2894 :
2895 : /*
2896 : set and save a value by name
2897 : */
2898 0 : bool AP_Param::set_and_save_by_name_ifchanged(const char *name, float value)
2899 : {
2900 : enum ap_var_type vtype;
2901 0 : AP_Param *vp = find(name, &vtype);
2902 0 : if (vp == nullptr) {
2903 0 : return false;
2904 : }
2905 0 : switch (vtype) {
2906 0 : case AP_PARAM_INT8:
2907 0 : ((AP_Int8 *)vp)->set_and_save_ifchanged(value);
2908 0 : return true;
2909 0 : case AP_PARAM_INT16:
2910 0 : ((AP_Int16 *)vp)->set_and_save_ifchanged(value);
2911 0 : return true;
2912 0 : case AP_PARAM_INT32:
2913 0 : ((AP_Int32 *)vp)->set_and_save_ifchanged(value);
2914 0 : return true;
2915 0 : case AP_PARAM_FLOAT:
2916 0 : ((AP_Float *)vp)->set_and_save_ifchanged(value);
2917 0 : return true;
2918 0 : default:
2919 0 : break;
2920 : }
2921 : // not a supported type
2922 0 : return false;
2923 : }
2924 :
2925 : #if AP_PARAM_DEFAULTS_ENABLED
2926 17679317 : void AP_Param::check_default(AP_Param *ap, float *default_value)
2927 : {
2928 17679317 : if (default_value == nullptr || ap == nullptr) {
2929 14774547 : return;
2930 : }
2931 2904770 : if (default_list != nullptr) {
2932 191414873 : for (defaults_list *item = default_list; item; item = item->next) {
2933 188552992 : if (item->ap == ap) {
2934 42889 : *default_value = item->val;
2935 42889 : return;
2936 : }
2937 : }
2938 : }
2939 : }
2940 :
2941 : /*
2942 : Remove default_list entries that are in param_overrides. Constructors may
2943 : call add_default() before param_overrides is populated, leaving stale nodes.
2944 : */
2945 : #if AP_PARAM_DEFAULTS_ENABLED
2946 3858 : void AP_Param::purge_defaults_list_overrides(void)
2947 : {
2948 3858 : defaults_list **prev = &default_list;
2949 3858 : defaults_list *item = default_list;
2950 179975 : while (item != nullptr) {
2951 176117 : bool found = false;
2952 13826622 : for (uint16_t i = 0; i < num_param_overrides; i++) {
2953 13653929 : if (item->ap == param_overrides[i].object_ptr) {
2954 3424 : found = true;
2955 3424 : break;
2956 : }
2957 : }
2958 176117 : if (found) {
2959 3424 : *prev = item->next;
2960 3424 : defaults_list *to_delete = item;
2961 3424 : item = item->next;
2962 3424 : delete to_delete;
2963 : } else {
2964 172693 : prev = &item->next;
2965 172693 : item = item->next;
2966 : }
2967 : }
2968 3858 : }
2969 : #endif // AP_PARAM_DEFAULTS_ENABLED
2970 :
2971 158573 : void AP_Param::add_default(AP_Param *ap, float v)
2972 : {
2973 : // Embedded defaults trump runtime, don't allow override
2974 5813470 : for (uint16_t i=0; i<num_param_overrides; i++) {
2975 5659090 : if (ap == param_overrides[i].object_ptr) {
2976 4193 : return;
2977 : }
2978 : }
2979 :
2980 154380 : if (default_list != nullptr) {
2981 : // check is param is already in list
2982 4934528 : for (defaults_list *item = default_list; item; item = item->next) {
2983 : // update existing entry
2984 4795229 : if (item->ap == ap) {
2985 12981 : item->val = v;
2986 12981 : return;
2987 : }
2988 : }
2989 : }
2990 :
2991 : // add to list
2992 141399 : defaults_list *new_item = NEW_NOTHROW defaults_list;
2993 141399 : if (new_item == nullptr) {
2994 0 : return;
2995 : }
2996 141399 : new_item->ap = ap;
2997 141399 : new_item->val = v;
2998 141399 : new_item->next = default_list;
2999 141399 : default_list = new_item;
3000 : }
3001 : #endif // AP_PARAM_DEFAULTS_ENABLED
3002 :
3003 :
3004 : #if AP_PARAM_KEY_DUMP
3005 : /*
3006 : do not remove this show_all() code, it is essential for debugging
3007 : and creating conversion tables
3008 : */
3009 :
3010 : // print the value of all variables
3011 : void AP_Param::show(const AP_Param *ap, const char *s,
3012 : enum ap_var_type type, AP_HAL::BetterStream *port)
3013 : {
3014 : switch (type) {
3015 : case AP_PARAM_INT8:
3016 : ::printf("%s: %d\n", s, (int)((AP_Int8 *)ap)->get());
3017 : break;
3018 : case AP_PARAM_INT16:
3019 : ::printf("%s: %d\n", s, (int)((AP_Int16 *)ap)->get());
3020 : break;
3021 : case AP_PARAM_INT32:
3022 : ::printf("%s: %ld\n", s, (long)((AP_Int32 *)ap)->get());
3023 : break;
3024 : case AP_PARAM_FLOAT:
3025 : ::printf("%s: %f\n", s, (double)((AP_Float *)ap)->get());
3026 : break;
3027 : default:
3028 : break;
3029 : }
3030 : }
3031 :
3032 : // print the value of all variables
3033 : void AP_Param::show(const AP_Param *ap, const ParamToken &token,
3034 : enum ap_var_type type, AP_HAL::BetterStream *port)
3035 : {
3036 : char s[AP_MAX_NAME_SIZE+1];
3037 : ap->copy_name_token(token, s, sizeof(s), true);
3038 : s[AP_MAX_NAME_SIZE] = 0;
3039 : show(ap, s, type, port);
3040 : }
3041 :
3042 : // print the value of all variables
3043 : void AP_Param::show_all(AP_HAL::BetterStream *port, bool showKeyValues)
3044 : {
3045 : ParamToken token;
3046 : AP_Param *ap;
3047 : enum ap_var_type type;
3048 : float default_value = NaNf; // from logger quiet_nanf
3049 :
3050 : for (ap=AP_Param::first(&token, &type, &default_value);
3051 : ap;
3052 : ap=AP_Param::next_scalar(&token, &type, &default_value)) {
3053 : if (showKeyValues) {
3054 : ::printf("Key %u: Index %u: GroupElement %u : Default %f :", (unsigned)var_info(token.key).key, (unsigned)token.idx, (unsigned)token.group_element, default_value);
3055 : default_value = NaNf;
3056 : }
3057 : show(ap, token, type, port);
3058 : hal.scheduler->delay(1);
3059 : }
3060 :
3061 : #if AP_PARAM_DEFAULTS_ENABLED
3062 : uint16_t list_len = 0;
3063 : if (default_list != nullptr) {
3064 : for (defaults_list *item = default_list; item; item = item->next) {
3065 : list_len++;
3066 : }
3067 : }
3068 : ::printf("Defaults list containts %i params (%li bytes)\n", list_len, list_len*sizeof(defaults_list));
3069 : #endif
3070 : }
3071 : #endif // AP_PARAM_KEY_DUMP
3072 :
3073 :
3074 : #if AP_PARAM_DYNAMIC_ENABLED
3075 : /*
3076 : allow for dynamically added parameter tables from scripts
3077 :
3078 : The layout we create is as follows:
3079 : - a top level Info with the given prefix, using one of the 10 possible slots in _var_info_dynamic
3080 : - a dynamically allocated GroupInfo table, never freed, of size (num_params+2)
3081 : - the GroupInfo table has an initial AP_Int32 hidden entry with a 32 bit CRC of the prefix
3082 : - the last GroupInfo is a footer
3083 : */
3084 75 : bool AP_Param::add_table(uint8_t _key, const char *prefix, uint8_t num_params)
3085 : {
3086 : // check if the key already exists. We only check base params to allow
3087 : // for scripting reload without a conflict
3088 75 : uint16_t key = uint16_t(_key) + AP_PARAM_DYNAMIC_KEY_BASE;
3089 9049 : for (uint16_t i=0; i<_num_vars_base; i++) {
3090 8974 : if (var_info(i).key == key) {
3091 0 : return false;
3092 : }
3093 : }
3094 75 : if (num_params > 63) {
3095 0 : return false;
3096 : }
3097 :
3098 : // we use a crc of the prefix to ensure the table key isn't re-used
3099 75 : const int32_t crc = int32_t(crc32_small(0, (const uint8_t *)prefix, strlen(prefix)));
3100 : int32_t current_crc;
3101 75 : if (load_int32(key, 0, current_crc) && current_crc != crc) {
3102 : // crc mismatch, we have a conflict with an existing use of this key
3103 0 : return false;
3104 : }
3105 :
3106 : // create the dynamic table if needed. This is never freed
3107 75 : if (_var_info_dynamic == nullptr) {
3108 43 : _var_info_dynamic = (Info *)calloc(AP_PARAM_MAX_DYNAMIC, sizeof(struct Info));
3109 43 : if (_var_info_dynamic == nullptr) {
3110 0 : return false;
3111 : }
3112 473 : for (uint8_t i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
3113 430 : auto &info = _var_info_dynamic[i];
3114 430 : info.type = AP_PARAM_NONE;
3115 430 : info.name = _empty_string;
3116 430 : info.key = 0xFFFF;
3117 430 : info.ptr = nullptr;
3118 430 : info.group_info = nullptr;
3119 430 : info.flags = 0;
3120 : }
3121 : // make tables available
3122 43 : _num_vars += AP_PARAM_MAX_DYNAMIC;
3123 : }
3124 :
3125 : // find existing key (allows for script reload)
3126 : uint8_t i;
3127 726 : for (i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
3128 661 : auto &info = _var_info_dynamic[i];
3129 661 : if (info.type != AP_PARAM_NONE && info.key == key) {
3130 10 : if (_dynamic_table_sizes[i] != 0 &&
3131 10 : num_params > _dynamic_table_sizes[i]) {
3132 : // can't expand the table at runtime
3133 0 : return false;
3134 : }
3135 10 : if (strcmp(prefix, info.name) != 0) {
3136 : // prefix has changed, reject as two scripts running
3137 : // with the same key
3138 0 : return false;
3139 : }
3140 10 : break;
3141 : }
3142 : }
3143 :
3144 75 : if (i == AP_PARAM_MAX_DYNAMIC) {
3145 : // find an unused slot
3146 108 : for (i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
3147 108 : auto &info = _var_info_dynamic[i];
3148 108 : if (info.type == AP_PARAM_NONE ) {
3149 65 : break;
3150 : }
3151 : }
3152 : }
3153 :
3154 75 : if (i == AP_PARAM_MAX_DYNAMIC) {
3155 : // no empty slots
3156 0 : return false;
3157 : }
3158 :
3159 75 : auto &info = _var_info_dynamic[i];
3160 :
3161 : // create memory for the array of floats if needed
3162 : // first float is used for the crc
3163 75 : if (info.ptr == nullptr) {
3164 65 : info.ptr = calloc(num_params+1, sizeof(float));
3165 65 : if (info.ptr == nullptr) {
3166 0 : return false;
3167 : }
3168 : }
3169 :
3170 : // allocate the name
3171 75 : if (info.name == _empty_string) {
3172 65 : info.name = strdup(prefix);
3173 65 : if (info.name == nullptr) {
3174 0 : free(const_cast<void*>(info.ptr));
3175 0 : info.ptr = nullptr;
3176 0 : info.name = _empty_string;
3177 0 : return false;
3178 : }
3179 : }
3180 :
3181 : // if it doesn't exist then create the table
3182 75 : if (info.group_info == nullptr) {
3183 65 : info.group_info = (GroupInfo *)calloc(num_params+2, sizeof(GroupInfo));
3184 65 : if (info.group_info == nullptr) {
3185 0 : free(const_cast<void*>(info.ptr));
3186 0 : free(const_cast<char*>(info.name));
3187 0 : info.ptr = nullptr;
3188 0 : info.name = _empty_string;
3189 0 : return false;
3190 : }
3191 : }
3192 : // fill in footer for all entries
3193 1305 : for (uint8_t gi=1; gi<num_params+2; gi++) {
3194 1230 : auto &ginfo = const_cast<GroupInfo*>(info.group_info)[gi];
3195 1230 : ginfo.name = _empty_string;
3196 1230 : ginfo.idx = 0xff;
3197 1230 : ginfo.flags = AP_PARAM_FLAG_HIDDEN;
3198 : }
3199 : // hidden first parameter containing AP_Int32 crc
3200 75 : auto &hinfo = const_cast<GroupInfo*>(info.group_info)[0];
3201 75 : hinfo.flags = AP_PARAM_FLAG_HIDDEN;
3202 75 : hinfo.name = _empty_string;
3203 75 : hinfo.idx = 0;
3204 75 : hinfo.offset = 0;
3205 75 : hinfo.type = AP_PARAM_INT32;
3206 : // fill in default value with the CRC. Relies on sizeof crc == sizeof float
3207 75 : memcpy((uint8_t *)&hinfo.def_value, (const uint8_t *)&crc, sizeof(crc));
3208 :
3209 : // remember the table size
3210 75 : if (_dynamic_table_sizes[i] == 0) {
3211 65 : _dynamic_table_sizes[i] = num_params;
3212 : }
3213 :
3214 : // make the group active
3215 75 : info.key = key;
3216 75 : info.type = AP_PARAM_GROUP;
3217 :
3218 75 : invalidate_count();
3219 :
3220 : // save the CRC
3221 75 : AP_Int32 *crc_param = const_cast<AP_Int32 *>((AP_Int32 *)info.ptr);
3222 75 : crc_param->set(crc);
3223 75 : crc_param->save(true);
3224 :
3225 75 : return true;
3226 : }
3227 :
3228 : /*
3229 : Load an AP_Int32 variable from EEPROM using top level key and group element. Used to confirm
3230 : a key in add_table()
3231 : */
3232 589 : bool AP_Param::load_int32(uint16_t key, uint32_t group_element, int32_t &value)
3233 : {
3234 : struct Param_header phdr;
3235 :
3236 589 : phdr.type = AP_PARAM_INT32;
3237 589 : set_key(phdr, key);
3238 589 : phdr.group_element = group_element;
3239 :
3240 : // scan EEPROM to find the right location
3241 : uint16_t ofs;
3242 589 : if (!scan(&phdr, &ofs)) {
3243 176 : return false;
3244 : }
3245 :
3246 : // found it
3247 413 : _storage.read_block(&value, ofs+sizeof(phdr), type_size(AP_PARAM_INT32));
3248 413 : return true;
3249 : }
3250 :
3251 : /*
3252 : add a parameter to a dynamic table
3253 : */
3254 514 : bool AP_Param::add_param(uint8_t _key, uint8_t param_num, const char *pname, float default_value)
3255 : {
3256 514 : if (_var_info_dynamic == nullptr) {
3257 : // No dynamic tables available
3258 0 : return false;
3259 : }
3260 :
3261 : // check for valid values
3262 514 : if (param_num == 0 || param_num > 63 || strlen(pname) > AP_MAX_NAME_SIZE) {
3263 0 : return false;
3264 : }
3265 :
3266 514 : uint16_t key = uint16_t(_key) + AP_PARAM_DYNAMIC_KEY_BASE;
3267 : // find the info
3268 : uint8_t i;
3269 653 : for (i=0; i<AP_PARAM_MAX_DYNAMIC; i++) {
3270 653 : auto &info = _var_info_dynamic[i];
3271 653 : if (info.key == key) {
3272 514 : break;
3273 : }
3274 : }
3275 514 : if (i == AP_PARAM_MAX_DYNAMIC) {
3276 : // not found
3277 0 : return false;
3278 : }
3279 :
3280 514 : if (param_num > _dynamic_table_sizes[i]) {
3281 0 : return false;
3282 : }
3283 :
3284 514 : auto &info = _var_info_dynamic[i];
3285 514 : if (info.ptr == nullptr) {
3286 0 : return false;
3287 : }
3288 :
3289 : // check CRC
3290 514 : const auto &hinfo = const_cast<GroupInfo*>(info.group_info)[0];
3291 514 : const int32_t crc = float_to_int32_le(hinfo.def_value);
3292 :
3293 : int32_t current_crc;
3294 514 : if (load_int32(key, 0, current_crc) && current_crc != crc) {
3295 : // crc mismatch, we have a conflict with an existing use of this key
3296 0 : return false;
3297 : }
3298 :
3299 : // Check length
3300 514 : char fullName[AP_MAX_NAME_SIZE+1] = {};
3301 514 : const int fullNameLength = hal.util->snprintf(fullName, sizeof(fullName), "%s%s", info.name, pname);
3302 514 : if ((fullNameLength < 0) || (fullNameLength > AP_MAX_NAME_SIZE)) {
3303 : // Param is too long with table prefix (or snprintf failed)
3304 0 : return false;
3305 : }
3306 :
3307 : // Get param object
3308 514 : AP_Float *pvalues = const_cast<AP_Float *>((const AP_Float *)info.ptr);
3309 514 : AP_Float &p = pvalues[param_num];
3310 :
3311 : // Check for conflicting name
3312 : enum ap_var_type existingType;
3313 514 : AP_Param* existingParam = find(fullName, &existingType);
3314 514 : if ((existingParam != nullptr) && ((existingType != AP_PARAM_FLOAT) || ((AP_Float*)existingParam != &p))) {
3315 : // There is a existing parameter with this name (which is not this parameter from a previous script run)
3316 0 : return false;
3317 : }
3318 :
3319 : // fill in idx of any gaps, leaving them hidden, this allows
3320 : // scripts to remove parameters
3321 4180 : for (uint8_t j=1; j<param_num; j++) {
3322 3666 : auto &g = const_cast<GroupInfo*>(info.group_info)[j];
3323 3666 : if (g.idx == 0xff) {
3324 110 : g.idx = j;
3325 110 : g.flags = AP_PARAM_FLAG_HIDDEN;
3326 110 : g.offset = j*sizeof(float);
3327 110 : g.type = AP_PARAM_FLOAT;
3328 : }
3329 : }
3330 :
3331 514 : auto &ginfo = const_cast<GroupInfo*>(info.group_info)[param_num];
3332 :
3333 514 : if (ginfo.name == _empty_string) {
3334 : // we don't allow name change while running
3335 514 : ginfo.name = strdup(pname);
3336 514 : if (ginfo.name == nullptr) {
3337 0 : ginfo.name = _empty_string;
3338 0 : return false;
3339 : }
3340 : }
3341 514 : ginfo.offset = param_num*sizeof(float);
3342 514 : ginfo.idx = param_num;
3343 514 : float *def_value = const_cast<float *>(&ginfo.def_value);
3344 514 : *def_value = default_value;
3345 514 : ginfo.type = AP_PARAM_FLOAT;
3346 :
3347 : // load from storage if available, the param is hidden during this
3348 : // load so the param is not visible to MAVLink until after it is
3349 : // loaded
3350 514 : p.set_default(default_value);
3351 514 : p.load();
3352 :
3353 : // clear the hidden flag if set and invalidate the count
3354 : // so we recount the parameters
3355 514 : ginfo.flags = 0;
3356 514 : invalidate_count();
3357 :
3358 514 : return true;
3359 : }
3360 : #endif
|
