ekf2: tide new covariance methods

src/modules/ekf2/ekf2_main.cpp

@@ -1422,27 +1422,19 @@ void Ekf2::run()
 					lpos.hagl_max = INFINITY;
 				}
 
-				// get pose covariance
-				float pose_covariance[36];
-				_ekf.get_pose_covariances(pose_covariance);
-
+				// get pose covariance and copy the URT to the uORB message
 				float *pos_p = &odom.pose_covariance[0];
 
-				for (unsigned x = 0; x < 6; x++)
+				for (unsigned x = 0; x < 6; x++) {
 					for (unsigned y = x; y < 6; y++) {
-						*pos_p++ = pose_covariance[x * 6 + y];
+						*pos_p++ = _ekf.pose_covariances()(x, y);
 					}
+				}
 
-				// get linear velocity covariance
+				// get the velocity covariances
+				// note: unknown angular velocity covariance matrix
 				matrix::SquareMatrix<float, 6> twist_cov = matrix::eye<float, 6>();
-
-				// unknown angular velocity covariance matrix
-				float lv_cov[9];
-				_ekf.get_velocity_covariances(lv_cov);
-				matrix::SquareMatrix<float, 3> linvel_cov(lv_cov);
-
-				// parse twist covariance
-				twist_cov.set(linvel_cov, 0, 0);
+				twist_cov.set(_ekf.velocity_covariances(), 0, 0);
 				float *vel_p = &odom.velocity_covariance[0];
 
 				for (unsigned x = 0; x < 6; x++) {
@@ -1539,7 +1531,7 @@ void Ekf2::run()
 			status.timestamp = now;
 			_ekf.get_state_delayed(status.states);
 			status.n_states = 24;
-			_ekf.get_covariances_diagonal(status.covariances);
+			*status.covariances = (*_ekf.covariances_diagonal().data());
 			_ekf.get_gps_check_status(&status.gps_check_fail_flags);
 			// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
 			// the GPS Fix bit, which is always checked)