diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 674d1163a9..5a9f8029cf 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -31,9 +31,9 @@ * ****************************************************************************/ -#include #include #include +#include #include #include #include @@ -121,7 +121,7 @@ private: /** * Throttle PID attenuation. */ - math::Vector<3> pid_attenuations(float tpa_breakpoint, float tpa_rate); + matrix::Vector3f pid_attenuations(float tpa_breakpoint, float tpa_rate); int _v_att_sub{-1}; /**< vehicle attitude subscription */ @@ -170,14 +170,14 @@ private: static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */ float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */ - math::Vector<3> _rates_prev; /**< angular rates on previous step */ - math::Vector<3> _rates_prev_filtered; /**< angular rates on previous step (low-pass filtered) */ - math::Vector<3> _rates_sp; /**< angular rates setpoint */ - math::Vector<3> _rates_int; /**< angular rates integral error */ - float _thrust_sp; /**< thrust setpoint */ - math::Vector<3> _att_control; /**< attitude control vector */ + matrix::Vector3f _rates_prev; /**< angular rates on previous step */ + matrix::Vector3f _rates_prev_filtered; /**< angular rates on previous step (low-pass filtered) */ + matrix::Vector3f _rates_sp; /**< angular rates setpoint */ + matrix::Vector3f _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + matrix::Vector3f _att_control; /**< attitude control vector */ - math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ DEFINE_PARAMETERS( (ParamFloat) _roll_p, @@ -236,16 +236,16 @@ private: (ParamFloat) _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 */ + matrix::Vector3f _attitude_p; /**< P gain for attitude control */ + matrix::Vector3f _rate_p; /**< P gain for angular rate error */ + matrix::Vector3f _rate_i; /**< I gain for angular rate error */ + matrix::Vector3f _rate_int_lim; /**< integrator state limit for rate loop */ + matrix::Vector3f _rate_d; /**< D gain for angular rate error */ + matrix::Vector3f _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 */ + matrix::Vector3f _mc_rate_max; /**< attitude rate limits in stabilized modes */ + matrix::Vector3f _auto_rate_max; /**< attitude rate limits in auto modes */ + matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ }; 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 f6b00ca839..b23ea997fd 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -49,6 +49,8 @@ #include #include #include +#include +#include #define MIN_TAKEOFF_THRUST 0.2f #define TPA_RATE_LOWER_LIMIT 0.05f @@ -189,13 +191,13 @@ MulticopterAttitudeControl::parameters_updated() _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); /* get transformation matrix from sensor/board to body frame */ - get_rot_matrix((enum Rotation)_board_rotation_param.get(), &_board_rotation); + _board_rotation = get_rot_matrix((enum Rotation)_board_rotation_param.get()); /* fine tune the rotation */ - math::Matrix<3, 3> board_rotation_offset; - 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()); + Dcmf board_rotation_offset(Eulerf( + 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; } @@ -415,17 +417,15 @@ MulticopterAttitudeControl::control_attitude(float dt) Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag(); /* calculate angular rates setpoint */ - eq = eq.emult(attitude_gain); - _rates_sp = math::Vector<3>(eq.data()); + _rates_sp = eq.emult(attitude_gain); /* Feed forward the yaw setpoint rate. We need to apply the yaw rate in the body frame. * 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 * _yaw_ff.get(); - yaw_feedforward_rate(2) *= yaw_w; - _rates_sp += math::Vector<3>(yaw_feedforward_rate.data()); + _rates_sp += yaw_feedforward_rate; /* limit rates */ @@ -458,14 +458,14 @@ MulticopterAttitudeControl::control_attitude(float dt) * Input: 'tpa_breakpoint', 'tpa_rate', '_thrust_sp' * Output: 'pidAttenuationPerAxis' vector */ -math::Vector<3> +Vector3f MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate) { /* throttle pid attenuation factor */ float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint) / (1.0f - tpa_breakpoint); tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa)); - math::Vector<3> pidAttenuationPerAxis; + Vector3f pidAttenuationPerAxis; pidAttenuationPerAxis(AXIS_INDEX_ROLL) = tpa; pidAttenuationPerAxis(AXIS_INDEX_PITCH) = tpa; pidAttenuationPerAxis(AXIS_INDEX_YAW) = 1.0; @@ -487,7 +487,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) } // get the raw gyro data and correct for thermal errors - math::Vector<3> rates; + Vector3f rates; if (_selected_gyro == 0) { rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0]; @@ -518,15 +518,15 @@ 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 = _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())); + Vector3f rates_p_scaled = _rate_p.emult(pid_attenuations(_tpa_breakpoint_p.get(), _tpa_rate_p.get())); + //Vector3f rates_i_scaled = _rate_i.emult(pid_attenuations(_tpa_breakpoint_i.get(), _tpa_rate_i.get())); + Vector3f 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; + Vector3f rates_err = _rates_sp - rates; /* apply low-pass filtering to the rates for D-term */ - math::Vector<3> rates_filtered( + Vector3f rates_filtered( _lp_filters_d[0].apply(rates(0)), _lp_filters_d[1].apply(rates(1)), _lp_filters_d[2].apply(rates(2))); @@ -566,7 +566,7 @@ 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 + // Perform the integration using a first order method and do not propagate the result if out of range or invalid float rate_i = _rates_int(i) + _rate_i(i) * rates_err(i) * dt; if (PX4_ISFINITE(rate_i) && rate_i > -_rate_int_lim(i) && rate_i < _rate_int_lim(i)) { @@ -704,12 +704,11 @@ MulticopterAttitudeControl::run() /* attitude controller disabled, poll rates setpoint topic */ 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, _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()); + Vector3f man_rate_sp( + math::superexpo(_manual_control_sp.y, _acro_expo.get(), _acro_superexpo.get()), + math::superexpo(-_manual_control_sp.x, _acro_expo.get(), _acro_superexpo.get()), + math::superexpo(_manual_control_sp.r, _acro_expo.get(), _acro_superexpo.get())); + _rates_sp = man_rate_sp.emult(_acro_rate_max); _thrust_sp = _manual_control_sp.z; /* publish attitude rates setpoint */