Merge branch 'inav_fix' into inav_flow

5 files changed, 97 insertions, 44 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 133646051..cba193208 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -94,6 +94,8 @@ extern device::Device *PX4IO_serial_interface() weak_function;
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 
+#define UPDATE_INTERVAL_MIN		2
+
 /**
  * The PX4IO class.
  *
@@ -790,8 +792,8 @@ PX4IO::task_main()
 
 		/* adjust update interval */
 		if (_update_interval != 0) {
-			if (_update_interval < 5)
-				_update_interval = 5;
+			if (_update_interval < UPDATE_INTERVAL_MIN)
+				_update_interval = UPDATE_INTERVAL_MIN;
 			if (_update_interval > 100)
 				_update_interval = 100;
 			orb_set_interval(_t_actuators, _update_interval);
@@ -1942,8 +1944,8 @@ int
 PX4IO::set_update_rate(int rate)
 {
 	int interval_ms = 1000 / rate;
-	if (interval_ms < 3) {
-		interval_ms = 3;
+	if (interval_ms < UPDATE_INTERVAL_MIN) {
+		interval_ms = UPDATE_INTERVAL_MIN;
 		warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
 	}
 
@@ -2317,7 +2319,7 @@ px4io_main(int argc, char *argv[])
 		if ((argc > 2)) {
 			g_dev->set_update_rate(atoi(argv[2]));
 		} else {
-			errx(1, "missing argument (50 - 400 Hz)");
+			errx(1, "missing argument (50 - 500 Hz)");
 			return 1;
 		}
 		exit(0);
diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c
index 621d3253f..3125ce246 100644
--- a/src/examples/flow_position_control/flow_position_control_main.c
+++ b/src/examples/flow_position_control/flow_position_control_main.c
@@ -67,6 +67,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 #include <poll.h>
+#include <mavlink/mavlink_log.h>
 
 #include "flow_position_control_params.h"
 
@@ -153,20 +154,28 @@ flow_position_control_thread_main(int argc, char *argv[])
 {
 	/* welcome user */
 	thread_running = true;
-	printf("[flow position control] starting\n");
+	static int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[fpc] started");
 
 	uint32_t counter = 0;
 	const float time_scale = powf(10.0f,-6.0f);
 
 	/* structures */
 	struct actuator_armed_s armed;
+	memset(&armed, 0, sizeof(armed));
 	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
 	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
 	struct filtered_bottom_flow_s filtered_flow;
+	memset(&filtered_flow, 0, sizeof(filtered_flow));
 	struct vehicle_local_position_s local_pos;
-
+	memset(&local_pos, 0, sizeof(local_pos));
 	struct vehicle_bodyframe_speed_setpoint_s speed_sp;
+	memset(&speed_sp, 0, sizeof(speed_sp));
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -216,6 +225,7 @@ flow_position_control_thread_main(int argc, char *argv[])
 	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err");
 
 	static bool sensors_ready = false;
+	static bool status_changed = false;
 
 	while (!thread_should_exit)
 	{
@@ -252,7 +262,7 @@ flow_position_control_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
 
 					parameters_update(&param_handles, &params);
-					printf("[flow position control] parameters updated.\n");
+					mavlink_log_info(mavlink_fd,"[fpc] parameters updated.");
 				}
 
 				/* only run controller if position/speed changed */
@@ -270,6 +280,8 @@ flow_position_control_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
 					/* get a local copy of local position */
 					orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
+					/* get a local copy of control mode */
+					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
 
 					if (control_mode.flag_control_velocity_enabled)
 					{
@@ -277,6 +289,11 @@ flow_position_control_thread_main(int argc, char *argv[])
 						float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
 						float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1
 
+						if(status_changed == false)
+							mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged");
+
+						status_changed = true;
+
 						/* calc dt */
 						if(last_time == 0)
 						{
@@ -296,7 +313,7 @@ flow_position_control_thread_main(int argc, char *argv[])
 						{
 							flow_sp_sumy = filtered_flow.sumy;
 							update_flow_sp_sumy = false;
-						}
+						} 
 
 						/* calc new bodyframe speed setpoints */
 						float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d;
@@ -518,6 +535,11 @@ flow_position_control_thread_main(int argc, char *argv[])
 					else
 					{
 						/* in manual or stabilized state just reset speed and flow sum setpoint */
+						//mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy);
+						if(status_changed == true)
+							mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged.");
+
+						status_changed = false;
 						speed_sp.vx = 0.0f;
 						speed_sp.vy = 0.0f;
 						flow_sp_sumx = filtered_flow.sumx;
@@ -558,20 +580,20 @@ flow_position_control_thread_main(int argc, char *argv[])
 			else if (ret == 0)
 			{
 				/* no return value, ignore */
-				printf("[flow position control] no attitude received.\n");
+				mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n");
 			}
 			else
 			{
 				if (fds[0].revents & POLLIN)
 				{
 					sensors_ready = true;
-					printf("[flow position control] initialized.\n");
+					mavlink_log_info(mavlink_fd,"[fpc] initialized.\n");
 				}
 			}
 		}
 	}
 
-	printf("[flow position control] ending now...\n");
+	mavlink_log_info(mavlink_fd,"[fpc] ending now...\n");
 
 	thread_running = false;
 
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index 5e80066a7..495c415f2 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -345,6 +345,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 						local_pos.y = local_pos.y + global_speed[1] * dt;
 						local_pos.vx = global_speed[0];
 						local_pos.vy = global_speed[1];
+						local_pos.xy_valid = true;
+						local_pos.v_xy_valid = true;
 					}
 					else
 					{
@@ -353,6 +355,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 						filtered_flow.vy = 0;
 						local_pos.vx = 0;
 						local_pos.vy = 0;
+						local_pos.xy_valid = false;
+						local_pos.v_xy_valid = false;
 					}
 
 					/* filtering ground distance */
@@ -361,6 +365,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 						/* not possible to fly */
 						sonar_valid = false;
 						local_pos.z = 0.0f;
+						local_pos.z_valid = false;
 					}
 					else
 					{
@@ -388,6 +393,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 						{
 							local_pos.z = -sonar_new;
 						}
+
+						local_pos.z_valid = true;
 					}
 
 					filtered_flow.timestamp = hrt_absolute_time();
diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c
index 6af955cd7..feed0d23c 100644
--- a/src/examples/flow_speed_control/flow_speed_control_main.c
+++ b/src/examples/flow_speed_control/flow_speed_control_main.c
@@ -65,6 +65,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 #include <poll.h>
+#include <mavlink/mavlink_log.h>
 
 #include "flow_speed_control_params.h"
 
@@ -151,17 +152,23 @@ flow_speed_control_thread_main(int argc, char *argv[])
 {
 	/* welcome user */
 	thread_running = true;
-	printf("[flow speed control] starting\n");
+	static int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd,"[fsc] started");
 
 	uint32_t counter = 0;
 
 	/* structures */
 	struct actuator_armed_s armed;
+	memset(&armed, 0, sizeof(armed));
 	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct filtered_bottom_flow_s filtered_flow;
+	memset(&filtered_flow, 0, sizeof(filtered_flow));
 	struct vehicle_bodyframe_speed_setpoint_s speed_sp;
-
+	memset(&speed_sp, 0, sizeof(speed_sp));
 	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -186,6 +193,7 @@ flow_speed_control_thread_main(int argc, char *argv[])
 	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err");
 
 	static bool sensors_ready = false;
+	static bool status_changed = false;
 
 	while (!thread_should_exit)
 	{
@@ -221,7 +229,7 @@ flow_speed_control_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
 
 					parameters_update(&param_handles, &params);
-					printf("[flow speed control] parameters updated.\n");
+					mavlink_log_info(mavlink_fd,"[fsp] parameters updated.");
 				}
 
 				/* only run controller if position/speed changed */
@@ -237,6 +245,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
 					/* get a local copy of bodyframe speed setpoint */
 					orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
+					/* get a local copy of control mode */
+					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
 
 					if (control_mode.flag_control_velocity_enabled)
 					{
@@ -244,6 +254,11 @@ flow_speed_control_thread_main(int argc, char *argv[])
 						float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
 						float roll_body  =  (speed_sp.vy - filtered_flow.vy) * params.speed_p;
 
+						if(status_changed == false)
+							mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged");
+
+						status_changed = true;
+
 						/* limit roll and pitch corrections */
 						if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch))
 						{
@@ -299,6 +314,11 @@ flow_speed_control_thread_main(int argc, char *argv[])
 					}
 					else
 					{
+						if(status_changed == true)
+							mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged.");
+
+						status_changed = false;
+
 						/* reset attitude setpoint */
 						att_sp.roll_body = 0.0f;
 						att_sp.pitch_body = 0.0f;
@@ -334,20 +354,20 @@ flow_speed_control_thread_main(int argc, char *argv[])
 			else if (ret == 0)
 			{
 				/* no return value, ignore */
-				printf("[flow speed control] no attitude received.\n");
+				mavlink_log_info(mavlink_fd,"[fsc] no attitude received.");
 			}
 			else
 			{
 				if (fds[0].revents & POLLIN)
 				{
 					sensors_ready = true;
-					printf("[flow speed control] initialized.\n");
+					mavlink_log_info(mavlink_fd,"[fsp] initialized.");
 				}
 			}
 		}
 	}
 
-	printf("[flow speed control] ending now...\n");
+	mavlink_log_info(mavlink_fd,"[fsc] ending now...");
 
 	thread_running = false;
 
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 950a47fd9..3d43f89ad 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -81,7 +81,7 @@ static const hrt_abstime sonar_timeout = 150000;	// sonar timeout = 150ms
 static const hrt_abstime sonar_valid_timeout = 1000000;	// assume that altitude == distance to surface during this time
 static const hrt_abstime flow_timeout = 1000000;	// optical flow topic timeout = 1s
 static const uint32_t updates_counter_len = 1000000;
-static const uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
+static const uint32_t pub_interval = 10000;	// limit publish rate to 100 Hz
 
 __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
 
@@ -315,14 +315,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	}
 
 	/* main loop */
-	struct pollfd fds[7] = {
-		{ .fd = parameter_update_sub, .events = POLLIN },
-		{ .fd = actuator_sub, .events = POLLIN },
-		{ .fd = armed_sub, .events = POLLIN },
+	struct pollfd fds[1] = {
 		{ .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 }
 	};
 
 	if (!thread_should_exit) {
@@ -330,7 +324,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	}
 
 	while (!thread_should_exit) {
-		int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+		int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
 		hrt_abstime t = hrt_absolute_time();
 
 		if (ret < 0) {
@@ -340,36 +334,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			continue;
 
 		} else if (ret > 0) {
+			/* act on attitude updates */
+
+			/* vehicle attitude */
+			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+			attitude_updates++;
+
+			bool updated;
+
 			/* parameter update */
-			if (fds[0].revents & POLLIN) {
-				/* read from param to clear updated flag */
+			orb_check(parameter_update_sub, &updated);
+
+			if (updated) {
 				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), parameter_update_sub,
-					 &update);
-				/* update parameters */
+				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
 				parameters_update(&pos_inav_param_handles, &params);
 			}
 
 			/* actuator */
-			if (fds[1].revents & POLLIN) {
+			orb_check(actuator_sub, &updated);
+
+			if (updated) {
 				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
 			}
 
 			/* armed */
-			if (fds[2].revents & POLLIN) {
-				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-			}
+			orb_check(armed_sub, &updated);
 
-			/* vehicle attitude */
-			if (fds[3].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-				attitude_updates++;
+			if (updated) {
+				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
 			}
 
 			/* sensor combined */
-			if (fds[4].revents & POLLIN) {
-				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+			orb_check(sensor_combined_sub, &updated);
 
+			if (updated) {
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 				if (sensor.accelerometer_counter > accel_counter) {
 					if (att.R_valid) {
 						/* correct accel bias, now only for Z */
@@ -406,7 +406,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 
 			/* optical flow */
-			if (fds[5].revents & POLLIN) {
+			orb_check(optical_flow_sub, &updated);
+			if (updated) {
 				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
 
 				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 || t < sonar_time + sonar_timeout)) {
@@ -488,7 +489,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 
 			/* vehicle GPS position */
-			if (fds[6].revents & POLLIN) {
+			orb_check(vehicle_gps_position_sub, &updated);
+			if (updated) {
 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 
 				if (gps.fix_type >= 3) {