attitude setpoint topic: cleanup of matrix class usage

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

src/drivers/bst/bst.cpp

@@ -51,6 +51,8 @@
 #include <uORB/topics/battery_status.h>
 #include <mathlib/math/Quaternion.hpp>
 
+using namespace matrix;
+
 #define BST_DEVICE_PATH "/dev/bst0"
 
 static const char commandline_usage[] = "usage: bst start|status|stop";
@@ -290,13 +292,13 @@ void BST::cycle()
 		if (updated) {
 			vehicle_attitude_s att;
 			orb_copy(ORB_ID(vehicle_attitude), _attitude_sub, &att);
-			matrix::Quaternion<float> q(&att.q[0]);
-			matrix::Euler<float> euler(q);
+			Quatf q(att.q);
+			Eulerf euler(q);
 			BSTPacket<BSTAttitude> bst_att = {};
 			bst_att.type = 0x1E;
-			bst_att.payload.roll = swap_int32(euler(0) * 10000);
-			bst_att.payload.pitch = swap_int32(euler(1) * 10000);
-			bst_att.payload.yaw = swap_int32(euler(2) * 10000);
+			bst_att.payload.roll = swap_int32(euler.phi() * 10000);
+			bst_att.payload.pitch = swap_int32(euler.theta() * 10000);
+			bst_att.payload.yaw = swap_int32(euler.psi() * 10000);
 			send_packet(bst_att);
 		}
 

src/drivers/vmount/output.cpp

@@ -187,8 +187,7 @@ void OutputBase::_calculate_output_angles(const hrt_abstime &t)
 		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
 	}
 
-	matrix::Quaternion<float> q(&vehicle_attitude.q[0]);
-	matrix::Euler<float> euler(q);
+	matrix::Eulerf euler = matrix::Quatf(vehicle_attitude.q);
 
 	for (int i = 0; i < 3; ++i) {
 		if (_stabilize[i]) {

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 */

src/modules/commander/accelerometer_calibration.cpp

@@ -687,10 +687,9 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
 		}
 
 		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-		matrix::Quaternion<float> q(&att.q[0]);
-		matrix::Euler<float> euler(q);
-		roll_mean += euler(0);
-		pitch_mean += euler(1);
+		matrix::Eulerf euler = matrix::Quatf(att.q);
+		roll_mean += euler.phi();
+		pitch_mean += euler.theta();
 		counter++;
 	}
 

src/modules/commander/commander.cpp

@@ -1168,9 +1168,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
 	home.y = localPosition.y;
 	home.z = localPosition.z;
 
-	matrix::Quaternion<float> q(&attitude.q[0]);
-	matrix::Euler<float> euler(q);
-	home.yaw = euler(2);
+	matrix::Eulerf euler = matrix::Quatf(attitude.q);
+	home.yaw = euler.psi();
 
 	PX4_INFO("home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 

src/modules/local_position_estimator/sensors/flow.cpp

@@ -64,10 +64,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 		return -1;
 	}
 
-	matrix::Quaternion<float> q(&_sub_att.get().q[0]);
-	matrix::Euler<float> euler(q);
+	matrix::Eulerf euler = matrix::Quatf(_sub_att.get().q);
 
-	float d = agl() * cosf(euler(0)) * cosf(euler(1));
+	float d = agl() * cosf(euler.phi()) * cosf(euler.theta());
 
 	// optical flow in x, y axis
 	// TODO consider making flow scale a states of the kalman filter

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -500,8 +500,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			/* sensor combined */
 			orb_check(sensor_combined_sub, &updated);
 
-			matrix::Quaternion<float> q(&att.q[0]);
-			matrix::Dcm<float> R(q);
+			matrix::Dcmf R = matrix::Quatf(att.q);
 
 			if (updated) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
@@ -944,8 +943,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 		}
 
-		matrix::Quaternion<float> q(&att.q[0]);
-		matrix::Dcm<float> R(q);
+		matrix::Dcm<float> R = matrix::Quatf(att.q);
 
 		/* check for timeout on FLOW topic */
 		if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {