Merge branch 'master' into offboard2_merge

Conflicts:
	src/modules/commander/commander.cpp

33 files changed, 423 insertions, 175 deletions

diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
new file mode 100644
index 000000000..3bf02fbff
--- /dev/null
+++ b/CONTRIBUTING.md
@@ -0,0 +1,44 @@
+# Contributing to PX4 Firmware
+
+We follow the [Github flow](https://guides.github.com/introduction/flow/) development model.
+
+### Fork the project, then clone your repo
+
+First [fork and clone](https://help.github.com/articles/fork-a-repo) the project project.
+
+### Create a feature branch
+
+*Always* branch off master for new features.
+
+```
+git checkout -b mydescriptivebranchname
+```
+
+### Edit and build the code
+
+The [developer guide](http://pixhawk.org/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://pixhawk.org/dev/code_style) when editing files.
+
+### Commit your changes
+
+Always write descriptive commit messages and add a fixes or relates note to them with an [issue number](https://github.com/px4/Firmware/issues) (Github will link these then conveniently)
+
+**Example:**
+
+```
+Change how the attitude controller works
+
+- Fixes rate feed forward
+- Allows a local body rate override
+
+Fixes issue #123
+```
+
+### Test your changes
+
+Since we care about safety, we will regularly ask you for test results. Best is to do a test flight (or bench test where it applies) and upload the logfile from it (on the microSD card in the logs directory) to Google Drive or Dropbox and share the link.
+
+### Push your changes
+
+Push changes to your repo and send a [pull request](https://github.com/PX4/Firmware/compare/).
+
+Make sure to provide some testing feedback and if possible the link to a flight log file.
diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
new file mode 100644
index 000000000..99ffd73a5
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
@@ -0,0 +1,28 @@
+#!nsh
+#
+# Hobbyking Micro Integrated PCB Quadcopter
+# with SimonK ESC firmware and Mystery A1510 motors
+#
+# Thomas Gubler <thomasgubler@gmail.com>
+#
+echo "HK Micro PCB Quad"
+
+sh /etc/init.d/4001_quad_x
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.1
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.003
+	param set MC_YAW_P 2.8
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_D 0.0
+fi
+
+set PWM_MIN 1200
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index b365bd642..5d9e9502c 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -136,6 +136,11 @@ then
 	sh /etc/init.d/4011_dji_f450
 fi
 
+if param compare SYS_AUTOSTART 4020
+then
+	sh /etc/init.d/4020_hk_micro_pcb
+fi
+
 #
 # Quad +
 #
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 4b6b0a4d2..8e83df391 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -33,7 +33,6 @@ MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
 MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
-MODULES		+= drivers/roboclaw
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 66b2157ed..66d9efbf8 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -25,6 +25,7 @@ MODULES		+= drivers/l3gd20
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/pca8574
+MODULES		+= drivers/roboclaw
 MODULES		+= systemcmds/perf
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/tests
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 92e089217..6b14f5945 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -293,7 +293,7 @@ BlinkM::BlinkM(int bus, int blinkm) :
 	safety_sub_fd(-1),
 	num_of_cells(0),
 	detected_cells_runcount(0),
-	t_led_color({0}),
+	t_led_color{0},
 	t_led_blink(0),
 	led_thread_runcount(0),
 	led_interval(1000),
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 241e3bdf3..401b65dd4 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -299,7 +299,6 @@ GPS::task_main()
 			_report_gps_pos.alt = (int32_t)1200e3f;
 			_report_gps_pos.timestamp_variance = hrt_absolute_time();
 			_report_gps_pos.s_variance_m_s = 10.0f;
-			_report_gps_pos.p_variance_m = 10.0f;
 			_report_gps_pos.c_variance_rad = 0.1f;
 			_report_gps_pos.fix_type = 3;
 			_report_gps_pos.eph = 0.9f;
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 44434a1df..d0854f5e9 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -95,7 +95,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate
 	_got_velned(false),
 	_disable_cmd_last(0),
 	_ack_waiting_msg(0),
-	_ubx_version(0)
+	_ubx_version(0),
+	_use_nav_pvt(false)
 {
 	decode_init();
 }
@@ -190,38 +191,45 @@ UBX::configure(unsigned &baudrate)
 
 	/* configure message rates */
 	/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
-	configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
 
+	/* try to set rate for NAV-PVT */
+	/* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */
+	configure_message_rate(UBX_MSG_NAV_PVT, 1);
 	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
-		return 1;
-	}
-
-	configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
-
-	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
-		return 1;
+		_use_nav_pvt = false;
+	} else {
+		_use_nav_pvt = true;
 	}
+	UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
 
-	configure_message_rate(UBX_MSG_NAV_SOL, 1);
+	if (!_use_nav_pvt) {
+		configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 
-	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
-		return 1;
-	}
+		configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 
-	configure_message_rate(UBX_MSG_NAV_VELNED, 1);
+		configure_message_rate(UBX_MSG_NAV_SOL, 1);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 
-	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
-		return 1;
+		configure_message_rate(UBX_MSG_NAV_VELNED, 1);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 	}
 
 	configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0);
-
 	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
 		return 1;
 	}
 
 	configure_message_rate(UBX_MSG_MON_HW, 1);
-
 	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
 		return 1;
 	}
@@ -454,11 +462,23 @@ UBX::payload_rx_init()
 	_rx_state = UBX_RXMSG_HANDLE;	// handle by default
 
 	switch (_rx_msg) {
+	case UBX_MSG_NAV_PVT:
+		if (   (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7)		/* u-blox 7 msg format */
+		    && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8))	/* u-blox 8+ msg format */
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (!_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if not using NAV-PVT
+		break;
+
 	case UBX_MSG_NAV_POSLLH:
 		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t))
 			_rx_state = UBX_RXMSG_ERROR_LENGTH;
 		else if (!_configured)
 			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
 		break;
 
 	case UBX_MSG_NAV_SOL:
@@ -466,6 +486,8 @@ UBX::payload_rx_init()
 			_rx_state = UBX_RXMSG_ERROR_LENGTH;
 		else if (!_configured)
 			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
 		break;
 
 	case UBX_MSG_NAV_TIMEUTC:
@@ -473,6 +495,8 @@ UBX::payload_rx_init()
 			_rx_state = UBX_RXMSG_ERROR_LENGTH;
 		else if (!_configured)
 			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
 		break;
 
 	case UBX_MSG_NAV_SVINFO:
@@ -489,6 +513,8 @@ UBX::payload_rx_init()
 			_rx_state = UBX_RXMSG_ERROR_LENGTH;
 		else if (!_configured)
 			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
 		break;
 
 	case UBX_MSG_MON_VER:
@@ -675,6 +701,68 @@ UBX::payload_rx_done(void)
 	// handle message
 	switch (_rx_msg) {
 
+	case UBX_MSG_NAV_PVT:
+		UBX_TRACE_RXMSG("Rx NAV-PVT\n");
+
+		_gps_position->fix_type		= _buf.payload_rx_nav_pvt.fixType;
+		_gps_position->satellites_used	= _buf.payload_rx_nav_pvt.numSV;
+
+		_gps_position->lat		= _buf.payload_rx_nav_pvt.lat;
+		_gps_position->lon		= _buf.payload_rx_nav_pvt.lon;
+		_gps_position->alt		= _buf.payload_rx_nav_pvt.hMSL;
+
+		_gps_position->eph		= (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f;
+		_gps_position->epv		= (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f;
+		_gps_position->s_variance_m_s	= (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f;
+
+		_gps_position->vel_m_s		= (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f;
+
+		_gps_position->vel_n_m_s	= (float)_buf.payload_rx_nav_pvt.velN * 1e-3f;
+		_gps_position->vel_e_m_s	= (float)_buf.payload_rx_nav_pvt.velE * 1e-3f;
+		_gps_position->vel_d_m_s	= (float)_buf.payload_rx_nav_pvt.velD * 1e-3f;
+		_gps_position->vel_ned_valid	= true;
+
+		_gps_position->cog_rad		= (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f;
+		_gps_position->c_variance_rad	= (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f;
+
+		{
+			/* convert to unix timestamp */
+			struct tm timeinfo;
+			timeinfo.tm_year	= _buf.payload_rx_nav_pvt.year - 1900;
+			timeinfo.tm_mon		= _buf.payload_rx_nav_pvt.month - 1;
+			timeinfo.tm_mday	= _buf.payload_rx_nav_pvt.day;
+			timeinfo.tm_hour	= _buf.payload_rx_nav_pvt.hour;
+			timeinfo.tm_min		= _buf.payload_rx_nav_pvt.min;
+			timeinfo.tm_sec		= _buf.payload_rx_nav_pvt.sec;
+			time_t epoch = mktime(&timeinfo);
+
+#ifndef CONFIG_RTC
+			//Since we lack a hardware RTC, set the system time clock based on GPS UTC
+			//TODO generalize this by moving into gps.cpp?
+			timespec ts;
+			ts.tv_sec = epoch;
+			ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
+			clock_settime(CLOCK_REALTIME, &ts);
+#endif
+
+			_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+			_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f);
+		}
+
+		_gps_position->timestamp_time		= hrt_absolute_time();
+		_gps_position->timestamp_velocity 	= hrt_absolute_time();
+		_gps_position->timestamp_variance 	= hrt_absolute_time();
+		_gps_position->timestamp_position	= hrt_absolute_time();
+
+		_rate_count_vel++;
+		_rate_count_lat_lon++;
+
+		_got_posllh = true;
+		_got_velned = true;
+
+		ret = 1;
+		break;
+
 	case UBX_MSG_NAV_POSLLH:
 		UBX_TRACE_RXMSG("Rx NAV-POSLLH\n");
 
@@ -697,7 +785,6 @@ UBX::payload_rx_done(void)
 
 		_gps_position->fix_type		= _buf.payload_rx_nav_sol.gpsFix;
 		_gps_position->s_variance_m_s	= (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f;	// from cm to m
-		_gps_position->p_variance_m	= (float)_buf.payload_rx_nav_sol.pAcc * 1e-2f;	// from cm to m
 		_gps_position->satellites_used	= _buf.payload_rx_nav_sol.numSV;
 
 		_gps_position->timestamp_variance = hrt_absolute_time();
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 1a9e7cb0c..219a5762a 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -104,6 +104,13 @@
 #define UBX_RX_NAV_PVT_FLAGS_PSMSTATE		0x1C	/**< psmState (Power Save Mode state (see Power Management)) */
 #define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID	0x20	/**< headVehValid (Heading of vehicle is valid) */
 
+/* RX NAV-TIMEUTC message content details */
+/*   Bitfield "valid" masks */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDTOW	0x01	/**< validTOW (1 = Valid Time of Week) */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDKWN	0x02	/**< validWKN (1 = Valid Week Number) */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC	0x04	/**< validUTC (1 = Valid UTC Time) */
+#define UBX_RX_NAV_TIMEUTC_VALID_UTCSTANDARD	0xF0	/**< utcStandard (0..15 = UTC standard identifier) */
+
 /* TX CFG-PRT message contents */
 #define UBX_TX_CFG_PRT_PORTID		0x01		/**< UART1 */
 #define UBX_TX_CFG_PRT_MODE		0x000008D0	/**< 0b0000100011010000: 8N1 */
@@ -176,7 +183,7 @@ typedef struct {
 	uint32_t	reserved2;
 } ubx_payload_rx_nav_sol_t;
 
-/* Rx NAV-PVT */
+/* Rx NAV-PVT (ubx8) */
 typedef struct {
 	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
 	uint16_t	year; 		/**< Year (UTC)*/
@@ -185,7 +192,7 @@ typedef struct {
 	uint8_t		hour; 		/**< Hour of day, range 0..23 (UTC) */
 	uint8_t		min; 		/**< Minute of hour, range 0..59 (UTC) */
 	uint8_t		sec;		/**< Seconds of minute, range 0..60 (UTC) */
-	uint8_t		valid; 		/**< Validity flags  (see UBX_RX_NAV_PVT_VALID_...) */
+	uint8_t		valid; 		/**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */
 	uint32_t	tAcc; 		/**< Time accuracy estimate (UTC) [ns] */
 	int32_t		nano;		/**< Fraction of second (UTC) [-1e9...1e9 ns] */
 	uint8_t		fixType;	/**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */
@@ -208,9 +215,11 @@ typedef struct {
 	uint16_t	pDOP;		/**< Position DOP [0.01] */
 	uint16_t	reserved2;
 	uint32_t	reserved3;
-	int32_t		headVeh;	/**< Heading of vehicle (2-D) [1e-5 deg] */
-	uint32_t	reserved4;
+	int32_t		headVeh;	/**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */
+	uint32_t	reserved4;	/**< (ubx8+ only) */
 } ubx_payload_rx_nav_pvt_t;
+#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7	(sizeof(ubx_payload_rx_nav_pvt_t) - 8)
+#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8	(sizeof(ubx_payload_rx_nav_pvt_t))
 
 /* Rx NAV-TIMEUTC */
 typedef struct {
@@ -223,7 +232,7 @@ typedef struct {
 	uint8_t		hour; 		/**< Hour of day, range 0..23 (UTC) */
 	uint8_t		min; 		/**< Minute of hour, range 0..59 (UTC) */
 	uint8_t		sec;		/**< Seconds of minute, range 0..60 (UTC) */
-	uint8_t		valid; 		/**< Validity Flags (see ubx documentation) */
+	uint8_t		valid; 		/**< Validity Flags (see UBX_RX_NAV_TIMEUTC_VALID_...) */
 } ubx_payload_rx_nav_timeutc_t;
 
 /* Rx NAV-SVINFO Part 1 */
@@ -388,6 +397,7 @@ typedef struct {
 
 /* General message and payload buffer union */
 typedef union {
+	ubx_payload_rx_nav_pvt_t		payload_rx_nav_pvt;
 	ubx_payload_rx_nav_posllh_t		payload_rx_nav_posllh;
 	ubx_payload_rx_nav_sol_t		payload_rx_nav_sol;
 	ubx_payload_rx_nav_timeutc_t		payload_rx_nav_timeutc;
@@ -526,6 +536,7 @@ private:
 	uint16_t		_ack_waiting_msg;
 	ubx_buf_t		_buf;
 	uint32_t		_ubx_version;
+	bool			_use_nav_pvt;
 };
 
 #endif /* UBX_H_ */
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 212e33ff8..e600976ce 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -294,23 +294,21 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
 		// calculate the position of the start and end points.  We should not be doing this often
 		// as this function generally will not be called repeatedly when we are out of the sector.
 
-		// TO DO - this is messed up and won't compile
-		float start_disp_x = radius * sinf(arc_start_bearing);
-		float start_disp_y = radius * cosf(arc_start_bearing);
-		float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep));
-		float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep));
-		float lon_start = lon_now + start_disp_x / 111111.0f;
-		float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f;
-		float lon_end = lon_now + end_disp_x / 111111.0f;
-		float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f;
-		float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
-		float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+		double start_disp_x = (double)radius * sin(arc_start_bearing);
+		double start_disp_y = (double)radius * cos(arc_start_bearing);
+		double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep)));
+		double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep)));
+		double lon_start = lon_now + start_disp_x / 111111.0;
+		double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
+		double lon_end = lon_now + end_disp_x / 111111.0;
+		double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0;
+		double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
+		double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
 
 
 		if (dist_to_start < dist_to_end) {
 			crosstrack_error->distance = dist_to_start;
 			crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
-
 		} else {
 			crosstrack_error->past_end = true;
 			crosstrack_error->distance = dist_to_end;
@@ -319,7 +317,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
 
 	}
 
-	crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing);
+	crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing);
 	return_value = OK;
 	return return_value;
 }
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 38297745d..bef92d8fc 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -478,7 +478,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
 			// We use an float epsilon delta to test float equality.
 			if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
-				mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
+				mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1);
 
 			} else {
 
@@ -640,7 +640,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* welcome user */
 	warnx("starting");
 
-	char *main_states_str[MAIN_STATE_MAX];
+	const char *main_states_str[MAIN_STATE_MAX];
 	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
 	main_states_str[MAIN_STATE_ALTCTL]			= "ALTCTL";
 	main_states_str[MAIN_STATE_POSCTL]			= "POSCTL";
@@ -650,7 +650,7 @@ int commander_thread_main(int argc, char *argv[])
 	main_states_str[MAIN_STATE_ACRO]			= "ACRO";
 	main_states_str[MAIN_STATE_OFFBOARD]			= "OFFBOARD";
 
-	char *arming_states_str[ARMING_STATE_MAX];
+	const char *arming_states_str[ARMING_STATE_MAX];
 	arming_states_str[ARMING_STATE_INIT]			= "INIT";
 	arming_states_str[ARMING_STATE_STANDBY]			= "STANDBY";
 	arming_states_str[ARMING_STATE_ARMED]			= "ARMED";
@@ -659,7 +659,7 @@ int commander_thread_main(int argc, char *argv[])
 	arming_states_str[ARMING_STATE_REBOOT]			= "REBOOT";
 	arming_states_str[ARMING_STATE_IN_AIR_RESTORE]		= "IN_AIR_RESTORE";
 
-	char *nav_states_str[NAVIGATION_STATE_MAX];
+	const char *nav_states_str[NAVIGATION_STATE_MAX];
 	nav_states_str[NAVIGATION_STATE_MANUAL]			= "MANUAL";
 	nav_states_str[NAVIGATION_STATE_ALTCTL]			= "ALTCTL";
 	nav_states_str[NAVIGATION_STATE_POSCTL]			= "POSCTL";
@@ -781,7 +781,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	bool updated = false;
 
-	bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
+	rc_calibration_check(mavlink_fd);
 
 	/* Subscribe to safety topic */
 	int safety_sub = orb_subscribe(ORB_ID(safety));
@@ -803,10 +803,16 @@ int commander_thread_main(int argc, char *argv[])
 	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
 	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
-	/* Subscribe to telemetry status */
-	int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
-	struct telemetry_status_s telemetry;
-	memset(&telemetry, 0, sizeof(telemetry));
+	/* Subscribe to telemetry status topics */
+	int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
+	uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
+	bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
+
+	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+		telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
+		telemetry_last_heartbeat[i] = 0;
+		telemetry_lost[i] = true;
+	}
 
 	/* Subscribe to global position */
 	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -888,7 +894,6 @@ int commander_thread_main(int argc, char *argv[])
 	bool arming_state_changed = false;
 	bool main_state_changed = false;
 	bool failsafe_old = false;
-	bool system_checked = false;
 
 	while (!thread_should_exit) {
 
@@ -936,7 +941,7 @@ int commander_thread_main(int argc, char *argv[])
 				status_changed = true;
 
 				/* re-check RC calibration */
-				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
+				rc_calibration_check(mavlink_fd);
 			}
 
 			/* navigation parameters */
@@ -945,15 +950,6 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
 		}
 
-		/* Perform system checks (again) once params are loaded and MAVLink is up. */
-		if (!system_checked && mavlink_fd && 
-			(telemetry.heartbeat_time > 0) &&
-			(hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) {
-
-			(void)rc_calibration_check(mavlink_fd);
-			system_checked = true;
-		}
-
 		orb_check(sp_man_sub, &updated);
 
 		if (updated) {
@@ -979,10 +975,26 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		orb_check(telemetry_sub, &updated);
+		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			orb_check(telemetry_subs[i], &updated);
 
-		if (updated) {
-			orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry);
+			if (updated) {
+				struct telemetry_status_s telemetry;
+				memset(&telemetry, 0, sizeof(telemetry));
+
+				orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry);
+
+				/* perform system checks when new telemetry link connected */
+				if (mavlink_fd &&
+					telemetry_last_heartbeat[i] == 0 &&
+					telemetry.heartbeat_time > 0 &&
+					hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
+
+					(void)rc_calibration_check(mavlink_fd);
+				}
+
+				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
+			}
 		}
 
 		orb_check(sensor_sub, &updated);
@@ -1386,18 +1398,35 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* data link check */
-		if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
+		/* data links check */
+		bool have_link = false;
+		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
+				/* handle the case where data link was regained */
+				if (telemetry_lost[i]) {
+					mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i);
+					telemetry_lost[i] = false;
+				}
+				have_link = true;
+
+			} else {
+				if (!telemetry_lost[i]) {
+					mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i);
+					telemetry_lost[i] = true;
+				}
+			}
+		}
+
+		if (have_link) {
 			/* handle the case where data link was regained */
 			if (status.data_link_lost) {
-				mavlink_log_critical(mavlink_fd, "#audio: data link regained");
 				status.data_link_lost = false;
 				status_changed = true;
 			}
 
 		} else {
 			if (!status.data_link_lost) {
-				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
+				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST");
 				status.data_link_lost = true;
 				status_changed = true;
 			}
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index e4f805dc0..5d768b73d 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1533,7 +1533,7 @@ FixedwingEstimator::start()
 	_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 40,
-					 6000,
+					 5000,
 					 (main_t)&FixedwingEstimator::task_main_trampoline,
 					 nullptr);
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index 9622f7e40..deaaf55fe 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -2503,12 +2503,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
 
     // Calculate initial Tbn matrix and rotate Mag measurements into NED
     // to set initial NED magnetic field states
-    Mat3f DCM;
-    quat2Tbn(DCM, initQuat);
+    quat2Tbn(Tbn, initQuat);
+    Tnb = Tbn.transpose();
     Vector3f initMagNED;
-    initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
-    initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
-    initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
+    initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z;
+    initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z;
+    initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z;
 
     magstate.q0 = initQuat[0];
     magstate.q1 = initQuat[1];
@@ -2521,7 +2521,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
     magstate.magYbias = magBias.y;
     magstate.magZbias = magBias.z;
     magstate.R_MAG = sq(magMeasurementSigma);
-    magstate.DCM = DCM;
+    magstate.DCM = Tbn;
 
     // write to state vector
     for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 000c02e3d..3d6c62434 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -414,6 +414,17 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_attitude_sp_pub(-1),
 	_nav_capabilities_pub(-1),
 
+	_att(),
+	_att_sp(),
+	_nav_capabilities(),
+	_manual(),
+	_airspeed(),
+	_control_mode(),
+	_global_pos(),
+	_pos_sp_triplet(),
+	_sensor_combined(),
+	_range_finder(),
+
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
 
@@ -433,18 +444,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_airspeed_valid(false),
 	_groundspeed_undershoot(0.0f),
 	_global_pos_valid(false),
-	_att(),
-	_att_sp(),
-	_nav_capabilities(),
-	_manual(),
-	_airspeed(),
-	_control_mode(),
-	_global_pos(),
-	_pos_sp_triplet(),
-	_sensor_combined(),
 	_mTecs(),
-	_was_pos_control_mode(false),
-	_range_finder()
+	_was_pos_control_mode(false)
 {
 	_nav_capabilities.turn_distance = 0.0f;
 
@@ -806,12 +807,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
-	// XXX re-visit
-	float baro_altitude = _global_pos.alt;
-
 	/* filter speed and altitude for controller */
 	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
-	math::Vector<3> accel_earth = _R_nb * accel_body;
 
 	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 
@@ -944,8 +941,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
-			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
-			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
+			/* Calculate altitude of last ordinary waypoint L */
 			float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
 
 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@@ -1449,7 +1445,7 @@ FixedwingPositionControl::start()
 	_control_task = task_spawn_cmd("fw_pos_control_l1",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       4048,
+				       3500,
 				       (main_t)&FixedwingPositionControl::task_main_trampoline,
 				       nullptr);
 
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index fc0a2551c..2e37d166e 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -220,7 +220,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
 	 * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
 	 * is running) */
-	bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
+	limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
 
 	/* Write part of the status message */
 	_status.flightPathAngleSp = flightPathAngleSp;
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
index e4e405227..c22e60ae0 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -56,9 +56,9 @@ class BlockOutputLimiter: public SuperBlock
 {
 public:
 // methods
-	BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+	BlockOutputLimiter(SuperBlock *parent, const char *name, bool fAngularLimit = false) :
 		SuperBlock(parent, name),
-		_isAngularLimit(isAngularLimit),
+		_isAngularLimit(fAngularLimit),
 		_min(this, "MIN"),
 		_max(this, "MAX")
 	{};
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index ca846a465..55a472bf6 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -346,7 +346,7 @@ MavlinkFTP::_workWrite(Request *req)
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workRemove(Request *req)
 {
-	auto hdr = req->header();
+	//auto hdr = req->header();
 
 	// for now, send error reply
 	return kErrPerm;
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 026a4d6c9..8dfa32ce8 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2238,7 +2238,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       1950,
+		       2700,
 		       (main_t)&Mavlink::start_helper,
 		       (const char **)argv);
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3747ad3ba..81966caf8 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -396,32 +396,35 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
-	mavlink_radio_status_t rstatus;
-	mavlink_msg_radio_status_decode(msg, &rstatus);
-
-	struct telemetry_status_s tstatus;
-	memset(&tstatus, 0, sizeof(tstatus));
-
-	tstatus.timestamp = hrt_absolute_time();
-	tstatus.heartbeat_time = _telemetry_heartbeat_time;
-	tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
-	tstatus.rssi = rstatus.rssi;
-	tstatus.remote_rssi = rstatus.remrssi;
-	tstatus.txbuf = rstatus.txbuf;
-	tstatus.noise = rstatus.noise;
-	tstatus.remote_noise = rstatus.remnoise;
-	tstatus.rxerrors = rstatus.rxerrors;
-	tstatus.fixed = rstatus.fixed;
-
-	if (_telemetry_status_pub < 0) {
-		_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
+	if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
+		mavlink_radio_status_t rstatus;
+		mavlink_msg_radio_status_decode(msg, &rstatus);
+
+		struct telemetry_status_s tstatus;
+		memset(&tstatus, 0, sizeof(tstatus));
+
+		tstatus.timestamp = hrt_absolute_time();
+		tstatus.heartbeat_time = _telemetry_heartbeat_time;
+		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
+		tstatus.rssi = rstatus.rssi;
+		tstatus.remote_rssi = rstatus.remrssi;
+		tstatus.txbuf = rstatus.txbuf;
+		tstatus.noise = rstatus.noise;
+		tstatus.remote_noise = rstatus.remnoise;
+		tstatus.rxerrors = rstatus.rxerrors;
+		tstatus.fixed = rstatus.fixed;
+
+		if (_telemetry_status_pub < 0) {
+			_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-	} else {
-		orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
-	}
+		} 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;
+		/* this means that heartbeats alone won't be published to the radio status no more */
+		_radio_status_available = true;
+	}
 }
 
 void
@@ -452,28 +455,31 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 void
 MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 {
-	mavlink_heartbeat_t hb;
-	mavlink_msg_heartbeat_decode(msg, &hb);
+	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
+	if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
+		mavlink_heartbeat_t hb;
+		mavlink_msg_heartbeat_decode(msg, &hb);
 
-	/* ignore own heartbeats, accept only heartbeats from GCS */
-	if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
-		_telemetry_heartbeat_time = hrt_absolute_time();
+		/* ignore own heartbeats, accept only heartbeats from GCS */
+		if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
+			_telemetry_heartbeat_time = hrt_absolute_time();
 
-		/* if no radio status messages arrive, lets at least publish that heartbeats were received */
-		if (!_radio_status_available) {
+			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
+			if (!_radio_status_available) {
 
-			struct telemetry_status_s tstatus;
-			memset(&tstatus, 0, sizeof(tstatus));
+				struct telemetry_status_s tstatus;
+				memset(&tstatus, 0, sizeof(tstatus));
 
-			tstatus.timestamp = _telemetry_heartbeat_time;
-			tstatus.heartbeat_time = _telemetry_heartbeat_time;
-			tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+				tstatus.timestamp = _telemetry_heartbeat_time;
+				tstatus.heartbeat_time = _telemetry_heartbeat_time;
+				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 
-			if (_telemetry_status_pub < 0) {
-				_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+				if (_telemetry_status_pub < 0) {
+					_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-			} else {
-				orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
+				} else {
+					orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
+				}
 			}
 		}
 	}
@@ -718,7 +724,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 
 	hil_gps.timestamp_variance = timestamp;
 	hil_gps.s_variance_m_s = 5.0f;
-	hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph;
 
 	hil_gps.timestamp_velocity = timestamp;
 	hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 76762c0fc..6d1d1fc2d 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -64,12 +64,15 @@
 #define rCCR		REG(STM32_I2C_CCR_OFFSET)
 #define rTRISE		REG(STM32_I2C_TRISE_OFFSET)
 
+void			i2c_reset(void);
 static int		i2c_interrupt(int irq, void *context);
 static void		i2c_rx_setup(void);
 static void		i2c_tx_setup(void);
 static void		i2c_rx_complete(void);
 static void		i2c_tx_complete(void);
+#ifdef DEBUG
 static void		i2c_dump(void);
+#endif
 
 static DMA_HANDLE	rx_dma;
 static DMA_HANDLE	tx_dma;
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index ca175bfbc..b00a96717 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -219,8 +219,8 @@ extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
-extern bool	sbus1_output(uint16_t *values, uint16_t num_values);
-extern bool	sbus2_output(uint16_t *values, uint16_t num_values);
+extern void	sbus1_output(uint16_t *values, uint16_t num_values);
+extern void	sbus2_output(uint16_t *values, uint16_t num_values);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 50108ea1b..b37259997 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -711,7 +711,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 {
 #define SELECT_PAGE(_page_name)							\
 	do {									\
-		*values = &_page_name[0];					\
+		*values = (uint16_t *)&_page_name[0];				\
 		*num_values = sizeof(_page_name) / sizeof(_page_name[0]);	\
 	} while(0)
 
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 70ccab180..0f0414148 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -116,14 +116,14 @@ sbus_init(const char *device)
 	return sbus_fd;
 }
 
-bool
+void
 sbus1_output(uint16_t *values, uint16_t num_values)
 {
 	char a = 'A';
 	write(sbus_fd, &a, 1);
 }
 
-bool
+void
 sbus2_output(uint16_t *values, uint16_t num_values)
 {
 	char b = 'B';
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 39e5b6c41..0d36fa2c6 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -979,7 +979,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GVSP_s log_GVSP;
 			struct log_BATT_s log_BATT;
 			struct log_DIST_s log_DIST;
-			struct log_TELE_s log_TELE;
+			struct log_TEL_s log_TEL;
 			struct log_EST0_s log_EST0;
 			struct log_EST1_s log_EST1;
 			struct log_PWR_s log_PWR;
@@ -1019,7 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int esc_sub;
 		int global_vel_sp_sub;
 		int battery_sub;
-		int telemetry_sub;
+		int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
 		int range_finder_sub;
 		int estimator_status_sub;
 		int tecs_status_sub;
@@ -1049,7 +1049,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
 	subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
 	subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
-	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
+	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+		subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
+	}
 	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 	subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
 	subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
@@ -1479,16 +1481,19 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- TELEMETRY --- */
-		if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
-			log_msg.msg_type = LOG_TELE_MSG;
-			log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
-			log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
-			log_msg.body.log_TELE.noise = buf.telemetry.noise;
-			log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
-			log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
-			log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
-			log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
-			LOGBUFFER_WRITE_AND_COUNT(TELE);
+		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) {
+				log_msg.msg_type = LOG_TEL0_MSG + i;
+				log_msg.body.log_TEL.rssi = buf.telemetry.rssi;
+				log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi;
+				log_msg.body.log_TEL.noise = buf.telemetry.noise;
+				log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise;
+				log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors;
+				log_msg.body.log_TEL.fixed = buf.telemetry.fixed;
+				log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf;
+				log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time;
+				LOGBUFFER_WRITE_AND_COUNT(TEL);
+			}
 		}
 
 		/* --- BOTTOM DISTANCE --- */
diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h
index dc5e6c8bd..aff0e3f48 100644
--- a/src/modules/sdlog2/sdlog2_format.h
+++ b/src/modules/sdlog2/sdlog2_format.h
@@ -91,6 +91,14 @@ struct log_format_s {
 						    .labels = _labels \
 	}
 
+#define LOG_FORMAT_S(_name, _struct_name, _format, _labels) { \
+		.type = LOG_##_name##_MSG, \
+			.length = sizeof(struct log_##_struct_name##_s) + LOG_PACKET_HEADER_LEN, \
+				  .name = #_name, \
+					  .format = _format, \
+						    .labels = _labels \
+	}
+
 #define LOG_FORMAT_MSG	  0x80
 
 #define LOG_PACKET_SIZE(_name)	LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s)
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 8c05e87c5..87f7f718f 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -276,18 +276,7 @@ struct log_DIST_s {
 	uint8_t flags;
 };
 
-/* --- TELE - TELEMETRY STATUS --- */
-#define LOG_TELE_MSG 22
-struct log_TELE_s {
-	uint8_t rssi;
-	uint8_t remote_rssi;
-	uint8_t noise;
-	uint8_t remote_noise;
-	uint16_t rxerrors;
-	uint16_t fixed;
-	uint8_t txbuf;
-};
-
+// ID 22 available
 // ID 23 available
 
 /* --- PWR - ONBOARD POWER SYSTEM --- */
@@ -385,6 +374,23 @@ struct log_EST1_s {
 	float s[16];
 };
 
+/* --- TEL0..3 - TELEMETRY STATUS --- */
+#define LOG_TEL0_MSG 34
+#define LOG_TEL1_MSG 35
+#define LOG_TEL2_MSG 36
+#define LOG_TEL3_MSG 37
+struct log_TEL_s {
+	uint8_t rssi;
+	uint8_t remote_rssi;
+	uint8_t noise;
+	uint8_t remote_noise;
+	uint16_t rxerrors;
+	uint16_t fixed;
+	uint8_t txbuf;
+	uint64_t heartbeat_time;
+};
+
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -432,7 +438,10 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
 	LOG_FORMAT(BATT, "ffff",		"V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB",			"Bottom,BottomRate,Flags"),
-	LOG_FORMAT(TELE, "BBBBHHB",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
+	LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
+	LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
+	LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
+	LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT(EST0, "ffffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
 	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
 	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index ccc516f39..7aa2f3a5f 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -67,7 +67,7 @@ __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pT
 
 __EXPORT struct system_load_s system_load;
 
-extern FAR struct _TCB *sched_gettcb(pid_t pid);
+extern FAR struct tcb_s *sched_gettcb(pid_t pid);
 
 void cpuload_initialize_once()
 {
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 9fff3eb88..90d8dd77c 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -54,6 +54,9 @@
 
 #include "systemlib.h"
 
+// Didn't seem right to include up_internal.h, so direct extern instead.
+extern void up_systemreset(void) noreturn_function;
+
 void
 systemreset(bool to_bootloader)
 {
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 687fc1d4a..9b118205e 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -186,7 +186,10 @@ ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
 
 #include "topics/telemetry_status.h"
-ORB_DEFINE(telemetry_status, struct telemetry_status_s);
+ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
+ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
+ORB_DEFINE(telemetry_status_2, struct telemetry_status_s);
+ORB_DEFINE(telemetry_status_3, struct telemetry_status_s);
 
 #include "topics/debug_key_value.h"
 ORB_DEFINE(debug_key_value, struct debug_key_value_s);
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index e9e00d76c..c4b99d520 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -72,6 +72,18 @@ struct telemetry_status_s {
  * @}
  */
 
-ORB_DECLARE(telemetry_status);
+ORB_DECLARE(telemetry_status_0);
+ORB_DECLARE(telemetry_status_1);
+ORB_DECLARE(telemetry_status_2);
+ORB_DECLARE(telemetry_status_3);
+
+#define TELEMETRY_STATUS_ORB_ID_NUM	4
+
+static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_ID_NUM] = {
+	ORB_ID(telemetry_status_0),
+	ORB_ID(telemetry_status_1),
+	ORB_ID(telemetry_status_2),
+	ORB_ID(telemetry_status_3),
+};
 
 #endif /* TOPIC_TELEMETRY_STATUS_H */
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index dbd59f4f3..80d65cd69 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -61,7 +61,6 @@ struct vehicle_gps_position_s {
 
 	uint64_t timestamp_variance;
 	float s_variance_m_s;				/**< speed accuracy estimate m/s */
-	float p_variance_m;				/**< position accuracy estimate m */
 	float c_variance_rad;				/**< course accuracy estimate rad */
 	uint8_t fix_type; 				/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
 
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index f8bff2f6f..235ab7186 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -46,6 +46,7 @@
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
+#include <math.h>
 #include <sys/stat.h>
 
 #include <arch/board/board.h>
@@ -228,8 +229,8 @@ do_show_print(void *arg, param_t param)
 	if (!(arg == NULL)) {
 
 		/* start search */
-		char *ss = search_string;
-		char *pp = p_name;
+		const char *ss = search_string;
+		const char *pp = p_name;
 		bool mismatch = false;
 
 		/* XXX this comparison is only ok for trailing wildcards */
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
index 00c649c75..df3ddb966 100644
--- a/src/systemcmds/tests/test_mathlib.cpp
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -96,8 +96,11 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Vector<3> %% Vector<3>", v1 % v2);
 		TEST_OP("Vector<3> length", v1.length());
 		TEST_OP("Vector<3> length squared", v1.length_squared());
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
 		TEST_OP("Vector<3> element read", volatile float a = v1(0));
 		TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]);
+#pragma GCC diagnostic pop
 		TEST_OP("Vector<3> element write", v1(0) = 1.0f);
 		TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
 	}