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

Minor cleanups in EKF estimator

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
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