dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #928 from PX4/ekf_auto_mag_decl 

attitude_estimator_ekf: auto detect mag declination using GPS
This commit is contained in:
Lorenz Meier 2014-06-21 20:05:43 +02:00
parent 4164e0a0ca 34184ff1fe
commit e0a6834606
2 changed files with 22 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/geo/geo.h
View File

@ -50,7 +50,7 @@
__BEGIN_DECLS __BEGIN_DECLS
#include "geo/geo_mag_declination.h" #include "geo_mag_declination.h"
#include <stdbool.h> #include <stdbool.h>

26
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -65,6 +65,7 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <systemlib/perf_counter.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 }; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
/* magnetic declination, in radians */
float mag_decl = 0.0f;
/* rotation matrix for magnetic declination */ /* rotation matrix for magnetic declination */
math::Matrix<3, 3> R_decl; math::Matrix<3, 3> R_decl;
R_decl.identity(); R_decl.identity();
@ -325,9 +329,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* update parameters */ /* update parameters */
parameters_update(&ekf_param_handles, &ekf_params); 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 */ /* 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); orb_check(sub_gps, &gps_updated);
if (gps_updated) { if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); 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; 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); parameters_update(&ekf_param_handles, &ekf_params);
/* update mag declination rotation matrix */ /* 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[0] = z_k[0];
x_aposteriori_k[1] = z_k[1]; 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.roll = euler[0];
att.pitch = euler[1]; att.pitch = euler[1];
att.yaw = euler[2] + ekf_params.mag_decl; att.yaw = euler[2] + mag_decl;
att.rollspeed = x_aposteriori[0]; att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1]; att.pitchspeed = x_aposteriori[1];