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

AHRS: perf improvement by caching declination vector

This commit is contained in:
Randy Mackay 2013-05-05 00:25:59 +09:00 committed by Andrew Tridgell
parent 943a1d8c8d
commit ee2daf25b6
2 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -244,12 +244,16 @@ AP_AHRS_DCM::yaw_error_compass(void)
return 0.0;
}
// get the earths magnetic field (only X and Y components needed)
Vector3f mag_earth = Vector3f(cosf(_compass->get_declination()),
sinf(_compass->get_declination()), 0);
// update vector holding earths magnetic field (if required)
if( _last_declination != _compass->get_declination() ) {
_last_declination = _compass->get_declination();
_mag_earth.x = cosf(_last_declination);
_mag_earth.y = sinf(_last_declination);
_mag_earth.z = 0;
}
// calculate the error term in earth frame
Vector3f error = rb % mag_earth;
Vector3f error = rb % _mag_earth;
return error.z;
}

10
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -14,7 +14,10 @@ class AP_AHRS_DCM : public AP_AHRS
{
public:
// Constructors
AP_AHRS_DCM(AP_InertialSensor *ins, GPS *&gps) : AP_AHRS(ins, gps)
AP_AHRS_DCM(AP_InertialSensor *ins, GPS *&gps) :
AP_AHRS(ins, gps),
_last_declination(0),
_mag_earth(1,0,0)
{
_dcm_matrix.identity();
@ -111,8 +114,9 @@ private:
float _ra_deltat;
uint32_t _ra_sum_start;
// current drift error in earth frame
Vector3f _drift_error_earth;
// the earths magnetic field
float _last_declination;
Vector3f _mag_earth;
// whether we have GPS lock
bool _have_gps_lock;