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

attitude_estimator_ekf: auto detect mag declination using GPS coordinates

This commit is contained in:
Anton Babushkin 2014-05-12 23:06:45 +02:00
parent 15e65fda26
commit bd9d58f565
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
#include "geo/geo_mag_declination.h"
#include "geo_mag_declination.h"
#include <stdbool.h>

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

@ -64,6 +64,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>
@ -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];