Skip to content

Commit

Permalink
GNSS yaw: unset yaw_align if fusion stops before takeoff
Browse files Browse the repository at this point in the history 
We do not want to rely on a potentially badly initialized heading as it
could lead to a flyaway directly after takeoff

Also remove the resetMagHeading() call from the GPS fusion control logic
as this is properly handled in mag_control.
  • Loading branch information
@bresch @priseborough
bresch authored and priseborough committed May 11, 2021
1 parent 7ee6970 commit a685987
Show file tree
Hide file tree
Showing 3 changed files with 69 additions and 21 deletions.
33 changes: 16 additions & 17 deletions EKF/control.cpp
View file
Expand Up @@ -524,27 +524,20 @@ void Ekf::controlGpsFusion()
const bool want_to_reset_mag_heading = !_control_status.flags.yaw_align ||
_control_status.flags.ev_yaw ||
_mag_inhibit_yaw_reset_req;
if (want_to_reset_mag_heading && canResetMagHeading()) {

if (want_to_reset_mag_heading) {
_mag_yaw_reset_req = true;
_control_status.flags.ev_yaw = false;
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
if (_mag_inhibit_yaw_reset_req) {
_mag_inhibit_yaw_reset_req = false;
// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
}
}

// If the heading is valid start using gps aiding
if (_control_status.flags.yaw_align) {
} else {
// If the heading is valid start using gps aiding
startGpsFusion();
}
}

} else if (!(_params.fusion_mode & MASK_USE_GPS)) {
_control_status.flags.gps = false;

} else if (_control_status.flags.gps
&& (!(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.yaw_align)) {
stopGpsFusion();
}

// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
Expand Down Expand Up @@ -725,14 +718,12 @@ void Ekf::controlGpsFusion()

} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
stopGpsFusion();
stopGpsYawFusion();
_warning_events.flags.gps_data_stopped = true;
ECL_WARN("GPS data stopped");
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
// Handle the case where we are fusing another position source along GPS,
// stop waiting for GPS after 1 s of lost signal
stopGpsFusion();
stopGpsYawFusion();
_warning_events.flags.gps_data_stopped_using_alternate = true;
ECL_WARN("GPS data stopped, using only EV, OF or air data" );
}
Expand Down Expand Up @@ -812,6 +803,14 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
// No yaw data in the message anymore. Stop until it comes back.
stopGpsYawFusion();
}

// Before takeoff, we do not want to continue to rely on the current heading
// if we had to stop the fusion
if (!_control_status.flags.in_air
&& !_control_status.flags.gps_yaw
&& _control_status_prev.flags.gps_yaw) {
_control_status.flags.yaw_align = false;
}
}

void Ekf::controlHeightSensorTimeouts()
Expand Down
38 changes: 34 additions & 4 deletions EKF/mag_control.cpp
View file
Expand Up @@ -67,12 +67,14 @@ void Ekf::controlMagFusion()
_num_bad_flight_yaw_events = 0;
}

if (_control_status.flags.mag_fault || !_control_status.flags.yaw_align) {
if (_control_status.flags.mag_fault || !_control_status.flags.tilt_align) {
stopMagFusion();
return;
}

_mag_yaw_reset_req |= otherHeadingSourcesHaveStopped();
_mag_yaw_reset_req |= !_control_status.flags.yaw_align;
_mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req;

if (noOtherYawAidingThanMag() && _mag_data_ready) {
if (_control_status.flags.in_air) {
Expand All @@ -84,6 +86,11 @@ void Ekf::controlMagFusion()
runOnGroundYawReset();
}

if (!_control_status.flags.yaw_align) {
// Having the yaw aligned is mandatory to continue
return;
}

// Determine if we should use simple magnetic heading fusion which works better when
// there are large external disturbances or the more accurate 3-axis fusion
switch (_params.mag_fusion_type) {
Expand Down Expand Up @@ -139,7 +146,18 @@ void Ekf::runOnGroundYawReset()
? resetMagHeading(_mag_lpf.getState())
: false;

_mag_yaw_reset_req = !has_realigned_yaw;
if (has_realigned_yaw) {
_mag_yaw_reset_req = false;
_control_status.flags.yaw_align = true;

// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
if (_mag_inhibit_yaw_reset_req) {
_mag_inhibit_yaw_reset_req = false;
// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
}
}
}
}

Expand All @@ -156,8 +174,20 @@ void Ekf::runInAirYawReset()
if (canRealignYawUsingGps()) { has_realigned_yaw = realignYawGPS(); }
else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); }

_mag_yaw_reset_req = !has_realigned_yaw;
_control_status.flags.mag_aligned_in_flight = has_realigned_yaw;
if (has_realigned_yaw) {
_mag_yaw_reset_req = false;
_control_status.flags.yaw_align = true;
_control_status.flags.mag_aligned_in_flight = true;

// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
if (_mag_inhibit_yaw_reset_req) {
_mag_inhibit_yaw_reset_req = false;
// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
}
}

}
}

Expand Down
19 changes: 19 additions & 0 deletions test/test_EKF_gps_yaw.cpp
View file
Expand Up @@ -239,3 +239,22 @@ TEST_F(EkfGpsHeadingTest, yaw_jump)
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
}

TEST_F(EkfGpsHeadingTest, stop_on_ground)
{
// GIVEN: the GPS yaw fusion activated and there is no mag data
_sensor_simulator._mag.stop();
float gps_heading = _ekf_wrapper.getYawAngle();
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(5);

// WHEN: the measurement stops
gps_heading = NAN;
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator.runSeconds(7.5);

// THEN: the fusion should stop and the GPS pos/vel aiding
// should stop as well because the yaw is not aligned anymore
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
}

0 comments on commit a685987

Please sign in to comment.