fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 16:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Compile fixes for EKF2

Browse Source
This commit is contained in:
Lorenz Meier
2016-03-11 21:59:38 +01:00
parent 83557ff0ba
commit 2fa43e35f5
1 changed files with 6 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+6 -5
View File
@@ -261,7 +261,8 @@ Ekf2::~Ekf2()
void Ekf2::print_status()
{
warnx("position OK %s", (_ekf->position_is_valid()) ? "[YES]" : "[NO]");
warnx("local position OK %s", (_ekf->local_position_is_valid()) ? "[YES]" : "[NO]");
warnx("global position OK %s", (_ekf->global_position_is_valid()) ? "[YES]" : "[NO]");
}
void Ekf2::task_main()
@@ -426,15 +427,15 @@ void Ekf2::task_main()
lpos.vz = vel[2];
// TODO: better status reporting
lpos.xy_valid = _ekf->position_is_valid();
lpos.xy_valid = _ekf->local_position_is_valid();
lpos.z_valid = true;
lpos.v_xy_valid = _ekf->position_is_valid();
lpos.v_xy_valid = _ekf->local_position_is_valid();
lpos.v_z_valid = true;
// Position of local NED origin in GPS / WGS84 frame
struct map_projection_reference_s ekf_origin = {};
_ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
lpos.xy_global = _ekf->position_is_valid();
lpos.xy_global = _ekf->global_position_is_valid();
lpos.z_global = true; // true if z is valid and has valid global reference (ref_alt)
lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
@@ -502,7 +503,7 @@ void Ekf2::task_main()
// generate and publish global position data
struct vehicle_global_position_s global_pos = {};
if (_ekf->position_is_valid()) {
if (_ekf->global_position_is_valid()) {
// TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator
global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start