Compare commits

...

1 Commits

Author SHA1 Message Date
Daniel Agar 9586ad0006 [RFC] broadcast parameter changes and ModuleParams skip full updateParams() when possible 2022-12-19 21:14:04 -05:00
54 changed files with 289 additions and 549 deletions
+6 -14
View File
@@ -122,21 +122,13 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template
for field in spec.parsed_fields():
field_name_and_type.update({field.name: field.type})
# assert if the timestamp field exists
try:
assert 'timestamp' in field_name_and_type
except AssertionError:
print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\tNo 'timestamp' field found in " +
spec.short_name + " msg definition!")
exit(1)
# assert if the timestamp field is of type uint64
try:
assert field_name_and_type.get('timestamp') == 'uint64'
except AssertionError:
print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name +
" msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!")
exit(1)
# try:
# assert field_name_and_type.get('timestamp') == 'uint64'
# except AssertionError:
# print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name +
# " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!")
# exit(1)
topics = get_topics(filename)
-2
View File
@@ -6,7 +6,6 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
CONFIG_DRIVERS_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_BMP388=y
@@ -60,7 +59,6 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL_L1=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y
CONFIG_MODULES_LAND_DETECTOR=y
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
CONFIG_MODULES_LOAD_MON=y
+1
View File
@@ -134,6 +134,7 @@ set(msg_files
OrbTest.msg
OrbTestLarge.msg
OrbTestMedium.msg
Parameter.msg
ParameterUpdate.msg
Ping.msg
PositionControllerLandingStatus.msg
+12
View File
@@ -0,0 +1,12 @@
char[17] name # parameter name
int16 index # -1 if the param_id field should be used as identifier
int16 index_used # -1 if the param_id field should be used as identifier
uint8 TYPE_INVALID = 0
uint8 TYPE_BOOL = 1
uint8 TYPE_INT32 = 2
uint8 TYPE_FLOAT32 = 3
uint8 type # parameter type
int32 int32_value # current value if param_type is int-like
float32 float32_value # current value if param_type is float-like
+4 -7
View File
@@ -4,11 +4,8 @@ uint64 timestamp # time since system start (microseconds)
uint32 instance # Instance count - constantly incrementing
uint32 get_count
uint32 set_count
uint32 find_count
uint32 export_count
uint16 count
uint16 active
uint16 changed
uint16 custom_default
Parameter changed_param
uint8 ORB_QUEUE_LENGTH = 8
@@ -42,6 +42,8 @@
#include <containers/List.hpp>
#include "param.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
class ModuleParams : public ListNode<ModuleParams *>
{
@@ -68,6 +70,8 @@ public:
virtual ~ModuleParams()
{
if (_parent) { _parent->_children.remove(this); }
_parameter_update_sub.unsubscribe();
}
// Disallow copy construction and move assignment.
@@ -83,20 +87,75 @@ protected:
*/
virtual void updateParams()
{
for (const auto &child : _children) {
child->updateParams();
bool update_all = true;
int parameter_updates = 0;
while (_parameter_update_sub.updated() && (parameter_updates < parameter_update_s::ORB_QUEUE_LENGTH)) {
parameter_updates++;
parameter_update_s parameter_update;
if (_parameter_update_sub.copy(&parameter_update)
&& (_parameter_update_instance > 0)
&& (parameter_update.instance == _parameter_update_instance + 1)
&& (parameter_update.changed_param.index >= 0)
&& (static_cast<uint16_t>(parameter_update.changed_param.index) != PARAM_INVALID)
) {
update_all = false;
for (const auto &child : _children) {
child->updateParams(parameter_update);
}
updateParamsImpl(parameter_update);
_parameter_update_instance = parameter_update.instance;
} else {
update_all = true;
break;
}
}
updateParamsImpl();
if (update_all) {
PX4_DEBUG("updateParams: updating all params");
parameter_update_s parameter_update;
if (_parameter_update_sub.copy(&parameter_update)) {
_parameter_update_instance = parameter_update.instance;
}
for (const auto &child : _children) {
child->updateParams();
}
updateParamsImpl();
} else {
PX4_DEBUG("updateParams: updating all params skipped");
}
}
virtual void updateParams(const parameter_update_s &parameter_update)
{
for (const auto &child : _children) {
child->updateParams(parameter_update);
}
updateParamsImpl(parameter_update);
}
/**
* @brief The implementation for this is generated with the macro DEFINE_PARAMETERS()
*/
virtual void updateParamsImpl() {}
virtual void updateParamsImpl(const parameter_update_s &parameter_update) {}
private:
/** @list _children The module parameter list of inheriting classes. */
List<ModuleParams *> _children;
ModuleParams *_parent{nullptr};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uint32_t _parameter_update_instance{0};
};
@@ -44,11 +44,13 @@
#include <math.h>
#include <parameters/px4_parameters.hpp>
#include <uORB/topics/parameter.h>
#include <uORB/topics/parameter_update.h>
/**
* get the parameter handle from a parameter enum
*/
inline static param_t param_handle(px4::params p)
inline static constexpr param_t param_handle(px4::params p)
{
return (param_t)p;
}
@@ -62,6 +64,9 @@ inline static param_t param_handle(px4::params p)
#define _CALL_UPDATE(x) \
STRIP(x).update();
#define _SET_PARAMETER_UPDATE(x) \
case STRIP(x).handle(): STRIP(x).setValue(parameter_update.changed_param); return;
// define the parameter update method, which will update all parameters.
// It is marked as 'final', so that wrong usages lead to a compile error (see below)
#define _DEFINE_PARAMETER_UPDATE_METHOD(...) \
@@ -69,6 +74,11 @@ inline static param_t param_handle(px4::params p)
void updateParamsImpl() final { \
APPLY_ALL(_CALL_UPDATE, __VA_ARGS__) \
} \
void updateParamsImpl(const parameter_update_s &parameter_update) { \
switch(parameter_update.changed_param.index) { \
APPLY_ALL(_SET_PARAMETER_UPDATE, __VA_ARGS__) \
} \
} \
private:
// Define a list of parameters. This macro also creates code to update parameters.
@@ -88,6 +98,12 @@ inline static param_t param_handle(px4::params p)
parent_class::updateParamsImpl(); \
APPLY_ALL(_CALL_UPDATE, __VA_ARGS__) \
} \
void updateParamsImpl(const parameter_update_s &parameter_update) { \
parent_class::updateParamsImpl(parameter_update); \
switch(parameter_update.changed_param.index) { \
APPLY_ALL(_SET_PARAMETER_UPDATE, __VA_ARGS__) \
} \
} \
private:
#define DEFINE_PARAMETERS_CUSTOM_PARENT(parent_class, ...) \
@@ -118,7 +134,6 @@ public:
Param()
{
param_set_used(handle());
update();
}
@@ -126,29 +141,22 @@ public:
const float &reference() const { return _val; }
/// Store the parameter value to the parameter storage (@see param_set())
bool commit() const { return param_set(handle(), &_val) == 0; }
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
bool commit_no_notification() const { return param_set_no_notification(handle(), &_val) == 0; }
/// Set and commit a new value. Returns true if the value changed.
bool commit_no_notification(float val)
bool set(float val)
{
if (fabsf(val - _val) > FLT_EPSILON) {
set(val);
commit_no_notification();
return true;
_val = val;
return (param_set(handle(), &_val) == 0);
}
return false;
}
void set(float val) { _val = val; }
bool update() { return param_get(handle(), &_val) == 0; }
param_t handle() const { return param_handle(p); }
void setValue(const parameter_s &parameter_update) { _val = parameter_update.float32_value; }
static constexpr param_t handle() { return param_handle(p); }
private:
float _val;
};
@@ -164,7 +172,6 @@ public:
Param(float &external_val)
: _val(external_val)
{
param_set_used(handle());
update();
}
@@ -172,29 +179,18 @@ public:
const float &reference() const { return _val; }
/// Store the parameter value to the parameter storage (@see param_set())
bool commit() const { return param_set(handle(), &_val) == 0; }
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
bool commit_no_notification() const { return param_set_no_notification(handle(), &_val) == 0; }
/// Set and commit a new value. Returns true if the value changed.
bool commit_no_notification(float val)
bool set(float val)
{
if (fabsf(val - _val) > FLT_EPSILON) {
set(val);
commit_no_notification();
return true;
}
return false;
_val = val;
return (param_set(handle(), &_val) == 0);
}
void set(float val) { _val = val; }
bool update() { return param_get(handle(), &_val) == 0; }
param_t handle() const { return param_handle(p); }
void setValue(const parameter_s &parameter_update) { _val = parameter_update.float32_value; }
static constexpr param_t handle() { return param_handle(p); }
private:
float &_val;
};
@@ -208,7 +204,6 @@ public:
Param()
{
param_set_used(handle());
update();
}
@@ -216,29 +211,18 @@ public:
const int32_t &reference() const { return _val; }
/// Store the parameter value to the parameter storage (@see param_set())
bool commit() const { return param_set(handle(), &_val) == 0; }
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
bool commit_no_notification() const { return param_set_no_notification(handle(), &_val) == 0; }
/// Set and commit a new value. Returns true if the value changed.
bool commit_no_notification(int32_t val)
bool set(int32_t val)
{
if (val != _val) {
set(val);
commit_no_notification();
return true;
}
return false;
_val = val;
return (param_set(handle(), &_val) == 0);
}
void set(int32_t val) { _val = val; }
bool update() { return param_get(handle(), &_val) == 0; }
param_t handle() const { return param_handle(p); }
void setValue(const parameter_s &parameter_update) { _val = parameter_update.int32_value; }
static constexpr param_t handle() { return param_handle(p); }
private:
int32_t _val;
};
@@ -254,7 +238,6 @@ public:
Param(int32_t &external_val)
: _val(external_val)
{
param_set_used(handle());
update();
}
@@ -263,28 +246,17 @@ public:
const int32_t &reference() const { return _val; }
/// Store the parameter value to the parameter storage (@see param_set())
bool commit() const { return param_set(handle(), &_val) == 0; }
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
bool commit_no_notification() const { return param_set_no_notification(handle(), &_val) == 0; }
/// Set and commit a new value. Returns true if the value changed.
bool commit_no_notification(int32_t val)
bool set(int32_t val)
{
if (val != _val) {
set(val);
commit_no_notification();
return true;
}
return false;
_val = val;
return (param_set(handle(), &_val) == 0);
}
void set(int32_t val) { _val = val; }
bool update() { return param_get(handle(), &_val) == 0; }
param_t handle() const { return param_handle(p); }
void setValue(const parameter_s &parameter_update) { _val = parameter_update.int32_value; }
static constexpr param_t handle() { return param_handle(p); }
private:
int32_t &_val;
};
@@ -298,7 +270,6 @@ public:
Param()
{
param_set_used(handle());
update();
}
@@ -306,34 +277,14 @@ public:
const bool &reference() const { return _val; }
/// Store the parameter value to the parameter storage (@see param_set())
bool commit() const
{
int32_t value_int = (int32_t)_val;
return param_set(handle(), &value_int) == 0;
}
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
bool commit_no_notification() const
{
int32_t value_int = (int32_t)_val;
return param_set_no_notification(handle(), &value_int) == 0;
}
/// Set and commit a new value. Returns true if the value changed.
bool commit_no_notification(bool val)
bool set(bool val)
{
if (val != _val) {
set(val);
commit_no_notification();
return true;
}
return false;
_val = val;
int32_t value_int = (int32_t)_val;
return (param_set(handle(), &value_int) == 0);
}
void set(bool val) { _val = val; }
bool update()
{
int32_t value_int;
@@ -347,7 +298,9 @@ public:
return false;
}
param_t handle() const { return param_handle(p); }
void setValue(const parameter_s &parameter_update) { _val = parameter_update.int32_value; }
static constexpr param_t handle() { return param_handle(p); }
private:
bool _val;
};
+1 -1
View File
@@ -385,7 +385,7 @@ uORB::DeviceNode::print_statistics(int max_topic_length)
lock();
const uint8_t instance = get_instance();
const int8_t sub_count = subscriber_count();
const int16_t sub_count = subscriber_count();
const uint8_t queue_size = get_queue_size();
unlock();
+2 -2
View File
@@ -197,7 +197,7 @@ public:
uint8_t get_queue_size() const { return _queue_size; }
int8_t subscriber_count() const { return _subscriber_count; }
int16_t subscriber_count() const { return _subscriber_count; }
/**
* Returns the number of updated data relative to the parameter 'generation'
@@ -296,7 +296,7 @@ private:
const uint8_t _instance; /**< orb multi instance identifier */
bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */
uint8_t _queue_size; /**< maximum number of elements in the queue */
int8_t _subscriber_count{0};
int16_t _subscriber_count{0};
// Determine the data range
@@ -107,7 +107,6 @@ public:
Param()
{
//param_set_used(handle());
update();
}
@@ -122,19 +121,12 @@ public:
//return param_set(handle(), &_val) == 0;
}
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
bool commit_no_notification() const
{
return true; // TODO
//return param_set_no_notification(handle(), &_val) == 0;
}
/// Set and commit a new value. Returns true if the value changed.
bool commit_no_notification(float val)
bool commit(float val)
{
if (fabsf(val - _val) > FLT_EPSILON) {
set(val);
commit_no_notification();
commit();
return true;
}
@@ -165,7 +157,6 @@ public:
Param(float &external_val)
: _val(external_val)
{
//param_set_used(handle()); // TODO
update();
}
@@ -180,19 +171,12 @@ public:
return true;
}
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
bool commit_no_notification() const
{
return true;
//return param_set_no_notification(handle(), &_val) == 0;
}
/// Set and commit a new value. Returns true if the value changed.
bool commit_no_notification(float val)
bool commit(float val)
{
if (fabsf(val - _val) > FLT_EPSILON) {
set(val);
commit_no_notification();
commit();
return true;
}
@@ -221,7 +205,6 @@ public:
Param()
{
//param_set_used(handle());
update();
}
@@ -236,19 +219,12 @@ public:
//return param_set(handle(), &_val) == 0;
}
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
bool commit_no_notification() const
{
//return param_set_no_notification(handle(), &_val) == 0;
return true;
}
/// Set and commit a new value. Returns true if the value changed.
bool commit_no_notification(int32_t val)
bool commit(int32_t val)
{
if (val != _val) {
set(val);
commit_no_notification();
commit();
return true;
}
@@ -279,7 +255,6 @@ public:
Param(int32_t &external_val)
: _val(external_val)
{
//param_set_used(handle());
update();
}
@@ -294,19 +269,12 @@ public:
//return param_set(handle(), &_val) == 0;
}
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
bool commit_no_notification() const
{
return true;
//return param_set_no_notification(handle(), &_val) == 0;
}
/// Set and commit a new value. Returns true if the value changed.
bool commit_no_notification(int32_t val)
bool commit(int32_t val)
{
if (val != _val) {
set(val);
commit_no_notification();
commit();
return true;
}
@@ -335,7 +303,6 @@ public:
Param()
{
//param_set_used(handle());
update();
}
@@ -351,20 +318,12 @@ public:
return true;
}
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
bool commit_no_notification() const
{
int32_t value_int = (int32_t)_val;
//return param_set_no_notification(handle(), &value_int) == 0;
return true;
}
/// Set and commit a new value. Returns true if the value changed.
bool commit_no_notification(bool val)
bool commit(bool val)
{
if (val != _val) {
set(val);
commit_no_notification();
commit();
return true;
}
@@ -308,7 +308,7 @@ CameraTrigger::CameraTrigger() :
_activation_time = 40.0f;
PX4_WARN("Trigger interval too low for PWM interface, setting to 40 ms");
param_set_no_notification(_p_activation_time, &(_activation_time));
param_set(_p_activation_time, &(_activation_time));
}
// Advertise critical publishers here, because we cannot advertise in interrupt context
@@ -581,7 +581,7 @@ CameraTrigger::Run()
*/
if (cmd.param1 > 0.0f) {
_distance = cmd.param1;
param_set_no_notification(_p_distance, &_distance);
param_set(_p_distance, &_distance);
_trigger_enabled = true;
_trigger_paused = false;
@@ -598,7 +598,7 @@ CameraTrigger::Run()
if (cmd.param2 > 0.0f) {
if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
_activation_time = cmd.param2;
param_set_no_notification(_p_activation_time, &(_activation_time));
param_set(_p_activation_time, &(_activation_time));
}
}
@@ -616,14 +616,14 @@ CameraTrigger::Run()
if (cmd.param1 > 0.0f) {
_interval = cmd.param1;
param_set_no_notification(_p_interval, &(_interval));
param_set(_p_interval, &_interval);
}
// We can only control the shutter integration time of the camera in GPIO mode
if (cmd.param2 > 0.0f) {
if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
_activation_time = cmd.param2;
param_set_no_notification(_p_activation_time, &_activation_time);
param_set(_p_activation_time, &_activation_time);
}
}
@@ -637,7 +637,7 @@ CameraTrigger::Run()
if (cmd.param1 > 0.0f) {
_distance = cmd.param1;
param_set_no_notification(_p_distance, &_distance);
param_set(_p_distance, &_distance);
_trigger_enabled = true;
_trigger_paused = false;
@@ -654,14 +654,14 @@ CameraTrigger::Run()
if (cmd.param2 > 0.0f) {
if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
_activation_time = cmd.param2;
param_set_no_notification(_p_activation_time, &(_activation_time));
param_set(_p_activation_time, &_activation_time);
}
}
// Set Param for minimum camera trigger interval
if (cmd.param3 > 0.0f) {
_min_interval = cmd.param3;
param_set_no_notification(_p_min_interval, &(_min_interval));
param_set(_p_min_interval, &_min_interval);
}
if (cmd.param4 >= 2.0f) {
+1 -1
View File
@@ -135,7 +135,7 @@ void DShot::enable_dshot_outputs(const bool enabled)
if (dshot_frequency_request != 0) {
if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) {
PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name);
param_set_no_notification(handle, &dshot_frequency_param);
param_set(handle, &dshot_frequency_param);
} else {
dshot_frequency = dshot_frequency_request;
-1
View File
@@ -773,7 +773,6 @@ void RCInput::Run()
) {
// RC_INPUT_PROTO auto => locked selection
_param_rc_input_proto.set(_rc_scan_state);
_param_rc_input_proto.commit();
}
}
}
@@ -106,8 +106,6 @@ int SagetechMXS::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
bool SagetechMXS::init()
{
ScheduleOnInterval(UPDATE_INTERVAL_US); // 50Hz
@@ -117,7 +115,6 @@ bool SagetechMXS::init()
if (vehicle_list == nullptr) {
if (_adsb_list_max.get() > MAX_VEHICLES_LIMIT) { // Safety Check
_adsb_list_max.set(MAX_VEHICLES_LIMIT);
_adsb_list_max.commit();
list_size_allocated = MAX_VEHICLES_LIMIT;
}
@@ -158,8 +155,7 @@ int SagetechMXS::custom_command(int argc, char *argv[])
}
if (!strcmp(verb, "ident")) {
get_instance()->_adsb_ident.set(1);
return get_instance()->_adsb_ident.commit();
return get_instance()->_adsb_ident.set(1);
}
if (!strcmp(verb, "opmode")) {
@@ -170,20 +166,16 @@ int SagetechMXS::custom_command(int argc, char *argv[])
return PX4_ERROR;
} else if (!strcmp(opmode, "off") || !strcmp(opmode, "0")) {
get_instance()->_mxs_op_mode.set(0);
return get_instance()->_mxs_op_mode.commit();
return get_instance()->_mxs_op_mode.set(0);
} else if (!strcmp(opmode, "on") || !strcmp(opmode, "1")) {
get_instance()->_mxs_op_mode.set(1);
return get_instance()->_mxs_op_mode.commit();
return get_instance()->_mxs_op_mode.set(1);
} else if (!strcmp(opmode, "stby") || !strcmp(opmode, "2")) {
get_instance()->_mxs_op_mode.set(2);
return get_instance()->_mxs_op_mode.commit();
return get_instance()->_mxs_op_mode.set(2);
} else if (!strcmp(opmode, "alt") || !strcmp(opmode, "3")) {
get_instance()->_mxs_op_mode.set(3);
return get_instance()->_mxs_op_mode.commit();
return get_instance()->_mxs_op_mode.set(3);
} else {
print_usage("Invalid Op Mode");
@@ -207,8 +199,7 @@ int SagetechMXS::custom_command(int argc, char *argv[])
return PX4_ERROR;
} else {
get_instance()->_adsb_squawk.set(sqk);
return get_instance()->_adsb_squawk.commit();
return get_instance()->_adsb_squawk.set(sqk);
}
}
@@ -305,7 +296,6 @@ void SagetechMXS::Run()
auto_config_installation();
auto_config_flightid();
_mxs_op_mode.set(sg_op_mode_t::modeStby);
_mxs_op_mode.commit();
send_targetreq_msg();
mxs_state.initialized = true;
}
@@ -751,7 +741,6 @@ void SagetechMXS::send_operating_msg()
if (mxs_state.op.identOn) {
_adsb_ident.set(0);
_adsb_ident.commit();
}
if (_gps.vel_ned_valid) {
@@ -1248,22 +1237,16 @@ int SagetechMXS::handle_fid(const char *fid)
int SagetechMXS::store_inst_resp()
{
_mxs_op_mode.set(mxs_state.ack.opMode);
_mxs_op_mode.commit();
_adsb_icao.set(mxs_state.inst.icao);
_adsb_icao.commit();
_adsb_len_width.set(mxs_state.inst.size);
_adsb_len_width.commit();
_adsb_emit_type.set(convert_sg_to_emitter_type(mxs_state.inst.emitter));
_adsb_emit_type.commit();
return PX4_OK;
}
void SagetechMXS::auto_config_operating()
{
mxs_state.op.opMode = sg_op_mode_t::modeOff;
_mxs_op_mode.set(sg_op_mode_t::modeOff);
_mxs_op_mode.commit();
if (check_valid_squawk(_adsb_squawk.get())) {
mxs_state.op.squawk = convert_base_to_decimal(BASE_OCTAL, _adsb_squawk.get());
-3
View File
@@ -90,9 +90,6 @@ public:
// Store the parameter value to the parameter storage (@see param_set())
bool commit() { return (param_set(_handle, &_val) == PX4_OK); }
// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
bool commit_no_notification() { return (param_set_no_notification(_handle, &_val) == PX4_OK); }
void set(T val) { _val = val; }
bool update() override { return (param_get(_handle, &_val) == PX4_OK); }
@@ -241,7 +241,7 @@ param_import_callback(bson_decoder_t decoder, bson_node_t node)
goto out;
}
if (param_set_external(param, v, true, true)) {
if (param_set_external(param, v, true)) {
debug("error setting value for '%s'", node->name);
goto out;
}
+1 -1
View File
@@ -58,7 +58,7 @@ __BEGIN_DECLS
*/
__EXPORT extern UT_array *param_values;
__EXPORT int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes);
__EXPORT int param_set_external(param_t param, const void *val, bool mark_saveds);
__EXPORT const void *param_get_value_ptr_external(param_t param);
/* The interface hooks to the Flash based storage. The caller is responsible for locking */
-31
View File
@@ -197,14 +197,6 @@ __EXPORT bool param_value_unsaved(param_t param);
*/
__EXPORT param_type_t param_type(param_t param);
/**
* Determine the size of a parameter.
*
* @param param A handle returned by param_find or passed by param_foreach.
* @return The size of the parameter's value.
*/
__EXPORT size_t param_size(param_t param);
/**
* Copy the value of a parameter.
*
@@ -250,29 +242,6 @@ __EXPORT int param_set(param_t param, const void *val);
*/
__EXPORT int param_set_default_value(param_t param, const void *val);
/**
* Mark a parameter as used. Only marked parameters will be sent to a GCS.
* A call to param_find() will mark a param as used as well.
*
* @param param A handle returned by param_find or passed by param_foreach.
*/
__EXPORT void param_set_used(param_t param);
/**
* Set the value of a parameter, but do not notify the system about the change.
*
* @param param A handle returned by param_find or passed by param_foreach.
* @param val The value to set; assumed to point to a variable of the parameter type.
* @return Zero if the parameter's value could be set from a scalar, nonzero otherwise.
*/
__EXPORT int param_set_no_notification(param_t param, const void *val);
/**
* Notify the system about parameter changes. Can be used for example after several calls to
* param_set_no_notification() to avoid unnecessary system notifications.
*/
__EXPORT void param_notify_changes(void);
/**
* Reset all parameters to their default values.
*/
+1 -1
View File
@@ -156,7 +156,7 @@ bool param_modify_on_import(bson_node_t node)
// If was in range height mode, set range aiding to "always"
if (node->i32 == 2) {
int32_t rng_mode = 2;
param_set_no_notification(param_find("EKF2_RNG_CTRL"), &rng_mode);
param_set(param_find("EKF2_RNG_CTRL"), &rng_mode);
}
PX4_INFO("param migrating EKF2_HGT_MODE (removed) -> EKF2_HGT_REF: value=%" PRId32, node->i32);
+37 -33
View File
@@ -246,18 +246,33 @@ param_find_changed(param_t param)
return nullptr;
}
void
param_notify_changes()
static void param_notify_changes(param_t param = PARAM_INVALID)
{
parameter_update_s pup{};
pup.instance = param_instance++;
pup.get_count = perf_event_count(param_get_perf);
pup.set_count = perf_event_count(param_set_perf);
pup.find_count = perf_event_count(param_find_perf);
pup.export_count = perf_event_count(param_export_perf);
pup.active = params_active.count();
pup.changed = params_changed.count();
pup.custom_default = params_custom_default.count();
pup.count = params_active.count();
if (param != PARAM_INVALID) {
pup.changed_param.index = param;
pup.changed_param.index_used = param_get_used_index(param);
memcpy(pup.changed_param.name, param_name(param), 16);
switch (param_type(param)) {
case PARAM_TYPE_INT32:
param_get(param, &pup.changed_param.int32_value);
pup.changed_param.type = parameter_s::TYPE_INT32;
break;
case PARAM_TYPE_FLOAT:
pup.changed_param.type = parameter_s::TYPE_FLOAT32;
param_get(param, &pup.changed_param.float32_value);
break;
}
} else {
pup.changed_param.index = -1;
}
pup.timestamp = hrt_absolute_time();
if (param_topic == nullptr) {
@@ -284,7 +299,7 @@ static param_t param_find_internal(const char *name, bool notification)
if (ret == 0) {
if (notification) {
param_set_used(middle);
params_active.set(middle, true);
}
return middle;
@@ -429,6 +444,7 @@ param_get(param_t param, void *val)
if (!params_active[param]) {
PX4_DEBUG("get: param %" PRId16 " (%s) not active", param, param_name(param));
params_active.set(param, true);
}
int result = PX4_ERROR;
@@ -665,7 +681,7 @@ param_control_autosave(bool enable)
}
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)
{
if (!handle_in_range(param)) {
PX4_ERR("set invalid param %d", param);
@@ -762,17 +778,17 @@ out:
* If we set something, now that we have unlocked, go ahead and advertise that
* a thing has been set.
*/
if ((result == PX4_OK) && param_changed && notify_changes) {
param_notify_changes();
if ((result == PX4_OK) && param_changed) {
param_notify_changes(param);
}
return result;
}
#if defined(FLASH_BASED_PARAMS)
int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes)
int param_set_external(param_t param, const void *val, bool mark_saved)
{
return param_set_internal(param, val, mark_saved, notify_changes);
return param_set_internal(param, val, mark_saved);
}
const void *param_get_value_ptr_external(param_t param)
@@ -783,12 +799,7 @@ const void *param_get_value_ptr_external(param_t param)
int param_set(param_t param, const void *val)
{
return param_set_internal(param, val, false, true);
}
int 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);
}
bool param_used(param_t param)
@@ -800,13 +811,6 @@ bool param_used(param_t param)
return false;
}
void param_set_used(param_t param)
{
if (handle_in_range(param)) {
params_active.set(param, true);
}
}
int param_set_default_value(param_t param, const void *val)
{
if (!handle_in_range(param)) {
@@ -908,7 +912,7 @@ int param_set_default_value(param_t param, const void *val)
if ((result == PX4_OK) && param_used(param)) {
// send notification if param is already in use
param_notify_changes();
param_notify_changes(param);
}
return result;
@@ -941,8 +945,8 @@ static int param_reset_internal(param_t param)
param_unlock_writer();
if (s != nullptr) {
param_notify_changes();
if (param_found) {
param_notify_changes(param);
}
return (!param_found);
@@ -1489,7 +1493,7 @@ param_import_callback(bson_decoder_t decoder, bson_node_t node)
case BSON_INT32: {
if (param_type(param) == PARAM_TYPE_INT32) {
int32_t i = node->i32;
param_set_internal(param, &i, true, true);
param_set_internal(param, &i, true);
PX4_DEBUG("Imported %s with value %" PRIi32, param_name(param), i);
} else {
@@ -1501,7 +1505,7 @@ param_import_callback(bson_decoder_t decoder, bson_node_t node)
case BSON_DOUBLE: {
if (param_type(param) == PARAM_TYPE_FLOAT) {
float f = node->d;
param_set_internal(param, &f, true, true);
param_set_internal(param, &f, true);
PX4_DEBUG("Imported %s with value %f", param_name(param), (double)f);
} else {
+1 -1
View File
@@ -96,7 +96,7 @@ bool param_is_volatile(param_t param)
return false;
}
size_t param_size(param_t param)
static size_t param_size(param_t param)
{
if (handle_in_range(param)) {
switch (param_type(param)) {
+1 -18
View File
@@ -50,11 +50,6 @@ int param_ioctl(unsigned int cmd, unsigned long arg)
int ret = OK;
switch (cmd) {
case PARAMIOCNOTIFY: {
param_notify_changes();
}
break;
case PARAMIOCFIND: {
paramiocfind_t *data = (paramiocfind_t *)arg;
@@ -111,13 +106,7 @@ int param_ioctl(unsigned int cmd, unsigned long arg)
case PARAMIOCSET: {
paramiocset_t *data = (paramiocset_t *)arg;
if (data->notification) {
data->ret = param_set(data->param, data->val);
} else {
data->ret = param_set_no_notification(data->param, data->val);
}
data->ret = param_set(data->param, data->val);
}
break;
@@ -127,12 +116,6 @@ int param_ioctl(unsigned int cmd, unsigned long arg)
}
break;
case PARAMIOCSETUSED: {
paramiocsetused_t *data = (paramiocsetused_t *)arg;
param_set_used(data->param);
}
break;
case PARAMIOCSETDEFAULT: {
paramiocsetdefault_t *data = (paramiocsetdefault_t *)arg;
data->ret = param_set_default_value(data->param, data->val);
-5
View File
@@ -106,11 +106,6 @@ typedef struct paramiocused {
bool ret;
} paramiocused_t;
#define PARAMIOCSETUSED _PARAMIOC(11)
typedef struct paramiocsetused {
const param_t param;
} paramiocsetused_t;
#define PARAMIOCSETDEFAULT _PARAMIOC(12)
typedef struct paramiocsetdefault {
const param_t param;
-19
View File
@@ -46,12 +46,6 @@
#include "parameters_common.cpp"
void
param_notify_changes()
{
boardctl(PARAMIOCNOTIFY, NULL);
}
param_t param_find(const char *name)
{
paramiocfind_t data = {name, true, PARAM_INVALID};
@@ -125,13 +119,6 @@ int param_set(param_t param, const void *val)
return data.ret;
}
int param_set_no_notification(param_t param, const void *val)
{
paramiocset_t data = {param, false, val, PX4_ERROR};
boardctl(PARAMIOCSET, reinterpret_cast<unsigned long>(&data));
return data.ret;
}
bool param_used(param_t param)
{
paramiocused_t data = {param, false};
@@ -139,12 +126,6 @@ bool param_used(param_t param)
return data.ret;
}
void param_set_used(param_t param)
{
paramiocsetused_t data = {param};
boardctl(PARAMIOCSETUSED, reinterpret_cast<unsigned long>(&data));
}
int param_set_default_value(param_t param, const void *val)
{
paramiocsetdefault_t data = {param, val, PX4_ERROR};
+1 -1
View File
@@ -195,7 +195,7 @@ bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type,
// eg CAL_MAGn_{X,Y,Z}OFF
sprintf(str, "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
if (param_set_no_notification(param_find(str), &values(axis)) != 0) {
if (param_set(param_find(str), &values(axis)) != 0) {
PX4_ERR("failed to set %s = %.4f", str, (double)values(axis));
ret = PX4_ERROR;
}
+1 -1
View File
@@ -90,7 +90,7 @@ bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t
// eg CAL_MAGn_ID/CAL_MAGn_ROT
sprintf(str, "CAL_%s%u_%s", sensor_type, instance, cal_type);
int ret = param_set_no_notification(param_find(str), &value);
int ret = param_set(param_find(str), &value);
if (ret != PX4_OK) {
PX4_ERR("failed to set %s", str);
@@ -409,17 +409,14 @@ AirspeedModule::Run()
switch (i) {
case 0:
_param_airspeed_scale_1.set(_airspeed_validator[i].get_CAS_scale_validated());
_param_airspeed_scale_1.commit_no_notification();
break;
case 1:
_param_airspeed_scale_2.set(_airspeed_validator[i].get_CAS_scale_validated());
_param_airspeed_scale_2.commit_no_notification();
break;
case 2:
_param_airspeed_scale_3.set(_airspeed_validator[i].get_CAS_scale_validated());
_param_airspeed_scale_3.commit_no_notification();
break;
}
}
-1
View File
@@ -1752,7 +1752,6 @@ void Commander::run()
if (_was_armed && !_arm_state_machine.isArmed()) {
const int32_t flight_uuid = _param_flight_uuid.get() + 1;
_param_flight_uuid.set(flight_uuid);
_param_flight_uuid.commit_no_notification();
_last_disarmed_timestamp = hrt_absolute_time();
@@ -363,7 +363,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
const Dcmf board_rotation_t = board_rotation.transpose();
bool param_save = false;
bool failed = true;
for (unsigned i = 0; i < MAX_ACCEL_SENS; i++) {
@@ -419,7 +418,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
if (calibrations[i].ParametersSave(i, true)) {
param_save = true;
failed = false;
} else {
@@ -434,10 +432,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
failed = true;
}
if (param_save) {
param_notify_changes();
}
if (!failed) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
px4_usleep(600000); // give this message enough time to propagate
@@ -455,7 +449,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
#if !defined(CONSTRAINED_FLASH)
PX4_INFO("Accelerometer quick calibration");
bool param_save = false;
bool failed = true;
FactoryCalibrationStorage factory_storage;
@@ -566,7 +559,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
if (calibration.ParametersSave(accel_index)) {
calibration.PrintStatus();
param_save = true;
failed = false;
} else {
@@ -582,10 +574,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
failed = true;
}
if (param_save) {
param_notify_changes();
}
if (!failed) {
return PX4_OK;
}
@@ -164,8 +164,6 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
return PX4_ERROR;
}
bool param_save = false;
for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
if ((calibration[instance].device_id() != 0) && (data_sum_count[instance] > 0)) {
@@ -210,16 +208,11 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
if (calibration[instance].ParametersSave(instance, true)) {
calibration[instance].PrintStatus();
param_save = true;
}
}
}
}
if (param_save) {
param_notify_changes();
}
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
return PX4_OK;
}
+1 -13
View File
@@ -89,19 +89,6 @@ int param_get(param_t param, void *val)
return -1;
}
void param_set_used(param_t param)
{
std::map<param_t, Param> &used_params = failsafe_instance.params();
if (used_params.find(param) != used_params.end()) {
return;
}
Param p;
memcpy(&p.val, &px4::parameters[param].val, sizeof(p.val));
used_params[param] = p;
}
std::vector<std::string> get_used_params()
{
std::vector<std::string> ret;
@@ -113,6 +100,7 @@ std::vector<std::string> get_used_params()
return ret;
}
param_value_u get_param_value(const std::string &name)
{
std::map<param_t, Param> &used_params = failsafe_instance.params();
@@ -248,7 +248,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (res == PX4_OK) {
// set offset parameters to new values
bool param_save = false;
bool failed = true;
for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) {
@@ -260,7 +259,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
calibration.PrintStatus();
if (calibration.ParametersSave(uorb_index, true)) {
param_save = true;
failed = false;
} else {
@@ -275,10 +273,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
failed = true;
}
if (param_save) {
param_notify_changes();
}
if (!failed) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
px4_usleep(600000); // give this message enough time to propagate
+2 -3
View File
@@ -165,9 +165,8 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)
PX4_INFO("Updating SENS_BOARD_Y_OFF %.1f -> %.1f degrees", (double)pitch_offset_current, (double)pitch_mean_degrees);
}
param_set_no_notification(roll_offset_handle, &roll_mean_degrees);
param_set_no_notification(pitch_offset_handle, &pitch_mean_degrees);
param_notify_changes();
param_set(roll_offset_handle, &roll_mean_degrees);
param_set(pitch_offset_handle, &pitch_mean_degrees);
success = true;
}
-12
View File
@@ -894,7 +894,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
}
if (result == calibrate_return_ok) {
bool param_save = false;
bool failed = true;
for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
@@ -918,7 +917,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
current_cal.PrintStatus();
if (current_cal.ParametersSave(cur_mag, true)) {
param_save = true;
failed = false;
} else {
@@ -933,10 +931,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
failed = true;
}
if (param_save) {
param_notify_changes();
}
if (failed) {
result = calibrate_return_error;
}
@@ -1005,7 +999,6 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
const Vector3f expected_field = Dcmf(euler).transpose() * mag_earth_pred;
bool param_save = false;
bool failed = true;
for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
@@ -1024,7 +1017,6 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
// save new calibration
if (cal.ParametersSave(cur_mag)) {
cal.PrintStatus();
param_save = true;
failed = false;
} else {
@@ -1035,10 +1027,6 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
}
}
if (param_save) {
param_notify_changes();
}
if (!failed && factory_storage.store() != PX4_OK) {
failed = true;
}
+12 -10
View File
@@ -610,9 +610,10 @@ void EKF2::VerifyParams()
{
if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS)
|| (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS_YAW)) {
_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_GPS |
SensorFusionMask::DEPRECATED_USE_GPS_YAW));
_param_ekf2_aid_mask.commit();
mavlink_log_critical(&_mavlink_log_pub, "Use EKF2_GPS_CTRL instead\n");
/* EVENT
* @description <param>EKF2_AID_MASK</param> is set to {1:.0}.
@@ -622,8 +623,9 @@ void EKF2::VerifyParams()
}
if ((_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::HPOS)) {
_param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() & ~GnssCtrl::VPOS);
_param_ekf2_gps_ctrl.commit();
mavlink_log_critical(&_mavlink_log_pub, "GPS lon/lat is required for altitude fusion\n");
/* EVENT
* @description <param>EKF2_GPS_CTRL</param> is set to {1:.0}.
@@ -633,8 +635,9 @@ void EKF2::VerifyParams()
}
if ((_param_ekf2_hgt_ref.get() == HeightSensor::BARO) && (_param_ekf2_baro_ctrl.get() == 0)) {
_param_ekf2_baro_ctrl.set(1);
_param_ekf2_baro_ctrl.commit();
mavlink_log_critical(&_mavlink_log_pub, "Baro enabled by EKF2_HGT_REF\n");
/* EVENT
* @description <param>EKF2_BARO_CTRL</param> is set to {1:.0}.
@@ -644,8 +647,9 @@ void EKF2::VerifyParams()
}
if ((_param_ekf2_hgt_ref.get() == HeightSensor::RANGE) && (_param_ekf2_rng_ctrl.get() == RngCtrl::DISABLED)) {
_param_ekf2_rng_ctrl.set(1);
_param_ekf2_rng_ctrl.commit();
mavlink_log_critical(&_mavlink_log_pub, "Range enabled by EKF2_HGT_REF\n");
/* EVENT
* @description <param>EKF2_RNG_CTRL</param> is set to {1:.0}.
@@ -655,8 +659,9 @@ void EKF2::VerifyParams()
}
if ((_param_ekf2_hgt_ref.get() == HeightSensor::GNSS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS)) {
_param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() | (GnssCtrl::VPOS | GnssCtrl::HPOS | GnssCtrl::VEL));
_param_ekf2_gps_ctrl.commit();
mavlink_log_critical(&_mavlink_log_pub, "GPS enabled by EKF2_HGT_REF\n");
/* EVENT
* @description <param>EKF2_GPS_CTRL</param> is set to {1:.0}.
@@ -669,10 +674,8 @@ void EKF2::VerifyParams()
if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)) {
_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::VEL));
_param_ekf2_ev_ctrl.commit();
_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL));
_param_ekf2_aid_mask.commit();
mavlink_log_critical(&_mavlink_log_pub, "EKF2 EV use EKF2_EV_CTRL instead of EKF2_AID_MASK\n");
/* EVENT
@@ -2204,7 +2207,6 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) {
_param_ekf2_mag_decl.set(declination_deg);
_param_ekf2_mag_decl.commit_no_notification();
}
_mag_decl_saved = true;
@@ -2245,7 +2247,7 @@ int EKF2::task_spawn(int argc, char *argv[])
if (imu_instances < 1 || imu_instances > 4) {
const int32_t imu_instances_limited = math::constrain(imu_instances, static_cast<int32_t>(1), static_cast<int32_t>(4));
PX4_WARN("EKF2_MULTI_IMU limited %" PRId32 " -> %" PRId32, imu_instances, imu_instances_limited);
param_set_no_notification(param_find("EKF2_MULTI_IMU"), &imu_instances_limited);
param_set(param_find("EKF2_MULTI_IMU"), &imu_instances_limited);
imu_instances = imu_instances_limited;
}
@@ -2261,7 +2263,7 @@ int EKF2::task_spawn(int argc, char *argv[])
if (mag_instances > 4) {
const int32_t mag_instances_limited = math::constrain(mag_instances, static_cast<int32_t>(1), static_cast<int32_t>(4));
PX4_WARN("EKF2_MULTI_MAG limited %" PRId32 " -> %" PRId32, mag_instances, mag_instances_limited);
param_set_no_notification(param_ekf2_mult_mag, &mag_instances_limited);
param_set(param_ekf2_mult_mag, &mag_instances_limited);
mag_instances = mag_instances_limited;
} else if (mag_instances <= 1) {
@@ -202,7 +202,6 @@ void FlightModeManager::start_flight_task()
if (_param_mpc_pos_mode.get() != 4) {
PX4_ERR("MPC_POS_MODE %" PRId32 " invalid, resetting", _param_mpc_pos_mode.get());
_param_mpc_pos_mode.set(4);
_param_mpc_pos_mode.commit();
}
error = switchTask(FlightTaskIndex::ManualAcceleration);
@@ -470,7 +470,6 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
mavlink_log_info(&mavlink_log_pub, "Autotune returned to idle");
_state = state::idle;
_param_fw_at_start.set(false);
_param_fw_at_start.commit();
}
break;
@@ -574,16 +573,6 @@ void FwAutotuneAttitudeControl::saveGainsToParams()
_param_fw_rr_i.set(_rate_k(0) * _rate_i(0));
_param_fw_rr_ff.set(_rate_ff(0));
_param_fw_r_tc.set(1.f / _att_p(0));
_param_fw_rr_p.commit_no_notification();
_param_fw_rr_i.commit_no_notification();
_param_fw_rr_ff.commit_no_notification();
if (_param_fw_at_axes.get() == Axes::roll) {
_param_fw_r_tc.commit();
} else {
_param_fw_r_tc.commit_no_notification();
}
}
if (_param_fw_at_axes.get() & Axes::pitch) {
@@ -591,25 +580,12 @@ void FwAutotuneAttitudeControl::saveGainsToParams()
_param_fw_pr_i.set(_rate_k(1) * _rate_i(1));
_param_fw_pr_ff.set(_rate_ff(1));
_param_fw_p_tc.set(1.f / _att_p(1));
_param_fw_pr_p.commit_no_notification();
_param_fw_pr_i.commit_no_notification();
_param_fw_pr_ff.commit_no_notification();
if (!(_param_fw_at_axes.get() & Axes::yaw)) {
_param_fw_p_tc.commit();
} else {
_param_fw_p_tc.commit_no_notification();
}
}
if (_param_fw_at_axes.get() & Axes::yaw) {
_param_fw_yr_p.set(_rate_k(2));
_param_fw_yr_i.set(_rate_k(2) * _rate_i(2));
_param_fw_yr_ff.set(_rate_ff(2));
_param_fw_yr_p.commit_no_notification();
_param_fw_yr_i.commit_no_notification();
_param_fw_yr_ff.commit();
}
}
@@ -261,20 +261,13 @@ void GyroCalibration::Run()
// save all calibrations
if (calibration_updated) {
bool param_save = false;
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
if (_gyro_calibration[gyro].device_id() != 0) {
if (_gyro_calibration[gyro].ParametersSave(gyro)) {
param_save = true;
}
_gyro_calibration[gyro].ParametersSave(gyro);
}
}
if (param_save) {
param_notify_changes();
}
Reset();
}
}
-1
View File
@@ -134,7 +134,6 @@ bool GyroFFT::init()
PX4_ERR("Invalid IMU_GYRO_FFT_LEN=%" PRId32 ", resetting", _param_imu_gyro_fft_len.get());
buffers_allocated = AllocateBuffers<256>();
_param_imu_gyro_fft_len.set(256);
_param_imu_gyro_fft_len.commit();
break;
}
@@ -189,14 +189,9 @@ void LandDetector::Run()
_takeoff_time = 0;
uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff;
_param_total_flight_time_high.set(flight_time);
_param_total_flight_time_high.commit_no_notification();
flight_time = _total_flight_time & 0xffffffff;
_param_total_flight_time_low.set(flight_time);
_param_total_flight_time_low.commit_no_notification();
}
_previous_armed_state = _armed;
@@ -131,7 +131,6 @@ void MulticopterLandDetector::_update_params()
(double)_param_lndmc_z_vel_max.get(), (double)(lndmc_upper_threshold));
_param_lndmc_z_vel_max.set(lndmc_upper_threshold);
_param_lndmc_z_vel_max.commit_no_notification();
}
int32_t use_hover_thrust_estimate = 0;
@@ -102,7 +102,6 @@ void ManualControl::Run()
if (rc_map_arm_sw > 0) {
_param_man_arm_gesture.set(0); // disable arm gesture
_param_man_arm_gesture.commit();
orb_advert_t mavlink_log_pub = nullptr;
mavlink_log_critical(&mavlink_log_pub, "Arm stick gesture disabled if arm switch in use\t")
-2
View File
@@ -194,7 +194,6 @@ Mavlink::mavlink_update_parameters()
if (_param_mav_type.get() < 0 || _param_mav_type.get() >= MAV_TYPE_ENUM_END) {
_param_mav_type.set(0);
_param_mav_type.commit_no_notification();
PX4_ERR("MAV_TYPE parameter invalid, resetting to 0.");
}
}
@@ -2788,7 +2787,6 @@ void Mavlink::configure_sik_radio()
/* reset param and save */
_param_sik_radio_id.set(0);
_param_sik_radio_id.commit_no_notification();
}
}
@@ -548,39 +548,23 @@ void McAutotuneAttitudeControl::saveGainsToParams()
_param_mc_rollrate_i.set(_rate_k(0) * _rate_i(0));
_param_mc_rollrate_d.set(_rate_k(0) * _rate_d(0));
_param_mc_roll_p.set(_att_p(0));
_param_mc_rollrate_p.commit_no_notification();
_param_mc_rollrate_k.commit_no_notification();
_param_mc_rollrate_i.commit_no_notification();
_param_mc_rollrate_d.commit_no_notification();
_param_mc_roll_p.commit_no_notification();
_param_mc_pitchrate_p.set(_rate_k(1));
_param_mc_pitchrate_k.set(1.f);
_param_mc_pitchrate_i.set(_rate_k(1) * _rate_i(1));
_param_mc_pitchrate_d.set(_rate_k(1) * _rate_d(1));
_param_mc_pitch_p.set(_att_p(1));
_param_mc_pitchrate_p.commit_no_notification();
_param_mc_pitchrate_k.commit_no_notification();
_param_mc_pitchrate_i.commit_no_notification();
_param_mc_pitchrate_d.commit_no_notification();
_param_mc_pitch_p.commit_no_notification();
_param_mc_yawrate_p.set(_rate_k(2));
_param_mc_yawrate_k.set(1.f);
_param_mc_yawrate_i.set(_rate_k(2) * _rate_i(2));
_param_mc_yawrate_d.set(_rate_k(2) * _rate_d(2));
_param_mc_yaw_p.set(_att_p(2));
_param_mc_yawrate_p.commit_no_notification();
_param_mc_yawrate_k.commit_no_notification();
_param_mc_yawrate_i.commit_no_notification();
_param_mc_yawrate_d.commit_no_notification();
_param_mc_yaw_p.commit();
}
void McAutotuneAttitudeControl::stopAutotune()
{
_param_mc_at_start.set(false);
_param_mc_at_start.commit();
_actuator_controls_sub.unregisterCallback();
}
@@ -91,57 +91,52 @@ void MulticopterPositionControl::parameters_update(bool force)
// make it less sensitive at the lower end
float responsiveness = _param_sys_vehicle_resp.get() * _param_sys_vehicle_resp.get();
num_changed += _param_mpc_acc_hor.commit_no_notification(math::lerp(1.f, 15.f, responsiveness));
num_changed += _param_mpc_acc_hor_max.commit_no_notification(math::lerp(2.f, 15.f, responsiveness));
num_changed += _param_mpc_man_y_max.commit_no_notification(math::lerp(80.f, 450.f, responsiveness));
num_changed += _param_mpc_acc_hor.set(math::lerp(1.f, 15.f, responsiveness));
num_changed += _param_mpc_acc_hor_max.set(math::lerp(2.f, 15.f, responsiveness));
num_changed += _param_mpc_man_y_max.set(math::lerp(80.f, 450.f, responsiveness));
if (responsiveness > 0.6f) {
num_changed += _param_mpc_man_y_tau.commit_no_notification(0.f);
num_changed += _param_mpc_man_y_tau.set(0.f);
} else {
num_changed += _param_mpc_man_y_tau.commit_no_notification(math::lerp(0.5f, 0.f, responsiveness / 0.6f));
num_changed += _param_mpc_man_y_tau.set(math::lerp(0.5f, 0.f, responsiveness / 0.6f));
}
if (responsiveness < 0.5f) {
num_changed += _param_mpc_tiltmax_air.commit_no_notification(45.f);
num_changed += _param_mpc_tiltmax_air.set(45.f);
} else {
num_changed += _param_mpc_tiltmax_air.commit_no_notification(math::min(MAX_SAFE_TILT_DEG, math::lerp(45.f, 70.f,
num_changed += _param_mpc_tiltmax_air.set(math::min(MAX_SAFE_TILT_DEG, math::lerp(45.f, 70.f,
(responsiveness - 0.5f) * 2.f)));
}
num_changed += _param_mpc_acc_down_max.commit_no_notification(math::lerp(0.8f, 15.f, responsiveness));
num_changed += _param_mpc_acc_up_max.commit_no_notification(math::lerp(1.f, 15.f, responsiveness));
num_changed += _param_mpc_jerk_max.commit_no_notification(math::lerp(2.f, 50.f, responsiveness));
num_changed += _param_mpc_jerk_auto.commit_no_notification(math::lerp(1.f, 25.f, responsiveness));
num_changed += _param_mpc_acc_down_max.set(math::lerp(0.8f, 15.f, responsiveness));
num_changed += _param_mpc_acc_up_max.set(math::lerp(1.f, 15.f, responsiveness));
num_changed += _param_mpc_jerk_max.set(math::lerp(2.f, 50.f, responsiveness));
num_changed += _param_mpc_jerk_auto.set(math::lerp(1.f, 25.f, responsiveness));
}
if (_param_mpc_xy_vel_all.get() >= 0.f) {
float xy_vel = _param_mpc_xy_vel_all.get();
num_changed += _param_mpc_vel_manual.commit_no_notification(xy_vel);
num_changed += _param_mpc_vel_man_back.commit_no_notification(-1.f);
num_changed += _param_mpc_vel_man_side.commit_no_notification(-1.f);
num_changed += _param_mpc_xy_cruise.commit_no_notification(xy_vel);
num_changed += _param_mpc_xy_vel_max.commit_no_notification(xy_vel);
num_changed += _param_mpc_vel_manual.set(xy_vel);
num_changed += _param_mpc_vel_man_back.set(-1.f);
num_changed += _param_mpc_vel_man_side.set(-1.f);
num_changed += _param_mpc_xy_cruise.set(xy_vel);
num_changed += _param_mpc_xy_vel_max.set(xy_vel);
}
if (_param_mpc_z_vel_all.get() >= 0.f) {
float z_vel = _param_mpc_z_vel_all.get();
num_changed += _param_mpc_z_v_auto_up.commit_no_notification(z_vel);
num_changed += _param_mpc_z_vel_max_up.commit_no_notification(z_vel);
num_changed += _param_mpc_z_v_auto_dn.commit_no_notification(z_vel * 0.75f);
num_changed += _param_mpc_z_vel_max_dn.commit_no_notification(z_vel * 0.75f);
num_changed += _param_mpc_tko_speed.commit_no_notification(z_vel * 0.6f);
num_changed += _param_mpc_land_speed.commit_no_notification(z_vel * 0.5f);
}
if (num_changed > 0) {
param_notify_changes();
num_changed += _param_mpc_z_v_auto_up.set(z_vel);
num_changed += _param_mpc_z_vel_max_up.set(z_vel);
num_changed += _param_mpc_z_v_auto_dn.set(z_vel * 0.75f);
num_changed += _param_mpc_z_vel_max_dn.set(z_vel * 0.75f);
num_changed += _param_mpc_tko_speed.set(z_vel * 0.6f);
num_changed += _param_mpc_land_speed.set(z_vel * 0.5f);
}
if (_param_mpc_tiltmax_air.get() > MAX_SAFE_TILT_DEG) {
_param_mpc_tiltmax_air.set(MAX_SAFE_TILT_DEG);
_param_mpc_tiltmax_air.commit();
mavlink_log_critical(&_mavlink_log_pub, "Tilt constrained to safe value\t");
/* EVENT
* @description <param>MPC_TILTMAX_AIR</param> is set to {1:.0}.
@@ -152,7 +147,6 @@ void MulticopterPositionControl::parameters_update(bool force)
if (_param_mpc_tiltmax_lnd.get() > _param_mpc_tiltmax_air.get()) {
_param_mpc_tiltmax_lnd.set(_param_mpc_tiltmax_air.get());
_param_mpc_tiltmax_lnd.commit();
mavlink_log_critical(&_mavlink_log_pub, "Land tilt has been constrained by max tilt\t");
/* EVENT
* @description <param>MPC_TILTMAX_LND</param> is set to {1:.0}.
@@ -171,7 +165,7 @@ void MulticopterPositionControl::parameters_update(bool force)
// Check that the design parameters are inside the absolute maximum constraints
if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) {
_param_mpc_xy_cruise.set(_param_mpc_xy_vel_max.get());
_param_mpc_xy_cruise.commit();
mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed\t");
/* EVENT
* @description <param>MPC_XY_CRUISE</param> is set to {1:.0}.
@@ -182,7 +176,6 @@ void MulticopterPositionControl::parameters_update(bool force)
if (_param_mpc_vel_manual.get() > _param_mpc_xy_vel_max.get()) {
_param_mpc_vel_manual.set(_param_mpc_xy_vel_max.get());
_param_mpc_vel_manual.commit();
mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed\t");
/* EVENT
* @description <param>MPC_VEL_MANUAL</param> is set to {1:.0}.
@@ -193,7 +186,6 @@ void MulticopterPositionControl::parameters_update(bool force)
if (_param_mpc_vel_man_back.get() > _param_mpc_vel_manual.get()) {
_param_mpc_vel_man_back.set(_param_mpc_vel_manual.get());
_param_mpc_vel_man_back.commit();
mavlink_log_critical(&_mavlink_log_pub, "Manual backward speed has been constrained by forward speed\t");
/* EVENT
* @description <param>MPC_VEL_MAN_BACK</param> is set to {1:.0}.
@@ -204,7 +196,6 @@ void MulticopterPositionControl::parameters_update(bool force)
if (_param_mpc_vel_man_side.get() > _param_mpc_vel_manual.get()) {
_param_mpc_vel_man_side.set(_param_mpc_vel_manual.get());
_param_mpc_vel_man_side.commit();
mavlink_log_critical(&_mavlink_log_pub, "Manual sideways speed has been constrained by forward speed\t");
/* EVENT
* @description <param>MPC_VEL_MAN_SIDE</param> is set to {1:.0}.
@@ -215,7 +206,6 @@ void MulticopterPositionControl::parameters_update(bool force)
if (_param_mpc_z_v_auto_up.get() > _param_mpc_z_vel_max_up.get()) {
_param_mpc_z_v_auto_up.set(_param_mpc_z_vel_max_up.get());
_param_mpc_z_v_auto_up.commit();
mavlink_log_critical(&_mavlink_log_pub, "Ascent speed has been constrained by max speed\t");
/* EVENT
* @description <param>MPC_Z_V_AUTO_UP</param> is set to {1:.0}.
@@ -226,7 +216,6 @@ void MulticopterPositionControl::parameters_update(bool force)
if (_param_mpc_z_v_auto_dn.get() > _param_mpc_z_vel_max_dn.get()) {
_param_mpc_z_v_auto_dn.set(_param_mpc_z_vel_max_dn.get());
_param_mpc_z_v_auto_dn.commit();
mavlink_log_critical(&_mavlink_log_pub, "Descent speed has been constrained by max speed\t");
/* EVENT
* @description <param>MPC_Z_V_AUTO_DN</param> is set to {1:.0}.
@@ -239,7 +228,6 @@ void MulticopterPositionControl::parameters_update(bool force)
_param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
_param_mpc_thr_max.get()));
_param_mpc_thr_hover.commit();
mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max\t");
/* EVENT
* @description <param>MPC_THR_HOVER</param> is set to {1:.0}.
@@ -383,7 +383,6 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
if (_param_imu_gyro_ratemax.get() <= 0) {
const int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get();
_param_imu_gyro_ratemax.set(400);
_param_imu_gyro_ratemax.commit();
PX4_WARN("IMU_GYRO_RATEMAX invalid (%" PRId32 "), resetting to default %" PRId32 ")", imu_gyro_ratemax,
_param_imu_gyro_ratemax.get());
}
@@ -394,7 +393,6 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
if (imu_gyro_ratemax != _param_imu_gyro_ratemax.get()) {
PX4_WARN("IMU_GYRO_RATEMAX updated %" PRId32 " -> %" PRIu32, _param_imu_gyro_ratemax.get(), imu_gyro_ratemax);
_param_imu_gyro_ratemax.set(imu_gyro_ratemax);
_param_imu_gyro_ratemax.commit_no_notification();
}
// gyro low pass cutoff frequency changed
@@ -151,7 +151,6 @@ bool VehicleIMU::ParametersUpdate(bool force)
if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
PX4_WARN("IMU_INTEG_RATE updated %" PRId32 " -> %" PRIu32, _param_imu_integ_rate.get(), imu_integration_rate_hz);
_param_imu_integ_rate.set(imu_integration_rate_hz);
_param_imu_integ_rate.commit_no_notification();
}
_imu_integration_interval_us = 1e6f / imu_integration_rate_hz;
@@ -843,9 +842,7 @@ void VehicleIMU::SensorCalibrationSaveAccel()
(double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
// save parameters with preferred calibration slot to current sensor index
if (_accel_calibration.ParametersSave(_sensor_accel_sub.get_instance())) {
param_notify_changes();
}
_accel_calibration.ParametersSave(_sensor_accel_sub.get_instance());
_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
}
@@ -895,9 +892,7 @@ void VehicleIMU::SensorCalibrationSaveGyro()
(double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
// save parameters with preferred calibration slot to current sensor index
if (_gyro_calibration.ParametersSave(_sensor_gyro_sub.get_instance())) {
param_notify_changes();
}
_gyro_calibration.ParametersSave(_sensor_gyro_sub.get_instance());
_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
}
@@ -200,7 +200,6 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
magnetometer_bias_estimate_s mag_bias_est;
if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) {
bool parameters_notify = false;
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) {
@@ -233,16 +232,11 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
_calibration_estimator_bias[mag_index].zero();
parameters_notify = true;
_last_calibration_update = hrt_absolute_time();
}
}
}
}
if (parameters_notify) {
param_notify_changes();
_last_calibration_update = hrt_absolute_time();
}
}
}
}
@@ -295,7 +289,6 @@ void VehicleMagnetometer::UpdateMagCalibration()
} else if (_in_flight_mag_cal_available) {
// not armed and mag cal available
bool calibration_param_save_needed = false;
// iterate through available bias estimates and fuse them sequentially using a Kalman Filter scheme
Vector3f state_variance{magb_vref, magb_vref, magb_vref};
@@ -332,17 +325,12 @@ void VehicleMagnetometer::UpdateMagCalibration()
_calibration_estimator_bias[mag_index].zero();
calibration_param_save_needed = true;
_last_calibration_update = hrt_absolute_time();
}
}
}
}
if (calibration_param_save_needed) {
param_notify_changes();
_last_calibration_update = hrt_absolute_time();
}
// clear all
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
_mag_cal[i] = {};
@@ -163,7 +163,7 @@ int TemperatureCalibrationAccel::finish()
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_A_ENABLE"), &enabled);
int result = param_set(param_find("TC_A_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_A_ENABLE (%i)", result);
@@ -214,7 +214,7 @@ int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int
for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
sprintf(str, "TC_A%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
param = (float)res[axis_index][coef_index];
result = param_set_no_notification(param_find(str), &param);
result = param_set(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
@@ -159,7 +159,7 @@ int TemperatureCalibrationBaro::finish()
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_B_ENABLE"), &enabled);
int result = param_set(param_find("TC_B_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_B_ENABLE (%i)", result);
@@ -199,7 +199,7 @@ int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int
for (unsigned coef_index = 0; coef_index <= POLYFIT_ORDER; coef_index++) {
sprintf(str, "TC_B%d_X%d", sensor_index, (POLYFIT_ORDER - coef_index));
param = (float)res[coef_index];
result = param_set_no_notification(param_find(str), &param);
result = param_set(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
@@ -98,7 +98,7 @@ int TemperatureCalibrationBase::set_parameter(const char *format_str, unsigned i
{
char param_str[30] {};
(void)sprintf(param_str, format_str, index);
int result = param_set_no_notification(param_find(param_str), value);
int result = param_set(param_find(param_str), value);
if (result != 0) {
PX4_ERR("unable to reset %s (%i)", param_str, result);
@@ -150,7 +150,7 @@ int TemperatureCalibrationGyro::finish()
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_G_ENABLE"), &enabled);
int result = param_set(param_find("TC_G_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_G_ENABLE (%i)", result);
@@ -199,7 +199,7 @@ int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int
for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
sprintf(str, "TC_G%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
param = (float)res[axis_index][coef_index];
result = param_set_no_notification(param_find(str), &param);
result = param_set(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
@@ -269,7 +269,6 @@ void TemperatureCalibration::task_main()
}
}
param_notify_changes();
int ret = param_save_default();
if (ret != 0) {
+34 -34
View File
@@ -282,16 +282,16 @@ bool ParameterTest::exportImport()
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = p;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
if (param_set(p, &set_val) != PX4_OK) {
PX4_ERR("param_set failed for: %d", p);
ut_assert("param_set failed", false);
}
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
@@ -300,16 +300,16 @@ bool ParameterTest::exportImport()
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = (float)p + MAGIC_FLOAT_VAL;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
if (param_set(p, &set_val) != PX4_OK) {
PX4_ERR("param_set failed for: %d", p);
ut_assert("param_set failed", false);
}
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
@@ -327,16 +327,16 @@ bool ParameterTest::exportImport()
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = 0;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
if (param_set(p, &set_val) != PX4_OK) {
PX4_ERR("param_set failed for: %d", p);
ut_assert("param_set failed", false);
}
int32_t get_val = -1;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
@@ -345,16 +345,16 @@ bool ParameterTest::exportImport()
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = 0.0f;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
if (param_set(p, &set_val) != PX4_OK) {
PX4_ERR("param_set failed for: %d", p);
ut_assert("param_set failed", false);
}
float get_val = -1.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare_float("value for param doesn't match default value", set_val, get_val, 0.001f);
@@ -375,7 +375,7 @@ bool ParameterTest::exportImport()
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
@@ -386,7 +386,7 @@ bool ParameterTest::exportImport()
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare_float("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL, 0.001f);
@@ -427,16 +427,16 @@ bool ParameterTest::exportImportAll()
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = p;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
if (param_set(p, &set_val) != PX4_OK) {
PX4_ERR("param_set failed for: %d", p);
ut_assert("param_set failed", false);
}
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
@@ -445,16 +445,16 @@ bool ParameterTest::exportImportAll()
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = (float)p + MAGIC_FLOAT_VAL;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
if (param_set(p, &set_val) != PX4_OK) {
PX4_ERR("param_set failed for: %d", p);
ut_assert("param_set failed", false);
}
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
@@ -475,16 +475,16 @@ bool ParameterTest::exportImportAll()
const int32_t set_val = 0;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
if (param_set(p, &set_val) != PX4_OK) {
PX4_ERR("param set failed: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
int32_t get_val = -1;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
@@ -493,16 +493,16 @@ bool ParameterTest::exportImportAll()
if (param_type(p) == PARAM_TYPE_FLOAT) {
float set_val = 0.0f;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
if (param_set(p, &set_val) != PX4_OK) {
PX4_ERR("param set failed: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
float get_val = -1.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
@@ -525,7 +525,7 @@ bool ParameterTest::exportImportAll()
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
@@ -536,7 +536,7 @@ bool ParameterTest::exportImportAll()
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
ut_assert("param_set failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);