mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-18 14:10:35 +08:00
ekf2: allow yaw estimator to perform yaw alignment
This can be useful if using a full EV + GNSS setup and you start indoors, then fly outside. Once GPS is good and the only missing requirement is yaw alignment the yaw estimator reset is performed and EV yaw will automatically stop itself.
This commit is contained in:
Daniel Agar
@@ -1135,8 +1135,6 @@ private:
|
||||
}
|
||||
|
||||
void stopGpsFusion();
|
||||
void stopGpsPosFusion();
|
||||
void stopGpsVelFusion();
|
||||
|
||||
void resetFakePosFusion();
|
||||
void stopFakePosFusion();
|
||||
|
||||
@@ -1160,37 +1160,29 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
|
||||
_time_last_heading_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
|
||||
// Resets the horizontal velocity and position to the default navigation sensor
|
||||
// Returns true if the reset was successful
|
||||
bool Ekf::resetYawToEKFGSF()
|
||||
{
|
||||
if (!isYawEmergencyEstimateAvailable()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// don't allow reset if there's just been a yaw reset
|
||||
const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
|
||||
const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
|
||||
|
||||
if (yaw_alignment_changed || quat_reset) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
|
||||
(double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
|
||||
|
||||
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
|
||||
|
||||
// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
|
||||
_flt_mag_align_start_time = _time_delayed_us;
|
||||
_control_status.flags.yaw_align = true;
|
||||
|
||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||
// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
|
||||
// and cause another navigation failure
|
||||
_control_status.flags.mag_fault = true;
|
||||
_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
|
||||
|
||||
} else if (_control_status.flags.gps_yaw) {
|
||||
_control_status.flags.gps_yaw_fault = true;
|
||||
_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
|
||||
|
||||
}
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
_inhibit_ev_yaw_use = true;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
_information_events.flags.yaw_aligned_to_imu_gps = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -119,35 +119,26 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
_aid_src_gnss_pos);
|
||||
_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start
|
||||
if (_control_status.flags.tilt_align && _NED_origin_initialised
|
||||
&& gps_checks_passing && !gps_checks_failing) {
|
||||
|
||||
if (!_control_status.flags.yaw_align) {
|
||||
if (_control_status.flags.ev_yaw && !_control_status.flags.yaw_align) {
|
||||
|
||||
// give mag a chance to start and yaw align if currently blocked by EV yaw
|
||||
const bool mag_enabled = (_params.mag_fusion_type <= MagFuseType::MAG_3D);
|
||||
const bool mag_available = (_mag_counter != 0);
|
||||
|
||||
if (mag_enabled && mag_available
|
||||
&& !_control_status.flags.mag_field_disturbed
|
||||
&& !_control_status.flags.mag_fault) {
|
||||
|
||||
stopEvYawFusion();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// Determine if we should use GPS aiding for velocity and horizontal position
|
||||
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
|
||||
const bool mandatory_conditions_passing = ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))
|
||||
&& _control_status.flags.tilt_align
|
||||
&& _control_status.flags.yaw_align
|
||||
&& _NED_origin_initialised;
|
||||
bool mandatory_conditions_passing = false;
|
||||
|
||||
if (((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))
|
||||
&& _control_status.flags.tilt_align
|
||||
&& _NED_origin_initialised
|
||||
) {
|
||||
// if GPS is otherwise ready to go other than yaw align
|
||||
if (!_control_status.flags.yaw_align && gps_checks_passing && !gps_checks_failing) {
|
||||
|
||||
if (resetYawToEKFGSF()) {
|
||||
ECL_INFO("GPS yaw aligned using IMU");
|
||||
}
|
||||
}
|
||||
|
||||
if (_control_status.flags.yaw_align) {
|
||||
mandatory_conditions_passing = true;
|
||||
}
|
||||
}
|
||||
|
||||
const bool continuing_conditions_passing = mandatory_conditions_passing && !gps_checks_failing;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing && gps_checks_passing;
|
||||
@@ -173,6 +164,31 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
*/
|
||||
if (resetYawToEKFGSF()) {
|
||||
ECL_WARN("GPS emergency yaw reset");
|
||||
|
||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||
// stop using the magnetometer in the main EKF otherwise it' fusion could drag the yaw around
|
||||
// and cause another navigation failure
|
||||
_control_status.flags.mag_fault = true;
|
||||
_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
_control_status.flags.gps_yaw_fault = true;
|
||||
_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
_inhibit_ev_yaw_use = true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
do_vel_pos_reset = true;
|
||||
}
|
||||
}
|
||||
@@ -203,16 +219,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// Do not use external vision for yaw if using GPS because yaw needs to be
|
||||
// defined relative to an NED reference frame
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
// Stop the vision for yaw fusion and do not allow it to start again
|
||||
stopEvYawFusion();
|
||||
_inhibit_ev_yaw_use = true;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
ECL_INFO("starting GPS fusion");
|
||||
_information_events.flags.starting_gps_fusion = true;
|
||||
|
||||
@@ -233,23 +239,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
|
||||
|
||||
_control_status.flags.gps = true;
|
||||
|
||||
} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) {
|
||||
// If no mag is used, align using the yaw estimator (if available)
|
||||
if (resetYawToEKFGSF()) {
|
||||
_information_events.flags.yaw_aligned_to_imu_gps = true;
|
||||
ECL_INFO("GPS yaw aligned using IMU, resetting vel and pos");
|
||||
|
||||
// reset velocity
|
||||
_information_events.flags.reset_vel_to_gps = true;
|
||||
resetVelocityTo(velocity, vel_obs_var);
|
||||
_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
|
||||
|
||||
// reset position
|
||||
_information_events.flags.reset_pos_to_gps = true;
|
||||
resetHorizontalPositionTo(position, pos_obs_var);
|
||||
_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -275,7 +264,7 @@ bool Ekf::shouldResetGpsFusion() const
|
||||
* with no aiding we need to do something
|
||||
*/
|
||||
bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max);
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max);
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
|
||||
@@ -379,9 +368,18 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
// Try to activate GPS yaw fusion
|
||||
startGpsYawFusion(gps_sample);
|
||||
if (resetYawToGps(gps_sample.yaw)) {
|
||||
|
||||
stopEvYawFusion();
|
||||
stopMagHdgFusion();
|
||||
stopMag3DFusion();
|
||||
|
||||
ECL_INFO("starting GPS yaw fusion");
|
||||
|
||||
_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
|
||||
_control_status.flags.gps_yaw = true;
|
||||
_control_status.flags.yaw_align = true;
|
||||
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
_nb_gps_yaw_reset_available = 1;
|
||||
}
|
||||
}
|
||||
@@ -393,72 +391,46 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
|
||||
// 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::startGpsYawFusion(const gpsSample &gps_sample)
|
||||
{
|
||||
if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
|
||||
ECL_INFO("starting GPS yaw fusion");
|
||||
_control_status.flags.yaw_align = true;
|
||||
_control_status.flags.mag_dec = false;
|
||||
|
||||
stopMagHdgFusion();
|
||||
stopMag3DFusion();
|
||||
_control_status.flags.gps_yaw = true;
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
void Ekf::stopGpsYawFusion()
|
||||
{
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
ECL_INFO("stopping GPS yaw fusion");
|
||||
|
||||
_control_status.flags.gps_yaw = false;
|
||||
resetEstimatorAidStatus(_aid_src_gnss_yaw);
|
||||
|
||||
// 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) {
|
||||
ECL_INFO("stopping GPS yaw fusion, clearing yaw alignment");
|
||||
_control_status.flags.yaw_align = false;
|
||||
|
||||
} else {
|
||||
ECL_INFO("stopping GPS yaw fusion");
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
}
|
||||
|
||||
void Ekf::stopGpsFusion()
|
||||
{
|
||||
if (_control_status.flags.gps) {
|
||||
stopGpsPosFusion();
|
||||
stopGpsVelFusion();
|
||||
ECL_INFO("stopping GPS position and velocity fusion");
|
||||
resetEstimatorAidStatus(_aid_src_gnss_pos);
|
||||
resetEstimatorAidStatus(_aid_src_gnss_vel);
|
||||
|
||||
_control_status.flags.gps = false;
|
||||
}
|
||||
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
stopGpsYawFusion();
|
||||
}
|
||||
stopGpsHgtFusion();
|
||||
stopGpsYawFusion();
|
||||
|
||||
// We do not need to know the true North anymore
|
||||
// EV yaw can start again
|
||||
_inhibit_ev_yaw_use = false;
|
||||
}
|
||||
|
||||
void Ekf::stopGpsPosFusion()
|
||||
{
|
||||
if (_control_status.flags.gps) {
|
||||
ECL_INFO("stopping GPS position fusion");
|
||||
_control_status.flags.gps = false;
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_gnss_pos);
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopGpsVelFusion()
|
||||
{
|
||||
ECL_INFO("stopping GPS velocity fusion");
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_gnss_vel);
|
||||
}
|
||||
|
||||
@@ -106,10 +106,10 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment)
|
||||
EXPECT_NEAR(yaw_est, yaw, tolerance_rad);
|
||||
EXPECT_LT(yaw_est_var, tolerance_rad);
|
||||
|
||||
// 2 resets: 1 after IMU+GNSS yaw alignment and 1 when starting GNSS aiding
|
||||
// 1 reset when starting GNSS aiding
|
||||
reset_logging_checker.capturePostResetState();
|
||||
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(2));
|
||||
EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(2));
|
||||
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
|
||||
EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1));
|
||||
|
||||
EXPECT_TRUE(_ekf->local_position_is_valid());
|
||||
EXPECT_TRUE(_ekf->global_position_is_valid());
|
||||
|
||||
Reference in New Issue
Block a user