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

7
libraries/AP_AHRS/AP_AHRS.h
View File

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