fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-12 09:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Enable GPS flight without magnetometer (#770)

Browse Source 
* EKF: Enable GPS flight without magnetometer

Enables takeoff in a non-GPS flight mode with mag fusion type set to MAG_FUSE_TYPE_NONE. After sufficient movement the EKF will reset the yaw tot he EKF-GSF estimate. After that GPS fusion will commence.

* EKF: Fix unconstrained yaw and yaw variance growth when on ground

* EKF: Ensure first yaw alignment can't be blocked

* EKF: Increase yaw variance limit allowed for alignment

Flight test data indicates that an uncertainty value of 15 deg still provides a reliable yaw estimate and enables an earlier alignment/reset if required.

* EKF: Remove unexecutable code

* EKF: Restructure heading fusion

* EKF: parameterise quarter variance check and retune default value

* EKF: Pass by reference instead of pointer

* EKF: Clarify reset logic

* EKF: Remove incorrect setting of mag field alignment flag

* EKF: Non-functional tidy up

* EKF: Fix non-use of function argument

The updateQuaternion function was using the _heading_innovation class variable instead of setting it using the innovation argument.

* EKF: Fix undefined variable

* EKF: Use single precision atan2

* EKF: remove unnecessary timer reset and unwanted execution of reset function

* EKF: Don't declare a mag fault when non-use is user selected

Doing so produces unnecessary user alerts.
This commit is contained in:
Paul Riseborough
2020-03-30 20:05:38 +11:00
committed by GitHub
parent 3bd09fdcd3
commit 89b5c77d5d
8 changed files with 442 additions and 295 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+18 -8
View File
@@ -644,17 +644,27 @@ void Ekf::controlGpsFusion()
ECL_WARN_TIMESTAMPED("GPS data quality poor - stopping use");
}
// handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before
// we can start using GPS
bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset && isTimedOut(_ekfgsf_yaw_reset_time, 5000000);
if (align_yaw_using_gsf) {
if (resetYawToEKFGSF()) {
_ekfgsf_yaw_reset_time = _time_last_imu;
_do_ekfgsf_yaw_reset = false;
}
}
// handle the case when we now have GPS, but have not been using it for an extended period
if (_control_status.flags.gps) {
// We are relying on aiding to constrain drift so after a specified time
// with no aiding we need to do something
bool do_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
bool do_vel_pos_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
// We haven't had an absolute position fix for a longer time so need to do something
do_reset = do_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
do_vel_pos_reset = do_vel_pos_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
// A reset to the EKF-GSF estimate can be performed after a recent takeoff which will enable
// recovery from a bad magnetometer or field estimate.
@@ -667,14 +677,14 @@ void Ekf::controlGpsFusion()
_time_last_on_ground_us = _time_last_imu;
}
const bool recent_takeoff = _control_status.flags.in_air && !isTimedOut(_time_last_on_ground_us, 30000000);
const bool reset_yaw_to_EKFGSF = (do_reset || _do_emergency_yaw_reset || stopped_following_gps_velocity) &&
const bool do_yaw_vel_pos_reset = (do_vel_pos_reset || _do_ekfgsf_yaw_reset || stopped_following_gps_velocity) &&
recent_takeoff &&
isTimedOut(_emergency_yaw_reset_time, 5000000);
isTimedOut(_ekfgsf_yaw_reset_time, 5000000);
if (reset_yaw_to_EKFGSF) {
if (do_yaw_vel_pos_reset) {
if (resetYawToEKFGSF()) {
_emergency_yaw_reset_time = _time_last_imu;
_do_emergency_yaw_reset = false;
_ekfgsf_yaw_reset_time = _time_last_imu;
_do_ekfgsf_yaw_reset = false;
// Reset the timeout counters
_time_last_hor_pos_fuse = _time_last_imu;
@@ -683,7 +693,7 @@ void Ekf::controlGpsFusion()
_time_last_of_fuse = _time_last_imu;
}
} else if (do_reset) {
} else if (do_vel_pos_reset) {
// use GPS velocity data to check and correct yaw angle if a FW vehicle
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
// if flying a fixed wing aircraft, do a complete reset that includes yaw