diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-07 08:40:23 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-07 08:40:23 +0200 |
| commit | 319ce3de1005d7199a992b227c7e75acb9db376b (patch) | |
| tree | bc387a6dc3c1f421b188644beeff0550b1c5878a /src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | |
| parent | 6cb96d074d282af92540b6481d843c2295426773 (diff) | |
| download | px4-firmware-319ce3de1005d7199a992b227c7e75acb9db376b.tar.gz px4-firmware-319ce3de1005d7199a992b227c7e75acb9db376b.tar.bz2 px4-firmware-319ce3de1005d7199a992b227c7e75acb9db376b.zip | |
Minor cleanups in EKF estimator
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 16 |
1 files changed, 8 insertions, 8 deletions
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 40729e94b..e563b4b87 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -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 |