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

AP_NavEKF : improved static and on-ground mode selection logic

This commit is contained in:
Paul Riseborough 2014-03-10 04:39:59 +11:00
parent 15a44571c2
commit b47a11edf6
2 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -541,14 +541,9 @@ void NavEKF::UpdateFilter()
// define rules used to set staticMode // define rules used to set staticMode
// staticMode enables ground operation without GPS by fusing zeros for position and height measurements // staticMode enables ground operation without GPS by fusing zeros for position and height measurements
// staticMode is always used when on-grond and magnetometer not used if (static_mode_demanded()) {
// if exisiting staticMode without a compass, a forced yaw alignment is performed
if (onGround && (static_mode_demanded() || !use_compass())) {
staticMode = true; staticMode = true;
} else { } else {
if (!use_compass() && staticMode) {
ForceYawAlignment();
}
staticMode = false; staticMode = false;
} }
@ -2598,14 +2593,18 @@ void NavEKF::OnGroundCheck()
uint8_t lowAirSpd = (!airspeed || !airspeed->use() || airspeed->get_airspeed() * airspeed->get_EAS2TAS() < 8.0f); uint8_t lowAirSpd = (!airspeed || !airspeed->use() || airspeed->get_airspeed() * airspeed->get_EAS2TAS() < 8.0f);
uint8_t lowGndSpd = (uint8_t)((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f); uint8_t lowGndSpd = (uint8_t)((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f);
uint8_t lowHgt = (uint8_t)(fabsf(hgtMea < 15.0f)); uint8_t lowHgt = (uint8_t)(fabsf(hgtMea < 15.0f));
// inhibit onGround mode if magnetomter calibration at low speed and height is enabled. // inhibit onGround mode if magnetomter calibration is enabled, movement is detected and static mode isn't demanded
// A static check is still applied to prevent excessive compass drift when sitting stationary if ((_magCal == 1) && (accNavMagHoriz > 0.5f) && !static_mode_demanded() && use_compass()) {
if ((_magCal == 1) && (accNavMagHoriz > 0.5f)) {
onGround = false; onGround = false;
} else { } else {
// Go with a majority vote from three criteria // Go with a majority vote from three criteria
onGround = ((lowAirSpd + lowGndSpd + lowHgt) >= 2); onGround = ((lowAirSpd + lowGndSpd + lowHgt) >= 2);
// force a yaw alignment if exiting onGround without a compass
if (!onGround && prevOnGround && !use_compass()) {
ForceYawAlignment();
}
} }
prevOnGround = onGround;
} }
void NavEKF::CovarianceInit(float roll, float pitch, float yaw) void NavEKF::CovarianceInit(float roll, float pitch, float yaw)

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -360,6 +360,7 @@ private:
ftype dt; // time lapsed since the last covariance prediction (sec) ftype dt; // time lapsed since the last covariance prediction (sec)
ftype hgtRate; // state for rate of change of height filter 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 onGround; // boolean true when the flight vehicle is on the ground (not flying)
bool prevOnGround; // value of onGround from previous update
Vector6 innovVelPos; // innovation output for a group of measurements Vector6 innovVelPos; // innovation output for a group of measurements
Vector6 varInnovVelPos; // innovation variance output for a group of measurements Vector6 varInnovVelPos; // innovation variance output for a group of measurements
bool fuseVelData; // this boolean causes the velNED measurements to be fused bool fuseVelData; // this boolean causes the velNED measurements to be fused