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

EKF: Improve detection of indoor flight condition

Browse Source
This commit is contained in:
Paul Riseborough
2018-05-16 12:14:08 +10:00
committed by Lorenz Meier
parent 565f99254c
commit ea9e8246ed
3 changed files with 9 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+3 -3
View File
@@ -1400,13 +1400,13 @@ void Ekf::controlMagFusion()
_control_status.flags.mag_dec = false; _control_status.flags.mag_dec = false;
} }
// If optical flow is the only aiding source and GPS checks are failing, then assume that we are operating // If optical flow is enabled there is no other aiding active and GPS checks are failing, then assume that we are operating
// indoors and the magnetometer is unreliable. Becasue the optical flow sensor is body fixed, absolute yaw // indoors and the magnetometer is unreliable. Becasue the optical flow sensor is body fixed, absolute yaw
// wrt North is not required for navigation and it is safer not to use the magnetometer. // wrt North is not required for navigation and it is safer not to use the magnetometer.
if ((_control_status.flags.opt_flow || _mag_use_inhibit) if (((_params.fusion_mode & MASK_USE_OF) || _mag_use_inhibit)
&& !_control_status.flags.gps && !_control_status.flags.gps
&& !_control_status.flags.ev_pos && !_control_status.flags.ev_pos
&& (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6)) { && ((_time_last_imu - _last_gps_pass_us > (uint64_t)5e6) || !(_params.fusion_mode & MASK_USE_GPS))) {
_mag_use_inhibit = true; _mag_use_inhibit = true;
} else { } else {
_mag_use_inhibit = false; _mag_use_inhibit = false;
EKF/ekf.h
+1
View File
@@ -326,6 +326,7 @@ private:
bool _mag_use_inhibit{false}; ///< true when magnetomer use is being inhibited bool _mag_use_inhibit{false}; ///< true when magnetomer use is being inhibited
bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetomer inhibit has been active for long enough to require a yaw reset when conditons improve. bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetomer inhibit has been active for long enough to require a yaw reset when conditons improve.
float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad) float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad)
bool _vehicle_at_rest_prev{false}; ///< true when the vehicle was at rest the previous time the status was checked
float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix
EKF/mag_fusion.cpp
+5 -1
View File
@@ -624,11 +624,15 @@ void Ekf::fuseHeading()
// Vehicle is not at rest so fuse a zero innovation and record the // Vehicle is not at rest so fuse a zero innovation and record the
// predicted heading to use as an observation when movement ceases. // predicted heading to use as an observation when movement ceases.
_heading_innov = 0.0f; _heading_innov = 0.0f;
_last_static_yaw = predicted_hdg; _vehicle_at_rest_prev = false;
} else { } else {
// Vehicle is at rest so use the last moving prediciton as an observation // Vehicle is at rest so use the last moving prediciton as an observation
// to prevent the heading from drifting and to enable yaw gyro bias learning // to prevent the heading from drifting and to enable yaw gyro bias learning
// before takeoff. // before takeoff.
if (!_vehicle_at_rest_prev) {
_last_static_yaw = predicted_hdg;
_vehicle_at_rest_prev = true;
}
_heading_innov = predicted_hdg - _last_static_yaw; _heading_innov = predicted_hdg - _last_static_yaw;
R_YAW = 0.01f; R_YAW = 0.01f;
innov_gate = 5.0f; innov_gate = 5.0f;