zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

refactor mc_att_control: use ModuleParams & Param classes

sbg
Beat Küng 7 years ago
parent
22e8204de8
commit
b04a3a969d
2 changed files with 139 additions and 302 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 163
      src/modules/mc_att_control/mc_att_control.hpp
  2. 278
      src/modules/mc_att_control/mc_att_control_main.cpp

163
src/modules/mc_att_control/mc_att_control.hpp
Unescape View File

@ -38,6 +38,7 @@ @@ -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[]); @@ -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: @@ -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.

278
src/modules/mc_att_control/mc_att_control_main.cpp
Unescape View File

@ -97,6 +97,7 @@ To reduce control latency, the module directly polls on the gyro topic published @@ -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() : @@ -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() : @@ -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() : @@ -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() @@ -349,7 +211,8 @@ MulticopterAttitudeControl::parameter_update_poll()
if (updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
parameters_update();
updateParams();
parameters_updated();
}
}
@ -419,10 +282,6 @@ MulticopterAttitudeControl::vehicle_status_poll() @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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());
}
}

Write Preview
Loading…
Cancel
Save