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

SITL: use AP_Declination code to get right compass dec for autotest

This commit is contained in:
Andrew Tridgell 2012-08-29 13:31:52 +10:00
parent 3403e1b78d
commit 39a4c6e861
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/Desktop/support/sitl_compass.cpp
View File

@ -13,6 +13,7 @@
#include <errno.h>
#include <AP_Math.h>
#include <AP_Compass.h>
#include <AP_Declination.h>
#include <SITL.h>
#include "desktop.h"
#include "util.h"
@ -21,9 +22,6 @@
#define MAG_OFS_Y 13.0
#define MAG_OFS_Z -18.0
// declination in Canberra (degrees)
#define MAG_DECLINATION 12.1
// inclination in Canberra (degrees)
#define MAG_INCLINATION -66
@ -43,11 +41,12 @@ static Vector3f heading_to_mag(float roll, float pitch, float yaw)
{
Vector3f Bearth, m;
Matrix3f R;
float declination = AP_Declination::get_declination(sitl.state.latitude, sitl.state.longitude);
// Bearth is the magnetic field in Canberra. We need to adjust
// it for inclination and declination
Bearth(MAG_FIELD_STRENGTH, 0, 0);
R.from_euler(0, -ToRad(MAG_INCLINATION), ToRad(MAG_DECLINATION));
R.from_euler(0, -ToRad(MAG_INCLINATION), ToRad(declination));
Bearth = R * Bearth;
// create a rotation matrix for the given attitude