attitude setpoint topic: cleanup of matrix class usage

Signed-off-by: Roman <bapstroman@gmail.com>

src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -611,10 +611,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					att.yawspeed = x_aposteriori[2];
 
 					/* magnetic declination */
-					matrix::Dcm<float> Ro(&Rot_matrix[0]);
-					matrix::Dcm<float> R_declination(&R_decl.data[0][0]);
+					matrix::Dcmf Ro(&Rot_matrix[0]);
+					matrix::Dcmf R_declination(&R_decl.data[0][0]);
 					Ro = R_declination * Ro;
-					matrix::Quaternion<float> q(Ro);
+					matrix::Quatf q = R_declination * Ro;
 
 					memcpy(&att.q[0],&q._data[0],sizeof(att.q));
 

src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -952,9 +952,8 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
 	_local_pos.xy_global = _gps_initialized; //TODO: Handle optical flow mode here
 
 	_local_pos.z_global = false;
-	matrix::Quaternion<float> q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
-	matrix::Euler<float> euler(q);
-	_local_pos.yaw = euler(2);
+	matrix::Eulerf euler = matrix::Quatf(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+	_local_pos.yaw = euler.psi();
 
 	if (!PX4_ISFINITE(_local_pos.x) ||
 	    !PX4_ISFINITE(_local_pos.y) ||

src/examples/fixedwing_control/main.cpp

@@ -177,19 +177,16 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
 	 * Calculate roll error and apply P gain
 	 */
 
-	matrix::Quaternion<float> qa(&att->q[0]);
-	matrix::Euler<float> att_euler(qa);
+	matrix::Eulerf att_euler = matrix::Quatf(att->q);
+	matrix::Eulerf att_sp_euler = matrix::Quatf(att_sp->q_d);
 
-	matrix::Quaternion<float> qd(&att_sp->q_d[0]);
-	matrix::Euler<float> att_sp_euler(qd);
-
-	float roll_err = att_euler(0) - att_sp_euler(0);
+	float roll_err = att_euler.phi() - att_sp_euler.phi();
 	actuators->control[0] = roll_err * p.roll_p;
 
 	/*
 	 * Calculate pitch error and apply P gain
 	 */
-	float pitch_err = att_euler(1) - att_sp_euler(1);
+	float pitch_err = att_euler.theta() - att_sp_euler.theta();
 	actuators->control[1] = pitch_err * p.pitch_p;
 }
 
@@ -203,11 +200,10 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p
 
 	float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon);
 
-	matrix::Quaternion<float> qa(&att->q[0]);
-	matrix::Euler<float> att_euler(qa);
+	matrix::Eulerf att_euler = matrix::Quatf(att->q);
 
 	/* calculate heading error */
-	float yaw_err = att_euler(2) - bearing;
+	float yaw_err = att_euler.psi() - bearing;
 	/* apply control gain */
 	float roll_body = yaw_err * p.hdng_p;
 
@@ -219,9 +215,9 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p
 		roll_body = 0.6f;
 	}
 
-	matrix::Euler<float> att_spe(roll_body, 0, bearing);
+	matrix::Eulerf att_spe(roll_body, 0, bearing);
 
-	matrix::Quaternion<float> qd(att_spe);
+	matrix::Quatf qd(att_spe);
 
 	att_sp->q_d[0] = qd(0);
 	att_sp->q_d[1] = qd(1);

src/examples/rover_steering_control/main.cpp

@@ -179,11 +179,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
 	/*
 	 * Calculate roll error and apply P gain
 	 */
-	matrix::Quaternion<float> q(&att->q[0]);
-	matrix::Euler<float> euler(q);
-	matrix::Quaternion<float> q_sp(&att_sp->q_d[0]);
-	matrix::Euler<float> euler_sp(q_sp);
-	float yaw_err = euler(2) - euler_sp(2);
+	float yaw_err = Eulerf(Quaternion(att->q)).psi() - Eulerf(Quaternion(att->q_d)).psi();
 	actuators->control[2] = yaw_err * pp.yaw_p;
 
 	/* copy throttle */