From 14b923b4548a8596a97fdec5b53e1779b69fee08 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Aug 2015 10:04:36 +0200 Subject: [PATCH] Att pos EKF: Fix init logic, only set local reference if GPS lock present --- .../AttitudePositionEstimatorEKF.h | 2 +- .../ekf_att_pos_estimator_main.cpp | 27 ++++++++++--------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 0e991a51a5..28b23f2069 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -342,7 +342,7 @@ private: /** * Initialize the reference position for the local coordinate frame */ - void initReferencePosition(hrt_abstime timestamp, + void initReferencePosition(hrt_abstime timestamp, bool gps_valid, double lat, double lon, float gps_alt, float baro_alt); /** diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 757069a613..b756e8a234 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -428,7 +428,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); + initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -738,7 +738,7 @@ void AttitudePositionEstimatorEKF::task_main() } void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, - double lat, double lon, float gps_alt, float baro_alt) + bool gps_valid, double lat, double lon, float gps_alt, float baro_alt) { // Reference altitude if (PX4_ISFINITE(_ekf->states[9])) { @@ -750,17 +750,20 @@ void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, } // init filtered gps and baro altitudes - _gps_alt_filt = gps_alt; _baro_alt_filt = baro_alt; - // Initialize projection - _local_pos.ref_lat = lat; - _local_pos.ref_lon = lon; - _local_pos.ref_alt = gps_alt; - _local_pos.ref_timestamp = timestamp; + if (gps_valid) { + _gps_alt_filt = gps_alt; - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + // Initialize projection + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; + _local_pos.ref_alt = gps_alt; + _local_pos.ref_timestamp = timestamp; + + map_projection_init(&_pos_ref, lat, lon); + mavlink_and_console_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + } } void AttitudePositionEstimatorEKF::initializeGPS() @@ -793,7 +796,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); + initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); #if 0 PX4_INFO("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, @@ -1370,7 +1373,7 @@ void AttitudePositionEstimatorEKF::pollData() } //Check if the GPS fix is good enough for us to use - if (_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { + if ((_gps.fix_type >= 3) && (_gps.eph < requiredAccuracy) && (_gps.epv < requiredAccuracy)) { _gpsIsGood = true; } else {