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

Att pos EKF: Fix init logic, only set local reference if GPS lock present

This commit is contained in:
Lorenz Meier 2015-08-06 10:04:36 +02:00
parent 3fd0ddf1b0
commit 14b923b454
2 changed files with 16 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -342,7 +342,7 @@ private:
/** /**
* Initialize the reference position for the local coordinate frame * 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); double lat, double lon, float gps_alt, float baro_alt);
/** /**

27
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -428,7 +428,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()
// Set up height correctly // Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); 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) { } else if (_ekf_logging) {
_ekf->GetFilterState(&ekf_report); _ekf->GetFilterState(&ekf_report);
@ -738,7 +738,7 @@ void AttitudePositionEstimatorEKF::task_main()
} }
void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, 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 // Reference altitude
if (PX4_ISFINITE(_ekf->states[9])) { if (PX4_ISFINITE(_ekf->states[9])) {
@ -750,17 +750,20 @@ void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
} }
// init filtered gps and baro altitudes // init filtered gps and baro altitudes
_gps_alt_filt = gps_alt;
_baro_alt_filt = baro_alt; _baro_alt_filt = baro_alt;
// Initialize projection if (gps_valid) {
_local_pos.ref_lat = lat; _gps_alt_filt = gps_alt;
_local_pos.ref_lon = lon;
_local_pos.ref_alt = gps_alt;
_local_pos.ref_timestamp = timestamp;
map_projection_init(&_pos_ref, lat, lon); // Initialize projection
mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); _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() void AttitudePositionEstimatorEKF::initializeGPS()
@ -793,7 +796,7 @@ void AttitudePositionEstimatorEKF::initializeGPS()
_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); _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 #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, 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 //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; _gpsIsGood = true;
} else { } else {