parameters: merge generated files ito single static constexpr header

- store parameter type and if volatile separately (saves kilobytes of flash)
 - use Bitset for tracking active and changed parameters
 - use atomic for autosave_enabled flag
 - compile at ${MAX_CUSTOM_OPT_LEVEL} (-O2 on non flash constrained boards)
This commit is contained in:
Daniel Agar
2021-02-08 11:32:13 -05:00
parent 71c7c69c9d
commit da1a38b44e
12 changed files with 194 additions and 335 deletions

3
.gitignore vendored
View File

@@ -101,9 +101,6 @@ msg/tmp/
msg/topics_sources/ msg/topics_sources/
platforms/posix/apps.cpp platforms/posix/apps.cpp
platforms/posix/apps.h platforms/posix/apps.h
src/lib/parameters/px4_parameters.c
src/lib/parameters/px4_parameters.h
src/lib/parameters/px4_parameters_public.h
src/lib/version/build_git_version.h src/lib/version/build_git_version.h
src/modules/simulator/simulator_config.h src/modules/simulator/simulator_config.h
src/systemcmds/topic_listener/listener_generated.cpp src/systemcmds/topic_listener/listener_generated.cpp

View File

@@ -1,3 +1,5 @@
# workaround for syslink parameter PARAM_DEFINE_INT32(SLNK_RADIO_ADDR2, 3890735079); // 0xE7E7E7E7
add_compile_options(-Wno-narrowing)
px4_add_board( px4_add_board(
PLATFORM nuttx PLATFORM nuttx

View File

@@ -1,3 +1,5 @@
# workaround for syslink parameter PARAM_DEFINE_INT32(SLNK_RADIO_ADDR2, 3890735079); // 0xE7E7E7E7
add_compile_options(-Wno-narrowing)
px4_add_board( px4_add_board(
PLATFORM nuttx PLATFORM nuttx

View File

@@ -41,7 +41,7 @@
#include "param_macros.h" #include "param_macros.h"
#include <parameters/px4_parameters_public.h> #include <parameters/px4_parameters.hpp>
/** /**
* get the parameter handle from a parameter enum * get the parameter handle from a parameter enum
@@ -112,7 +112,7 @@ class Param<float, p>
{ {
public: public:
// static type-check // static type-check
static_assert(px4::param_types_array[(int)p] == PARAM_TYPE_FLOAT, "parameter type must be float"); static_assert(px4::parameters_type[(int)p] == PARAM_TYPE_FLOAT, "parameter type must be float");
Param() Param()
{ {
@@ -151,7 +151,7 @@ class Param<float &, p>
{ {
public: public:
// static type-check // static type-check
static_assert(px4::param_types_array[(int)p] == PARAM_TYPE_FLOAT, "parameter type must be float"); static_assert(px4::parameters_type[(int)p] == PARAM_TYPE_FLOAT, "parameter type must be float");
Param(float &external_val) Param(float &external_val)
: _val(external_val) : _val(external_val)
@@ -190,7 +190,7 @@ class Param<int32_t, p>
{ {
public: public:
// static type-check // static type-check
static_assert(px4::param_types_array[(int)p] == PARAM_TYPE_INT32, "parameter type must be int32_t"); static_assert(px4::parameters_type[(int)p] == PARAM_TYPE_INT32, "parameter type must be int32_t");
Param() Param()
{ {
@@ -229,7 +229,7 @@ class Param<int32_t &, p>
{ {
public: public:
// static type-check // static type-check
static_assert(px4::param_types_array[(int)p] == PARAM_TYPE_INT32, "parameter type must be int32_t"); static_assert(px4::parameters_type[(int)p] == PARAM_TYPE_INT32, "parameter type must be int32_t");
Param(int32_t &external_val) Param(int32_t &external_val)
: _val(external_val) : _val(external_val)
@@ -268,7 +268,7 @@ class Param<bool, p>
{ {
public: public:
// static type-check // static type-check
static_assert(px4::param_types_array[(int)p] == PARAM_TYPE_INT32, "parameter type must be int32_t"); static_assert(px4::parameters_type[(int)p] == PARAM_TYPE_INT32, "parameter type must be int32_t");
Param() Param()
{ {

View File

@@ -31,6 +31,8 @@
# #
############################################################################ ############################################################################
add_compile_options(${MAX_CUSTOM_OPT_LEVEL})
add_subdirectory(tinybson) add_subdirectory(tinybson)
if (NOT PARAM_DEFAULT_OVERRIDES) if (NOT PARAM_DEFAULT_OVERRIDES)
@@ -113,16 +115,14 @@ add_custom_command(OUTPUT ${parameters_xml} ${parameters_json} ${parameters_json
) )
add_custom_target(parameters_xml DEPENDS ${parameters_xml}) add_custom_target(parameters_xml DEPENDS ${parameters_xml})
# generate px4_parameters.c and px4_parameters{,_public}.h # generate px4_parameters.hpp
add_custom_command(OUTPUT px4_parameters.c px4_parameters.h px4_parameters_public.h add_custom_command(OUTPUT px4_parameters.hpp
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_generate_params.py COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_generate_params.py
--xml ${parameters_xml} --dest ${CMAKE_CURRENT_BINARY_DIR} --xml ${parameters_xml} --dest ${CMAKE_CURRENT_BINARY_DIR}
DEPENDS DEPENDS
${PX4_BINARY_DIR}/parameters.xml ${PX4_BINARY_DIR}/parameters.xml
px_generate_params.py px_generate_params.py
templates/px4_parameters.c.jinja templates/px4_parameters.hpp.jinja
templates/px4_parameters.h.jinja
templates/px4_parameters_public.h.jinja
) )
set(SRCS) set(SRCS)
@@ -147,10 +147,8 @@ endif()
if (NOT "${PX4_BOARD}" MATCHES "px4_io") if (NOT "${PX4_BOARD}" MATCHES "px4_io")
add_library(parameters add_library(parameters
${SRCS} ${SRCS}
px4_parameters.c px4_parameters.hpp
px4_parameters.h )
px4_parameters_public.h
)
if ("${CONFIG_SHMEM}" STREQUAL "1") if ("${CONFIG_SHMEM}" STREQUAL "1")
target_link_libraries(parameters PRIVATE px4_layer) target_link_libraries(parameters PRIVATE px4_layer)

View File

@@ -435,25 +435,7 @@ union param_value_u {
* instead. * instead.
*/ */
struct param_info_s { struct param_info_s {
const char *name const char *name;
// GCC 4.8 and higher don't implement proper alignment of static data on
// 64-bit. This means that the 24-byte param_info_s variables are
// 16 byte aligned by GCC and that messes up the assumption that
// sequential items in the __param segment can be addressed as an array.
// The assumption is that the address of the second parameter is at
// &param[0]+sizeof(param[0]). When compiled with clang it is
// true, with gcc is is not true.
// See https://llvm.org/bugs/show_bug.cgi?format=multiple&id=18006
// The following hack is for GCC >=4.8 only. Clang works fine without
// this.
#ifdef __PX4_POSIX
__attribute__((aligned(16)));
#else
;
#endif
param_type_t type;
uint16_t volatile_param: 1;
union param_value_u val; union param_value_u val;
}; };

View File

@@ -44,13 +44,14 @@
#define PARAM_IMPLEMENTATION #define PARAM_IMPLEMENTATION
#include "param.h" #include "param.h"
#include "param_translation.h" #include "param_translation.h"
#include <parameters/px4_parameters.h> #include <parameters/px4_parameters.hpp>
#include "tinybson/tinybson.h" #include "tinybson/tinybson.h"
#include <crc32.h> #include <crc32.h>
#include <float.h> #include <float.h>
#include <math.h> #include <math.h>
#include <containers/Bitset.hpp>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h> #include <px4_platform_common/px4_config.h>
@@ -91,50 +92,20 @@ static char *param_user_file = nullptr;
/* autosaving variables */ /* autosaving variables */
static hrt_abstime last_autosave_timestamp = 0; static hrt_abstime last_autosave_timestamp = 0;
static struct work_s autosave_work {}; static struct work_s autosave_work {};
static volatile bool autosave_scheduled = false; static px4::atomic<bool> autosave_scheduled{false};
static bool autosave_disabled = false; static bool autosave_disabled = false;
/** static constexpr uint16_t param_info_count = sizeof(px4::parameters) / sizeof(param_info_s);
* Array of static parameter info. static px4::Bitset<param_info_count> params_active; // params found
*/ static px4::Bitset<param_info_count> params_changed; // params non-default
static const param_info_s *param_info_base = (const param_info_s *) &px4_parameters;
#define param_info_count px4_parameters.param_count
/** // Storage for modified parameters.
* Storage for modified parameters.
*/
struct param_wbuf_s { struct param_wbuf_s {
union param_value_u val; union param_value_u val;
param_t param; param_t param;
bool unsaved; bool unsaved;
}; };
uint8_t *param_changed_storage = nullptr;
int size_param_changed_storage_bytes = 0;
const int bits_per_allocation_unit = (sizeof(*param_changed_storage) * 8);
static unsigned
get_param_info_count()
{
/* Singleton creation of and array of bits to track changed values */
if (!param_changed_storage) {
/* Note that we have a (highly unlikely) race condition here: in the worst case the allocation is done twice */
size_param_changed_storage_bytes = (param_info_count / bits_per_allocation_unit) + 1;
param_changed_storage = (uint8_t *)calloc(size_param_changed_storage_bytes, 1);
/* If the allocation fails we need to indicate failure in the
* API by returning PARAM_INVALID
*/
if (param_changed_storage == nullptr) {
return 0;
}
}
return param_info_count;
}
/** flexible array holding modified parameter values */ /** flexible array holding modified parameter values */
UT_array *param_values{nullptr}; UT_array *param_values{nullptr};
@@ -145,10 +116,6 @@ const UT_icd param_icd = {sizeof(param_wbuf_s), nullptr, nullptr, nullptr};
static orb_advert_t param_topic = nullptr; static orb_advert_t param_topic = nullptr;
static unsigned int param_instance = 0; static unsigned int param_instance = 0;
static void param_set_used_internal(param_t param);
static param_t param_find_internal(const char *name, bool notification);
// the following implements an RW-lock using 2 semaphores (used as mutexes). It gives // the following implements an RW-lock using 2 semaphores (used as mutexes). It gives
// priority to readers, meaning a writer could suffer from starvation, but in our use-case // priority to readers, meaning a writer could suffer from starvation, but in our use-case
// we only have short periods of reads and writes are rare. // we only have short periods of reads and writes are rare.
@@ -226,10 +193,10 @@ param_init()
px4_sem_init(&param_sem_save, 0, 1); px4_sem_init(&param_sem_save, 0, 1);
px4_sem_init(&reader_lock_holders_lock, 0, 1); px4_sem_init(&reader_lock_holders_lock, 0, 1);
param_export_perf = perf_alloc(PC_ELAPSED, "param_export"); param_export_perf = perf_alloc(PC_ELAPSED, "param: export");
param_find_perf = perf_alloc(PC_COUNT, "param_find"); param_find_perf = perf_alloc(PC_COUNT, "param: find");
param_get_perf = perf_alloc(PC_COUNT, "param_get"); param_get_perf = perf_alloc(PC_COUNT, "param: get");
param_set_perf = perf_alloc(PC_ELAPSED, "param_set"); param_set_perf = perf_alloc(PC_ELAPSED, "param: set");
} }
/** /**
@@ -238,15 +205,10 @@ param_init()
* @param param The parameter handle to test. * @param param The parameter handle to test.
* @return True if the handle is valid. * @return True if the handle is valid.
*/ */
static bool static constexpr bool handle_in_range(param_t param) { return (param < param_info_count); }
handle_in_range(param_t param)
{
unsigned count = get_param_info_count();
return (count && param < count);
}
/** /**
* Compare two modifid parameter structures to determine ordering. * Compare two modified parameter structures to determine ordering.
* *
* This function is suitable for passing to qsort or bsearch. * This function is suitable for passing to qsort or bsearch.
*/ */
@@ -277,23 +239,21 @@ param_compare_values(const void *a, const void *b)
static param_wbuf_s * static param_wbuf_s *
param_find_changed(param_t param) param_find_changed(param_t param)
{ {
param_wbuf_s *s = nullptr;
param_assert_locked(); param_assert_locked();
if (param_values != nullptr) { if (params_changed[param] && (param_values != nullptr)) {
param_wbuf_s key{}; param_wbuf_s key{};
key.param = param; key.param = param;
s = (param_wbuf_s *)utarray_find(param_values, &key, param_compare_values); return (param_wbuf_s *)utarray_find(param_values, &key, param_compare_values);
} }
return s; return nullptr;
} }
static void void
_param_notify_changes() param_notify_changes()
{ {
parameter_update_s pup = {}; parameter_update_s pup{};
pup.timestamp = hrt_absolute_time(); pup.timestamp = hrt_absolute_time();
pup.instance = param_instance++; pup.instance = param_instance++;
@@ -309,30 +269,23 @@ _param_notify_changes()
} }
} }
void static param_t param_find_internal(const char *name, bool notification)
param_notify_changes()
{
_param_notify_changes();
}
param_t
param_find_internal(const char *name, bool notification)
{ {
perf_count(param_find_perf); perf_count(param_find_perf);
param_t middle; param_t middle;
param_t front = 0; param_t front = 0;
param_t last = get_param_info_count(); param_t last = param_info_count;
/* perform a binary search of the known parameters */ /* perform a binary search of the known parameters */
while (front <= last) { while (front <= last) {
middle = front + (last - front) / 2; middle = front + (last - front) / 2;
int ret = strcmp(name, param_info_base[middle].name); int ret = strcmp(name, param_name(middle));
if (ret == 0) { if (ret == 0) {
if (notification) { if (notification) {
param_set_used_internal(middle); param_set_used(middle);
} }
perf_end(param_find_perf); perf_end(param_find_perf);
@@ -354,78 +307,50 @@ param_find_internal(const char *name, bool notification)
return PARAM_INVALID; return PARAM_INVALID;
} }
param_t param_t param_find(const char *name)
param_find(const char *name)
{ {
return param_find_internal(name, true); return param_find_internal(name, true);
} }
param_t param_t param_find_no_notification(const char *name)
param_find_no_notification(const char *name)
{ {
return param_find_internal(name, false); return param_find_internal(name, false);
} }
unsigned unsigned param_count()
param_count()
{ {
return get_param_info_count(); return param_info_count;
} }
unsigned unsigned param_count_used()
param_count_used()
{ {
unsigned count = 0; return params_active.count();
// ensure the allocation has been done
if (get_param_info_count()) {
for (int i = 0; i < size_param_changed_storage_bytes; i++) {
for (int j = 0; j < bits_per_allocation_unit; j++) {
if (param_changed_storage[i] & (1 << j)) {
count++;
}
}
}
}
return count;
} }
param_t param_t param_for_index(unsigned index)
param_for_index(unsigned index)
{ {
unsigned count = get_param_info_count(); if (index < param_info_count) {
if (count && index < count) {
return (param_t)index; return (param_t)index;
} }
return PARAM_INVALID; return PARAM_INVALID;
} }
param_t param_t param_for_used_index(unsigned index)
param_for_used_index(unsigned index)
{ {
int count = get_param_info_count(); // walk all params and count used params
if (index < param_info_count) {
if (count && (int)index < count) {
/* walk all params and count used params */
unsigned used_count = 0; unsigned used_count = 0;
for (int i = 0; i < size_param_changed_storage_bytes; i++) { for (int i = 0; i < params_active.size(); i++) {
for (int j = 0; j < bits_per_allocation_unit; j++) { if (params_active[i]) {
if (param_changed_storage[i] & (1 << j)) { // we found the right used count,
// return the param value
/* we found the right used count, if (index == used_count) {
* return the param value return static_cast<param_t>(i);
*/
if (index == used_count) {
return (param_t)(i * bits_per_allocation_unit + j);
}
used_count++;
} }
used_count++;
} }
} }
} }
@@ -433,8 +358,7 @@ param_for_used_index(unsigned index)
return PARAM_INVALID; return PARAM_INVALID;
} }
int int param_get_index(param_t param)
param_get_index(param_t param)
{ {
if (handle_in_range(param)) { if (handle_in_range(param)) {
return (unsigned)param; return (unsigned)param;
@@ -443,8 +367,7 @@ param_get_index(param_t param)
return -1; return -1;
} }
int int param_get_used_index(param_t param)
param_get_used_index(param_t param)
{ {
/* this tests for out of bounds and does a constant time lookup */ /* this tests for out of bounds and does a constant time lookup */
if (!param_used(param)) { if (!param_used(param)) {
@@ -454,42 +377,46 @@ param_get_used_index(param_t param)
/* walk all params and count, now knowing that it has a valid index */ /* walk all params and count, now knowing that it has a valid index */
int used_count = 0; int used_count = 0;
for (int i = 0; i < size_param_changed_storage_bytes; i++) { for (int i = 0; i < params_active.size(); i++) {
for (int j = 0; j < bits_per_allocation_unit; j++) { if (params_active[i]) {
if (param_changed_storage[i] & (1 << j)) {
if ((int)param == i * bits_per_allocation_unit + j) { if (param == i) {
return used_count; return used_count;
}
used_count++;
} }
used_count++;
} }
} }
return -1; return -1;
} }
const char * const char *param_name(param_t param)
param_name(param_t param)
{ {
return handle_in_range(param) ? param_info_base[param].name : nullptr; return handle_in_range(param) ? px4::parameters[param].name : nullptr;
} }
bool param_type_t param_type(param_t param)
param_is_volatile(param_t param)
{ {
return handle_in_range(param) ? param_info_base[param].volatile_param : false; return handle_in_range(param) ? px4::parameters_type[param] : PARAM_TYPE_UNKNOWN;
} }
bool bool param_is_volatile(param_t param)
param_value_is_default(param_t param)
{ {
struct param_wbuf_s *s; if (handle_in_range(param)) {
param_lock_reader(); for (const auto &p : px4::parameters_volatile) {
s = param_find_changed(param); if (static_cast<px4::params>(param) == p) {
param_unlock_reader(); return true;
return s == nullptr; }
}
}
return false;
}
bool param_value_is_default(param_t param)
{
return handle_in_range(param) && !params_changed[param];
} }
bool bool
@@ -503,14 +430,7 @@ param_value_unsaved(param_t param)
return ret; return ret;
} }
param_type_t size_t param_size(param_t param)
param_type(param_t param)
{
return handle_in_range(param) ? param_info_base[param].type : PARAM_TYPE_UNKNOWN;
}
size_t
param_size(param_t param)
{ {
if (handle_in_range(param)) { if (handle_in_range(param)) {
switch (param_type(param)) { switch (param_type(param)) {
@@ -551,7 +471,7 @@ param_get_value_ptr(param_t param)
v = &s->val; v = &s->val;
} else { } else {
v = &param_info_base[param].val; v = &px4::parameters[param].val;
} }
result = v; result = v;
@@ -564,10 +484,21 @@ int
param_get(param_t param, void *val) param_get(param_t param, void *val)
{ {
perf_count(param_get_perf); perf_count(param_get_perf);
if (!handle_in_range(param)) {
PX4_ERR("get: param %d invalid", param);
return -1;
}
if (!params_active[param]) {
PX4_WARN("get: param %d (%s) not active", param, param_name(param));
}
int result = -1; int result = -1;
if (val) { if (val) {
param_lock_reader(); param_lock_reader();
const void *v = param_get_value_ptr(param); const void *v = param_get_value_ptr(param);
if (v) { if (v) {
@@ -587,11 +518,11 @@ param_get_default_value(param_t param, void *default_val)
if (default_val && handle_in_range(param)) { if (default_val && handle_in_range(param)) {
switch (param_type(param)) { switch (param_type(param)) {
case PARAM_TYPE_INT32: case PARAM_TYPE_INT32:
memcpy(default_val, &param_info_base[param].val.i, param_size(param)); memcpy(default_val, &px4::parameters[param].val.i, param_size(param));
return 0; return 0;
case PARAM_TYPE_FLOAT: case PARAM_TYPE_FLOAT:
memcpy(default_val, &param_info_base[param].val.f, param_size(param)); memcpy(default_val, &px4::parameters[param].val.f, param_size(param));
return 0; return 0;
} }
} }
@@ -621,7 +552,7 @@ autosave_worker(void *arg)
param_lock_writer(); param_lock_writer();
last_autosave_timestamp = hrt_absolute_time(); last_autosave_timestamp = hrt_absolute_time();
autosave_scheduled = false; autosave_scheduled.store(false);
disabled = autosave_disabled; disabled = autosave_disabled;
param_unlock_writer(); param_unlock_writer();
@@ -646,7 +577,7 @@ autosave_worker(void *arg)
static void static void
param_autosave() param_autosave()
{ {
if (autosave_scheduled || autosave_disabled) { if (autosave_scheduled.load() || autosave_disabled) {
return; return;
} }
@@ -663,7 +594,7 @@ param_autosave()
delay = rate_limit - last_save_elapsed; delay = rate_limit - last_save_elapsed;
} }
autosave_scheduled = true; autosave_scheduled.store(true);
work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(delay)); work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(delay));
} }
@@ -672,9 +603,9 @@ param_control_autosave(bool enable)
{ {
param_lock_writer(); param_lock_writer();
if (!enable && autosave_scheduled) { if (!enable && autosave_scheduled.load()) {
work_cancel(LPWORK, &autosave_work); work_cancel(LPWORK, &autosave_work);
autosave_scheduled = false; autosave_scheduled.store(false);
} }
autosave_disabled = !enable; autosave_disabled = !enable;
@@ -685,13 +616,19 @@ static int
param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes) param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes)
{ {
int result = -1; int result = -1;
bool params_changed = false; bool param_changed = false;
param_lock_writer(); param_lock_writer();
perf_begin(param_set_perf); perf_begin(param_set_perf);
// create the parameter store if it doesn't exist
if (param_values == nullptr) { if (param_values == nullptr) {
utarray_new(param_values, &param_icd); utarray_new(param_values, &param_icd);
// mark all parameters unchanged (default)
for (int i = 0; i < params_changed.size(); i++) {
params_changed.set(i, false);
}
} }
if (param_values == nullptr) { if (param_values == nullptr) {
@@ -705,11 +642,11 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_
switch (param_type(param)) { switch (param_type(param)) {
case PARAM_TYPE_INT32: case PARAM_TYPE_INT32:
set_to_default = (param_info_base[param].val.i == *(int32_t *)val); set_to_default = (px4::parameters[param].val.i == *(int32_t *)val);
break; break;
case PARAM_TYPE_FLOAT: case PARAM_TYPE_FLOAT:
set_to_default = (fabsf(param_info_base[param].val.f - * (float *)val) < FLT_EPSILON); set_to_default = (fabsf(px4::parameters[param].val.f - * (float *)val) < FLT_EPSILON);
break; break;
} }
@@ -720,7 +657,8 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_
// param in memory and set to non-default value, clear // param in memory and set to non-default value, clear
int pos = utarray_eltidx(param_values, s); int pos = utarray_eltidx(param_values, s);
utarray_erase(param_values, pos, 1); utarray_erase(param_values, pos, 1);
params_changed = true; params_changed.set(param, false);
param_changed = true;
} }
// do nothing if param not already set and being set to default // do nothing if param not already set and being set to default
@@ -728,14 +666,15 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_
} else { } else {
if (s == nullptr) { if (s == nullptr) {
/* construct a new parameter */ /* construct a new parameter */
param_wbuf_s buf = {}; param_wbuf_s buf{};
buf.param = param; buf.param = param;
params_changed = true; param_changed = true;
/* add it to the array and sort */ /* add it to the array and sort */
utarray_push_back(param_values, &buf); utarray_push_back(param_values, &buf);
utarray_sort(param_values, param_compare_values); utarray_sort(param_values, param_compare_values);
params_changed.set(param, true);
/* find it after sorting */ /* find it after sorting */
s = param_find_changed(param); s = param_find_changed(param);
@@ -744,12 +683,12 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_
/* update the changed value */ /* update the changed value */
switch (param_type(param)) { switch (param_type(param)) {
case PARAM_TYPE_INT32: case PARAM_TYPE_INT32:
params_changed = params_changed || s->val.i != *(int32_t *)val; param_changed = param_changed || s->val.i != *(int32_t *)val;
s->val.i = *(int32_t *)val; s->val.i = *(int32_t *)val;
break; break;
case PARAM_TYPE_FLOAT: case PARAM_TYPE_FLOAT:
params_changed = params_changed || fabsf(s->val.f - * (float *)val) > FLT_EPSILON; param_changed = param_changed || fabsf(s->val.f - * (float *)val) > FLT_EPSILON;
s->val.f = *(float *)val; s->val.f = *(float *)val;
break; break;
@@ -775,8 +714,8 @@ out:
* If we set something, now that we have unlocked, go ahead and advertise that * If we set something, now that we have unlocked, go ahead and advertise that
* a thing has been set. * a thing has been set.
*/ */
if (params_changed && notify_changes) { if (param_changed && notify_changes) {
_param_notify_changes(); param_notify_changes();
} }
return result; return result;
@@ -794,47 +733,30 @@ const void *param_get_value_ptr_external(param_t param)
} }
#endif #endif
int int param_set(param_t param, const void *val)
param_set(param_t param, const void *val)
{ {
return param_set_internal(param, val, false, true); return param_set_internal(param, val, false, true);
} }
int int param_set_no_notification(param_t param, const void *val)
param_set_no_notification(param_t param, const void *val)
{ {
return param_set_internal(param, val, false, false); return param_set_internal(param, val, false, false);
} }
bool bool param_used(param_t param)
param_used(param_t param)
{ {
int param_index = param_get_index(param); if (handle_in_range(param)) {
return params_active[param];
if (param_index < 0) {
return false;
} }
return param_changed_storage[param_index / bits_per_allocation_unit] & return false;
(1 << param_index % bits_per_allocation_unit);
} }
void param_set_used(param_t param) void param_set_used(param_t param)
{ {
param_set_used_internal(param); if (handle_in_range(param)) {
} params_active.set(param, true);
void param_set_used_internal(param_t param)
{
int param_index = param_get_index(param);
if (param_index < 0) {
return;
} }
// FIXME: this needs locking too
param_changed_storage[param_index / bits_per_allocation_unit] |=
(1 << param_index % bits_per_allocation_unit);
} }
static int param_reset_internal(param_t param, bool notify = true) static int param_reset_internal(param_t param, bool notify = true)
@@ -855,6 +777,8 @@ static int param_reset_internal(param_t param, bool notify = true)
utarray_erase(param_values, pos, 1); utarray_erase(param_values, pos, 1);
} }
params_changed.set(param, false);
param_found = true; param_found = true;
} }
@@ -863,7 +787,7 @@ static int param_reset_internal(param_t param, bool notify = true)
param_unlock_writer(); param_unlock_writer();
if (s != nullptr && notify) { if (s != nullptr && notify) {
_param_notify_changes(); param_notify_changes();
} }
return (!param_found); return (!param_found);
@@ -879,6 +803,10 @@ param_reset_all_internal(bool auto_save)
if (param_values != nullptr) { if (param_values != nullptr) {
utarray_free(param_values); utarray_free(param_values);
for (int i = 0; i < params_changed.size(); i++) {
params_changed.set(i, false);
}
} }
/* mark as reset / deleted */ /* mark as reset / deleted */
@@ -890,7 +818,7 @@ param_reset_all_internal(bool auto_save)
param_unlock_writer(); param_unlock_writer();
_param_notify_changes(); param_notify_changes();
} }
void void
@@ -902,9 +830,7 @@ param_reset_all()
void void
param_reset_excludes(const char *excludes[], int num_excludes) param_reset_excludes(const char *excludes[], int num_excludes)
{ {
param_t param; for (param_t param = 0; handle_in_range(param); param++) {
for (param = 0; handle_in_range(param); param++) {
const char *name = param_name(param); const char *name = param_name(param);
bool exclude = false; bool exclude = false;
@@ -929,9 +855,7 @@ param_reset_excludes(const char *excludes[], int num_excludes)
void void
param_reset_specific(const char *resets[], int num_resets) param_reset_specific(const char *resets[], int num_resets)
{ {
param_t param; for (param_t param = 0; handle_in_range(param); param++) {
for (param = 0; handle_in_range(param); param++) {
const char *name = param_name(param); const char *name = param_name(param);
bool reset = false; bool reset = false;

View File

@@ -50,9 +50,7 @@ def generate(xml_file, dest='.'):
os.path.mkdir(dest) os.path.mkdir(dest)
template_files = [ template_files = [
'px4_parameters.h.jinja', 'px4_parameters.hpp.jinja',
'px4_parameters_public.h.jinja',
'px4_parameters.c.jinja',
] ]
for template_file in template_files: for template_file in template_files:
template = env.get_template(template_file) template = env.get_template(template_file)

View File

@@ -1,35 +0,0 @@
{# jinja syntax: http://jinja.pocoo.org/docs/2.9/templates/ #}
#include "px4_parameters.h"
// DO NOT EDIT
// This file is autogenerated from paramaters.xml
__BEGIN_DECLS
const
struct px4_parameters_t px4_parameters = {
{% for param in params %}
{
"{{ param.attrib["name"] }}",
PARAM_TYPE_{{ param.attrib["type"] }},
{%- if param.attrib["volatile"] == "true" %}
.volatile_param = 1,
{%- else %}
.volatile_param = 0,
{%- endif %}
{%- if param.attrib["type"] == "FLOAT" %}
.val.f = {{ param.attrib["default"] }}
{%- elif param.attrib["type"] == "INT32" %}
.val.i = {{ param.attrib["default"] }}
{%- endif %}
},
{% endfor %}
{{ params | length }}
};
//extern const struct px4_parameters_t px4_parameters;
__END_DECLS
{# vim: set noet ft=jinja fenc=utf-8 ff=unix sts=4 sw=4 ts=4 : #}

View File

@@ -1,21 +0,0 @@
{# jinja syntax: http://jinja.pocoo.org/docs/2.9/templates/ #}
#include <stdint.h>
#include <parameters/param.h>
// DO NOT EDIT
// This file is autogenerated from parameters.xml
__BEGIN_DECLS
struct px4_parameters_t {
{%- for param in params %}
const struct param_info_s __param__{{ param.attrib["name"] }};
{%- endfor %}
const unsigned int param_count;
};
extern const struct px4_parameters_t px4_parameters;
__END_DECLS
{# vim: set noet ft=jinja fenc=utf-8 ff=unix sts=4 sw=4 ts=4 : #}

View File

@@ -0,0 +1,49 @@
{# jinja syntax: http://jinja.pocoo.org/docs/2.9/templates/ #}
#include <stdint.h>
#include <parameters/param.h>
// DO NOT EDIT
// This file is autogenerated from parameters.xml
namespace px4 { {# wrap the enum in a namespace, otherwise we get shadowing errors for MAV_TYPE #}
/// Enum with all parameters
enum class params : uint16_t {
{# enums are guaranteed to start with 0 (if the value for the first is not
specified), and then incremented by 1 (the implementation depends on that!) #}
{%- for param in params %}
{{ param.attrib["name"] }},
{%- endfor %}
};
static constexpr param_info_s parameters[] = {
{% for param in params %}
{
"{{ param.attrib["name"] }}",
{%- if param.attrib["type"] == "FLOAT" %}
.val = {{ "{" }} .f = {{ param.attrib["default"] }} {{ "}" }},
{%- elif param.attrib["type"] == "INT32" %}
.val = {{ "{" }} .i = {{ param.attrib["default"] }}{{ "}" }},
{%- endif %}
},
{% endfor %}
};
static constexpr param_type_t parameters_type[] = {
{% for param in params %}
PARAM_TYPE_{{ param.attrib["type"] }},
{%- endfor -%}
};
static constexpr params parameters_volatile[] = {
{% for param in params %}
{%- if param.attrib["volatile"] == "true" %}
params::{{ param.attrib["name"] }},
{%- endif -%}
{% endfor %}
};
} // namespace px4

View File

@@ -1,37 +0,0 @@
{# jinja syntax: http://jinja.pocoo.org/docs/2.9/templates/ #}
#include <stdint.h>
#include <parameters/param.h>
// DO NOT EDIT
// This file is autogenerated from parameters.xml
#ifdef __cplusplus
namespace px4 { {# wrap the enum in a namespace, otherwise we get shadowing errors for MAV_TYPE #}
/// Enum with all parameters
enum class params {
{# enums are guaranteed to start with 0 (if the value for the first is not
specified), and then incremented by 1 (the implementation depends on that!) #}
{%- for param in params %}
{{ param.attrib["name"] }},
{%- endfor %}
_COUNT
};
// All parameter types
{# (px4_parameters is marked as extern, so we cannot use it as constexpr) #}
static const constexpr int param_types_array[] = {
{%- for param in params %}
PARAM_TYPE_{{ param.attrib["type"] }}, // {{ param.attrib["name"] }}
{%- endfor %}
};
} // namespace px4
#endif /* __cplusplus */