Sonar added to position_estimator_inav

5 files changed, 93 insertions, 20 deletions

diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
index acaf693f1..13328edb4 100644
--- a/src/modules/position_estimator_inav/inertial_filter.c
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -13,9 +13,12 @@ void inertial_filter_predict(float dt, float x[3])
 	x[1] += x[2] * dt;
 }
 
-void inertial_filter_correct(float edt, float x[3], int i, float w)
+void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
 {
-	float ewdt = w * edt;
+	float ewdt = w * dt;
+	if (ewdt > 1.0f)
+		ewdt = 1.0f;	// prevent over-correcting
+	ewdt *= e;
 	x[i] += ewdt;
 
 	if (i == 0) {
diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h
index dca6375dc..761c17097 100644
--- a/src/modules/position_estimator_inav/inertial_filter.h
+++ b/src/modules/position_estimator_inav/inertial_filter.h
@@ -10,4 +10,4 @@
 
 void inertial_filter_predict(float dt, float x[3]);
 
-void inertial_filter_correct(float edt, float x[3], int i, float w);
+void inertial_filter_correct(float e, float dt, float x[3], int i, float w);
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 278a319b5..fb09948ec 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -59,6 +59,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/optical_flow.h>
 #include <mavlink/mavlink_log.h>
 #include <poll.h>
 #include <systemlib/geo/geo.h>
@@ -183,6 +184,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	memset(&att, 0, sizeof(att));
 	struct vehicle_local_position_s local_pos;
 	memset(&local_pos, 0, sizeof(local_pos));
+	struct optical_flow_s flow;
+	memset(&flow, 0, sizeof(flow));
 	struct vehicle_global_position_s global_pos;
 	memset(&global_pos, 0, sizeof(global_pos));
 
@@ -191,6 +194,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 
 	/* advertise */
@@ -263,12 +267,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	hrt_abstime t_prev = 0;
 
 	uint16_t accel_updates = 0;
-	hrt_abstime accel_t = 0;
 	uint16_t baro_updates = 0;
-	hrt_abstime baro_t = 0;
-	hrt_abstime gps_t = 0;
 	uint16_t gps_updates = 0;
 	uint16_t attitude_updates = 0;
+	uint16_t flow_updates = 0;
 
 	hrt_abstime updates_counter_start = hrt_absolute_time();
 	uint32_t updates_counter_len = 1000000;
@@ -283,6 +285,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			{ 0.0f, 0.0f },		// N (pos, vel)
 			{ 0.0f, 0.0f },		// E (pos, vel)
 	};
+	float sonar_corr = 0.0f;
+	float sonar_corr_filtered = 0.0f;
+	float flow_corr[] = { 0.0f, 0.0f };	// X, Y
+
+	float sonar_prev = 0.0f;
+	hrt_abstime sonar_time = 0;
 
 	/* main loop */
 	struct pollfd fds[5] = {
@@ -290,6 +298,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		{ .fd = vehicle_status_sub, .events = POLLIN },
 		{ .fd = vehicle_attitude_sub, .events = POLLIN },
 		{ .fd = sensor_combined_sub, .events = POLLIN },
+		{ .fd = optical_flow_sub, .events = POLLIN },
 		{ .fd = vehicle_gps_position_sub, .events = POLLIN }
 	};
 
@@ -297,7 +306,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	warnx("main loop started.");
 
 	while (!thread_should_exit) {
-		int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+		int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate
 		hrt_abstime t = hrt_absolute_time();
 
 		if (ret < 0) {
@@ -355,13 +364,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				}
 
 				if (sensor.baro_counter > baro_counter) {
-					baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[0];
+					baro_corr = - sensor.baro_alt_meter - z_est[0];
 					baro_counter = sensor.baro_counter;
 					baro_updates++;
 				}
 			}
 
-			if (params.use_gps && fds[4].revents & POLLIN) {
+			/* optical flow */
+			if (fds[4].revents & POLLIN) {
+				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
+				if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) {
+					if (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_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
+						if (fabsf(sonar_corr) > params.sonar_err) {
+							// correction is too large: spike or new ground level?
+							if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
+								// spike detected, ignore
+								sonar_corr = 0.0f;
+							} else {
+								// new ground level
+								baro_alt0 += sonar_corr;
+								warnx("new home: alt = %.3f", baro_alt0);
+								mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
+								local_pos.home_alt = baro_alt0;
+								local_pos.home_timestamp = hrt_absolute_time();
+								sonar_corr = 0.0f;
+								sonar_corr_filtered = 0.0;
+							}
+						}
+					}
+				} else {
+					sonar_corr = 0.0f;
+				}
+				flow_updates++;
+			}
+
+			if (params.use_gps && fds[5].revents & POLLIN) {
 				/* vehicle GPS position */
 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 				if (gps.fix_type >= 3) {
@@ -393,8 +434,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		inertial_filter_predict(dt, z_est);
 
 		/* inertial filter correction for altitude */
-		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);
+		baro_alt0 += sonar_corr * params.w_alt_sonar * dt;
+		inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro);
+		inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
+		inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
 
 		/* dont't try to estimate position when no any position source available */
 		bool can_estimate_pos = params.use_gps && gps.fix_type >= 3;
@@ -404,15 +447,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_predict(dt, y_est);
 
 			/* inertial filter correction for position */
-			inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc);
-			inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc);
+			inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
+			inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
 
 			if (params.use_gps && gps.fix_type >= 3) {
-				inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p);
-				inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p);
+				inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
+				inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
 				if (gps.vel_ned_valid) {
-					inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v);
-					inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v);
+					inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
+					inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
 				}
 			}
 		}
@@ -421,17 +464,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			/* print updates rate */
 			if (t - updates_counter_start > updates_counter_len) {
 				float updates_dt = (t - updates_counter_start) * 0.000001f;
-				printf(
-					"[inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n",
+				warnx(
+					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
 					accel_updates / updates_dt,
 					baro_updates / updates_dt,
 					gps_updates / updates_dt,
-					attitude_updates / updates_dt);
+					attitude_updates / updates_dt,
+					flow_updates / updates_dt);
 				updates_counter_start = t;
 				accel_updates = 0;
 				baro_updates = 0;
 				gps_updates = 0;
 				attitude_updates = 0;
+				flow_updates = 0;
 			}
 		}
 
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 49dc7f51f..eac2fc1ce 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -43,18 +43,28 @@
 PARAM_DEFINE_INT32(INAV_USE_GPS, 1);
 PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f);
 PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
 PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f);
 PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
 	h->use_gps = param_find("INAV_USE_GPS");
 	h->w_alt_baro = param_find("INAV_W_ALT_BARO");
 	h->w_alt_acc = param_find("INAV_W_ALT_ACC");
+	h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
 	h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P");
 	h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
 	h->w_pos_acc = param_find("INAV_W_POS_ACC");
+	h->w_pos_flow = param_find("INAV_W_POS_FLOW");
+	h->flow_k = param_find("INAV_FLOW_K");
+	h->sonar_filt = param_find("INAV_SONAR_FILT");
+	h->sonar_err = param_find("INAV_SONAR_ERR");
 
 	return OK;
 }
@@ -64,9 +74,14 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 	param_get(h->use_gps, &(p->use_gps));
 	param_get(h->w_alt_baro, &(p->w_alt_baro));
 	param_get(h->w_alt_acc, &(p->w_alt_acc));
+	param_get(h->w_alt_sonar, &(p->w_alt_sonar));
 	param_get(h->w_pos_gps_p, &(p->w_pos_gps_p));
 	param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
 	param_get(h->w_pos_acc, &(p->w_pos_acc));
+	param_get(h->w_pos_flow, &(p->w_pos_flow));
+	param_get(h->flow_k, &(p->flow_k));
+	param_get(h->sonar_filt, &(p->sonar_filt));
+	param_get(h->sonar_err, &(p->sonar_err));
 
 	return OK;
 }
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index 870227fef..cca172b5d 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -44,18 +44,28 @@ struct position_estimator_inav_params {
 	int use_gps;
 	float w_alt_baro;
 	float w_alt_acc;
+	float w_alt_sonar;
 	float w_pos_gps_p;
 	float w_pos_gps_v;
 	float w_pos_acc;
+	float w_pos_flow;
+	float flow_k;
+	float sonar_filt;
+	float sonar_err;
 };
 
 struct position_estimator_inav_param_handles {
 	param_t use_gps;
 	param_t w_alt_baro;
 	param_t w_alt_acc;
+	param_t w_alt_sonar;
 	param_t w_pos_gps_p;
 	param_t w_pos_gps_v;
 	param_t w_pos_acc;
+	param_t w_pos_flow;
+	param_t flow_k;
+	param_t sonar_filt;
+	param_t sonar_err;
 };
 
 /**