From 90c3d230d97eabcf3971f53f02b255cac130de08 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 21 Mar 2012 09:05:58 +1100 Subject: [PATCH] SITL: improve the simulated compass this implements a much more accurate model of a compass using matrix rotations, instead of trying to calculate components directly --- libraries/Desktop/support/desktop.h | 2 +- libraries/Desktop/support/sitl.cpp | 4 +- libraries/Desktop/support/sitl_compass.cpp | 55 +++++++++++----------- 3 files changed, 30 insertions(+), 31 deletions(-) diff --git a/libraries/Desktop/support/desktop.h b/libraries/Desktop/support/desktop.h index 8ff6756ddf..814117b996 100644 --- a/libraries/Desktop/support/desktop.h +++ b/libraries/Desktop/support/desktop.h @@ -20,7 +20,7 @@ void sitl_input(void); void sitl_setup(void); int sitl_gps_pipe(void); ssize_t sitl_gps_read(int fd, void *buf, size_t count); -void sitl_update_compass(float heading, float roll, float pitch, float yaw); +void sitl_update_compass(float roll, float pitch, float yaw); void sitl_update_gps(double latitude, double longitude, float altitude, double speedN, double speedE, bool have_lock); void sitl_update_adc(float roll, float pitch, float yaw, diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index 7e0976620a..205522898d 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -278,7 +278,7 @@ static void timer_handler(int signum) sim_state.xAccel, sim_state.yAccel, sim_state.zAccel, sim_state.airspeed); sitl_update_barometer(sim_state.altitude); - sitl_update_compass(sim_state.heading, sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading); + sitl_update_compass(sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading); sei(); } @@ -326,7 +326,7 @@ void sitl_setup(void) // setup some initial values sitl_update_barometer(desktop_state.initial_height); sitl_update_adc(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0); - sitl_update_compass(0, 0, 0, 0); + sitl_update_compass(0, 0, 0); sitl_update_gps(0, 0, 0, 0, 0, false); } diff --git a/libraries/Desktop/support/sitl_compass.cpp b/libraries/Desktop/support/sitl_compass.cpp index 46a3653def..8d1d319a37 100644 --- a/libraries/Desktop/support/sitl_compass.cpp +++ b/libraries/Desktop/support/sitl_compass.cpp @@ -20,40 +20,40 @@ #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 + +// 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 */ -static Vector3f heading_to_mag(float heading, float roll, float pitch, float yaw) +static Vector3f heading_to_mag(float roll, float pitch, float yaw) { - Vector3f v; - double headX, headY, cos_roll, sin_roll, cos_pitch, sin_pitch, scale; - const double magnitude = 665; + Vector3f Bearth, m; + Matrix3f R; - cos_roll = cos(roll); - sin_roll = sin(roll); - cos_pitch = cos(pitch); - sin_pitch = sin(pitch); + // 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)); + Bearth = R * Bearth; + // create a rotation matrix for the given attitude + R.from_euler(roll, pitch, yaw); - headY = -sin(heading); - headX = cos(heading); - - if (fabs(cos_roll) < 1.0e-20) { - cos_roll = 1.0e-10; - } - if (fabs(cos_pitch) < 1.0e-20) { - cos_pitch = 1.0e-10; - } - - v.z = -0.6*cos(roll)*cos(pitch); - v.y = (headY + v.z*sin_roll) / cos_roll; - v.x = (headX - (v.y*sin_roll*sin_pitch + v.z*cos_roll*sin_pitch)) / cos_pitch; - scale = magnitude / sqrt((v.x*v.x) + (v.y*v.y) + (v.z*v.z)); - v *= scale; - return v; + // 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; } @@ -62,12 +62,11 @@ static Vector3f heading_to_mag(float heading, float roll, float pitch, float yaw setup the compass with new input all inputs are in degrees */ -void sitl_update_compass(float heading, float roll, float pitch, float yaw) +void sitl_update_compass(float roll, float pitch, float yaw) { extern AP_Compass_HIL compass; - Vector3f m = heading_to_mag(ToRad(heading), - ToRad(roll), + Vector3f m = heading_to_mag(ToRad(roll), ToRad(pitch), ToRad(yaw)); - compass.setHIL(m.x - MAG_OFS_X, m.y - MAG_OFS_Y, m.z - MAG_OFS_Z); + compass.setHIL(m.x, m.y, m.z); }