diff options
| -rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 6 |
1 files changed, 5 insertions, 1 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 488b53453..5b312941e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -478,7 +478,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; flow_prev = flow.flow_timestamp; - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7f && (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { + if ((flow.ground_distance_m > 0.31f) && + (flow.ground_distance_m < 4.0f) && + (att.R[2][2] > 0.7f) && + (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { + sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; |