position_estimator_inav: use independent estimate for distance to ground (sonar), WIP

4 files changed, 26 insertions, 23 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 506bc1bf0..6df7c69b2 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -180,7 +180,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 	int baro_init_cnt = 0;
 	int baro_init_num = 200;
-	float baro_offset = 0.0f;		// baro offset from reference altitude, it's not the same as local_position.ref_alt if sonar available
+	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized only once
+	float surface_offset = 0.0f;	// ground level offset from reference altitude
 	float alt_avg = 0.0f;
 	bool landed = true;
 	hrt_abstime landed_time = 0;
@@ -365,7 +366,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
 
 				/* reset ground level on arm if sonar is invalid */
-				if (armed.armed && !flag_armed && t > sonar_valid_time + sonar_valid_timeout) {
+				if (armed.armed && !flag_armed) {
 					flag_armed = armed.armed;
 					baro_offset -= z_est[0];
 					z_est[0] = 0.0f;
@@ -410,7 +411,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				}
 
 				if (sensor.baro_counter > baro_counter) {
-					baro_corr = - sensor.baro_alt_meter - z_est[0];	// should be shifted by baro_offset before applying correction
+					baro_corr = baro_offset - sensor.baro_alt_meter - z_est[0];
 					baro_counter = sensor.baro_counter;
 					baro_updates++;
 				}
@@ -425,7 +426,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
 					sonar_time = t;
 					sonar_prev = flow.ground_distance_m;
-					sonar_corr = -flow.ground_distance_m - z_est[0];
+					sonar_corr = flow.ground_distance_m + surface_offset + z_est[0];
 					sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
 
 					if (fabsf(sonar_corr) > params.sonar_err) {
@@ -437,12 +438,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 						} else {
 							/* new ground level */
-							baro_offset += sonar_corr;
-							mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_offset);
-							local_pos.ref_alt += sonar_corr;
-							local_pos.ref_timestamp = t;
-							z_est[0] += sonar_corr;
-							alt_avg -= sonar_corr;
+							surface_offset -= sonar_corr;
+							mavlink_log_info(mavlink_fd, "[inav] new surface level: %.3f", surface_offset);
+							alt_avg -= sonar_corr;	// TODO check this
 							sonar_corr = 0.0f;
 							sonar_corr_filtered = 0.0f;
 							sonar_valid_time = t;
@@ -459,7 +457,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				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) {
 					/* distance to surface */
-					float flow_dist = -z_est[0] / att.R[2][2];
+					float flow_dist = (- z_est[0] - surface_offset) / 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 };
@@ -626,7 +624,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		/* baro offset correction if sonar available,
 		 * don't touch reference altitude, local_pos.ref_alt != baro_offset after this */
 		if (sonar_valid) {
-			baro_offset -= (baro_corr + baro_offset) * params.w_alt_sonar * dt;
+			surface_offset -= sonar_corr * params.w_alt_sonar * dt;
 		}
 
 		/* accelerometer bias correction */
@@ -644,11 +642,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			accel_bias_corr[1] -= flow_corr[1] * params.w_pos_flow;
 		}
 
-		accel_bias_corr[2] -= (baro_corr + baro_offset) * params.w_alt_baro * params.w_alt_baro;
-
-		if (sonar_valid) {
-			accel_bias_corr[2] -= sonar_corr * params.w_alt_sonar * params.w_alt_sonar;
-		}
+		accel_bias_corr[2] -= baro_corr * params.w_alt_baro * params.w_alt_baro;
 
 		/* transform error vector from NED frame to body frame */
 		for (int i = 0; i < 3; i++) {
@@ -665,11 +659,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		inertial_filter_predict(dt, z_est);
 
 		/* inertial filter correction for altitude */
-		if (sonar_valid) {
-			inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
-		}
-
-		inertial_filter_correct(baro_corr + baro_offset, dt, z_est, 0, params.w_alt_baro);
+		inertial_filter_correct(baro_corr, dt, z_est, 0, params.w_alt_baro);
 		inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
 
 		if (can_estimate_xy) {
@@ -773,6 +763,10 @@ 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;
+			if (local_pos.dist_bottom_valid) {
+				local_pos.dist_bottom = -z_est[0] - surface_offset;
+			}
 
 			local_pos.timestamp = t;
 
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ec8033202..7541910d1 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1046,8 +1046,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
 				log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
 				log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
+				log_msg.body.log_LPOS.dist_bottom = buf.local_pos.dist_bottom;
 				log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
 				log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
+				log_msg.body.log_LPOS.dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
 				log_msg.body.log_LPOS.landed = buf.local_pos.landed;
 				LOGBUFFER_WRITE_AND_COUNT(LPOS);
 			}
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 0e88da054..32dd4f714 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -109,8 +109,10 @@ struct log_LPOS_s {
 	int32_t ref_lat;
 	int32_t ref_lon;
 	float ref_alt;
+	float dist_bottom;
 	uint8_t xy_flags;
 	uint8_t z_flags;
+	uint8_t dist_flags;
 	uint8_t landed;
 };
 
@@ -281,7 +283,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
-	LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
+	LOG_FORMAT(LPOS, "ffffffLLffBBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,DstB,XYFl,ZFl,DstFl,Land"),
 	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 427153782..807fc6c09 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -77,6 +77,11 @@ struct vehicle_local_position_s
 	int32_t ref_lon;		/**< Reference point longitude in 1E7 degrees */
 	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
 	bool landed;			/**< true if vehicle is landed */
+	/* Distance to surface */
+	float dist_bottom;		/**< Distance to bottom surface (ground) */
+	float v_dist_bottom;		/**< Distance to bottom surface (ground) change rate */
+	uint64_t surface_bottom_timestamp;		/**< Time when new bottom surface found */
+	bool dist_bottom_valid;	/**< true if distance to bottom surface is valid */
 };
 
 /**