From 5d556f185023b2c52dd98093f255cb9cee65e406 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 13 Oct 2013 19:43:17 +0200 Subject: position_estimator_inav: distance to surface estimation fixes --- .../position_estimator_inav_main.c | 35 ++++++++++++++-------- .../position_estimator_inav_params.c | 2 +- 2 files changed, 23 insertions(+), 14 deletions(-) (limited to 'src/modules/position_estimator_inav') 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 f9fb5ab1b..8f789542f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -435,15 +435,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { /* spike detected, ignore */ sonar_corr = 0.0f; - surface_offset_rate = 0.0f; sonar_valid = false; } else { /* new ground level */ surface_offset -= sonar_corr; + surface_offset_rate = 0.0f; sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; - surface_offset_rate = 0.0f; sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; @@ -454,7 +453,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* correction is ok, use it */ sonar_valid_time = t; sonar_valid = true; - surface_offset_rate = -sonar_corr * params.w_alt_sonar; } } @@ -501,10 +499,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); flow_w = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist); + /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) flow_w *= 0.05f; + flow_valid = true; } else { @@ -610,7 +610,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* check for sonar measurement timeout */ if (sonar_valid && t > sonar_time + sonar_timeout) { sonar_corr = 0.0f; - surface_offset_rate = 0.0f; sonar_valid = false; } @@ -621,30 +620,39 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) 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 position during some time after position sources lost */ if (use_gps || use_flow) { - xy_src_time = t; + xy_src_time = t; } + bool can_estimate_xy = (t < xy_src_time + xy_src_timeout); - /* baro offset correction if sonar available, - * don't touch reference altitude, local_pos.ref_alt != baro_offset after this */ - if (sonar_valid) { + bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); + + if (dist_bottom_valid) { + /* surface distance prediction */ surface_offset += surface_offset_rate * dt; - } - /* accelerometer bias correction */ - float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; + /* surface distance correction */ + if (sonar_valid) { + surface_offset_rate -= sonar_corr * params.w_alt_sonar * params.w_alt_sonar * dt; + surface_offset -= sonar_corr * params.w_alt_sonar * dt; + } + } + /* reduce GPS weight if optical flow is good */ float w_pos_gps_p = params.w_pos_gps_p; float w_pos_gps_v = params.w_pos_gps_v; - /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_pos_gps_p *= params.w_gps_flow; w_pos_gps_v *= params.w_gps_flow; } + /* accelerometer bias correction */ + float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; + if (use_gps) { accel_bias_corr[0] -= gps_corr[0][0] * w_pos_gps_p * w_pos_gps_p; accel_bias_corr[0] -= gps_corr[0][1] * w_pos_gps_v; @@ -778,7 +786,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vz = z_est[1]; local_pos.landed = landed; local_pos.yaw = att.yaw; - local_pos.dist_bottom_valid = t < sonar_valid_time + sonar_valid_timeout; + local_pos.dist_bottom_valid = dist_bottom_valid; + if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 0a00ae6bb..b3f46fb99 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -51,7 +51,7 @@ PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); -PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); -- cgit v1.2.3