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

Merge pull request #255 from PX4/pr-ekfBugFix

Browse Source 
EKF: fix bugs preventing preflight checks passing
This commit is contained in:
Paul Riseborough 2017-03-21 07:27:43 +10:00 committed by GitHub
commit 6d9b556b04
3 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/covariance.cpp
View File

@ -737,7 +737,7 @@ void Ekf::fixCovarianceErrors()
// record the pass/fail
if (!bad_acc_bias) {
_fault_status.flags.bad_acc_bias = true;
_fault_status.flags.bad_acc_bias = false;
_time_acc_bias_check = _time_last_imu;
} else {
_fault_status.flags.bad_acc_bias = true;

2
EKF/estimator_interface.cpp
View File

@ -435,7 +435,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
_time_last_range = 0;
_time_last_airspeed = 0;
_time_last_optflow = 0;
memset(&_fault_status.flags, 0, sizeof(_fault_status.flags));
_fault_status.value = 0;
_time_last_ext_vision = 0;
return true;
}

11
EKF/gps_checks.cpp
View File

@ -57,11 +57,12 @@
bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
{
// If we have defined the WGS-84 position of the NED origin, run gps quality checks until they pass, then define the origins WGS-84 position using the last GPS fix
if (!_NED_origin_initialised) {
// we have good GPS data so can now set the origin's WGS-84 position
if (gps_is_good(gps) && !_NED_origin_initialised) {
// Set the origin's WGS-84 position to the last gps fix
// Run GPS checks whenever the WGS-84 origin is not set or the vehicle is not using GPS
// Also run checks if the vehicle is on-ground as the check data can be used by vehicle pre-flight checks
if (!_control_status.flags.in_air || !_NED_origin_initialised || !_control_status.flags.gps) {
bool gps_checks_pass = gps_is_good(gps);
if (!_NED_origin_initialised && gps_checks_pass) {
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
double lat = gps->lat / 1.0e7;
double lon = gps->lon / 1.0e7;
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);