Browse Source

AP_NavEKF: Add GPS glitching monitor to EKF status report

master
Paul Riseborough 10 years ago committed by Randy Mackay
parent
commit
d48d4ac950
  1. 1
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 1
      libraries/AP_NavEKF/AP_Nav_Common.h

1
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -4972,6 +4972,7 @@ void NavEKF::getFilterStatus(nav_filter_status &status) const @@ -4972,6 +4972,7 @@ void NavEKF::getFilterStatus(nav_filter_status &status) const
status.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
status.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
status.flags.using_gps = (imuSampleTime_ms - lastPosPassTime) < 4000;
status.flags.gps_glitching = !gpsAccuracyGood; // The GPS is glitching
}
// send an EKF_STATUS message to GCS

1
libraries/AP_NavEKF/AP_Nav_Common.h

@ -35,6 +35,7 @@ union nav_filter_status { @@ -35,6 +35,7 @@ union nav_filter_status {
uint16_t takeoff : 1; // 11 - true if filter is compensating for baro errors during takeoff
uint16_t touchdown : 1; // 12 - true if filter is compensating for baro errors during touchdown
uint16_t using_gps : 1; // 13 - true if we are using GPS position
uint16_t gps_glitching : 1; // 14 - true if the the GPS is glitching
} flags;
uint16_t value;
};

Loading…
Cancel
Save