diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-08-17 12:37:41 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-08-17 12:37:41 +0200 |
commit | 2be5240b9f70803417c9648133490409ba40ba55 (patch) | |
tree | a511a38870d63fe5dfd6b41f80ccf6ad753bfaa9 /src/modules/position_estimator_inav/position_estimator_inav_main.c | |
parent | c543f89ec17048c1b5264623a885a9247a05304c (diff) | |
download | px4-firmware-2be5240b9f70803417c9648133490409ba40ba55.tar.gz px4-firmware-2be5240b9f70803417c9648133490409ba40ba55.tar.bz2 px4-firmware-2be5240b9f70803417c9648133490409ba40ba55.zip |
commander, multirotor_att_control, position_estimator_inav: position valid flag fixed, other fixes and cleaunup
Diffstat (limited to 'src/modules/position_estimator_inav/position_estimator_inav_main.c')
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 15 |
1 files changed, 7 insertions, 8 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 0530c2dea..0e7e0ef5d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -75,6 +75,7 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; +static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -490,7 +491,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); /* dont't try to estimate position when no any position source available */ - bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; + bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout; if (can_estimate_pos) { /* inertial filter prediction for position */ @@ -501,14 +502,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); - if (params.use_gps && gps.fix_type >= 3) { - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); - if (gps.vel_ned_valid) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); - } + if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) { + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); } } |