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

EKF: Publish global position also if GPS is not yet valid so that controllers can get a valid altitude

This commit is contained in:
Lorenz Meier 2015-06-21 19:55:04 +02:00 committed by tumbili
parent 5d92927991
commit 6a00fce009
1 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -221,6 +221,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
/* indicate consumers that the current position data is not valid */
_gps.eph = 10000.0f;
_gps.epv = 10000.0f;
/* fetch initial parameter values */
parameters_update();
@ -686,21 +690,21 @@ void AttitudePositionEstimatorEKF::task_main()
continue;
}
//Run EKF data fusion steps
// Run EKF data fusion steps
updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
//Publish attitude estimations
// Publish attitude estimations
publishAttitude();
//Publish Local Position estimations
// Publish Local Position estimations
publishLocalPosition();
//Publish Global Position, but only if it's any good
if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) {
// Publish Global Position, but only if it's any good
if (_gpsIsGood || _global_pos.dead_reckoning) {
publishGlobalPosition();
}
//Publish wind estimates
// Publish wind estimates
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
publishWindEstimate();
}
@ -891,6 +895,10 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
_global_pos.lat = est_lat;
_global_pos.lon = est_lon;
_global_pos.time_utc_usec = _gps.time_utc_usec;
} else {
_global_pos.lat = 0.0;
_global_pos.lon = 0.0;
_global_pos.time_utc_usec = 0;
}
if (_local_pos.v_xy_valid) {
@ -907,6 +915,8 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz;
} else {
_global_pos.vel_d = 0.0f;
}
/* terrain altitude */