Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: allow calling calculate_heading() for different instances

Browse Source
This commit is contained in:
Gustavo Jose de Sousa 2016-01-22 15:30:59 -02:00 committed by Lucas De Marchi
parent f4ccf94dfc
commit 0efbe8c80c
2 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Compass/Compass.cpp
View File

@ -623,12 +623,12 @@ Compass::get_declination() const
calculate a compass heading given the attitude from DCM and the mag vector
*/
float
Compass::calculate_heading(const Matrix3f &dcm_matrix) const
Compass::calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const
{
float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);
// Tilt compensated magnetic field Y component:
const Vector3f &field = get_field();
const Vector3f &field = get_field(i);
float headY = field.y * dcm_matrix.c.z - field.z * dcm_matrix.c.y;

5
libraries/AP_Compass/Compass.h
View File

@ -82,7 +82,10 @@ public:
///
/// @returns heading in radians
///
float calculate_heading(const Matrix3f &dcm_matrix) const;
float calculate_heading(const Matrix3f &dcm_matrix) const {
return calculate_heading(dcm_matrix, get_primary());
}
float calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const;
/// Sets offset x/y/z values.
///