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

AP_NavEKF: use ahrs->get_armed() for static mode demanded

This commit is contained in:
Andrew Tridgell 2014-02-19 10:52:57 +11:00
parent 3b1f9a4bbf
commit c9d0c1face
2 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -499,7 +499,7 @@ void NavEKF::UpdateFilter()
OnGroundCheck(); OnGroundCheck();
// Define rules used to set staticMode // Define rules used to set staticMode
if (onGround && !_ahrs->get_correct_centrifugal()) { if (onGround && static_mode_demanded()) {
staticMode = true; staticMode = true;
} else { } else {
staticMode = false; staticMode = false;
@ -2231,7 +2231,7 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const
void NavEKF::getAccelBias(Vector3f &accelBias) const void NavEKF::getAccelBias(Vector3f &accelBias) const
{ {
accelBias.x = staticMode? 1.0f : 0.0f; accelBias.x = staticMode? 1.0f : 0.0f;
accelBias.y = _ahrs->get_correct_centrifugal()? 0.0f : 1.0f; accelBias.y = static_mode_demanded()? 1.0f : 0.0f;
accelBias.z = states[13] / dtIMU; accelBias.z = states[13] / dtIMU;
} }
@ -2659,4 +2659,12 @@ bool NavEKF::useAirspeed(void) const
return _ahrs->get_airspeed()->use(); return _ahrs->get_airspeed()->use();
} }
/*
see if the vehicle code has demanded static mode
*/
bool NavEKF::static_mode_demanded(void) const
{
return !_ahrs->get_armed() || !_ahrs->get_correct_centrifugal();
}
#endif // HAL_CPU_CLASS #endif // HAL_CPU_CLASS

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -255,6 +255,9 @@ private:
// return true if we should use the airspeed sensor // return true if we should use the airspeed sensor
bool useAirspeed(void) const; bool useAirspeed(void) const;
// check if static mode has been demanded by vehicle code
bool static_mode_demanded(void) const;
private: private:
// EKF Mavlink Tuneable Parameters // EKF Mavlink Tuneable Parameters
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s