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

SITL: use new HIL compass API

This commit is contained in:
Andrew Tridgell 2013-05-02 15:01:47 +10:00
parent 0d97f417ec
commit a09c53a3ea
2 changed files with 7 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_HAL_AVR_SITL/SITL_State.h
View File

@ -54,7 +54,7 @@ private:
// these methods are static as they are called
// from the timer
static void _update_barometer(float height);
static void _update_compass(float roll, float pitch, float yaw);
static void _update_compass(float rollDeg, float pitchDeg, float yawDeg);
struct gps_data {
double latitude;
@ -89,7 +89,6 @@ private:
static float _gyro_drift(void);
static float _rand_float(void);
static Vector3f _rand_vec3f(void);
static Vector3f _heading_to_mag(float roll, float pitch, float yaw);
// signal handlers
static void _sig_fpe(int signum);

51
libraries/AP_HAL_AVR_SITL/sitl_compass.cpp
View File

@ -21,60 +21,21 @@
using namespace AVR_SITL;
#define MAG_OFS_X 5.0
#define MAG_OFS_Y 13.0
#define MAG_OFS_Z -18.0
// inclination in Canberra (degrees)
#define MAG_INCLINATION -66
// magnetic field strength in Canberra as observed
// using an APM1 with 5883L compass
#define MAG_FIELD_STRENGTH 818
/*
given a magnetic heading, and roll, pitch, yaw values,
calculate consistent magnetometer components
All angles are in radians
*/
Vector3f SITL_State::_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(declination));
Bearth = R * Bearth;
// create a rotation matrix for the given attitude
R.from_euler(roll, pitch, yaw);
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
return m + (_rand_vec3f() * _sitl->mag_noise);
}
/*
setup the compass with new input
all inputs are in degrees
*/
void SITL_State::_update_compass(float roll, float pitch, float yaw)
void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
{
if (_compass == NULL) {
// no compass in this sketch
return;
}
Vector3f m = _heading_to_mag(ToRad(roll),
ToRad(pitch),
ToRad(yaw));
_compass->setHIL(m.x, m.y, m.z);
_compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg));
Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
_compass->mag_x += noise.x;
_compass->mag_y += noise.y;
_compass->mag_z += noise.z;
}
#endif