diff --git a/EKF/control.cpp b/EKF/control.cpp index 59d186e389..8dbe5028a9 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -48,9 +48,6 @@ void Ekf::controlFusionModes() // Store the status to enable change detection _control_status_prev.value = _control_status.value; - // Get the magnetic declination - calcMagDeclination(); - // monitor the tilt alignment if (!_control_status.flags.tilt_align) { // whilst we are aligning the tilt, monitor the variances diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 2c01bef64a..708e9e31d1 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -254,8 +254,6 @@ bool Ekf::initialiseFilter() _R_to_earth = quat_to_invrotmat(_state.quat_nominal); // calculate the initial magnetic field and yaw alignment - // Get the magnetic declination - calcMagDeclination(); _control_status.flags.yaw_align = resetMagHeading(_mag_filt_state, false, false); // initialise the state covariance matrix diff --git a/EKF/ekf.h b/EKF/ekf.h index 001b3ed6cd..1625df6db4 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -389,8 +389,6 @@ private: bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited Vector2f _flowRadXYcomp; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive - float _mag_declination{0.0f}; ///< magnetic declination used by reset and fusion functions (rad) - // output predictor states Vector3f _delta_angle_corr; ///< delta angle correction vector (rad) imuSample _imu_down_sampled{}; ///< down sampled imu data (sensor rate -> filter update rate) @@ -553,7 +551,7 @@ private: bool realignYawGPS(); // calculate the magnetic declination to be used by the alignment and fusion processing - void calcMagDeclination(); + float getMagDeclination(); // reset position states of the ekf (only horizontal position) bool resetPosition(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 9bb32e8f48..996ee9ee17 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -610,7 +610,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update // rotate the magnetometer measurements into earth frame using a zero yaw angle Vector3f mag_earth_pred = R_to_earth * mag_init; // the angle of the projection onto the horizontal gives the yaw angle - euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) { // we are operating without knowing the earth frame yaw angle @@ -668,7 +668,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update // rotate the magnetometer measurements into earth frame using a zero yaw angle Vector3f mag_earth_pred = R_to_earth * mag_init; // the angle of the projection onto the horizontal gives the yaw angle - euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) { // we are operating without knowing the earth frame yaw angle @@ -767,28 +767,28 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update } // Calculate the magnetic declination to be used by the alignment and fusion processing -void Ekf::calcMagDeclination() +float Ekf::getMagDeclination() { // set source of magnetic declination for internal use if (_control_status.flags.mag_align_complete) { // Use value consistent with earth field state - _mag_declination = atan2f(_state.mag_I(1), _state.mag_I(0)); + return atan2f(_state.mag_I(1), _state.mag_I(0)); } else if (_params.mag_declination_source & MASK_USE_GEO_DECL) { // use parameter value until GPS is available, then use value returned by geo library if (_NED_origin_initialised) { - _mag_declination = _mag_declination_gps; - _mag_declination_to_save_deg = math::degrees(_mag_declination); + _mag_declination_to_save_deg = math::degrees(_mag_declination_gps); + return _mag_declination_gps; } else { - _mag_declination = math::radians(_params.mag_declination_deg); _mag_declination_to_save_deg = _params.mag_declination_deg; + return math::radians(_params.mag_declination_deg); } } else { // always use the parameter value - _mag_declination = math::radians(_params.mag_declination_deg); _mag_declination_to_save_deg = _params.mag_declination_deg; + return math::radians(_params.mag_declination_deg); } } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index cae1eddd81..871a26d6b2 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -491,7 +491,7 @@ void Ekf::fuseHeading() } // the angle of the projection onto the horizontal gives the yaw angle - measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); } else if (_control_status.flags.ev_yaw) { // calculate the yaw angle for a 321 sequence @@ -584,7 +584,7 @@ void Ekf::fuseHeading() } // the angle of the projection onto the horizontal gives the yaw angle - measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); } else if (_control_status.flags.ev_yaw) { // calculate the yaw angle for a 312 sequence @@ -873,7 +873,7 @@ void Ekf::fuseDeclination(float decl_sigma) Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); // calculate innovation and constrain - float innovation = atan2f(magE, magN) - _mag_declination; + float innovation = atan2f(magE, magN) - getMagDeclination(); innovation = math::constrain(innovation, -0.5f, 0.5f); // apply covariance correction via P_new = (I -K*H)*P @@ -963,8 +963,9 @@ void Ekf::limitDeclination() _state.mag_I(1) *= h_scaler; } else { // too small to scale radially so set to expected value - _state.mag_I(0) = 2.0f * h_field_min * cosf(_mag_declination); - _state.mag_I(1) = 2.0f * h_field_min * sinf(_mag_declination); + float mag_declination = getMagDeclination(); + _state.mag_I(0) = 2.0f * h_field_min * cosf(mag_declination); + _state.mag_I(1) = 2.0f * h_field_min * sinf(mag_declination); } h_field = h_field_min; }