Merge branch 'master' of github.com:PX4/Firmware into ekf_varweight

10 files changed, 147 insertions, 55 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 6c2c03070..74deda8cc 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -129,7 +129,7 @@ extern struct system_load_s system_load;
 #define POSITION_TIMEOUT		(2 * 1000 * 1000)	/**< consider the local or global position estimate invalid after 600ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
 #define RC_TIMEOUT			500000
-#define DL_TIMEOUT			5 * 1000* 1000
+#define DL_TIMEOUT			(10 * 1000 * 1000)
 #define OFFBOARD_TIMEOUT		500000
 #define DIFFPRESS_TIMEOUT		2000000
 
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 2faf8ab76..ed7e879d3 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -217,6 +217,8 @@ Mavlink::Mavlink() :
 		errx(1, "instance ID is out of range");
 		break;
 	}
+
+	_rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 }
 
 Mavlink::~Mavlink()
@@ -1227,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[])
 				_mode = MAVLINK_MODE_CUSTOM;
 
 			} else if (strcmp(optarg, "camera") == 0) {
-				_mode = MAVLINK_MODE_CAMERA;
+				// left in here for compatibility
+				_mode = MAVLINK_MODE_ONBOARD;
+			} else if (strcmp(optarg, "onboard") == 0) {
+				_mode = MAVLINK_MODE_ONBOARD;
 			}
 
 			break;
@@ -1287,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[])
 		warnx("mode: CUSTOM");
 		break;
 
-	case MAVLINK_MODE_CAMERA:
-		warnx("mode: CAMERA");
+	case MAVLINK_MODE_ONBOARD:
+		warnx("mode: ONBOARD");
 		break;
 
 	default:
@@ -1391,9 +1396,10 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
 		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
+		configure_stream("OPTICAL_FLOW", 0.5f);
 		break;
 
-	case MAVLINK_MODE_CAMERA:
+	case MAVLINK_MODE_ONBOARD:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("ATTITUDE", 15.0f);
 		configure_stream("GLOBAL_POSITION_INT", 15.0f);
@@ -1653,6 +1659,8 @@ Mavlink::display_status()
 		printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
 	}
 
+	printf("\tmavlink chan: #%u\n", _channel);
+
 	if (_rstatus.timestamp > 0) {
 
 		printf("\ttype:\t\t");
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1e2e94cef..7f9d225bb 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -127,7 +127,7 @@ public:
 	enum MAVLINK_MODE {
 		MAVLINK_MODE_NORMAL = 0,
 		MAVLINK_MODE_CUSTOM,
-		MAVLINK_MODE_CAMERA
+		MAVLINK_MODE_ONBOARD
 	};
 
 	void			set_mode(enum MAVLINK_MODE);
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 7a761588a..d436c95e9 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -58,6 +58,10 @@
 #endif
 static const int ERROR = -1;
 
+#define CHECK_SYSID_COMPID_MISSION(_msg)		(_msg.target_system == mavlink_system.sysid && \
+						((_msg.target_component == mavlink_system.compid) || \
+						(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
+						(_msg.target_component == MAV_COMP_ID_ALL)))
 
 MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_state(MAVLINK_WPM_STATE_IDLE),
@@ -79,8 +83,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
 	_mission_result_sub(-1),
 	_offboard_mission_pub(-1),
 	_slow_rate_limiter(_interval / 10.0f),
-	_verbose(false),
-	_comp_id(MAV_COMP_ID_MISSIONPLANNER)
+	_verbose(false)
 {
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
@@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
 	mavlink_mission_ack_t wpa;
 	mavlink_msg_mission_ack_decode(msg, &wpa);
 
-	if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpa)) {
 		if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
 			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
 				_time_last_recv = hrt_absolute_time();
@@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
 	mavlink_mission_set_current_t wpc;
 	mavlink_msg_mission_set_current_decode(msg, &wpc);
 
-	if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpc)) {
 		if (_state == MAVLINK_WPM_STATE_IDLE) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
 	mavlink_mission_request_list_t wprl;
 	mavlink_msg_mission_request_list_decode(msg, &wprl);
 
-	if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wprl)) {
 		if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
 	mavlink_mission_request_t wpr;
 	mavlink_msg_mission_request_decode(msg, &wpr);
 
-	if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpr)) {
 		if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
 			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
 				_time_last_recv = hrt_absolute_time();
@@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 	mavlink_mission_count_t wpc;
 	mavlink_msg_mission_count_decode(msg, &wpc);
 
-	if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpc)) {
 		if (_state == MAVLINK_WPM_STATE_IDLE) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
 	mavlink_mission_item_t wp;
 	mavlink_msg_mission_item_decode(msg, &wp);
 
-	if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
+	if (CHECK_SYSID_COMPID_MISSION(wp)) {
 		if (_state == MAVLINK_WPM_STATE_GETLIST) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
 	mavlink_mission_clear_all_t wpca;
 	mavlink_msg_mission_clear_all_decode(msg, &wpca);
 
-	if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
+	if (CHECK_SYSID_COMPID_MISSION(wpca)) {
 
 		if (_state == MAVLINK_WPM_STATE_IDLE) {
 			/* don't touch mission items storage itself, but only items count in mission state */
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index 8ff27f87d..a7bb94c22 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -126,8 +126,6 @@ private:
 
 	bool _verbose;
 
-	uint8_t			_comp_id;
-
 	/* do not allow top copying this class */
 	MavlinkMissionManager(MavlinkMissionManager &);
 	MavlinkMissionManager& operator = (const MavlinkMissionManager &);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c0fae0a2f..a602344fd 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
-	_radio_status_available(false),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
@@ -137,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_command_long(msg);
 		break;
 
+	case MAVLINK_MSG_ID_COMMAND_INT:
+		handle_message_command_int(msg);
+		break;
+
 	case MAVLINK_MSG_ID_OPTICAL_FLOW:
 		handle_message_optical_flow(msg);
 		break;
@@ -277,6 +280,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
+{
+	/* command */
+	mavlink_command_int_t cmd_mavlink;
+	mavlink_msg_command_int_decode(msg, &cmd_mavlink);
+
+	if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
+			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+		//check for MAVLINK terminate command
+		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
+			/* This is the link shutdown command, terminate mavlink */
+			warnx("terminated by remote command");
+			fflush(stdout);
+			usleep(50000);
+
+			/* terminate other threads and this thread */
+			_mavlink->_task_should_exit = true;
+
+		} else {
+
+			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				      mavlink_system.sysid, mavlink_system.compid);
+				return;
+			}
+
+			struct vehicle_command_s vcmd;
+			memset(&vcmd, 0, sizeof(vcmd));
+
+			/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
+			vcmd.param1 = cmd_mavlink.param1;
+			vcmd.param2 = cmd_mavlink.param2;
+			vcmd.param3 = cmd_mavlink.param3;
+			vcmd.param4 = cmd_mavlink.param4;
+			/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
+			vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
+			vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
+			vcmd.param7 = cmd_mavlink.z;
+			// XXX do proper translation
+			vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
+			vcmd.target_system = cmd_mavlink.target_system;
+			vcmd.target_component = cmd_mavlink.target_component;
+			vcmd.source_system = msg->sysid;
+			vcmd.source_component = msg->compid;
+
+			if (_cmd_pub < 0) {
+				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
+			} else {
+				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
+			}
+		}
+	}
+}
+
+void
 MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 {
 	/* optical flow */
@@ -430,9 +489,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 		} else {
 			orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
 		}
-
-		/* this means that heartbeats alone won't be published to the radio status no more */
-		_radio_status_available = true;
 	}
 }
 
@@ -474,25 +530,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 
 			struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
 
-			hrt_abstime tnow = hrt_absolute_time();
-
-			/* always set heartbeat, publish only if telemetry link not up */
-			tstatus.heartbeat_time = tnow;
-
-			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
-			if (!_radio_status_available) {
+			/* set heartbeat time and topic time and publish -
+			 * the telem status also gets updated on telemetry events
+			 */
+			tstatus.timestamp = hrt_absolute_time();
+			tstatus.heartbeat_time = tstatus.timestamp;
 
-				tstatus.timestamp = tnow;
-				/* telem_time indicates the timestamp of a telemetry status packet and we got none */
-				tstatus.telem_time = 0;
-				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+			if (_telemetry_status_pub < 0) {
+				_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-				if (_telemetry_status_pub < 0) {
-					_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
-
-				} else {
-					orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
-				}
+			} else {
+				orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
 			}
 		}
 	}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 014193100..91125736c 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -110,6 +110,7 @@ private:
 
 	void handle_message(mavlink_message_t *msg);
 	void handle_message_command_long(mavlink_message_t *msg);
+	void handle_message_command_int(mavlink_message_t *msg);
 	void handle_message_optical_flow(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
@@ -151,7 +152,6 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
-	bool _radio_status_available;
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 331a9a728..042c46afd 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -353,6 +353,8 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ACRO:
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
+			case NAVIGATION_STATE_LAND:
+			case NAVIGATION_STATE_TERMINATION:
 				_navigation_mode = nullptr;
 				_can_loiter_at_sp = false;
 				break;
@@ -368,8 +370,6 @@ Navigator::task_main()
 			case NAVIGATION_STATE_AUTO_RTGS:
 				_navigation_mode = &_rtl; /* TODO: change this to something else */
 				break;
-			case NAVIGATION_STATE_LAND:
-			case NAVIGATION_STATE_TERMINATION:
 			case NAVIGATION_STATE_OFFBOARD:
 				_navigation_mode = &_offboard;
 				break;
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 7ce6ef5ef..229bfe3ce 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
 
+/**
+ * QNH for barometer
+ *
+ * @min 500
+ * @max 1500
+ * @group Sensor Calibration
+ * @unit hPa
+ */
+PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
+
 
 /**
  * Board rotation
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4e8a8c01d..f40034d79 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -143,6 +143,12 @@
 
 #define STICK_ON_OFF_LIMIT 0.75f
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
 /**
  * Sensor app start / stop handling function
  *
@@ -235,7 +241,7 @@ private:
 	math::Matrix<3, 3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
-	
+
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
 	hrt_abstime _battery_current_timestamp;	/**< timestamp of last battery current reading */
 
@@ -258,7 +264,7 @@ private:
 
 		int board_rotation;
 		int external_mag_rotation;
-		
+
 		float board_offset[3];
 
 		int rc_map_roll;
@@ -301,6 +307,8 @@ private:
 		float battery_voltage_scaling;
 		float battery_current_scaling;
 
+		float baro_qnh;
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -354,9 +362,11 @@ private:
 
 		param_t board_rotation;
 		param_t external_mag_rotation;
-		
+
 		param_t board_offset[3];
 
+		param_t baro_qnh;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -462,12 +472,6 @@ private:
 namespace sensors
 {
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 Sensors	*g_sensors = nullptr;
 }
 
@@ -611,12 +615,15 @@ Sensors::Sensors() :
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
-	
+
 	/* rotation offsets */
 	_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
 	_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
 	_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
 
+	/* Barometer QNH */
+	_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -828,19 +835,37 @@ Sensors::parameters_update()
 
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
-	
+
 	param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
 	param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
 	param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
-	
+
 	/** fine tune board offset on parameter update **/
-	math::Matrix<3, 3> board_rotation_offset; 
+	math::Matrix<3, 3> board_rotation_offset;
 	board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
 							 M_DEG_TO_RAD_F * _parameters.board_offset[1],
 							 M_DEG_TO_RAD_F * _parameters.board_offset[2]);
-	
+
 	_board_rotation = _board_rotation * board_rotation_offset;
 
+	/* update barometer qnh setting */
+	param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
+	int	fd;
+	fd = open(BARO_DEVICE_PATH, 0);
+	if (fd < 0) {
+		warn("%s", BARO_DEVICE_PATH);
+		errx(1, "FATAL: no barometer found");
+
+	} else {
+		int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
+		if (ret) {
+			warnx("qnh could not be set");
+			close(fd);
+			return ERROR;
+		}
+		close(fd);
+	}
+
 	return OK;
 }