Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AHRS: perf improvement by caching declination vector

Browse Source
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; return 0.0;
} }
// get the earths magnetic field (only X and Y components needed) // update vector holding earths magnetic field (if required)
Vector3f mag_earth = Vector3f(cosf(_compass->get_declination()), if( _last_declination != _compass->get_declination() ) {
sinf(_compass->get_declination()), 0); _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 // calculate the error term in earth frame
Vector3f error = rb % mag_earth; Vector3f error = rb % _mag_earth;
return error.z; 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: public:
// Constructors // 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(); _dcm_matrix.identity();
@ -111,8 +114,9 @@ private:
float _ra_deltat; float _ra_deltat;
uint32_t _ra_sum_start; uint32_t _ra_sum_start;
// current drift error in earth frame // the earths magnetic field
Vector3f _drift_error_earth; float _last_declination;
Vector3f _mag_earth;
// whether we have GPS lock // whether we have GPS lock
bool _have_gps_lock; bool _have_gps_lock;