diff --git a/EKF/common.h b/EKF/common.h index 7be9a48de60a..1a8116cc8d05 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -448,7 +448,7 @@ union filter_control_status_u { uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data from a GPS receiver is being fused - uint32_t flt_mag_align_complete : 1; ///< 23 - true when the in-flight mag field alignment has been completed + uint32_t mag_align_complete : 1; ///< 23 - true when the in-flight mag field alignment has been completed } flags; uint32_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 0deb062ea15f..e04f66f3c983 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1349,7 +1349,7 @@ void Ekf::controlMagFusion() // Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude if (!_control_status.flags.in_air) { _last_on_ground_posD = _state.pos(2); - _control_status.flags.flt_mag_align_complete = false; + _control_status.flags.mag_align_complete = false; _num_bad_flight_yaw_events = 0; } @@ -1359,7 +1359,7 @@ void Ekf::controlMagFusion() // We need to reset the yaw angle after climbing away from the ground to enable // recovery from ground level magnetic interference. - if (!_control_status.flags.flt_mag_align_complete) { + if (!_control_status.flags.mag_align_complete) { // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. _mag_yaw_reset_req |= (_last_on_ground_posD - _state.pos(2)) > 1.5f; @@ -1368,7 +1368,7 @@ void Ekf::controlMagFusion() // perform a yaw reset if requested by other functions if (_mag_yaw_reset_req) { if (!_mag_use_inhibit ) { - _control_status.flags.flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag) && _control_status.flags.in_air; + _control_status.flags.mag_align_complete = resetMagHeading(_mag_sample_delayed.mag) && _control_status.flags.in_air; } _mag_yaw_reset_req = false; } @@ -1426,16 +1426,16 @@ void Ekf::controlMagFusion() // decide whether 3-axis magnetomer fusion can be used bool use_3D_fusion = _control_status.flags.tilt_align && // Use of 3D fusion requires valid tilt estimates _control_status.flags.in_air && // don't use when on the ground becasue of magnetic anomalies - (_control_status.flags.flt_mag_align_complete || height_achieved) && // once in-flight field alignment has been performed, ignore relative height + (_control_status.flags.mag_align_complete || height_achieved) && // once in-flight field alignment has been performed, ignore relative height ((_imu_sample_delayed.time_us - _time_last_movement) < 2 * 1000 * 1000); // Using 3-axis fusion for a minimum period after to allow for false negatives // perform switch-over if (use_3D_fusion) { if (!_control_status.flags.mag_3D) { - if (!_control_status.flags.flt_mag_align_complete) { + if (!_control_status.flags.mag_align_complete) { // If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { - _control_status.flags.flt_mag_align_complete = realignYawGPS(); + _control_status.flags.mag_align_complete = realignYawGPS(); if (_velpos_reset_request) { resetVelocity(); @@ -1444,10 +1444,10 @@ void Ekf::controlMagFusion() } } else { - _control_status.flags.flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag); + _control_status.flags.mag_align_complete = resetMagHeading(_mag_sample_delayed.mag); } - _control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.flt_mag_align_complete; + _control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.mag_align_complete; } else { // reset the mag field covariances @@ -1462,7 +1462,7 @@ void Ekf::controlMagFusion() } // only use one type of mag fusion at the same time - _control_status.flags.mag_3D = _control_status.flags.flt_mag_align_complete; + _control_status.flags.mag_3D = _control_status.flags.mag_align_complete; _control_status.flags.mag_hdg = !_control_status.flags.mag_3D; } else { @@ -1510,13 +1510,13 @@ void Ekf::controlMagFusion() } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) { // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states - if (!_control_status.flags.mag_3D || !_control_status.flags.flt_mag_align_complete) { - _control_status.flags.flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag); - _control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.flt_mag_align_complete; + if (!_control_status.flags.mag_3D || !_control_status.flags.mag_align_complete) { + _control_status.flags.mag_align_complete = resetMagHeading(_mag_sample_delayed.mag); + _control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.mag_align_complete; } // use 3-axis mag fusion if reset was successful - _control_status.flags.mag_3D = _control_status.flags.flt_mag_align_complete; + _control_status.flags.mag_3D = _control_status.flags.mag_align_complete; _control_status.flags.mag_hdg = false; } else { diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index c5a94df33f5a..6ffa53cd0ee2 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -429,7 +429,7 @@ bool Ekf::realignYawGPS() ECL_WARN("EKF bad yaw corrected using GPS course"); // declare the magnetomer as failed if a bad yaw has occurred more than once - if (_control_status.flags.flt_mag_align_complete && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) { + if (_control_status.flags.mag_align_complete && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) { ECL_WARN("EKF stopping magnetometer use"); _control_status.flags.mag_fault = true; } @@ -448,11 +448,11 @@ bool Ekf::realignYawGPS() Eulerf euler321(_state.quat_nominal); // apply yaw correction - if (!_control_status.flags.flt_mag_align_complete) { + if (!_control_status.flags.mag_align_complete) { // This is our first flight aligment so we can assume that the recent change in velocity has occurred due to a // forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error euler321(2) += courseYawError; - _control_status.flags.flt_mag_align_complete = true; + _control_status.flags.mag_align_complete = true; } else if (_control_status.flags.wind) { // we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is @@ -765,7 +765,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) void Ekf::calcMagDeclination() { // set source of magnetic declination for internal use - if (_control_status.flags.flt_mag_align_complete) { + 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));