attitude message cleanup: more cleanup

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

src/lib/terrain_estimation/terrain_estimator.cpp

@@ -66,8 +66,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
 			       const struct sensor_combined_s *sensor,
 			       const struct distance_sensor_s *distance)
 {
-	matrix::Quaternion<float> q(&attitude->q[0]);
+	matrix::Dcmf R_att = matrix::Quatf(attitude->q);
-	matrix::Dcm<float> R_att(q);
 	matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]);
 	matrix::Vector<float, 3> u;
 	u = R_att * a;
@@ -112,8 +111,8 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl
 	}
 
 	if (distance->timestamp > _time_last_distance) {
-		matrix::Quaternion<float> q(&attitude->q[0]);
+		matrix::Quatf q(attitude->q);
-		matrix::Euler<float> euler(q);
+		matrix::Eulerf euler(q);
 		float d = distance->current_distance;
 
 		matrix::Matrix<float, 1, n_x> C;
@@ -122,7 +121,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl
 		float R = 0.009f;
 
 		matrix::Vector<float, 1> y;
-		y(0) = d * cosf(euler(0)) * cosf(euler(1));
+		y(0) = d * cosf(euler.phi()) * cosf(euler.theta());
 
 		// residual
 		matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());

src/modules/ekf2/ekf2_main.cpp

@@ -772,8 +772,8 @@ void Ekf2::task_main()
 			lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
 
 			// The rotation of the tangent plane vs. geographical north
-			matrix::Euler<float> euler(q);
+			matrix::Eulerf euler(q);
-			lpos.yaw = euler(2);
+			lpos.yaw = euler.psi();
 
 			float terrain_vpos;
 			lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos);

src/modules/mavlink/mavlink_messages.cpp

@@ -841,12 +841,11 @@ protected:
 
 		if (_att_sub->update(&_att_time, &att)) {
 			mavlink_attitude_t msg;
-			matrix::Quaternion<float> q(&att.q[0]);
+			matrix::Eulerf euler = matrix::Quatf(att.q);
-			matrix::Euler<float> euler(q);
 			msg.time_boot_ms = att.timestamp / 1000;
-			msg.roll = euler(0);
+			msg.roll = euler.phi();
-			msg.pitch = euler(1);
+			msg.pitch = euler.theta();
-			msg.yaw = euler(2);
+			msg.yaw = euler.psi();
 			msg.rollspeed = att.rollspeed;
 			msg.pitchspeed = att.pitchspeed;
 			msg.yawspeed = att.yawspeed;
@@ -1015,11 +1014,10 @@ protected:
 
 		if (updated) {
 			mavlink_vfr_hud_t msg;
-			matrix::Quaternion<float> q(&att.q[0]);
+			matrix::Eulerf euler = matrix::Quatf(att.q);
-			matrix::Euler<float> euler(q);
 			msg.airspeed = airspeed.indicated_airspeed_m_s;
 			msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
-			msg.heading = _wrap_2pi(euler(2)) * M_RAD_TO_DEG_F;
+			msg.heading = _wrap_2pi(euler.psi()) * M_RAD_TO_DEG_F;
 			msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
 
 			if (_pos_time > 0) {

src/modules/mavlink/mavlink_receiver.cpp

@@ -1942,8 +1942,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	{
 		struct vehicle_global_position_s hil_global_pos;
 		memset(&hil_global_pos, 0, sizeof(hil_global_pos));
-		matrix::Quaternion<float> q(&hil_attitude.q[0]);
+		matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
-		matrix::Euler<float> euler(q);
 		hil_global_pos.timestamp = timestamp;
 		hil_global_pos.lat = hil_state.lat / ((double)1e7);
 		hil_global_pos.lon = hil_state.lon / ((double)1e7);
@@ -1951,7 +1950,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_global_pos.vel_n = hil_state.vx / 100.0f;
 		hil_global_pos.vel_e = hil_state.vy / 100.0f;
 		hil_global_pos.vel_d = hil_state.vz / 100.0f;
-		hil_global_pos.yaw = euler(2);
+		hil_global_pos.yaw = euler.psi();
 		hil_global_pos.eph = 2.0f;
 		hil_global_pos.epv = 4.0f;
 
@@ -1992,9 +1991,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_local_pos.vx = hil_state.vx / 100.0f;
 		hil_local_pos.vy = hil_state.vy / 100.0f;
 		hil_local_pos.vz = hil_state.vz / 100.0f;
-		matrix::Quaternion<float> q(&hil_attitude.q[0]);
+		matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
-		matrix::Euler<float> euler(q);
+		hil_local_pos.yaw = euler.psi();
-		hil_local_pos.yaw = euler(2);
 		hil_local_pos.xy_global = true;
 		hil_local_pos.z_global = true;
 

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -1330,8 +1330,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			local_pos.vy = y_est[1];
 			local_pos.z = z_est[0];
 			local_pos.vz = z_est[1];
-			matrix::Euler<float> euler(R);
+			matrix::Eulerf euler(R);
-			local_pos.yaw = euler(2);
+			local_pos.yaw = euler.psi();
 			local_pos.dist_bottom_valid = dist_bottom_valid;
 			local_pos.eph = eph;
 			local_pos.epv = epv;

src/modules/vtol_att_control/tailsitter.cpp

@@ -131,9 +131,8 @@ void Tailsitter::update_vtol_state()
 	 * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle.
 	*/
 
-	matrix::Quaternion<float> q(&_v_att->q[0]);
+	matrix::Eulerf euler = matrix::Quatf(_v_att->q);
-	matrix::Euler<float> euler(q);
+	float pitch = euler.theta();
-	float pitch = euler(1);
 
 	if (!_attc->is_fixed_wing_requested()) {