adapted to new vehicle attitude message

msg/vehicle_attitude.msg

@@ -4,5 +4,6 @@ float32 rollspeed	# Angular velocity about body north axis (x) in rad/s
 float32 pitchspeed	# Angular velocity about body east axis (y) in rad/s
 float32 yawspeed	# Angular velocity about body down axis (z) in rad/s
 float32[4] q		# Quaternion (NED)
+bool q_valid		# Quaternion valid
 
 # TOPICS vehicle_attitude vehicle_attitude_groundtruth

src/drivers/bst/bst.cpp

@@ -49,6 +49,7 @@
 #include <math.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/battery_status.h>
+#include <mathlib/math/Quaternion.hpp>
 
 #define BST_DEVICE_PATH "/dev/bst0"
 
@@ -289,11 +290,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);
 			BSTPacket<BSTAttitude> bst_att = {};
 			bst_att.type = 0x1E;
-			bst_att.payload.roll = swap_int32(att.roll * 10000);
+			bst_att.payload.roll = swap_int32(euler(0) * 10000);
-			bst_att.payload.pitch = swap_int32(att.pitch * 10000);
+			bst_att.payload.pitch = swap_int32(euler(1) * 10000);
-			bst_att.payload.yaw = swap_int32(att.yaw * 10000);
+			bst_att.payload.yaw = swap_int32(euler(2) * 10000);
 			send_packet(bst_att);
 		}
 

src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -820,15 +820,15 @@ void AttitudePositionEstimatorEKF::initializeGPS()
 void AttitudePositionEstimatorEKF::publishAttitude()
 {
 	// Output results
-	math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+	matrix::Quaternion<float> q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
-	math::Matrix<3, 3> R = q.to_dcm();
+	//math::Matrix<3, 3> R = q.to_dcm();
-	math::Vector<3> euler = R.to_euler();
+	//math::Vector<3> euler = R.to_euler();
 
-	for (int i = 0; i < 3; i++) {
+	/*for (int i = 0; i < 3; i++) {
 		for (int j = 0; j < 3; j++) {
 			PX4_R(_att.R, i, j) = R(i, j);
 		}
-	}
+	}*/
 
 	_att.timestamp = _last_sensor_timestamp;
 	_att.q[0] = _ekf->states[0];
@@ -836,21 +836,21 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	_att.q[2] = _ekf->states[2];
 	_att.q[3] = _ekf->states[3];
 	_att.q_valid = true;
-	_att.R_valid = true;
+	//_att.R_valid = true;
 
-	_att.timestamp = _last_sensor_timestamp;
+	//_att.timestamp = _last_sensor_timestamp;
-	_att.roll = euler(0);
+	//_att.roll = euler(0);
-	_att.pitch = euler(1);
+	//_att.pitch = euler(1);
-	_att.yaw = euler(2);
+	//_att.yaw = euler(2);
 
 	_att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
 	_att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;
 	_att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt;
 
 	// gyro offsets
-	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
+	//_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
-	_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt;
+	//_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt;
-	_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt;
+	//_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt;
 
 	/* lazily publish the attitude only once available */
 	if (_att_pub != nullptr) {
@@ -973,7 +973,9 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
 	_local_pos.xy_global = _gps_initialized; //TODO: Handle optical flow mode here
 
 	_local_pos.z_global = false;
-	_local_pos.yaw = _att.yaw;
+	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);
 
 	if (!PX4_ISFINITE(_local_pos.x) ||
 	    !PX4_ISFINITE(_local_pos.y) ||

src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp

@@ -87,11 +87,13 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 
 	/* construct attitude setpoint rotation matrix */
 	math::Matrix<3, 3> R_sp;
-	R_sp.set(_v_att_sp->data().R_body);
+	matrix::Quaternion<float> q_sp(&_v_att_sp->data().q_d[0]);
+	R_sp.set(&q_sp._data[0][0]);
 
 	/* rotation matrix for current state */
 	math::Matrix<3, 3> R;
-	R.set(_v_att->data().R);
+	matrix::Quaternion<float> q(&_v_att->data().q[0]);
+	R.set(&q._data[0][0]);
 
 	/* all input data is ready, run controller itself */
 

src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp

@@ -250,7 +250,9 @@ MulticopterPositionControlMultiplatform::reset_alt_sp()
 
 		//XXX hack until #1741 is in/ported
 		/* reset yaw sp */
-		_att_sp_msg.data().yaw_body = _att->data().yaw;
+		matrix::Quaternion<float> q(&_att->data().q[0]);
+		matrix::Euler<float> euler(q);
+		_att_sp_msg.data().yaw_body = euler(2);
 
 		//XXX: port this once a mavlink like interface is available
 		// mavlink_log_info(&_mavlink_log_pub, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
@@ -582,6 +584,10 @@ void  MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
 	static bool was_armed = false;
 	static uint64_t t_prev = 0;
 
+	matrix::Quaternion<float> q(&_att->data().q[0]);
+	matrix::Euler<float> euler(q);
+	matrix::Dcm<float> R(q);
+
 	uint64_t t = get_time_micros();
 	float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.005f;
 	t_prev = t;
@@ -641,7 +647,7 @@ void  MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
 
 			_att_sp_msg.data().roll_body = 0.0f;
 			_att_sp_msg.data().pitch_body = 0.0f;
-			_att_sp_msg.data().yaw_body = _att->data().yaw;
+			_att_sp_msg.data().yaw_body = euler(2);
 			_att_sp_msg.data().thrust = 0.0f;
 
 			_att_sp_msg.data().timestamp = get_time_micros();
@@ -815,11 +821,11 @@ void  MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
 					/* thrust compensation for altitude only control mode */
 					float att_comp;
 
-					if (PX4_R(_att->data().R, 2, 2) > TILT_COS_MAX) {
+					if (R(2, 2) > TILT_COS_MAX) {
-						att_comp = 1.0f / PX4_R(_att->data().R, 2, 2);
+						att_comp = 1.0f / R(2, 2);
 
-					} else if (PX4_R(_att->data().R, 2, 2) > 0.0f) {
+					} else if (R(2, 2) > 0.0f) {
-						att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().R, 2, 2) + 1.0f;
+						att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * R(2, 2) + 1.0f;
 						saturation_z = true;
 
 					} else {
@@ -1005,7 +1011,7 @@ void  MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
 		/* reset yaw setpoint to current position if needed */
 		if (reset_yaw_sp) {
 			reset_yaw_sp = false;
-			_att_sp_msg.data().yaw_body = _att->data().yaw;
+			_att_sp_msg.data().yaw_body = euler(2);
 		}
 
 		/* do not move yaw while arming */
@@ -1014,13 +1020,13 @@ void  MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
 
 			_att_sp_msg.data().yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
 			_att_sp_msg.data().yaw_body = _wrap_pi(_att_sp_msg.data().yaw_body + _att_sp_msg.data().yaw_sp_move_rate * dt);
-			float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw);
+			float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - euler(2));
 
 			if (yaw_offs < - YAW_OFFSET_MAX) {
-				_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - YAW_OFFSET_MAX);
+				_att_sp_msg.data().yaw_body = _wrap_pi(euler(2) - YAW_OFFSET_MAX);
 
 			} else if (yaw_offs > YAW_OFFSET_MAX) {
-				_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + YAW_OFFSET_MAX);
+				_att_sp_msg.data().yaw_body = _wrap_pi(euler(2) + YAW_OFFSET_MAX);
 			}
 		}
 

src/examples/px4_simple_app/px4_simple_app.c

@@ -107,9 +107,13 @@ int px4_simple_app_main(int argc, char *argv[])
 					 (double)raw.accelerometer_m_s2[2]);
 
 				/* set att and publish this information for other apps */
-				att.roll = raw.accelerometer_m_s2[0];
+				//att.roll = raw.accelerometer_m_s2[0];
-				att.pitch = raw.accelerometer_m_s2[1];
+				//att.pitch = raw.accelerometer_m_s2[1];
-				att.yaw = raw.accelerometer_m_s2[2];
+				//att.yaw = raw.accelerometer_m_s2[2];
+				att.q[0] = raw.accelerometer_m_s2[0];
+				att.q[1] = raw.accelerometer_m_s2[1];
+				att.q[2] = raw.accelerometer_m_s2[2];
+
 				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
 			}
 

src/lib/terrain_estimation/terrain_estimator.cpp

@@ -65,9 +65,10 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
 			       const struct sensor_combined_s *sensor,
 			       const struct distance_sensor_s *distance)
 {
-	if (attitude->R_valid) {
+	if (attitude->q_valid) {
-		matrix::Matrix<float, 3, 3> R_att(attitude->R);
+		matrix::Quaternion<float> q(&attitude->q[0]);
-		matrix::Vector<float, 3> a(sensor->accelerometer_m_s2);
+		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;
 		_u_z = u(2) + 9.81f; // compensate for gravity
@@ -115,7 +116,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::Euler<float> euler(q);
 		float d = distance->current_distance;
 
 		matrix::Matrix<float, 1, n_x> C;
@@ -124,7 +126,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(attitude->roll) * cosf(attitude->pitch);
+		y(0) = d * cosf(euler(0)) * cosf(euler(1));
 
 		// residual
 		matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -468,26 +468,26 @@ void AttitudeEstimatorQ::task_main()
 		struct vehicle_attitude_s att = {};
 		att.timestamp = sensors.timestamp;
 
-		att.roll = euler(0);
+		//att.roll = euler(0);
-		att.pitch = euler(1);
+		//att.pitch = euler(1);
-		att.yaw = euler(2);
+		//att.yaw = euler(2);
 
 		att.rollspeed = _rates(0);
 		att.pitchspeed = _rates(1);
 		att.yawspeed = _rates(2);
 
-		for (int i = 0; i < 3; i++) {
+		//for (int i = 0; i < 3; i++) {
-			att.g_comp[i] = _accel(i) - _pos_acc(i);
+		//	att.g_comp[i] = _accel(i) - _pos_acc(i);
-		}
+		//}
 
-		/* copy offsets */
+		///* copy offsets */
-		memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));
+		//memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));
 
-		Matrix<3, 3> R = _q.to_dcm();
+		//Matrix<3, 3> R = _q.to_dcm();
 
-		/* copy rotation matrix */
+		///* copy rotation matrix */
-		memcpy(&att.R[0], R.data, sizeof(att.R));
+		//memcpy(&att.R[0], R.data, sizeof(att.R));
-		att.R_valid = true;
+		//att.R_valid = true;
 		memcpy(&att.q[0], _q.data, sizeof(att.q));
 		att.q_valid = true;
 

src/modules/commander/accelerometer_calibration.cpp

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

src/modules/commander/commander.cpp

@@ -78,6 +78,15 @@
 #include <systemlib/state_table.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/hysteresis/hysteresis.h>
+
+#include <sys/stat.h>
+#include <string.h>
+#include <math.h>
+#include <poll.h>
+#include <float.h>
+#include <matrix/math.hpp>
+
+#include <uORB/uORB.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
@@ -106,7 +115,6 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
-#include <uORB/uORB.h>
 
 typedef enum VEHICLE_MODE_FLAG
 {
@@ -1160,7 +1168,9 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
 	home.y = localPosition.y;
 	home.z = localPosition.z;
 
-	home.yaw = attitude.yaw;
+	matrix::Quaternion<float> q(&attitude.q[0]);
+	matrix::Euler<float> euler(q);
+	home.yaw = euler(2);
 
 	PX4_INFO("home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 

src/modules/ekf2/ekf2_main.cpp

@@ -721,10 +721,10 @@ void Ekf2::task_main()
 
 			// generate remaining vehicle attitude data
 			att.timestamp = hrt_absolute_time();
-			matrix::Euler<float> euler(q);
+			//matrix::Euler<float> euler(q);
-			att.roll = euler(0);
+			//att.roll = euler(0);
-			att.pitch = euler(1);
+			//att.pitch = euler(1);
-			att.yaw = euler(2);
+			//att.yaw = euler(2);
 
 			att.q[0] = q(0);
 			att.q[1] = q(1);
@@ -777,7 +777,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
-			lpos.yaw = att.yaw;
+			matrix::Euler<float> euler(q);
+			lpos.yaw = euler(2);
 
 			float terrain_vpos;
 			lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos);

src/modules/mavlink/mavlink_messages.cpp

@@ -841,11 +841,12 @@ protected:
 
 		if (_att_sub->update(&_att_time, &att)) {
 			mavlink_attitude_t msg;
-
+			matrix::Quaternion<float> q(&att.q[0]);
+			matrix::Euler<float> euler(q);
 			msg.time_boot_ms = att.timestamp / 1000;
-			msg.roll = att.roll;
+			msg.roll = euler(0);
-			msg.pitch = att.pitch;
+			msg.pitch = euler(1);
-			msg.yaw = att.yaw;
+			msg.yaw = euler(2);
 			msg.rollspeed = att.rollspeed;
 			msg.pitchspeed = att.pitchspeed;
 			msg.yawspeed = att.yawspeed;
@@ -1014,10 +1015,11 @@ protected:
 
 		if (updated) {
 			mavlink_vfr_hud_t msg;
-
+			matrix::Quaternion<float> q(&att.q[0]);
+			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(att.yaw) * M_RAD_TO_DEG_F;
+			msg.heading = _wrap_2pi(euler(2)) * 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

@@ -1920,8 +1920,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		math::Vector<3> euler = C_nb.to_euler();
 
 		hil_attitude.timestamp = timestamp;
-		memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R));
+		//memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R));
-		hil_attitude.R_valid = true;
+		//hil_attitude.R_valid = true;
 
 		hil_attitude.q[0] = q(0);
 		hil_attitude.q[1] = q(1);
@@ -1929,9 +1929,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_attitude.q[3] = q(3);
 		hil_attitude.q_valid = true;
 
-		hil_attitude.roll = euler(0);
+		//hil_attitude.roll = euler(0);
-		hil_attitude.pitch = euler(1);
+		//hil_attitude.pitch = euler(1);
-		hil_attitude.yaw = euler(2);
+		//hil_attitude.yaw = euler(2);
 		hil_attitude.rollspeed = hil_state.rollspeed;
 		hil_attitude.pitchspeed = hil_state.pitchspeed;
 		hil_attitude.yawspeed = hil_state.yawspeed;
@@ -1948,7 +1948,8 @@ 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::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);
@@ -1956,7 +1957,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 = hil_attitude.yaw;
+		hil_global_pos.yaw = euler(1);
 		hil_global_pos.eph = 2.0f;
 		hil_global_pos.epv = 4.0f;
 
@@ -1997,7 +1998,9 @@ 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;
-		hil_local_pos.yaw = hil_attitude.yaw;
+		matrix::Quaternion<float> q(&hil_attitude.q[0]);
+		matrix::Euler<float> euler(q);
+		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

@@ -500,11 +500,14 @@ 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);
+
 			if (updated) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
 				if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {
-					if (att.R_valid) {
+					if (att.q_valid) {
 						/* correct accel bias */
 						sensor.accelerometer_m_s2[0] -= acc_bias[0];
 						sensor.accelerometer_m_s2[1] -= acc_bias[1];
@@ -515,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							acc[i] = 0.0f;
 
 							for (int j = 0; j < 3; j++) {
-								acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
+								acc[i] += R(i, j) * sensor.accelerometer_m_s2[j];
 							}
 						}
 
@@ -548,9 +551,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			if (updated) { //check if altitude estimation for lidar is enabled and new sensor data
 
-				if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance
+				if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance
-				    && lidar.current_distance < lidar.max_distance
+			    		&& (R(2, 2) > 0.7f)) {
-				    && (PX4_R(att.R, 2, 2) > 0.7f)) {
 
 					if (!use_lidar_prev && use_lidar) {
 						lidar_first = true;
@@ -559,7 +561,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					use_lidar_prev = use_lidar;
 
 					lidar_time = t;
-					dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance
+					dist_ground = lidar.current_distance * R(2, 2); //vertical distance
 
 					if (lidar_first) {
 						lidar_first = false;
@@ -602,7 +604,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				float flow_q = flow.quality / 255.0f;
 				float dist_bottom = lidar.current_distance;
 
-				if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {
+				if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && R(2, 2) > 0.7f) {
 					/* distance to surface */
 					//float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar
 					float flow_dist = dist_bottom; //use this if using lidar
@@ -612,7 +614,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					float body_v_est[2] = { 0.0f, 0.0f };
 
 					for (int i = 0; i < 2; i++) {
-						body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
+						body_v_est[i] = R( 0, i) * x_est[1] + R(1, i) * y_est[1] + R(2, i) * z_est[1];
 					}
 
 					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
@@ -706,7 +708,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					/* project measurements vector to NED basis, skip Z component */
 					for (int i = 0; i < 2; i++) {
 						for (int j = 0; j < 3; j++) {
-							flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
+							flow_v[i] += R(i, j) * flow_m[j];
 						}
 					}
 
@@ -715,7 +717,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					corr_flow[1] = flow_v[1] - y_est[1];
 					/* adjust correction weight */
 					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
-					w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
+					w_flow = R(2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
 
 
 					/* if flow is not accurate, reduce weight for it */
@@ -946,6 +948,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 		}
 
+		matrix::Quaternion<float> q(&att.q[0]);
+		matrix::Dcm<float> R(q);
+
 		/* check for timeout on FLOW topic */
 		if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {
 			flow_valid = false;
@@ -1110,7 +1115,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			float c = 0.0f;
 
 			for (int j = 0; j < 3; j++) {
-				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
+				c += R(j, i) * accel_bias_corr[j];
 			}
 
 			if (PX4_ISFINITE(c)) {
@@ -1140,7 +1145,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			float c = 0.0f;
 
 			for (int j = 0; j < 3; j++) {
-				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
+				c += R(j, i) * accel_bias_corr[j];
 			}
 
 			if (PX4_ISFINITE(c)) {
@@ -1311,7 +1316,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			est_buf[buf_ptr][2][1] = z_est[1];
 
 			/* push current rotation matrix to buffer */
-			memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
+			memcpy(R_buf[buf_ptr], &R._data[0][0], sizeof(R._data));
 
 			buf_ptr++;
 
@@ -1331,7 +1336,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];
-			local_pos.yaw = att.yaw;
+			matrix::Euler<float> euler(R);
+			local_pos.yaw = euler(0);
 			local_pos.dist_bottom_valid = dist_bottom_valid;
 			local_pos.eph = eph;
 			local_pos.epv = epv;

src/modules/sdlog2/sdlog2.c

@@ -2269,15 +2269,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_ATT.q_x = buf.att.q[1];
 			log_msg.body.log_ATT.q_y = buf.att.q[2];
 			log_msg.body.log_ATT.q_z = buf.att.q[3];
-			log_msg.body.log_ATT.roll = buf.att.roll;
+			log_msg.body.log_ATT.roll = 0;
-			log_msg.body.log_ATT.pitch = buf.att.pitch;
+			log_msg.body.log_ATT.pitch = 0;
-			log_msg.body.log_ATT.yaw = buf.att.yaw;
+			log_msg.body.log_ATT.yaw = 0;
 			log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
 			log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
 			log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
-			log_msg.body.log_ATT.gx = buf.att.g_comp[0];
+			log_msg.body.log_ATT.gx = 0;
-			log_msg.body.log_ATT.gy = buf.att.g_comp[1];
+			log_msg.body.log_ATT.gy = 0;
-			log_msg.body.log_ATT.gz = buf.att.g_comp[2];
+			log_msg.body.log_ATT.gz = 0;
 			LOGBUFFER_WRITE_AND_COUNT(ATT);
 		}
 

src/modules/vtol_att_control/tailsitter.cpp

@@ -131,6 +131,10 @@ 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::Euler<float> euler(q);
+	float pitch = euler(1);
+
 	if (!_attc->is_fixed_wing_requested()) {
 
 
@@ -157,7 +161,7 @@ void Tailsitter::update_vtol_state()
 		case TRANSITION_BACK:
 
 			// check if we have reached pitch angle to switch to MC mode
-			if (_v_att->pitch >= PITCH_TRANSITION_BACK) {
+			if (pitch >= PITCH_TRANSITION_BACK) {
 				_vtol_schedule.flight_mode = MC_MODE;
 			}
 
@@ -180,7 +184,7 @@ void Tailsitter::update_vtol_state()
 
 			// check if we have reached airspeed  and pitch angle to switch to TRANSITION P2 mode
 			if ((_airspeed->indicated_airspeed_m_s >= _params_tailsitter.airspeed_trans
-			     && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) {
+			     && pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) {
 				_vtol_schedule.flight_mode = FW_MODE;
 				//_vtol_schedule.transition_start = hrt_absolute_time();
 			}