mc_att_control: refactor: switch to matrix library

src/modules/mc_att_control/mc_att_control.hpp

@@ -31,9 +31,9 @@
  *
  ****************************************************************************/
 
-#include <lib/mathlib/mathlib.h>
 #include <lib/mixer/mixer.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <matrix/matrix/math.hpp>
 #include <systemlib/perf_counter.h>
 #include <px4_config.h>
 #include <px4_defines.h>
@@ -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<px4::params::MC_ROLL_P>) _roll_p,
@@ -236,16 +236,16 @@ private:
 		(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 */
+	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 */
 
 };
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -49,6 +49,8 @@
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <systemlib/circuit_breaker.h>
+#include <mathlib/math/Limits.hpp>
+#include <mathlib/math/Functions.hpp>
 
 #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 */