Make mag_innov/-var a Matrix::Vector3f

EKF/ekf.h

@@ -412,8 +412,8 @@ private:
 	float _heading_innov{0.0f};	///< heading measurement innovation (rad)
 	float _heading_innov_var{0.0f};	///< heading measurement innovation variance (rad**2)
 
-	float _mag_innov[3] {};		///< earth magnetic field innovations (Gauss)
+	Vector3f _mag_innov;		///< earth magnetic field innovations (Gauss)
-	float _mag_innov_var[3] {};	///< earth magnetic field innovation variance (Gauss**2)
+	Vector3f _mag_innov_var;	///< earth magnetic field innovation variance (Gauss**2)
 
 	float _drag_innov[2] {};	///< multirotor drag measurement innovation (m/sec**2)
 	float _drag_innov_var[2] {};	///< multirotor drag measurement innovation variance ((m/sec**2)**2)

EKF/ekf_helper.cpp

@@ -776,17 +776,17 @@ void Ekf::getHeadingInnovRatio(float &heading_innov_ratio) const
 
 void Ekf::getMagInnov(float mag_innov[3]) const
 {
-	memcpy(mag_innov, _mag_innov, sizeof(_mag_innov));
+	_mag_innov.copyTo(mag_innov);
 }
 
 void Ekf::getMagInnovVar(float mag_innov_var[3]) const
 {
-	memcpy(mag_innov_var, _mag_innov_var, sizeof(_mag_innov_var));
+	_mag_innov_var.copyTo(mag_innov_var);
 }
 
 void Ekf::getMagInnovRatio(float &mag_innov_ratio) const
 {
-	mag_innov_ratio = math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2]);
+	mag_innov_ratio = _mag_test_ratio.max();
 }
 
 void Ekf::getDragInnov(float drag_innov[2]) const
@@ -1105,7 +1105,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	// return the integer bitmask containing the consistency check pass/fail status
 	status = _innov_check_fail_status.value;
 	// return the largest magnetometer innovation test ratio
-	mag = sqrtf(math::max(_yaw_test_ratio, math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2])));
+	mag = sqrtf(math::max(_yaw_test_ratio,_mag_test_ratio.max()));
 	// return the largest velocity innovation test ratio
 	vel = math::max(sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1))),
 			sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1))));
@@ -1139,7 +1139,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
 	bool gps_vel_innov_bad = (_gps_vel_test_ratio(0) > 1.0f) || (_gps_vel_test_ratio(1) > 1.0f);
 	bool gps_pos_innov_bad = (_gps_pos_test_ratio(0) > 1.0f);
-	bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f);
+	bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f);
 	soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
 	soln_status.flags.accel_error = _bad_vert_accel_detected;
 	*status = soln_status.value;

EKF/estimator_interface.h

@@ -479,7 +479,7 @@ protected:
 
 	// innovation consistency check monitoring ratios
 	float _yaw_test_ratio{};		// yaw innovation consistency check ratio
-	float _mag_test_ratio[3] {};		// magnetometer XYZ innovation consistency check ratios
+	Vector3f _mag_test_ratio;		// magnetometer XYZ innovation consistency check ratios
 	Vector2f _gps_vel_test_ratio;	// GPS velocity innovation consistency check ratios
 	Vector2f _gps_pos_test_ratio;	// GPS position innovation consistency check ratios
 	Vector2f _ev_vel_test_ratio;		// EV velocity innovation consistency check ratios

EKF/gps_yaw_fusion.cpp

@@ -195,7 +195,7 @@ void Ekf::fuseGpsAntYaw()
 	_yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var);
 
 	// we are no longer using 3-axis fusion so set the reported test levels to zero
-	memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
+	_mag_test_ratio.setZero();
 
 	if (_yaw_test_ratio > 1.0f) {
 		_innov_check_fail_status.flags.reject_yaw = true;