diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp
index 37aa5301c2c1c42d814cd5c507c191a04ef2edec..5d3c5c2957b99eed139a52d0f036662617554da2 100644
--- a/src/modules/mc_att_control/mc_att_control.hpp
+++ b/src/modules/mc_att_control/mc_att_control.hpp
@@ -38,6 +38,7 @@
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_module.h>
+#include <px4_module_params.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <uORB/topics/actuator_controls.h>
@@ -63,7 +64,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
#define MAX_GYRO_COUNT 3
-class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
+class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>, public ModuleParams
{
public:
MulticopterAttitudeControl();
@@ -140,100 +141,80 @@ private:
float _thrust_sp; /**< thrust setpoint */
math::Vector<3> _att_control; /**< attitude control vector */
- math::Matrix<3, 3> _board_rotation = {}; /**< rotation matrix for the orientation that the board is mounted */
-
- struct {
- param_t roll_p;
- param_t roll_rate_p;
- param_t roll_rate_i;
- param_t roll_rate_integ_lim;
- param_t roll_rate_d;
- param_t roll_rate_ff;
- param_t pitch_p;
- param_t pitch_rate_p;
- param_t pitch_rate_i;
- param_t pitch_rate_integ_lim;
- param_t pitch_rate_d;
- param_t pitch_rate_ff;
- param_t d_term_cutoff_freq;
- param_t tpa_breakpoint_p;
- param_t tpa_breakpoint_i;
- param_t tpa_breakpoint_d;
- param_t tpa_rate_p;
- param_t tpa_rate_i;
- param_t tpa_rate_d;
- param_t yaw_p;
- param_t yaw_rate_p;
- param_t yaw_rate_i;
- param_t yaw_rate_integ_lim;
- param_t yaw_rate_d;
- param_t yaw_rate_ff;
- param_t yaw_ff;
- param_t roll_rate_max;
- param_t pitch_rate_max;
- param_t yaw_rate_max;
- param_t yaw_auto_max;
-
- param_t acro_roll_max;
- param_t acro_pitch_max;
- param_t acro_yaw_max;
- param_t acro_expo;
- param_t acro_superexpo;
- param_t rattitude_thres;
-
- param_t vtol_wv_yaw_rate_scale;
-
- param_t bat_scale_en;
-
- param_t board_rotation;
-
- param_t board_offset[3];
-
- } _params_handles; /**< handles for interesting parameters */
-
- struct {
- matrix::Vector3f attitude_p; /**< P gain for attitude control */
- math::Vector<3> rate_p; /**< P gain for angular rate error */
- math::Vector<3> rate_i; /**< I gain for angular rate error */
- math::Vector<3> rate_int_lim; /**< integrator state limit for rate loop */
- math::Vector<3> rate_d; /**< D gain for angular rate error */
- math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */
- float yaw_ff; /**< yaw control feed-forward */
-
- float d_term_cutoff_freq; /**< Cutoff frequency for the D-term filter */
-
- float tpa_breakpoint_p; /**< Throttle PID Attenuation breakpoint */
- float tpa_breakpoint_i; /**< Throttle PID Attenuation breakpoint */
- float tpa_breakpoint_d; /**< Throttle PID Attenuation breakpoint */
- float tpa_rate_p; /**< Throttle PID Attenuation slope */
- float tpa_rate_i; /**< Throttle PID Attenuation slope */
- float tpa_rate_d; /**< Throttle PID Attenuation slope */
-
- float roll_rate_max;
- float pitch_rate_max;
- float yaw_rate_max;
- float yaw_auto_max;
- math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */
- math::Vector<3> auto_rate_max; /**< attitude rate limits in auto modes */
- matrix::Vector3f acro_rate_max; /**< max attitude rates in acro mode */
- float acro_expo; /**< function parameter for expo stick curve shape */
- float acro_superexpo; /**< function parameter for superexpo stick curve shape */
- float rattitude_thres;
-
- float vtol_wv_yaw_rate_scale; /**< Scale value [0, 1] for yaw rate setpoint */
-
- int32_t bat_scale_en;
-
- int32_t board_rotation;
-
- float board_offset[3];
-
- } _params;
+ math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
+
+ DEFINE_PARAMETERS(
+ (ParamFloat<px4::params::MC_ROLL_P>) _roll_p,
+ (ParamFloat<px4::params::MC_ROLLRATE_P>) _roll_rate_p,
+ (ParamFloat<px4::params::MC_ROLLRATE_I>) _roll_rate_i,
+ (ParamFloat<px4::params::MC_RR_INT_LIM>) _roll_rate_integ_lim,
+ (ParamFloat<px4::params::MC_ROLLRATE_D>) _roll_rate_d,
+ (ParamFloat<px4::params::MC_ROLLRATE_FF>) _roll_rate_ff,
+
+ (ParamFloat<px4::params::MC_PITCH_P>) _pitch_p,
+ (ParamFloat<px4::params::MC_PITCHRATE_P>) _pitch_rate_p,
+ (ParamFloat<px4::params::MC_PITCHRATE_I>) _pitch_rate_i,
+ (ParamFloat<px4::params::MC_PR_INT_LIM>) _pitch_rate_integ_lim,
+ (ParamFloat<px4::params::MC_PITCHRATE_D>) _pitch_rate_d,
+ (ParamFloat<px4::params::MC_PITCHRATE_FF>) _pitch_rate_ff,
+
+ (ParamFloat<px4::params::MC_YAW_P>) _yaw_p,
+ (ParamFloat<px4::params::MC_YAWRATE_P>) _yaw_rate_p,
+ (ParamFloat<px4::params::MC_YAWRATE_I>) _yaw_rate_i,
+ (ParamFloat<px4::params::MC_YR_INT_LIM>) _yaw_rate_integ_lim,
+ (ParamFloat<px4::params::MC_YAWRATE_D>) _yaw_rate_d,
+ (ParamFloat<px4::params::MC_YAWRATE_FF>) _yaw_rate_ff,
+
+ (ParamFloat<px4::params::MC_YAW_FF>) _yaw_ff, /**< yaw control feed-forward */
+
+ (ParamFloat<px4::params::MC_DTERM_CUTOFF>) _d_term_cutoff_freq, /**< Cutoff frequency for the D-term filter */
+
+ (ParamFloat<px4::params::MC_TPA_BREAK_P>) _tpa_breakpoint_p, /**< Throttle PID Attenuation breakpoint */
+ (ParamFloat<px4::params::MC_TPA_BREAK_I>) _tpa_breakpoint_i, /**< Throttle PID Attenuation breakpoint */
+ (ParamFloat<px4::params::MC_TPA_BREAK_D>) _tpa_breakpoint_d, /**< Throttle PID Attenuation breakpoint */
+ (ParamFloat<px4::params::MC_TPA_RATE_P>) _tpa_rate_p, /**< Throttle PID Attenuation slope */
+ (ParamFloat<px4::params::MC_TPA_RATE_I>) _tpa_rate_i, /**< Throttle PID Attenuation slope */
+ (ParamFloat<px4::params::MC_TPA_RATE_D>) _tpa_rate_d, /**< Throttle PID Attenuation slope */
+
+ (ParamFloat<px4::params::MC_ROLLRATE_MAX>) _roll_rate_max,
+ (ParamFloat<px4::params::MC_PITCHRATE_MAX>) _pitch_rate_max,
+ (ParamFloat<px4::params::MC_YAWRATE_MAX>) _yaw_rate_max,
+ (ParamFloat<px4::params::MC_YAWRAUTO_MAX>) _yaw_auto_max,
+
+ (ParamFloat<px4::params::MC_ACRO_R_MAX>) _acro_roll_max,
+ (ParamFloat<px4::params::MC_ACRO_P_MAX>) _acro_pitch_max,
+ (ParamFloat<px4::params::MC_ACRO_Y_MAX>) _acro_yaw_max,
+ (ParamFloat<px4::params::MC_ACRO_EXPO>) _acro_expo, /**< function parameter for expo stick curve shape */
+ (ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _acro_superexpo, /**< function parameter for superexpo stick curve shape */
+
+ (ParamFloat<px4::params::MC_RATT_TH>) _rattitude_thres,
+
+ (ParamBool<px4::params::MC_BAT_SCALE_EN>) _bat_scale_en,
+
+ (ParamInt<px4::params::SENS_BOARD_ROT>) _board_rotation_param,
+
+ (ParamFloat<px4::params::SENS_BOARD_X_OFF>) _board_offset_x,
+ (ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _board_offset_y,
+ (ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _board_offset_z,
+
+ (ParamFloat<px4::params::VT_WV_YAWR_SCL>) _vtol_wv_yaw_rate_scale /**< Scale value [0, 1] for yaw rate setpoint */
+ )
+
+ matrix::Vector3f _attitude_p; /**< P gain for attitude control */
+ math::Vector<3> _rate_p; /**< P gain for angular rate error */
+ math::Vector<3> _rate_i; /**< I gain for angular rate error */
+ math::Vector<3> _rate_int_lim; /**< integrator state limit for rate loop */
+ math::Vector<3> _rate_d; /**< D gain for angular rate error */
+ math::Vector<3> _rate_ff; /**< Feedforward gain for desired rates */
+
+ math::Vector<3> _mc_rate_max; /**< attitude rate limits in stabilized modes */
+ math::Vector<3> _auto_rate_max; /**< attitude rate limits in auto modes */
+ matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */
/**
* Update our local parameter cache.
*/
- void parameters_update();
+ void parameters_updated();
/**
* Check for parameter update and handle it.
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index ccf3c1474ef5dc3d6707a73d75bff3019fd00abf..15ee533de07bf95adfa1ffa515aa09d28412db38 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -97,6 +97,7 @@ To reduce control latency, the module directly polls on the gyro topic published
}
MulticopterAttitudeControl::MulticopterAttitudeControl() :
+ ModuleParams(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
@@ -115,28 +116,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_v_att.q[0] = 1.f;
_v_att_sp.q_d[0] = 1.f;
- _params.rate_p.zero();
- _params.rate_i.zero();
- _params.rate_int_lim.zero();
- _params.rate_d.zero();
- _params.rate_ff.zero();
- _params.yaw_ff = 0.0f;
- _params.roll_rate_max = 0.0f;
- _params.pitch_rate_max = 0.0f;
- _params.yaw_rate_max = 0.0f;
- _params.mc_rate_max.zero();
- _params.auto_rate_max.zero();
- _params.acro_rate_max.zero();
- _params.rattitude_thres = 1.0f;
-
- _params.bat_scale_en = 0;
-
- _params.board_rotation = 0;
-
- _params.board_offset[0] = 0.0f;
- _params.board_offset[1] = 0.0f;
- _params.board_offset[2] = 0.0f;
-
_rates_prev.zero();
_rates_prev_filtered.zero();
_rates_sp.zero();
@@ -144,65 +123,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_thrust_sp = 0.0f;
_att_control.zero();
- _board_rotation.identity();
-
- _params_handles.roll_p = param_find("MC_ROLL_P");
- _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
- _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I");
- _params_handles.roll_rate_integ_lim = param_find("MC_RR_INT_LIM");
- _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D");
- _params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF");
-
- _params_handles.pitch_p = param_find("MC_PITCH_P");
- _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P");
- _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I");
- _params_handles.pitch_rate_integ_lim = param_find("MC_PR_INT_LIM");
- _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
- _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF");
-
- _params_handles.d_term_cutoff_freq = param_find("MC_DTERM_CUTOFF");
-
- _params_handles.tpa_breakpoint_p = param_find("MC_TPA_BREAK_P");
- _params_handles.tpa_breakpoint_i = param_find("MC_TPA_BREAK_I");
- _params_handles.tpa_breakpoint_d = param_find("MC_TPA_BREAK_D");
- _params_handles.tpa_rate_p = param_find("MC_TPA_RATE_P");
- _params_handles.tpa_rate_i = param_find("MC_TPA_RATE_I");
- _params_handles.tpa_rate_d = param_find("MC_TPA_RATE_D");
-
- _params_handles.yaw_p = param_find("MC_YAW_P");
- _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
- _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
- _params_handles.yaw_rate_integ_lim = param_find("MC_YR_INT_LIM");
- _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
- _params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF");
- _params_handles.yaw_ff = param_find("MC_YAW_FF");
-
- _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX");
- _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX");
- _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
- _params_handles.yaw_auto_max = param_find("MC_YAWRAUTO_MAX");
-
- _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX");
- _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
- _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
- _params_handles.acro_expo = param_find("MC_ACRO_EXPO");
- _params_handles.acro_superexpo = param_find("MC_ACRO_SUPEXPO");
-
- _params_handles.rattitude_thres = param_find("MC_RATT_TH");
-
- _params_handles.bat_scale_en = param_find("MC_BAT_SCALE_EN");
-
- /* rotations */
- _params_handles.board_rotation = param_find("SENS_BOARD_ROT");
-
- /* rotation offsets */
- _params_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
- _params_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
- _params_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
-
- /* fetch initial parameter values */
- parameters_update();
-
/* initialize thermal corrections as we might not immediately get a topic update (only non-zero values) */
for (unsigned i = 0; i < 3; i++) {
// used scale factors to unity
@@ -210,131 +130,73 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_sensor_correction.gyro_scale_1[i] = 1.0f;
_sensor_correction.gyro_scale_2[i] = 1.0f;
}
+
+ parameters_updated();
}
void
-MulticopterAttitudeControl::parameters_update()
+MulticopterAttitudeControl::parameters_updated()
{
- float v;
+ /* Store some of the parameters in a more convenient way & precompute often-used values */
/* roll gains */
- param_get(_params_handles.roll_p, &v);
- _params.attitude_p(0) = v;
- param_get(_params_handles.roll_rate_p, &v);
- _params.rate_p(0) = v;
- param_get(_params_handles.roll_rate_i, &v);
- _params.rate_i(0) = v;
- param_get(_params_handles.roll_rate_integ_lim, &v);
- _params.rate_int_lim(0) = v;
- param_get(_params_handles.roll_rate_d, &v);
- _params.rate_d(0) = v;
- param_get(_params_handles.roll_rate_ff, &v);
- _params.rate_ff(0) = v;
+ _attitude_p(0) = _roll_p.get();
+ _rate_p(0) = _roll_rate_p.get();
+ _rate_i(0) = _roll_rate_i.get();
+ _rate_int_lim(0) = _roll_rate_integ_lim.get();
+ _rate_d(0) = _roll_rate_d.get();
+ _rate_ff(0) = _roll_rate_ff.get();
/* pitch gains */
- param_get(_params_handles.pitch_p, &v);
- _params.attitude_p(1) = v;
- param_get(_params_handles.pitch_rate_p, &v);
- _params.rate_p(1) = v;
- param_get(_params_handles.pitch_rate_i, &v);
- _params.rate_i(1) = v;
- param_get(_params_handles.pitch_rate_integ_lim, &v);
- _params.rate_int_lim(1) = v;
- param_get(_params_handles.pitch_rate_d, &v);
- _params.rate_d(1) = v;
- param_get(_params_handles.pitch_rate_ff, &v);
- _params.rate_ff(1) = v;
-
- param_get(_params_handles.d_term_cutoff_freq, &_params.d_term_cutoff_freq);
-
- if (fabsf(_lp_filters_d[0].get_cutoff_freq() - _params.d_term_cutoff_freq) > 0.01f) {
- _lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
- _lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
- _lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
+ _attitude_p(1) = _pitch_p.get();
+ _rate_p(1) = _pitch_rate_p.get();
+ _rate_i(1) = _pitch_rate_i.get();
+ _rate_int_lim(1) = _pitch_rate_integ_lim.get();
+ _rate_d(1) = _pitch_rate_d.get();
+ _rate_ff(1) = _pitch_rate_ff.get();
+
+ /* yaw gains */
+ _attitude_p(2) = _yaw_p.get();
+ _rate_p(2) = _yaw_rate_p.get();
+ _rate_i(2) = _yaw_rate_i.get();
+ _rate_int_lim(2) = _yaw_rate_integ_lim.get();
+ _rate_d(2) = _yaw_rate_d.get();
+ _rate_ff(2) = _yaw_rate_ff.get();
+
+ if (fabsf(_lp_filters_d[0].get_cutoff_freq() - _d_term_cutoff_freq.get()) > 0.01f) {
+ _lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
+ _lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
+ _lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d[0].reset(_rates_prev(0));
_lp_filters_d[1].reset(_rates_prev(1));
_lp_filters_d[2].reset(_rates_prev(2));
}
- param_get(_params_handles.tpa_breakpoint_p, &_params.tpa_breakpoint_p);
- param_get(_params_handles.tpa_breakpoint_i, &_params.tpa_breakpoint_i);
- param_get(_params_handles.tpa_breakpoint_d, &_params.tpa_breakpoint_d);
- param_get(_params_handles.tpa_rate_p, &_params.tpa_rate_p);
- param_get(_params_handles.tpa_rate_i, &_params.tpa_rate_i);
- param_get(_params_handles.tpa_rate_d, &_params.tpa_rate_d);
-
- /* yaw gains */
- param_get(_params_handles.yaw_p, &v);
- _params.attitude_p(2) = v;
- param_get(_params_handles.yaw_rate_p, &v);
- _params.rate_p(2) = v;
- param_get(_params_handles.yaw_rate_i, &v);
- _params.rate_i(2) = v;
- param_get(_params_handles.yaw_rate_integ_lim, &v);
- _params.rate_int_lim(2) = v;
- param_get(_params_handles.yaw_rate_d, &v);
- _params.rate_d(2) = v;
- param_get(_params_handles.yaw_rate_ff, &v);
- _params.rate_ff(2) = v;
-
- param_get(_params_handles.yaw_ff, &_params.yaw_ff);
-
/* angular rate limits */
- param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
- _params.mc_rate_max(0) = math::radians(_params.roll_rate_max);
- param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
- _params.mc_rate_max(1) = math::radians(_params.pitch_rate_max);
- param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
- _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
+ _mc_rate_max(0) = math::radians(_roll_rate_max.get());
+ _mc_rate_max(1) = math::radians(_pitch_rate_max.get());
+ _mc_rate_max(2) = math::radians(_yaw_rate_max.get());
/* auto angular rate limits */
- param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
- _params.auto_rate_max(0) = math::radians(_params.roll_rate_max);
- param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
- _params.auto_rate_max(1) = math::radians(_params.pitch_rate_max);
- param_get(_params_handles.yaw_auto_max, &_params.yaw_auto_max);
- _params.auto_rate_max(2) = math::radians(_params.yaw_auto_max);
+ _auto_rate_max(0) = math::radians(_roll_rate_max.get());
+ _auto_rate_max(1) = math::radians(_pitch_rate_max.get());
+ _auto_rate_max(2) = math::radians(_yaw_auto_max.get());
/* manual rate control acro mode rate limits and expo */
- param_get(_params_handles.acro_roll_max, &v);
- _params.acro_rate_max(0) = math::radians(v);
- param_get(_params_handles.acro_pitch_max, &v);
- _params.acro_rate_max(1) = math::radians(v);
- param_get(_params_handles.acro_yaw_max, &v);
- _params.acro_rate_max(2) = math::radians(v);
- param_get(_params_handles.acro_expo, &_params.acro_expo);
- param_get(_params_handles.acro_superexpo, &_params.acro_superexpo);
-
- /* stick deflection needed in rattitude mode to control rates not angles */
- param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
-
- if (_vehicle_status.is_vtol) {
- param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale);
-
- } else {
- _params.vtol_wv_yaw_rate_scale = 0.f;
- }
-
- param_get(_params_handles.bat_scale_en, &_params.bat_scale_en);
+ _acro_rate_max(0) = math::radians(_acro_roll_max.get());
+ _acro_rate_max(1) = math::radians(_acro_pitch_max.get());
+ _acro_rate_max(2) = math::radians(_acro_yaw_max.get());
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
- /* rotation of the autopilot relative to the body */
- param_get(_params_handles.board_rotation, &(_params.board_rotation));
-
- /* fine adjustment of the rotation */
- param_get(_params_handles.board_offset[0], &(_params.board_offset[0]));
- param_get(_params_handles.board_offset[1], &(_params.board_offset[1]));
- param_get(_params_handles.board_offset[2], &(_params.board_offset[2]));
-
/* get transformation matrix from sensor/board to body frame */
- get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation);
+ get_rot_matrix((enum Rotation)_board_rotation_param.get(), &_board_rotation);
/* fine tune the rotation */
math::Matrix<3, 3> board_rotation_offset;
- board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _params.board_offset[0],
- M_DEG_TO_RAD_F * _params.board_offset[1],
- M_DEG_TO_RAD_F * _params.board_offset[2]);
+ board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _board_offset_x.get(),
+ M_DEG_TO_RAD_F * _board_offset_y.get(),
+ M_DEG_TO_RAD_F * _board_offset_z.get());
_board_rotation = board_rotation_offset * _board_rotation;
}
@@ -349,7 +211,8 @@ MulticopterAttitudeControl::parameter_update_poll()
if (updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);
- parameters_update();
+ updateParams();
+ parameters_updated();
}
}
@@ -419,10 +282,6 @@ MulticopterAttitudeControl::vehicle_status_poll()
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
- _params_handles.vtol_wv_yaw_rate_scale = param_find("VT_WV_YAWR_SCL");
-
- parameters_update();
-
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
@@ -512,7 +371,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
_thrust_sp = _v_att_sp.thrust;
/* prepare yaw weight from the ratio between roll/pitch and yaw gains */
- Vector3f attitude_gain = _params.attitude_p;
+ Vector3f attitude_gain = _attitude_p;
const float roll_pitch_gain = (attitude_gain(0) + attitude_gain(1)) / 2.f;
const float yaw_w = math::constrain(attitude_gain(2) / roll_pitch_gain, 0.f, 1.f);
attitude_gain(2) = roll_pitch_gain;
@@ -564,7 +423,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
* We infer the body z axis by taking the last column of R.transposed (== q.inversed)
* because it's the rotation axis for body yaw and multiply it by the rate and gain. */
Vector3f yaw_feedforward_rate = q.inversed().dcm_z();
- yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _params.yaw_ff;
+ yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _yaw_ff.get();
yaw_feedforward_rate(2) *= yaw_w;
_rates_sp += math::Vector<3>(yaw_feedforward_rate.data());
@@ -574,10 +433,10 @@ MulticopterAttitudeControl::control_attitude(float dt)
for (int i = 0; i < 3; i++) {
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
!_v_control_mode.flag_control_manual_enabled) {
- _rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));
+ _rates_sp(i) = math::constrain(_rates_sp(i), -_auto_rate_max(i), _auto_rate_max(i));
} else {
- _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
+ _rates_sp(i) = math::constrain(_rates_sp(i), -_mc_rate_max(i), _mc_rate_max(i));
}
}
@@ -585,7 +444,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (_vehicle_status.is_vtol && _v_att_sp.disable_mc_yaw_control) {
if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) {
- const float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
+ const float wv_yaw_rate_max = _auto_rate_max(2) * _vtol_wv_yaw_rate_scale.get();
_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
// prevent integrator winding up in weathervane mode
@@ -660,9 +519,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
rates(1) -= _sensor_bias.gyro_y_bias;
rates(2) -= _sensor_bias.gyro_z_bias;
- math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p));
- //math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i));
- math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d));
+ math::Vector<3> rates_p_scaled = _rate_p.emult(pid_attenuations(_tpa_breakpoint_p.get(), _tpa_rate_p.get()));
+ //math::Vector<3> rates_i_scaled = _rate_i.emult(pid_attenuations(_tpa_breakpoint_i.get(), _tpa_rate_i.get()));
+ math::Vector<3> rates_d_scaled = _rate_d.emult(pid_attenuations(_tpa_breakpoint_d.get(), _tpa_rate_d.get()));
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
@@ -676,7 +535,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
_att_control = rates_p_scaled.emult(rates_err) +
_rates_int -
rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt +
- _params.rate_ff.emult(_rates_sp);
+ _rate_ff.emult(_rates_sp);
_rates_prev = rates;
_rates_prev_filtered = rates_filtered;
@@ -709,9 +568,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
}
// Perform the integration using a first order method and do not propaate the result if out of range or invalid
- float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
+ float rate_i = _rates_int(i) + _rate_i(i) * rates_err(i) * dt;
- if (PX4_ISFINITE(rate_i) && rate_i > -_params.rate_int_lim(i) && rate_i < _params.rate_int_lim(i)) {
+ if (PX4_ISFINITE(rate_i) && rate_i > -_rate_int_lim(i) && rate_i < _rate_int_lim(i)) {
_rates_int(i) = rate_i;
}
@@ -720,7 +579,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
/* explicitly limit the integrator state */
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
- _rates_int(i) = math::constrain(_rates_int(i), -_params.rate_int_lim(i), _params.rate_int_lim(i));
+ _rates_int(i) = math::constrain(_rates_int(i), -_rate_int_lim(i), _rate_int_lim(i));
}
}
@@ -755,9 +614,6 @@ MulticopterAttitudeControl::run()
_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
- /* initialize parameters cache */
- parameters_update();
-
/* wakeup source: gyro data from sensor selected by the sensor app */
px4_pollfd_struct_t poll_fds = {};
poll_fds.events = POLLIN;
@@ -821,8 +677,8 @@ MulticopterAttitudeControl::run()
* or roll (yaw can rotate 360 in normal att control). If both are true don't
* even bother running the attitude controllers */
if (_v_control_mode.flag_control_rattitude_enabled) {
- if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
- fabsf(_manual_control_sp.x) > _params.rattitude_thres) {
+ if (fabsf(_manual_control_sp.y) > _rattitude_thres.get() ||
+ fabsf(_manual_control_sp.x) > _rattitude_thres.get()) {
_v_control_mode.flag_control_attitude_enabled = false;
}
}
@@ -850,10 +706,10 @@ MulticopterAttitudeControl::run()
if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control - ACRO mode */
matrix::Vector3f man_rate_sp;
- man_rate_sp(0) = math::superexpo(_manual_control_sp.y, _params.acro_expo, _params.acro_superexpo);
- man_rate_sp(1) = math::superexpo(-_manual_control_sp.x, _params.acro_expo, _params.acro_superexpo);
- man_rate_sp(2) = math::superexpo(_manual_control_sp.r, _params.acro_expo, _params.acro_superexpo);
- man_rate_sp = man_rate_sp.emult(_params.acro_rate_max);
+ man_rate_sp(0) = math::superexpo(_manual_control_sp.y, _acro_expo.get(), _acro_superexpo.get());
+ man_rate_sp(1) = math::superexpo(-_manual_control_sp.x, _acro_expo.get(), _acro_superexpo.get());
+ man_rate_sp(2) = math::superexpo(_manual_control_sp.r, _acro_expo.get(), _acro_superexpo.get());
+ man_rate_sp = man_rate_sp.emult(_acro_rate_max);
_rates_sp = math::Vector<3>(man_rate_sp.data());
_thrust_sp = _manual_control_sp.z;
@@ -894,7 +750,7 @@ MulticopterAttitudeControl::run()
_actuators.timestamp_sample = _sensor_gyro.timestamp;
/* scale effort by battery status */
- if (_params.bat_scale_en && _battery_status.scale > 0.0f) {
+ if (_bat_scale_en.get() && _battery_status.scale > 0.0f) {
for (int i = 0; i < 4; i++) {
_actuators.control[i] *= _battery_status.scale;
}
@@ -965,9 +821,9 @@ MulticopterAttitudeControl::run()
_loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
dt_accumulator = 0;
loop_counter = 0;
- _lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
- _lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
- _lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq);
+ _lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
+ _lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
+ _lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
}
}