I’m trying to display the VIO quality in QGC. However, it seems like px4 doesn’t actually handle/calculate the quality for odometry data and just sets it to zero.
void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sample)
{
// generate vehicle odometry data
vehicle_odometry_s odom;
odom.timestamp_sample = imu_sample.time_us;
// position
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
_ekf.getPosition().copyTo(odom.position);
// orientation quaternion
_ekf.getQuaternion().copyTo(odom.q);
// velocity
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
_ekf.getVelocity().copyTo(odom.velocity);
// angular_velocity
const Vector3f rates{imu_sample.delta_ang / imu_sample.delta_ang_dt};
const Vector3f angular_velocity = rates - _ekf.getGyroBias();
angular_velocity.copyTo(odom.angular_velocity);
// velocity covariances
_ekf.getVelocityVariance().copyTo(odom.velocity_variance);
// position covariances
_ekf.getPositionVariance().copyTo(odom.position_variance);
// orientation covariance
_ekf.getQuaternionVariance().copyTo(odom.orientation_variance);
odom.reset_counter = _ekf.get_quat_reset_count()
+ _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count()
+ _ekf.get_posNE_reset_count() + _ekf.get_posD_reset_count();
odom.quality = 0;
// publish vehicle odometry data
odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_odometry_pub.publish(odom);
}
Is this how it’s supposed to work? How is the quality field of the ODOMETRY mavlink message supposed to be populated?