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

EKF: Ensure we start with zero local altitude and zero GPS offset. Since the filter is not publishing any data at this point this is not relevant in operation, but might be important later if we publish a separate altitude estimate topic

This commit is contained in:
Lorenz Meier 2015-06-11 12:30:05 +02:00
parent 45cd05b57a
commit e8a9c20056
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -631,7 +631,10 @@ void AttitudePositionEstimatorEKF::task_main()
// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
// maintain heavily filtered values for both baro and gps altitude
// Assume the filtered output should be identical for both sensors
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
if (_gpsIsGood) {
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
}
// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
// _last_debug_print = hrt_absolute_time();
// perf_print_counter(_perf_baro);
@ -658,6 +661,8 @@ void AttitudePositionEstimatorEKF::task_main()
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
_filter_ref_offset = -_baro.altitude;
warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset);
} else {