Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: fixed detection of airspeed sensor 

the get_airspeed() AHRS call can change as the user enables/disables
the airspeed sensor, plus it only gets enabled after the NavEKF
constructor runs.
This commit is contained in:
Andrew Tridgell 2014-02-19 09:19:46 +11:00
parent 1ade39977a
commit 869e41fd03
2 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -241,7 +241,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
hgtRate = 0.0f;
mag_state.q0 = 1;
mag_state.DCM.identity();
useAirspeed = _ahrs->get_airspeed();
}
bool NavEKF::healthy(void) const
@ -576,7 +575,7 @@ void NavEKF::SelectVelPosFusion()
// reset the counter used to schedule updates so that we always fuse data on the frame GPS data arrives
skipCounter = velPosFuseStepRatio;
// If a long time since last GPS update, then reset position and velocity and reset stored state history
if ((hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeUseTAS && useAirspeed) || (hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeNoTAS && !useAirspeed)) {
if ((hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeUseTAS && useAirspeed()) || (hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeNoTAS && !useAirspeed())) {
ResetPosition();
ResetVelocity();
StoreStatesReset();
@ -650,7 +649,7 @@ void NavEKF::SelectTasFusion()
{
readAirSpdData();
// Determine if data is waiting to be fused
tasDataWaiting = (statesInitialised && useAirspeed && !onGround && (tasDataWaiting || newDataTas));
tasDataWaiting = (statesInitialised && useAirspeed() && !onGround && (tasDataWaiting || newDataTas));
bool timeout = ((IMUmsec - TASmsecPrev) >= TASmsecMax);
// Fuse Airspeed Measurements - hold off if magnetometer fusion has been performed, unless maximum time interval exceeded
if (tasDataWaiting && (!magFusePerformed || timeout || fuseMeNow))
@ -1458,7 +1457,7 @@ void NavEKF::FuseVelPosNED()
// set the GPS data timeout depending on whether airspeed data is present
uint32_t gpsRetryTime;
if (useAirspeed) gpsRetryTime = _gpsRetryTimeUseTAS;
if (useAirspeed()) gpsRetryTime = _gpsRetryTimeUseTAS;
else gpsRetryTime = _gpsRetryTimeNoTAS;
// Form the observation vector
@ -2366,7 +2365,7 @@ void NavEKF::CopyAndFixCovariances()
}
// if we flying, but not using airspeed, we want all the off-diagonals for the wind
// states to remain zero and want to keep the old variances for these states
else if (!useAirspeed) {
else if (!useAirspeed()) {
// copy calculated variances we want to propagate
for (uint8_t i=0; i<=13; i++) {
P[i][i] = nextP[i][i];
@ -2657,4 +2656,12 @@ void NavEKF::ZeroVariables()
memset(&posNE[0], 0, sizeof(posNE));
}
bool NavEKF::useAirspeed(void) const
{
if (_ahrs->get_airspeed() == NULL) {
return false;
}
return _ahrs->get_airspeed()->use();
}
#endif // HAL_CPU_CLASS

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -256,6 +256,8 @@ private:
// reset the vertical position state using the last height measurement
void ResetHeight(void);
// return true if we should use the airspeed sensor
bool useAirspeed(void) const;
private:
// EKF Mavlink Tuneable Parameters
@ -330,7 +332,6 @@ private:
ftype dt; // time lapsed since the last covariance prediction (sec)
ftype hgtRate; // state for rate of change of height filter
bool onGround; // boolean true when the flight vehicle is on the ground (not flying)
bool useAirspeed; // boolean true if airspeed data is being used
const bool useCompass; // boolean true if magnetometer data is being used
Vector6 innovVelPos; // innovation output for a group of measurements
Vector6 varInnovVelPos; // innovation variance output for a group of measurements