position_estimator_inav: "landed" detector implemented, bugfixes

6 files changed, 118 insertions, 46 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 50acec7e0..04e6dd2cb 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -820,6 +820,17 @@ int commander_thread_main(int argc, char *argv[])
 		/* update condition_local_position_valid and condition_local_altitude_valid */
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
+		if (status.condition_local_altitude_valid) {
+			if (status.condition_landed != local_position.landed) {
+				status.condition_landed = local_position.landed;
+				status_changed = true;
+				if (status.condition_landed) {
+					mavlink_log_info(mavlink_fd, "[cmd] LANDED");
+				} else {
+					mavlink_log_info(mavlink_fd, "[cmd] IN AIR");
+				}
+			}
+		}
 
 		/* update battery status */
 		orb_check(battery_sub, &updated);
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 0d5a537ea..a6ebeeacb 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -459,6 +459,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					/* publish local position setpoint as projection of global position setpoint */
 					orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
 				}
+
+				/* reset setpoints after non-manual modes */
+				reset_sp_xy = true;
+				reset_sp_z = true;
 			}
 
 			/* run position & altitude controllers, calculate velocity setpoint */
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index 0b09c9ea7..9c1ef2edb 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -41,8 +41,8 @@
 #include "multirotor_pos_control_params.h"
 
 /* controller parameters */
-PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f);
-PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f);
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
 PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
 PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
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 c0cfac880..3466841c4 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -52,10 +52,11 @@
 #include <limits.h>
 #include <math.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -169,14 +170,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	int baro_init_cnt = 0;
 	int baro_init_num = 200;
 	float baro_alt0 = 0.0f; /* to determine while start up */
+	float alt_avg = 0.0f;
+	bool landed = true;
+	hrt_abstime landed_time = 0;
 
 	uint32_t accel_counter = 0;
 	uint32_t baro_counter = 0;
 
 	/* declare and safely initialize all structs */
-	struct vehicle_status_s vehicle_status;
-	memset(&vehicle_status, 0, sizeof(vehicle_status));
-	/* make sure that baroINITdone = false */
+	struct actuator_controls_effective_s actuator;
+	memset(&actuator, 0, sizeof(actuator));
+	struct actuator_armed_s armed;
+	memset(&armed, 0, sizeof(armed));
 	struct sensor_combined_s sensor;
 	memset(&sensor, 0, sizeof(sensor));
 	struct vehicle_gps_position_s gps;
@@ -192,7 +197,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 	/* subscribe */
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	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));
@@ -294,9 +300,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	hrt_abstime sonar_time = 0;
 
 	/* main loop */
-	struct pollfd fds[6] = {
+	struct pollfd fds[7] = {
 		{ .fd = parameter_update_sub, .events = POLLIN },
-		{ .fd = vehicle_status_sub, .events = POLLIN },
+		{ .fd = actuator_sub, .events = POLLIN },
+		{ .fd = armed_sub, .events = POLLIN },
 		{ .fd = vehicle_attitude_sub, .events = POLLIN },
 		{ .fd = sensor_combined_sub, .events = POLLIN },
 		{ .fd = optical_flow_sub, .events = POLLIN },
@@ -308,7 +315,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	}
 
 	while (!thread_should_exit) {
-		int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+		int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate
 		hrt_abstime t = hrt_absolute_time();
 
 		if (ret < 0) {
@@ -328,20 +335,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				parameters_update(&pos_inav_param_handles, &params);
 			}
 
-			/* vehicle status */
+			/* actuator */
 			if (fds[1].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_status), vehicle_status_sub,
-					 &vehicle_status);
+				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator);
 			}
 
-			/* vehicle attitude */
+			/* armed */
 			if (fds[2].revents & POLLIN) {
+				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+			}
+
+			/* vehicle attitude */
+			if (fds[3].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
 				attitude_updates++;
 			}
 
 			/* sensor combined */
-			if (fds[3].revents & POLLIN) {
+			if (fds[4].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
 				if (sensor.accelerometer_counter > accel_counter) {
@@ -378,7 +389,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 
 			/* optical flow */
-			if (fds[4].revents & POLLIN) {
+			if (fds[5].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)) {
@@ -415,33 +426,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				flow_updates++;
 			}
 
-			if (fds[5].revents & POLLIN) {
-				/* vehicle GPS position */
+			/* vehicle GPS position */
+			if (fds[6].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 
-				if (gps.fix_type >= 3) {
+				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
 					/* initialize reference position if needed */
-					if (ref_xy_inited) {
-						/* project GPS lat lon to plane */
-						float gps_proj[2];
-						map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
-						/* calculate correction for position */
-						gps_corr[0][0] = gps_proj[0] - x_est[0];
-						gps_corr[1][0] = gps_proj[1] - y_est[0];
-
-						/* calculate correction for velocity */
-						if (gps.vel_ned_valid) {
-							gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
-							gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
-
-						} else {
-							gps_corr[0][1] = 0.0f;
-							gps_corr[1][1] = 0.0f;
-						}
-
-					} else {
-						hrt_abstime t = hrt_absolute_time();
-
+					if (!ref_xy_inited) {
 						if (ref_xy_init_start == 0) {
 							ref_xy_init_start = t;
 
@@ -462,12 +453,32 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						}
 					}
 
-					gps_updates++;
+					if (ref_xy_inited) {
+						/* project GPS lat lon to plane */
+						float gps_proj[2];
+						map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
+						/* calculate correction for position */
+						gps_corr[0][0] = gps_proj[0] - x_est[0];
+						gps_corr[1][0] = gps_proj[1] - y_est[0];
+
+						/* calculate correction for velocity */
+						if (gps.vel_ned_valid) {
+							gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
+							gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
+
+						} else {
+							gps_corr[0][1] = 0.0f;
+							gps_corr[1][1] = 0.0f;
+						}
+					}
 
 				} else {
 					/* no GPS lock */
 					memset(gps_corr, 0, sizeof(gps_corr));
+					ref_xy_init_start = 0;
 				}
+
+				gps_updates++;
 			}
 		}
 
@@ -516,9 +527,40 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/* detect land */
+		alt_avg += (z_est[0] - alt_avg) * dt / params.land_t;
+		float alt_disp = z_est[0] - alt_avg;
+		alt_disp = alt_disp * alt_disp;
+		float land_disp2 = params.land_disp * params.land_disp;
+		/* get actual thrust output */
+		float thrust = armed.armed ? actuator.control_effective[3] : 0.0f;
+
+		if (landed) {
+			if (alt_disp > land_disp2 && thrust > params.land_thr) {
+				landed = false;
+				landed_time = 0;
+			}
+
+		} else {
+			if (alt_disp < land_disp2 && thrust < params.land_thr) {
+				if (landed_time == 0) {
+					landed_time = t;    // land detected first time
+
+				} else {
+					if (t > landed_time + params.land_t * 1000000.0f) {
+						landed = true;
+						landed_time = 0;
+					}
+				}
+
+			} else {
+				landed_time = 0;
+			}
+		}
+
 		if (verbose_mode) {
 			/* print updates rate */
-			if (t - updates_counter_start > updates_counter_len) {
+			if (t > updates_counter_start + updates_counter_len) {
 				float updates_dt = (t - updates_counter_start) * 0.000001f;
 				warnx(
 					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
@@ -536,7 +578,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 		}
 
-		if (t - pub_last > pub_interval) {
+		if (t > pub_last + pub_interval) {
 			pub_last = t;
 			/* publish local position */
 			local_pos.timestamp = t;
@@ -549,7 +591,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			local_pos.vy = y_est[1];
 			local_pos.z = z_est[0];
 			local_pos.vz = z_est[1];
-			local_pos.landed = false;	// TODO
+			local_pos.landed = landed;
 
 			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
 
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 801e20781..4f9ddd009 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -40,7 +40,7 @@
 
 #include "position_estimator_inav_params.h"
 
-PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f);
 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, 1.0f);
@@ -51,6 +51,9 @@ PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
 PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
 PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
+PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
@@ -65,6 +68,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 	h->flow_k = param_find("INAV_FLOW_K");
 	h->sonar_filt = param_find("INAV_SONAR_FILT");
 	h->sonar_err = param_find("INAV_SONAR_ERR");
+	h->land_t = param_find("INAV_LAND_T");
+	h->land_disp = param_find("INAV_LAND_DISP");
+	h->land_thr = param_find("INAV_LAND_THR");
 
 	return OK;
 }
@@ -82,6 +88,9 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 	param_get(h->flow_k, &(p->flow_k));
 	param_get(h->sonar_filt, &(p->sonar_filt));
 	param_get(h->sonar_err, &(p->sonar_err));
+	param_get(h->land_t, &(p->land_t));
+	param_get(h->land_disp, &(p->land_disp));
+	param_get(h->land_thr, &(p->land_thr));
 
 	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 ed6f61e04..61570aea7 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -52,6 +52,9 @@ struct position_estimator_inav_params {
 	float flow_k;
 	float sonar_filt;
 	float sonar_err;
+	float land_t;
+	float land_disp;
+	float land_thr;
 };
 
 struct position_estimator_inav_param_handles {
@@ -66,6 +69,9 @@ struct position_estimator_inav_param_handles {
 	param_t flow_k;
 	param_t sonar_filt;
 	param_t sonar_err;
+	param_t land_t;
+	param_t land_disp;
+	param_t land_thr;
 };
 
 /**