mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-23 21:07:35 +08:00
Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 9586ad0006 | |||
| b6157c89a0 |
@@ -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)
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -134,6 +134,7 @@ set(msg_files
|
||||
OrbTest.msg
|
||||
OrbTestLarge.msg
|
||||
OrbTestMedium.msg
|
||||
Parameter.msg
|
||||
ParameterUpdate.msg
|
||||
Ping.msg
|
||||
PositionControllerLandingStatus.msg
|
||||
|
||||
@@ -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,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(¶meter_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(¶meter_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 ¶meter_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 ¶meter_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 ¶meter_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 ¶meter_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,35 +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; }
|
||||
|
||||
void reset()
|
||||
{
|
||||
param_reset_no_notification(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
bool update() { return param_get(handle(), &_val) == 0; }
|
||||
|
||||
param_t handle() const { return param_handle(p); }
|
||||
void setValue(const parameter_s ¶meter_update) { _val = parameter_update.float32_value; }
|
||||
|
||||
static constexpr param_t handle() { return param_handle(p); }
|
||||
private:
|
||||
float _val;
|
||||
};
|
||||
@@ -170,7 +172,6 @@ public:
|
||||
Param(float &external_val)
|
||||
: _val(external_val)
|
||||
{
|
||||
param_set_used(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
@@ -178,35 +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;
|
||||
}
|
||||
|
||||
void set(float val) { _val = val; }
|
||||
|
||||
void reset()
|
||||
{
|
||||
param_reset_no_notification(handle());
|
||||
update();
|
||||
_val = val;
|
||||
return (param_set(handle(), &_val) == 0);
|
||||
}
|
||||
|
||||
bool update() { return param_get(handle(), &_val) == 0; }
|
||||
|
||||
param_t handle() const { return param_handle(p); }
|
||||
void setValue(const parameter_s ¶meter_update) { _val = parameter_update.float32_value; }
|
||||
|
||||
static constexpr param_t handle() { return param_handle(p); }
|
||||
private:
|
||||
float &_val;
|
||||
};
|
||||
@@ -220,7 +204,6 @@ public:
|
||||
|
||||
Param()
|
||||
{
|
||||
param_set_used(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
@@ -228,35 +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;
|
||||
}
|
||||
|
||||
void set(int32_t val) { _val = val; }
|
||||
|
||||
void reset()
|
||||
{
|
||||
param_reset_no_notification(handle());
|
||||
update();
|
||||
_val = val;
|
||||
return (param_set(handle(), &_val) == 0);
|
||||
}
|
||||
|
||||
bool update() { return param_get(handle(), &_val) == 0; }
|
||||
|
||||
param_t handle() const { return param_handle(p); }
|
||||
void setValue(const parameter_s ¶meter_update) { _val = parameter_update.int32_value; }
|
||||
|
||||
static constexpr param_t handle() { return param_handle(p); }
|
||||
private:
|
||||
int32_t _val;
|
||||
};
|
||||
@@ -272,7 +238,6 @@ public:
|
||||
Param(int32_t &external_val)
|
||||
: _val(external_val)
|
||||
{
|
||||
param_set_used(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
@@ -281,34 +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;
|
||||
}
|
||||
|
||||
void set(int32_t val) { _val = val; }
|
||||
|
||||
void reset()
|
||||
{
|
||||
param_reset_no_notification(handle());
|
||||
update();
|
||||
_val = val;
|
||||
return (param_set(handle(), &_val) == 0);
|
||||
}
|
||||
|
||||
bool update() { return param_get(handle(), &_val) == 0; }
|
||||
|
||||
param_t handle() const { return param_handle(p); }
|
||||
void setValue(const parameter_s ¶meter_update) { _val = parameter_update.int32_value; }
|
||||
|
||||
static constexpr param_t handle() { return param_handle(p); }
|
||||
private:
|
||||
int32_t &_val;
|
||||
};
|
||||
@@ -322,7 +270,6 @@ public:
|
||||
|
||||
Param()
|
||||
{
|
||||
param_set_used(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
@@ -330,38 +277,12 @@ 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;
|
||||
}
|
||||
|
||||
void set(bool val) { _val = val; }
|
||||
|
||||
void reset()
|
||||
{
|
||||
param_reset_no_notification(handle());
|
||||
update();
|
||||
_val = val;
|
||||
int32_t value_int = (int32_t)_val;
|
||||
return (param_set(handle(), &value_int) == 0);
|
||||
}
|
||||
|
||||
bool update()
|
||||
@@ -377,7 +298,9 @@ public:
|
||||
return false;
|
||||
}
|
||||
|
||||
param_t handle() const { return param_handle(p); }
|
||||
void setValue(const parameter_s ¶meter_update) { _val = parameter_update.int32_value; }
|
||||
|
||||
static constexpr param_t handle() { return param_handle(p); }
|
||||
private:
|
||||
bool _val;
|
||||
};
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -143,12 +135,6 @@ public:
|
||||
|
||||
void set(float val) { _val = val; }
|
||||
|
||||
void reset()
|
||||
{
|
||||
//param_reset_no_notification(handle()); // TODO
|
||||
update();
|
||||
}
|
||||
|
||||
bool update()
|
||||
{
|
||||
return true; // TODO
|
||||
@@ -171,7 +157,6 @@ public:
|
||||
Param(float &external_val)
|
||||
: _val(external_val)
|
||||
{
|
||||
//param_set_used(handle()); // TODO
|
||||
update();
|
||||
}
|
||||
|
||||
@@ -186,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;
|
||||
}
|
||||
|
||||
@@ -207,12 +185,6 @@ public:
|
||||
|
||||
void set(float val) { _val = val; }
|
||||
|
||||
void reset()
|
||||
{
|
||||
//param_reset_no_notification(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
bool update()
|
||||
{
|
||||
return true;
|
||||
@@ -233,7 +205,6 @@ public:
|
||||
|
||||
Param()
|
||||
{
|
||||
//param_set_used(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
@@ -248,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;
|
||||
}
|
||||
|
||||
@@ -269,12 +233,6 @@ public:
|
||||
|
||||
void set(int32_t val) { _val = val; }
|
||||
|
||||
void reset()
|
||||
{
|
||||
//param_reset_no_notification(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
bool update()
|
||||
{
|
||||
//return param_get(handle(), &_val) == 0;
|
||||
@@ -297,7 +255,6 @@ public:
|
||||
Param(int32_t &external_val)
|
||||
: _val(external_val)
|
||||
{
|
||||
//param_set_used(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
@@ -312,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;
|
||||
}
|
||||
|
||||
@@ -333,12 +283,6 @@ public:
|
||||
|
||||
void set(int32_t val) { _val = val; }
|
||||
|
||||
void reset()
|
||||
{
|
||||
//param_reset_no_notification(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
bool update()
|
||||
{
|
||||
//return param_get(handle(), &_val) == 0;
|
||||
@@ -359,7 +303,6 @@ public:
|
||||
|
||||
Param()
|
||||
{
|
||||
//param_set_used(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
@@ -375,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;
|
||||
}
|
||||
|
||||
@@ -397,12 +332,6 @@ public:
|
||||
|
||||
void set(bool val) { _val = val; }
|
||||
|
||||
void reset()
|
||||
{
|
||||
//param_reset_no_notification(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
bool update()
|
||||
{
|
||||
int32_t value_int;
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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());
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -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,45 +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 a parameter to its default value.
|
||||
*
|
||||
* @param param A handle returned by param_find or passed by param_foreach.
|
||||
* @return Zero on success, nonzero on failure
|
||||
*/
|
||||
__EXPORT int param_reset(param_t param);
|
||||
|
||||
/**
|
||||
* Reset a parameter to its default value, but do not notify the system about the change.
|
||||
*
|
||||
* @param param A handle returned by param_find or passed by param_foreach.
|
||||
* @return Zero on success, nonzero on failure
|
||||
*/
|
||||
__EXPORT int param_reset_no_notification(param_t param);
|
||||
|
||||
/**
|
||||
* Reset all parameters to their default values.
|
||||
*/
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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,13 +912,13 @@ 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;
|
||||
}
|
||||
|
||||
static int param_reset_internal(param_t param, bool notify = true)
|
||||
static int param_reset_internal(param_t param)
|
||||
{
|
||||
param_wbuf_s *s = nullptr;
|
||||
bool param_found = false;
|
||||
@@ -941,16 +945,13 @@ static int param_reset_internal(param_t param, bool notify = true)
|
||||
|
||||
param_unlock_writer();
|
||||
|
||||
if (s != nullptr && notify) {
|
||||
param_notify_changes();
|
||||
if (param_found) {
|
||||
param_notify_changes(param);
|
||||
}
|
||||
|
||||
return (!param_found);
|
||||
}
|
||||
|
||||
int param_reset(param_t param) { return param_reset_internal(param, true); }
|
||||
int param_reset_no_notification(param_t param) { return param_reset_internal(param, false); }
|
||||
|
||||
static void
|
||||
param_reset_all_internal(bool auto_save)
|
||||
{
|
||||
@@ -1000,7 +1001,7 @@ param_reset_excludes(const char *excludes[], int num_excludes)
|
||||
}
|
||||
|
||||
if (!exclude) {
|
||||
param_reset(param);
|
||||
param_reset_internal(param);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1025,7 +1026,7 @@ param_reset_specific(const char *resets[], int num_resets)
|
||||
}
|
||||
|
||||
if (reset) {
|
||||
param_reset(param);
|
||||
param_reset_internal(param);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1492,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 {
|
||||
@@ -1504,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 {
|
||||
|
||||
@@ -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)) {
|
||||
|
||||
@@ -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,30 +116,12 @@ 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);
|
||||
}
|
||||
break;
|
||||
|
||||
case PARAMIOCRESET: {
|
||||
paramiocreset_t *data = (paramiocreset_t *)arg;
|
||||
|
||||
if (data->notification) {
|
||||
data->ret = param_reset(data->param);
|
||||
|
||||
} else {
|
||||
data->ret = param_reset_no_notification(data->param);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case PARAMIOCRESETGROUP: {
|
||||
paramiocresetgroup_t *data = (paramiocresetgroup_t *)arg;
|
||||
|
||||
|
||||
@@ -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;
|
||||
@@ -118,13 +113,6 @@ typedef struct paramiocsetdefault {
|
||||
int ret;
|
||||
} paramiocsetdefault_t;
|
||||
|
||||
#define PARAMIOCRESET _PARAMIOC(13)
|
||||
typedef struct paramiocreset {
|
||||
const param_t param;
|
||||
const bool notification;
|
||||
int ret;
|
||||
} paramiocreset_t;
|
||||
|
||||
#define PARAMIOCRESETGROUP _PARAMIOC(14)
|
||||
typedef enum {
|
||||
PARAM_RESET_ALL,
|
||||
|
||||
@@ -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};
|
||||
@@ -152,20 +133,6 @@ int param_set_default_value(param_t param, const void *val)
|
||||
return data.ret;
|
||||
}
|
||||
|
||||
int param_reset(param_t param)
|
||||
{
|
||||
paramiocreset_t data = {param, true, PX4_ERROR};
|
||||
boardctl(PARAMIOCRESET, reinterpret_cast<unsigned long>(&data));
|
||||
return data.ret;
|
||||
}
|
||||
|
||||
int param_reset_no_notification(param_t param)
|
||||
{
|
||||
paramiocreset_t data = {param, false, PX4_ERROR};
|
||||
boardctl(PARAMIOCRESET, reinterpret_cast<unsigned long>(&data));
|
||||
return data.ret;
|
||||
}
|
||||
|
||||
void
|
||||
param_reset_all()
|
||||
{
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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
@@ -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 ×tamp)
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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}.
|
||||
|
||||
@@ -162,42 +162,48 @@ void RCUpdate::parameters_updated()
|
||||
if (param_get(param_find("RC_MAP_MODE_SW"), &rc_map_value) == PX4_OK) {
|
||||
if (rc_map_value != 0) {
|
||||
PX4_WARN("RC_MAP_MODE_SW deprecated");
|
||||
param_reset(param_find("RC_MAP_MODE_SW"));
|
||||
rc_map_value = 0;
|
||||
param_set(param_find("RC_MAP_MODE_SW"), &rc_map_value);
|
||||
}
|
||||
}
|
||||
|
||||
if (param_get(param_find("RC_MAP_RATT_SW"), &rc_map_value) == PX4_OK) {
|
||||
if (rc_map_value != 0) {
|
||||
PX4_WARN("RC_MAP_RATT_SW deprecated");
|
||||
param_reset(param_find("RC_MAP_RATT_SW"));
|
||||
rc_map_value = 0;
|
||||
param_set(param_find("RC_MAP_RATT_SW"), &rc_map_value);
|
||||
}
|
||||
}
|
||||
|
||||
if (param_get(param_find("RC_MAP_POSCTL_SW"), &rc_map_value) == PX4_OK) {
|
||||
if (rc_map_value != 0) {
|
||||
PX4_WARN("RC_MAP_POSCTL_SW deprecated");
|
||||
param_reset(param_find("RC_MAP_POSCTL_SW"));
|
||||
rc_map_value = 0;
|
||||
param_set(param_find("RC_MAP_POSCTL_SW"), &rc_map_value);
|
||||
}
|
||||
}
|
||||
|
||||
if (param_get(param_find("RC_MAP_ACRO_SW"), &rc_map_value) == PX4_OK) {
|
||||
if (rc_map_value != 0) {
|
||||
PX4_WARN("RC_MAP_ACRO_SW deprecated");
|
||||
param_reset(param_find("RC_MAP_ACRO_SW"));
|
||||
rc_map_value = 0;
|
||||
param_set(param_find("RC_MAP_ACRO_SW"), &rc_map_value);
|
||||
}
|
||||
}
|
||||
|
||||
if (param_get(param_find("RC_MAP_STAB_SW"), &rc_map_value) == PX4_OK) {
|
||||
if (rc_map_value != 0) {
|
||||
PX4_WARN("RC_MAP_STAB_SW deprecated");
|
||||
param_reset(param_find("RC_MAP_STAB_SW"));
|
||||
rc_map_value = 0;
|
||||
param_set(param_find("RC_MAP_STAB_SW"), &rc_map_value);
|
||||
}
|
||||
}
|
||||
|
||||
if (param_get(param_find("RC_MAP_MAN_SW"), &rc_map_value) == PX4_OK) {
|
||||
if (rc_map_value != 0) {
|
||||
PX4_WARN("RC_MAP_MAN_SW deprecated");
|
||||
param_reset(param_find("RC_MAP_MAN_SW"));
|
||||
rc_map_value = 0;
|
||||
param_set(param_find("RC_MAP_MAN_SW"), &rc_map_value);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -382,7 +382,7 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
|
||||
// IMU_GYRO_RATEMAX
|
||||
if (_param_imu_gyro_ratemax.get() <= 0) {
|
||||
const int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get();
|
||||
_param_imu_gyro_ratemax.reset();
|
||||
_param_imu_gyro_ratemax.set(400);
|
||||
PX4_WARN("IMU_GYRO_RATEMAX invalid (%" PRId32 "), resetting to default %" PRId32 ")", imu_gyro_ratemax,
|
||||
_param_imu_gyro_ratemax.get());
|
||||
}
|
||||
@@ -393,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), ¶m);
|
||||
result = param_set(param_find(str), ¶m);
|
||||
|
||||
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), ¶m);
|
||||
result = param_set(param_find(str), ¶m);
|
||||
|
||||
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), ¶m);
|
||||
result = param_set(param_find(str), ¶m);
|
||||
|
||||
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) {
|
||||
|
||||
@@ -57,7 +57,10 @@ test_param(int argc, char *argv[])
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (param_reset(p) != OK) {
|
||||
// default value 12345678
|
||||
int32_t test_params_default_value = 12345678;
|
||||
|
||||
if (param_set(p, &test_params_default_value) != OK) {
|
||||
warnx("failed param reset");
|
||||
return 1;
|
||||
}
|
||||
|
||||
@@ -226,11 +226,17 @@ bool ParameterTest::ResetAllExcludesWildcard()
|
||||
|
||||
bool ParameterTest::CustomDefaults()
|
||||
{
|
||||
int32_t value = 0;
|
||||
// reset to default
|
||||
const int32_t test_1_default_value = 2;
|
||||
int32_t value = test_1_default_value;
|
||||
param_t param_test_1 = param_find("TEST_1");
|
||||
param_reset(param_test_1);
|
||||
param_set(param_test_1, &value);
|
||||
param_set_default_value(param_test_1, &value);
|
||||
|
||||
// TEST_1 default value 2
|
||||
value = 0;
|
||||
param_get(param_test_1, &value);
|
||||
ut_compare("value for param doesn't match default value", value, 2); // TEST_1 default value 2
|
||||
ut_compare("value for param doesn't match default value", value, test_1_default_value);
|
||||
|
||||
// verify underlying default value
|
||||
int32_t default_value = 0;
|
||||
@@ -247,10 +253,11 @@ bool ParameterTest::CustomDefaults()
|
||||
param_get_default_value(param_test_1, &default_value);
|
||||
ut_compare("value for param default doesn't match custom default value", default_value, 123456789);
|
||||
|
||||
// verify new value
|
||||
value = 0;
|
||||
param_get(param_test_1, &value);
|
||||
ut_compare("param value not custom default", value, 123456789);
|
||||
// // verify new value
|
||||
// TODO: arguably once the parameter is set to default value initially it should "track" the new default value
|
||||
// value = 0;
|
||||
// param_get(param_test_1, &value);
|
||||
// ut_compare("param value not custom default", value, 123456789);
|
||||
|
||||
// set to new value and verify
|
||||
value = 987654321;
|
||||
@@ -259,12 +266,6 @@ bool ParameterTest::CustomDefaults()
|
||||
param_get(param_test_1, &value);
|
||||
ut_compare("param value not saved", value, 987654321);
|
||||
|
||||
// reset (to custom default)
|
||||
param_reset(param_test_1);
|
||||
value = 0;
|
||||
param_get(param_test_1, &value);
|
||||
ut_compare("param value not reset to custom default", value, 123456789);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -281,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);
|
||||
@@ -299,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);
|
||||
@@ -326,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);
|
||||
@@ -344,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);
|
||||
@@ -374,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);
|
||||
@@ -385,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);
|
||||
@@ -426,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);
|
||||
@@ -444,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);
|
||||
@@ -474,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);
|
||||
@@ -492,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);
|
||||
@@ -524,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);
|
||||
@@ -535,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);
|
||||
|
||||
Reference in New Issue
Block a user