vehicle_attitude: add RPY variance

msg/VehicleAttitude.msg

@@ -1,12 +1,12 @@
-# This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use
-
 uint64 timestamp                # time since system start (microseconds)
-
 uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
 float32[4] q                    # Quaternion rotation from the FRD body frame to the NED earth frame
+
 float32[4] delta_q_reset        # Amount by which quaternion has changed during last reset
 uint8 quat_reset_counter        # Quaternion reset counter
 
+float32[3] variance             # roll/pitch/yaw variance (rad^2)
+
 # TOPICS vehicle_attitude vehicle_attitude_groundtruth external_ins_attitude
 # TOPICS estimator_attitude

msg/VehicleLocalPosition.msg

@@ -44,8 +44,6 @@ float32 delta_heading			# Heading delta caused by latest heading reset [rad]
 uint8 heading_reset_counter		# Index of latest heading reset
 bool heading_good_for_control
 
-float32 tilt_var
-
 # Position of reference point (local NED frame origin) in global (GPS / WGS84) frame
 bool xy_global				# true if position (x, y) has a valid global reference (ref_lat, ref_lon)
 bool z_global				# true if z has a valid global reference (ref_alt)

src/modules/ekf2/EKF2.cpp

@@ -920,6 +920,9 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 		_ekf.getQuaternion().copyTo(att.q);
 
 		_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
+
+		_ekf.getRotVarNed().copyTo(att.variance);
+
 		att.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_attitude_pub.publish(att);
 
@@ -1582,7 +1585,6 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	lpos.heading_var = _ekf.getYawVar();
 	lpos.delta_heading = Eulerf(delta_q_reset).psi();
 	lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
-	lpos.tilt_var = _ekf.getTiltVariance();
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive