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

AP_NavEKF: use AHRS vehicle class for sideslip calculation 

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
Andrew Tridgell 2014-04-21 17:12:15 +10:00
parent 5acd17b843
commit 25667a11a0
2 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -786,7 +786,7 @@ void NavEKF::SelectBetaFusion()
// we are a fly forward vehicle type AND NOT using a full range of sensors with healthy position // we are a fly forward vehicle type AND NOT using a full range of sensors with healthy position
// AND NOT on the ground AND enough time has lapsed since our last fusion // AND NOT on the ground AND enough time has lapsed since our last fusion
// AND (we have not fused magnetometer data on this time step OR the immediate fusion flag is set) // AND (we have not fused magnetometer data on this time step OR the immediate fusion flag is set)
if (_ahrs->get_fly_forward() && !(use_compass() && useAirspeed() && posHealth) && !onGround && ((IMUmsec - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) { if (assume_zero_sideslip() && !(use_compass() && useAirspeed() && posHealth) && !onGround && ((IMUmsec - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) {
FuseSideslip(); FuseSideslip();
BETAmsecPrev = IMUmsec; BETAmsecPrev = IMUmsec;
} }
@ -2166,7 +2166,7 @@ void NavEKF::FuseMagnetometer()
magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f); magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
// Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle // Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle
// In this case we might as well try using the magnetometer, but with a reduced weighting // In this case we might as well try using the magnetometer, but with a reduced weighting
if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !_ahrs->get_fly_forward() && magTimeout)) if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !assume_zero_sideslip() && magTimeout))
{ {
// correct the state vector // correct the state vector
for (uint8_t j= 0; j<=indexLimit; j++) for (uint8_t j= 0; j<=indexLimit; j++)
@ -2745,7 +2745,7 @@ void NavEKF::OnGroundCheck()
onGround = true; onGround = true;
} }
// force a yaw alignment if exiting onGround without a compass or if compass is timed out and we are a fly forward vehicle // force a yaw alignment if exiting onGround without a compass or if compass is timed out and we are a fly forward vehicle
if (!onGround && prevOnGround && (!use_compass() || (magTimeout && _ahrs->get_fly_forward()))) { if (!onGround && prevOnGround && (!use_compass() || (magTimeout && assume_zero_sideslip()))) {
alignYawGPS(); alignYawGPS();
} }
// If we are flying a fly-forward type vehicle without an airspeed sensor and exiting onGround // If we are flying a fly-forward type vehicle without an airspeed sensor and exiting onGround
@ -2753,7 +2753,7 @@ void NavEKF::OnGroundCheck()
// wind speed is equal to the 6m/s. This prevents gains being too high at the start // wind speed is equal to the 6m/s. This prevents gains being too high at the start
// of flight if launching into a headwind until the first turn when the EKF can form a wind speed // of flight if launching into a headwind until the first turn when the EKF can form a wind speed
// estimate // estimate
if (!onGround && prevOnGround && !useAirspeed() && _ahrs->get_fly_forward()) { if (!onGround && prevOnGround && !useAirspeed() && assume_zero_sideslip()) {
setWindVelStates(); setWindVelStates();
} }
} }
@ -2850,7 +2850,7 @@ void NavEKF::CopyAndFixCovariances()
} }
// if we have a non-forward flight vehicle type and no airspeed sensor, we want the wind // if we have a non-forward flight vehicle type and no airspeed sensor, we want the wind
// states to remain zero and want to keep the old variances for these states // states to remain zero and want to keep the old variances for these states
else if (!useAirspeed() && !_ahrs->get_fly_forward()) { else if (!useAirspeed() && !assume_zero_sideslip()) {
// copy calculated variances we want to propagate // copy calculated variances we want to propagate
for (uint8_t i=0; i<=13; i++) { for (uint8_t i=0; i<=13; i++) {
P[i][i] = nextP[i][i]; P[i][i] = nextP[i][i];
@ -3332,4 +3332,15 @@ void NavEKF::decayGpsOffset()
} }
} }
/*
should we assume zero sideslip?
*/
bool NavEKF::assume_zero_sideslip(void) const
{
// we don't assume zero sideslip for ground vehicles as EKF could
// be quite sensitive to a rapid spin of the ground vehicle if
// traction is lost
return _ahrs->get_fly_forward() && _ahrs->get_vehicle_class() != AHRS_VEHICLE_GROUND;
}
#endif // HAL_CPU_CLASS #endif // HAL_CPU_CLASS

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -480,6 +480,9 @@ private:
// should we use the compass? // should we use the compass?
bool use_compass(void) const; bool use_compass(void) const;
// should we assume zero sideslip?
bool assume_zero_sideslip(void) const;
}; };
#if CONFIG_HAL_BOARD != HAL_BOARD_PX4 #if CONFIG_HAL_BOARD != HAL_BOARD_PX4