aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: use vectors for ground vector complimentary filter

Browse Source
This commit is contained in:
Andrew Tridgell 2013-05-13 11:24:48 +10:00
parent 9f309a2aa6
commit a17b85c661
2 changed files with 9 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -184,15 +184,14 @@ Vector2f AP_AHRS::groundspeed_vector(void)
if (gotAirspeed) {
Vector3f wind = wind_estimate();
Vector2f wind2d = Vector2f(wind.x, wind.y);
Vector2f airspeed_vector = Vector2f(cos(yaw), sin(yaw)) * airspeed;
Vector2f airspeed_vector = Vector2f(cosf(yaw), sinf(yaw)) * airspeed;
gndVelADS = airspeed_vector - wind2d;
}
// Generate estimate of ground speed vector using GPS
if (gotGPS) {
Vector2f v;
float cog = radians(_gps->ground_course*0.01f);
gndVelGPS = Vector2f(cos(cog), sin(cog)) * _gps->ground_speed * 0.01f;
gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * _gps->ground_speed * 0.01f;
}
// If both ADS and GPS data is available, apply a complementary filter
if (gotAirspeed && gotGPS) {
@ -207,15 +206,13 @@ Vector2f AP_AHRS::groundspeed_vector(void)
// To-Do - set Tau as a function of GPS lag.
const float alpha = 1.0f - beta;
// Run LP filters
_xlp = beta*gndVelGPS.x + alpha*_xlp;
_ylp = beta*gndVelGPS.y + alpha*_ylp;
_lp = gndVelGPS * beta + _lp * alpha;
// Run HP filters
_xhp = gndVelADS.x - _lastGndVelADS.x + alpha*_xhp;
_yhp = gndVelADS.y - _lastGndVelADS.y + alpha*_yhp;
_hp = (gndVelADS - _lastGndVelADS) + _hp * alpha;
// Save the current ADS ground vector for the next time step
_lastGndVelADS = gndVelADS;
// Sum the HP and LP filter outputs
return Vector2f(_xhp + _xlp, _yhp + _ylp);
return _hp + _lp;
}
// Only ADS data is available return ADS estimate
if (gotAirspeed && !gotGPS) {

11
libraries/AP_AHRS/AP_AHRS.h
View File

@ -219,13 +219,10 @@ protected:
// Declare filter states for HPF and LPF used by complementary
// filter in AP_AHRS::groundspeed_vector
float _xlp; // x component low-pass filter
float _ylp; // y component low-pass filter
float _xhp; // x component high-pass filter
float _yhp; // y component high-pass filter
Vector2f _lastGndVelADS; // previous HPF input
};
Vector2f _lp; // ground vector low-pass filter
Vector2f _hp; // ground vector high-pass filter
Vector2f _lastGndVelADS; // previous HPF input
};
#include <AP_AHRS_DCM.h>
#include <AP_AHRS_MPU6000.h>