diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-10-11 20:36:40 +0200 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-10-11 20:36:40 +0200 |
| commit | 2b1a11b16dd77a5b2a427e7a32a2e398b249ebad (patch) | |
| tree | d8e099be2ad95440d4e738cecd551337de7f40e5 /src/modules/position_estimator_inav/position_estimator_inav_main.c | |
| parent | 74af8d2c459f1509e2b1361f0b699c0dd758a34d (diff) | |
| download | px4-firmware-2b1a11b16dd77a5b2a427e7a32a2e398b249ebad.tar.gz px4-firmware-2b1a11b16dd77a5b2a427e7a32a2e398b249ebad.tar.bz2 px4-firmware-2b1a11b16dd77a5b2a427e7a32a2e398b249ebad.zip | |
position_estimator_inav: bug fixed, allow to disable GPS by setting INAV_W_POS_GPS_P = 0
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 | 7 |
1 files changed, 4 insertions, 3 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 8cd161075..81271ab52 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -454,10 +454,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float flow_q = flow.quality / 255.0f; + float dist_bottom = - z_est[0] - surface_offset; - if (z_est[0] < - 0.31f && flow_q > params.flow_q_min && t < sonar_valid_time + sonar_valid_timeout && att.R[2][2] > 0.7) { + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) { /* distance to surface */ - float flow_dist = (- z_est[0] - surface_offset) / att.R[2][2]; + float flow_dist = dist_bottom / att.R[2][2]; /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; @@ -612,7 +613,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) t_prev = t; /* use GPS if it's valid and reference position initialized */ - bool use_gps = ref_xy_inited && gps_valid; + bool use_gps = ref_xy_inited && gps_valid && params.w_pos_gps_p > 0.00001f; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps); /* try to estimate xy even if no absolute position source available, |