Merge branch 'master' into mpc_rc

5 files changed, 39 insertions, 12 deletions

diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index ad435b251..e49288a74 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -62,6 +62,12 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
  * @group MAVLink
  */
 PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
+/**
+ * Use/Accept HIL GPS message (even if not in HIL mode)
+ * If set to 1 incomming HIL GPS messages are parsed.
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
 
 mavlink_system_t mavlink_system = {
 	100,
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 227e99b48..7ecca0d65 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -208,6 +208,7 @@ Mavlink::Mavlink() :
 	_mavlink_fd(-1),
 	_task_running(false),
 	_hil_enabled(false),
+	_use_hil_gps(false),
 	_is_usb_uart(false),
 	_wait_to_transmit(false),
 	_received_messages(false),
@@ -487,11 +488,13 @@ void Mavlink::mavlink_update_system(void)
 	static param_t param_system_id;
 	static param_t param_component_id;
 	static param_t param_system_type;
+	static param_t param_use_hil_gps;
 
 	if (!initialized) {
 		param_system_id = param_find("MAV_SYS_ID");
 		param_component_id = param_find("MAV_COMP_ID");
 		param_system_type = param_find("MAV_TYPE");
+		param_use_hil_gps = param_find("MAV_USEHILGPS");
 		initialized = true;
 	}
 
@@ -516,6 +519,11 @@ void Mavlink::mavlink_update_system(void)
 	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
 		mavlink_system.type = system_type;
 	}
+
+	int32_t use_hil_gps;
+	param_get(param_use_hil_gps, &use_hil_gps);
+
+	_use_hil_gps = (bool)use_hil_gps;
 }
 
 int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 66d82b471..1bf51fd31 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -157,6 +157,8 @@ public:
 
 	bool		get_hil_enabled() { return _hil_enabled; }
 
+	bool		get_use_hil_gps() { return _use_hil_gps; }
+
 	bool		get_flow_control_enabled() { return _flow_control_enabled; }
 
 	bool		get_forwarding_on() { return _forwarding_on; }
@@ -223,6 +225,7 @@ private:
 
 	/* states */
 	bool		_hil_enabled;		/**< Hardware In the Loop mode */
+	bool		_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
 	bool		_is_usb_uart;		/**< Port is USB */
 	bool        _wait_to_transmit;  /**< Wait to transmit until received messages. */
 	bool        _received_messages; /**< Whether we've received valid mavlink messages. */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 0762b76f2..33a4fef12 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -160,6 +160,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	 * The HIL mode is enabled by the HIL bit flag
 	 * in the system mode. Either send a set mode
 	 * COMMAND_LONG message or a SET_MODE message
+	 *
+	 * Accept HIL GPS messages if use_hil_gps flag is true.
+	 * This allows to provide fake gps measurements to the system.
 	 */
 	if (_mavlink->get_hil_enabled()) {
 		switch (msg->msgid) {
@@ -167,10 +170,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 			handle_message_hil_sensor(msg);
 			break;
 
-		case MAVLINK_MSG_ID_HIL_GPS:
-			handle_message_hil_gps(msg);
-			break;
-
 		case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
 			handle_message_hil_state_quaternion(msg);
 			break;
@@ -180,7 +179,20 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		}
 	}
 
-	/* If we've received a valid message, mark the flag indicating so.
+
+   	if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
+		switch (msg->msgid) {
+		case MAVLINK_MSG_ID_HIL_GPS:
+			handle_message_hil_gps(msg);
+			break;
+
+		default:
+			break;
+		}
+
+	}
+	
+    /* If we've received a valid message, mark the flag indicating so.
 	   This is used in the '-w' command-line flag. */
 	_mavlink->set_has_received_messages(true);
 }
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 41e2148ac..a73599526 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1242,7 +1242,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				}
 			}
 			_last_adc = t;
-			if (_battery_status.voltage_v > 0.0f) {
+			if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) {
 				/* announce the battery status if needed, just publish else */
 				if (_battery_pub > 0) {
 					orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
@@ -1527,12 +1527,10 @@ Sensors::task_main()
 
 	while (!_task_should_exit) {
 
-		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		/* wait for up to 50ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
 
-		/* timed out - periodic check for _task_should_exit, etc. */
-		if (pret == 0)
-			continue;
+		/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -1571,7 +1569,7 @@ Sensors::task_main()
 		perf_end(_loop_perf);
 	}
 
-	printf("[sensors] exiting.\n");
+	warnx("[sensors] exiting.");
 
 	_sensors_task = -1;
 	_exit(0);