diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-21 20:05:43 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-21 20:05:43 +0200 |
commit | e0a6834606f0aa7302c9e8fc3984d5b2018b9d1a (patch) | |
tree | 2379723ea8ed8d5fd7d222585fc1b35879371e8e /src | |
parent | 4164e0a0ca64b32612ba4254139c801274db98b4 (diff) | |
parent | 34184ff1fed3b8c24a955068b2ea2f6447494969 (diff) | |
download | px4-firmware-e0a6834606f0aa7302c9e8fc3984d5b2018b9d1a.tar.gz px4-firmware-e0a6834606f0aa7302c9e8fc3984d5b2018b9d1a.tar.bz2 px4-firmware-e0a6834606f0aa7302c9e8fc3984d5b2018b9d1a.zip |
Merge pull request #928 from PX4/ekf_auto_mag_decl
attitude_estimator_ekf: auto detect mag declination using GPS
Diffstat (limited to 'src')
-rw-r--r-- | src/lib/geo/geo.h | 2 | ||||
-rwxr-xr-x | src/modules/attitude_estimator_ekf/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 e2f3da6f8..87c1cf460 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 <stdbool.h> 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 66a949af1..5bcce1b4d 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -65,6 +65,7 @@ #include <drivers/drv_hrt.h> #include <lib/mathlib/mathlib.h> +#include <lib/geo/geo.h> #include <systemlib/systemlib.h> #include <systemlib/perf_counter.h> @@ -287,6 +288,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; + /* magnetic declination, in radians */ + float mag_decl = 0.0f; + /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); @@ -325,9 +329,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 */ @@ -340,6 +341,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; @@ -469,7 +477,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]; @@ -515,7 +531,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]; |