Remove q_valid flag from attitude topic

msg/vehicle_attitude.msg

@@ -1,8 +1,7 @@
-# This is similar to the mavlink message ATTITUDE, but for onboard use
+# This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use
 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/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -617,7 +617,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					matrix::Quaternion<float> q(Ro);
 
 					memcpy(&att.q[0],&q._data[0],sizeof(att.q));
-					att.q_valid = true;
 
 					if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1])
 						&& PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) {
@@ -625,7 +624,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
 
 					} else {
-						warnx("ERR: NaN estimate!");
+						PX4_ERR("ERR: NaN estimate!");
 					}
 
 					perf_end(ekf_loop_perf);

src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -826,7 +826,6 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	_att.q[1] = _ekf->states[1];
 	_att.q[2] = _ekf->states[2];
 	_att.q[3] = _ekf->states[3];
-	_att.q_valid = true;
 
 	_att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
 	_att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;

src/lib/terrain_estimation/terrain_estimator.cpp

@@ -37,6 +37,7 @@
  */
 
 #include "terrain_estimator.h"
+#include <geo/geo.h>
 
 #define DISTANCE_TIMEOUT 100000		// time in usec after which laser is considered dead
 
@@ -65,17 +66,12 @@ 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->q_valid) {
-		matrix::Quaternion<float> q(&attitude->q[0]);
-		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
-
-	} else {
-		_u_z = 0.0f;
-	}
+	matrix::Quaternion<float> q(&attitude->q[0]);
+	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) + CONSTANTS_ONE_G; // compensate for gravity
 
 	// dynamics matrix
 	matrix::Matrix<float, n_x, n_x> A;

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -473,7 +473,6 @@ void AttitudeEstimatorQ::task_main()
 		att.yawspeed = _rates(2);
 
 		memcpy(&att.q[0], _q.data, sizeof(att.q));
-		att.q_valid = true;
 
 		/* the instance count is not used here */
 		int att_inst;

src/modules/ekf2/ekf2_main.cpp

@@ -726,7 +726,6 @@ void Ekf2::task_main()
 			att.q[1] = q(1);
 			att.q[2] = q(2);
 			att.q[3] = q(3);
-			att.q_valid = true;
 
 			att.rollspeed = gyro_rad[0];
 			att.pitchspeed = gyro_rad[1];

src/modules/ekf2_replay/ekf2_replay_main.cpp

@@ -535,15 +535,15 @@ void Ekf2Replay::logIfUpdated()
 	log_message.body.att.q_x = att.q[1];
 	log_message.body.att.q_y = att.q[2];
 	log_message.body.att.q_z = att.q[3];
-	log_message.body.att.roll = att.roll;
-	log_message.body.att.pitch = att.pitch;
-	log_message.body.att.yaw = att.yaw;
+	log_message.body.att.roll = 0;
+	log_message.body.att.pitch = 0;
+	log_message.body.att.yaw = 0;
 	log_message.body.att.roll_rate = att.rollspeed;
 	log_message.body.att.pitch_rate = att.pitchspeed;
 	log_message.body.att.yaw_rate = att.yawspeed;
-	log_message.body.att.gx = att.g_comp[0];
-	log_message.body.att.gy = att.g_comp[1];
-	log_message.body.att.gz = att.g_comp[2];
+	log_message.body.att.gx = 0;
+	log_message.body.att.gy = 0;
+	log_message.body.att.gz = 0;
 
 	writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_ATT_MSG].length);
 

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -929,7 +929,7 @@ void BlockLocalPositionEstimator::predict()
 	// state or covariance
 	if (!_validXY && !_validZ) { return; }
 
-	if (integrate && _sub_att.get().q_valid) {
+	if (integrate) {
 		matrix::Quaternion<float> q(&_sub_att.get().q[0]);
 		_eul = matrix::Euler<float>(q);
 		_R_att = matrix::Dcm<float>(q);

src/modules/mavlink/mavlink_receiver.cpp

@@ -1925,7 +1925,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_attitude.q[1] = q(1);
 		hil_attitude.q[2] = q(2);
 		hil_attitude.q[3] = q(3);
-		hil_attitude.q_valid = true;
 
 		hil_attitude.rollspeed = hil_state.rollspeed;
 		hil_attitude.pitchspeed = hil_state.pitchspeed;

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -507,27 +507,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
 				if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {
-					if (att.q_valid) {
-						/* correct accel bias */
-						sensor.accelerometer_m_s2[0] -= acc_bias[0];
-						sensor.accelerometer_m_s2[1] -= acc_bias[1];
-						sensor.accelerometer_m_s2[2] -= acc_bias[2];
+					/* correct accel bias */
+					sensor.accelerometer_m_s2[0] -= acc_bias[0];
+					sensor.accelerometer_m_s2[1] -= acc_bias[1];
+					sensor.accelerometer_m_s2[2] -= acc_bias[2];
 
-						/* transform acceleration vector from body frame to NED frame */
-						for (int i = 0; i < 3; i++) {
-							acc[i] = 0.0f;
+					/* transform acceleration vector from body frame to NED frame */
+					for (int i = 0; i < 3; i++) {
+						acc[i] = 0.0f;
 
-							for (int j = 0; j < 3; j++) {
-								acc[i] += R(i, j) * sensor.accelerometer_m_s2[j];
-							}
+						for (int j = 0; j < 3; j++) {
+							acc[i] += R(i, j) * sensor.accelerometer_m_s2[j];
 						}
-
-						acc[2] += CONSTANTS_ONE_G;
-
-					} else {
-						memset(acc, 0, sizeof(acc));
 					}
 
+					acc[2] += CONSTANTS_ONE_G;
+
 					accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative;
 					accel_updates++;
 				}
@@ -551,8 +546,9 @@ 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 && lidar.current_distance < lidar.max_distance
-			    		&& (R(2, 2) > 0.7f)) {
+				if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance
+				    && lidar.current_distance < lidar.max_distance
+				    && (R(2, 2) > 0.7f)) {
 
 					if (!use_lidar_prev && use_lidar) {
 						lidar_first = true;
@@ -614,7 +610,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] = R( 0, i) * x_est[1] + R(1, i) * y_est[1] + 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 */

src/modules/simulator/simulator_mavlink.cpp

@@ -321,7 +321,6 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 			hil_attitude.q[1] = q(1);
 			hil_attitude.q[2] = q(2);
 			hil_attitude.q[3] = q(3);
-			hil_attitude.q_valid = true;
 
 			hil_attitude.rollspeed = hil_state.rollspeed;
 			hil_attitude.pitchspeed = hil_state.pitchspeed;