Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: use NavEKF for ground vector when available

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-02 14:25:41 +11:00
parent 2cd6efa23e
commit b39a5062e9
3 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_AHRS/AP_AHRS.h
View File

@ -210,7 +210,7 @@ public:
}
// return a ground vector estimate in meters/second, in North/East order
Vector2f groundspeed_vector(void);
virtual Vector2f groundspeed_vector(void);
// return ground speed estimate in meters/second. Used by ground vehicles.
float groundspeed(void) const {

11
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -178,4 +178,15 @@ bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc)
return false;
}
// EKF has a better ground speed vector estimate
Vector2f AP_AHRS_NavEKF::groundspeed_vector(void)
{
if (!ekf_started || !_ekf_use) {
return AP_AHRS_DCM::groundspeed_vector();
}
Vector3f vec;
EKF.getVelNED(vec);
return Vector2f(vec.x, vec.y);
}
#endif // AP_AHRS_NAVEKF_AVAILABLE

3
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -79,6 +79,9 @@ public:
// return secondary position solution if available
bool get_secondary_position(struct Location &loc);
// EKF has a better ground speed vector estimate
Vector2f groundspeed_vector(void);
private:
NavEKF EKF;
AP_Baro &_baro;