position_estimator_inav: distance to surface estimation fixes

2 files changed, 23 insertions, 14 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 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);