general fixes on VIO data access

ROMFS/px4fmu_common/init.d-posix/1013_iris_vision

@@ -11,4 +11,7 @@ if [ $AUTOCNF = yes ]
 then
 	param set EKF2_AID_MASK 8
 	param set EKF2_EV_DELAY 5
+
+	# LPE: Vision + baro
+	param set LPE_FUSION 132
 fi

msg/vehicle_odometry.msg

@@ -5,7 +5,7 @@ float32 x			# North position
 float32 y			# East position
 float32 z			# Down position
 
-# Orientation quaternion. NaN if invalid/unknown
+# Orientation quaternion. First value NaN if invalid/unknown
 float32[4] q			# Quaternion rotation from NED earth-fixed frame to XYZ body frame
 
 # Row-major representation of 6x6 pose cross-covariance matrix URT.

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -349,9 +349,10 @@ void AttitudeEstimatorQ::task_main()
 
 			if (orb_copy(ORB_ID(vehicle_visual_odometry), _vision_odom_sub, &vision) == PX4_OK) {
 				// validation check for vision attitude data
-				bool vision_att_valid = !PX4_ISFINITE(vision.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(vision.pose_covariance[15],
-							fmaxf(vision.pose_covariance[18],
-							      vision.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true;
+				bool vision_att_valid = PX4_ISFINITE(vision.q[0])
+							&& (PX4_ISFINITE(vision.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(vision.pose_covariance[15],
+									fmaxf(vision.pose_covariance[18],
+											vision.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true);
 
 				if (vision_att_valid) {
 					Dcmf Rvis = Quatf(vision.q);
@@ -379,9 +380,10 @@ void AttitudeEstimatorQ::task_main()
 
 			if (orb_copy(ORB_ID(vehicle_mocap_odometry), _mocap_odom_sub, &mocap) == PX4_OK) {
 				// validation check for mocap attitude data
-				bool mocap_att_valid = !PX4_ISFINITE(mocap.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(mocap.pose_covariance[15],
-						       fmaxf(mocap.pose_covariance[18],
-							     mocap.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true;
+				bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
+						       && (PX4_ISFINITE(mocap.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(mocap.pose_covariance[15],
+								       fmaxf(mocap.pose_covariance[18],
+										       mocap.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true);
 
 				if (mocap_att_valid) {
 					Dcmf Rmoc = Quatf(mocap.q);

src/modules/ekf2/ekf2_main.cpp

@@ -221,7 +221,7 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
 	// set pose/velocity as invalid if standard deviation is bigger than max_std_dev
 	// TODO: the user should be allowed to set these values by a parameter
 	static constexpr float ep_max_std_dev = 100.0f;	///< Maximum permissible standard deviation for estimated position
-	//static constexpr float eo_max_std_dev = 100.0f;	///< Maximum permissible standard deviation for estimated orientation
+	static constexpr float eo_max_std_dev = 100.0f;	///< Maximum permissible standard deviation for estimated orientation
 	//static constexpr float ev_max_std_dev = 100.0f;	///< Maximum permissible standard deviation for estimated velocity
 
 	// GPS blending and switching
@@ -1160,35 +1160,43 @@ void Ekf2::run()
 
 		if (visual_odometry_updated) {
 			// copy both attitude & position, we need both to fill a single ext_vision_message
-			vehicle_odometry_s ev_odom = {};
+			vehicle_odometry_s ev_odom;
 			orb_copy(ORB_ID(vehicle_visual_odometry), _ev_odom_sub, &ev_odom);
 
 			ext_vision_message ev_data;
-			ev_data.posNED(0) = ev_odom.x;
-			ev_data.posNED(1) = ev_odom.y;
-			ev_data.posNED(2) = ev_odom.z;
-			ev_data.quat = matrix::Quatf(ev_odom.q);
-
-			// position measurement error from parameters
-			if (!PX4_ISFINITE(ev_odom.pose_covariance[0])) {
-				ev_data.posErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[0], ev_odom.pose_covariance[6])));
-				ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[11]));
-			} else {
-				ev_data.posErr = _ev_pos_noise.get();
-				ev_data.hgtErr = _ev_pos_noise.get();
+
+			// check for valid position data
+			if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
+				ev_data.posNED(0) = ev_odom.x;
+				ev_data.posNED(1) = ev_odom.y;
+				ev_data.posNED(2) = ev_odom.z;
+
+				// position measurement error from parameters
+				if (PX4_ISFINITE(ev_odom.pose_covariance[0])) {
+					ev_data.posErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[0], ev_odom.pose_covariance[6])));
+					ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[11]));
+				} else {
+					ev_data.posErr = _ev_pos_noise.get();
+					ev_data.hgtErr = _ev_pos_noise.get();
+				}
 			}
 
-			// orientation measurement error from parameters
-			if (!PX4_ISFINITE(ev_odom.pose_covariance[0])) {
-				ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmaxf(ev_odom.pose_covariance[18],
-						       ev_odom.pose_covariance[20]))));
+			// check for valid orientation data
+			if (PX4_ISFINITE(ev_odom.q[0])) {
+				ev_data.quat = matrix::Quatf(ev_odom.q);
 
-			} else {
-				ev_data.angErr = _ev_ang_noise.get();
+				// orientation measurement error from parameters
+				if (PX4_ISFINITE(ev_odom.pose_covariance[15])) {
+					ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmaxf(ev_odom.pose_covariance[18],
+							       ev_odom.pose_covariance[20]))));
+
+				} else {
+					ev_data.angErr = _ev_ang_noise.get();
+				}
 			}
 
-			// only set data if all positions are valid
-			if (sqrtf(ev_odom.pose_covariance[0]) < ep_max_std_dev && sqrtf(ev_odom.pose_covariance[6]) < ep_max_std_dev) {
+			// only set data if all positions and orientation are valid
+			if (ev_data.posErr < ep_max_std_dev && ev_data.angErr < eo_max_std_dev) {
 				// use timestamp from external computer, clocks are synchronized when using MAVROS
 				_ekf.setExtVisionData(ev_odom.timestamp, &ev_data);
 			}

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -599,7 +599,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
 			_pub_lpos.get().z = xLP(X_z);	// down
 		}
 
-		_pub_gpos.get().yaw = matrix::Eulerf(_q).psi();
+		_pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
 
 		_pub_lpos.get().vx = xLP(X_vx);		// north
 		_pub_lpos.get().vy = xLP(X_vy);		// east
@@ -723,7 +723,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().vel_n = xLP(X_vx);
 		_pub_gpos.get().vel_e = xLP(X_vy);
 		_pub_gpos.get().vel_d = xLP(X_vz);
-		_pub_gpos.get().yaw = matrix::Eulerf(_q).psi();
+		_pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
 		_pub_gpos.get().eph = eph;
 		_pub_gpos.get().epv = epv;
 		_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
@@ -827,8 +827,7 @@ void BlockLocalPositionEstimator::updateSSParams()
 void BlockLocalPositionEstimator::predict()
 {
 	// get acceleration
-	_q = matrix::Quatf(&_sub_att.get().q[0]);
-	_R_att = matrix::Dcm<float>(_q);
+	_R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
 	Vector3f a(_sub_sensor.get().accelerometer_m_s2);
 	// note, bias is removed in dynamics function
 	_u = _R_att * a;

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -436,7 +436,6 @@ class BlockLocalPositionEstimator : public control::SuperBlock, public ModulePar
 	Vector<float, n_u>  _u;	// input vector
 	Matrix<float, n_x, n_x>  _P;	// state covariance matrix
 
-	matrix::Quatf _q;
 	matrix::Dcm<float> _R_att;
 
 	Matrix<float, n_x, n_x>  _A;	// dynamics matrix

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -60,7 +60,7 @@ void BlockLocalPositionEstimator::mocapInit()
 
 int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
 {
-	if (!PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) {
+	if (PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) {
 		// check if the mocap data is valid based on the covariances
 		_mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[0], _sub_mocap_odom.get().pose_covariance[6]));
 		_mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[11]);
@@ -75,7 +75,7 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
 
 	if (!_mocap_xy_valid || !_mocap_z_valid) {
 		_time_last_mocap = _sub_mocap_odom.get().timestamp;
-		return !OK;
+		return -1;
 
 	} else {
 		y.setZero();

src/modules/local_position_estimator/sensors/vision.cpp

@@ -65,7 +65,7 @@ void BlockLocalPositionEstimator::visionInit()
 
 int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
 {
-	if (!PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) {
+	if (PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) {
 		// check if the vision data is valid based on the covariances
 		_vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[0], _sub_visual_odom.get().pose_covariance[6]));
 		_vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[11]);
@@ -80,7 +80,7 @@ int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
 
 	if (!_vision_xy_valid || !_vision_z_valid) {
 		_time_last_vision_p = _sub_visual_odom.get().timestamp;
-		return !OK;
+		return -1;
 
 	} else {
 		y.setZero();

src/modules/mavlink/mavlink_messages.cpp

@@ -2109,7 +2109,7 @@ class MavlinkStreamVisionPositionEstimate : public MavlinkStream
 
 	bool send(const hrt_abstime t)
 	{
-		vehicle_odometry_s vodom = {};
+		vehicle_odometry_s vodom;
 
 		if (_odom_sub->update(&_odom_time, &vodom)) {
 			mavlink_vision_position_estimate_t vmsg = {};

src/modules/simulator/simulator.h

@@ -344,8 +344,7 @@ class Simulator : public ModuleParams
 	// class methods
 	int publish_sensor_topics(mavlink_hil_sensor_t *imu);
 	int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
-	template<typename T>
-	int publish_odometry_topic(T *odom_mavlink);
+	int publish_odometry_topic(mavlink_message_t *odom_mavlink);
 	int publish_distance_topic(mavlink_distance_sensor_t *dist);
 
 #ifndef __PX4_QURT

src/modules/simulator/simulator_mavlink.cpp

@@ -1123,18 +1123,17 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
 	return OK;
 }
 
-template<typename T>
-int Simulator::publish_odometry_topic(T *msg)
+int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
 {
 	uint64_t timestamp = hrt_absolute_time();
 
-	struct vehicle_odometry_s odom = {};
+	struct vehicle_odometry_s odom;
 
 	odom.timestamp = timestamp;
 
-	if (msg->msgid == MAVLINK_MSG_ID_ODOMETRY) {
+	if (odom_mavlink->msgid == MAVLINK_MSG_ID_ODOMETRY) {
 		mavlink_odometry_t odom_msg;
-		mavlink_msg_odometry_decode(msg, &odom_msg);
+		mavlink_msg_odometry_decode(odom_mavlink, &odom_msg);
 
 		/* Dcm rotation matrix from body frame to local NED frame */
 		matrix::Dcm<float> Rbl;
@@ -1175,9 +1174,9 @@ int Simulator::publish_odometry_topic(T *msg)
 			odom.velocity_covariance[i] = odom_msg.twist_covariance[i];
 		}
 
-	} else if (msg->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
+	} else if (odom_mavlink->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
 		mavlink_vision_position_estimate_t ev;
-		mavlink_msg_vision_position_estimate_decode(msg, &ev);
+		mavlink_msg_vision_position_estimate_decode(odom_mavlink, &ev);
 		/* The position in the local NED frame */
 		odom.x = ev.x;
 		odom.y = ev.y;