From bd9d58f565383598db785908a7cc08f87f6a07f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 12 May 2014 23:06:45 +0200 Subject: [PATCH] attitude_estimator_ekf: auto detect mag declination using GPS coordinates --- src/lib/geo/geo.h | 2 +- .../attitude_estimator_ekf_main.cpp | 26 +++++++++++++++---- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index e2f3da6f80..87c1cf4605 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -50,7 +50,7 @@ __BEGIN_DECLS -#include "geo/geo_mag_declination.h" +#include "geo_mag_declination.h" #include diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index c61b6ff3fe..e10c7de560 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -64,6 +64,7 @@ #include #include +#include #include #include @@ -292,6 +293,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; + /* magnetic declination, in radians */ + float mag_decl = 0.0f; + /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); @@ -330,9 +334,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); - - /* update mag declination rotation matrix */ - R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); } /* only run filter if sensor values changed */ @@ -345,6 +346,13 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_check(sub_gps, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); + + if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); + + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, mag_decl); + } } bool global_pos_updated; @@ -474,7 +482,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ - R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); + if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); + + } else { + mag_decl = ekf_params.mag_decl; + } + + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, mag_decl); x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; @@ -522,7 +538,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.roll = euler[0]; att.pitch = euler[1]; - att.yaw = euler[2] + ekf_params.mag_decl; + att.yaw = euler[2] + mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1];