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

Minor cleanups in EKF estimator

Browse Source
This commit is contained in:
Lorenz Meier
2014-05-07 08:40:23 +02:00
parent 6cb96d074d
commit 319ce3de10
1 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+8 -8
View File
@@ -625,8 +625,8 @@ FixedwingEstimator::task_main()
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
/* system is in HIL now, we got plenty of time - make sure real sensors are off */
usleep(250000);
/* system is in HIL now, wait for measurements to come in one last round */
usleep(65000);
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
@@ -1399,13 +1399,13 @@ int FixedwingEstimator::trip_nan() {
_ekf->states[5] = nan_val;
usleep(100000);
// warnx("tripping covariance #1 with NaN values");
// KH[2][2] = nan_val; // intermediate result used for covariance updates
// usleep(100000);
warnx("tripping covariance #1 with NaN values");
_ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates
usleep(100000);
// warnx("tripping covariance #2 with NaN values");
// KHP[5][5] = nan_val; // intermediate result used for covariance updates
// usleep(100000);
warnx("tripping covariance #2 with NaN values");
_ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates
usleep(100000);
warnx("tripping covariance #3 with NaN values");
_ekf->P[3][3] = nan_val; // covariance matrix