Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-25 10:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: use new HIL compass API

Browse Source
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 // these methods are static as they are called
// from the timer // from the timer
static void _update_barometer(float height); 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 { struct gps_data {
double latitude; double latitude;
@ -89,7 +89,6 @@ private:
static float _gyro_drift(void); static float _gyro_drift(void);
static float _rand_float(void); static float _rand_float(void);
static Vector3f _rand_vec3f(void); static Vector3f _rand_vec3f(void);
static Vector3f _heading_to_mag(float roll, float pitch, float yaw);
// signal handlers // signal handlers
static void _sig_fpe(int signum); 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; 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 setup the compass with new input
all inputs are in degrees 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) { if (_compass == NULL) {
// no compass in this sketch // no compass in this sketch
return; return;
} }
Vector3f m = _heading_to_mag(ToRad(roll), _compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg));
ToRad(pitch), Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
ToRad(yaw)); _compass->mag_x += noise.x;
_compass->setHIL(m.x, m.y, m.z); _compass->mag_y += noise.y;
_compass->mag_z += noise.z;
} }
#endif #endif