Merge branch 'master' into offboard2_merge

Conflicts:
	src/modules/commander/commander.cpp

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.

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

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 +
 #

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

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

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),

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;

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;
+		_use_nav_pvt = false;
+	} else {
+		_use_nav_pvt = true;
 	}
+	UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
 
-	configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
+	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) {
+		configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
-		return 1;
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
-	}
+			return 1;
+		}
 
-	configure_message_rate(UBX_MSG_NAV_SOL, 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) {
+		configure_message_rate(UBX_MSG_NAV_VELNED, 1);
-		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();

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] */
+	int32_t		headVeh;	/**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */
-	uint32_t	reserved4;
+	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_ */

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
+		double start_disp_x = (double)radius * sin(arc_start_bearing);
-		float start_disp_x = radius * sinf(arc_start_bearing);
+		double start_disp_y = (double)radius * cos(arc_start_bearing);
-		float start_disp_y = radius * cosf(arc_start_bearing);
+		double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep)));
-		float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep));
+		double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep)));
-		float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep));
+		double lon_start = lon_now + start_disp_x / 111111.0;
-		float lon_start = lon_now + start_disp_x / 111111.0f;
+		double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
-		float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f;
+		double lon_end = lon_now + end_disp_x / 111111.0;
-		float lon_end = lon_now + end_disp_x / 111111.0f;
+		double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0;
-		float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f;
+		double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
-		float 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);
-		float 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;
 }

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 */
+	/* Subscribe to telemetry status topics */
-	int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
+	int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
-	struct telemetry_status_s telemetry;
+	uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
-	memset(&telemetry, 0, sizeof(telemetry));
+	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) {
+			if (updated) {
-			orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry);
+				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 */
+		/* data links check */
-		if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
+		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;
 			}

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);
 

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(Tbn, initQuat);
-    quat2Tbn(DCM, initQuat);
+    Tnb = Tbn.transpose();
     Vector3f initMagNED;
-    initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
+    initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z;
-    initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
+    initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z;
-    initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.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

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),
+	_was_pos_control_mode(false)
-	_range_finder()
 {
 	_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 */
+			/* Calculate 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));
 			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);
 

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;

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")
 	{};

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;

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);
 

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;
+	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
-	mavlink_msg_radio_status_decode(msg, &rstatus);
+	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;
+		struct telemetry_status_s tstatus;
-	memset(&tstatus, 0, sizeof(tstatus));
+		memset(&tstatus, 0, sizeof(tstatus));
 
-	tstatus.timestamp = hrt_absolute_time();
+		tstatus.timestamp = hrt_absolute_time();
-	tstatus.heartbeat_time = _telemetry_heartbeat_time;
+		tstatus.heartbeat_time = _telemetry_heartbeat_time;
-	tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
+		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
-	tstatus.rssi = rstatus.rssi;
+		tstatus.rssi = rstatus.rssi;
-	tstatus.remote_rssi = rstatus.remrssi;
+		tstatus.remote_rssi = rstatus.remrssi;
-	tstatus.txbuf = rstatus.txbuf;
+		tstatus.txbuf = rstatus.txbuf;
-	tstatus.noise = rstatus.noise;
+		tstatus.noise = rstatus.noise;
-	tstatus.remote_noise = rstatus.remnoise;
+		tstatus.remote_noise = rstatus.remnoise;
-	tstatus.rxerrors = rstatus.rxerrors;
+		tstatus.rxerrors = rstatus.rxerrors;
-	tstatus.fixed = rstatus.fixed;
+		tstatus.fixed = rstatus.fixed;
 
-	if (_telemetry_status_pub < 0) {
+		if (_telemetry_status_pub < 0) {
-		_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+			_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-	} else {
+		} else {
-		orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
+			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;
+	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
-	mavlink_msg_heartbeat_decode(msg, &hb);
+	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 */
+		/* ignore own heartbeats, accept only heartbeats from GCS */
-	if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
+		if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
-		_telemetry_heartbeat_time = hrt_absolute_time();
+			_telemetry_heartbeat_time = hrt_absolute_time();
 
-		/* if no radio status messages arrive, lets at least publish that heartbeats were received */
+			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
-		if (!_radio_status_available) {
+			if (!_radio_status_available) {
 
-			struct telemetry_status_s tstatus;
+				struct telemetry_status_s tstatus;
-			memset(&tstatus, 0, sizeof(tstatus));
+				memset(&tstatus, 0, sizeof(tstatus));
 
-			tstatus.timestamp = _telemetry_heartbeat_time;
+				tstatus.timestamp = _telemetry_heartbeat_time;
-			tstatus.heartbeat_time = _telemetry_heartbeat_time;
+				tstatus.heartbeat_time = _telemetry_heartbeat_time;
-			tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 
-			if (_telemetry_status_pub < 0) {
+				if (_telemetry_status_pub < 0) {
-				_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+					_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-			} else {
+				} else {
-				orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
+					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

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;

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 void	sbus1_output(uint16_t *values, uint16_t num_values);
-extern bool	sbus2_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;

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)
 

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';

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)) {
+		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
-			log_msg.msg_type = LOG_TELE_MSG;
+			if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) {
-			log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
+				log_msg.msg_type = LOG_TEL0_MSG + i;
-			log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
+				log_msg.body.log_TEL.rssi = buf.telemetry.rssi;
-			log_msg.body.log_TELE.noise = buf.telemetry.noise;
+				log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi;
-			log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
+				log_msg.body.log_TEL.noise = buf.telemetry.noise;
-			log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
+				log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise;
-			log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
+				log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors;
-			log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
+				log_msg.body.log_TEL.fixed = buf.telemetry.fixed;
-			LOGBUFFER_WRITE_AND_COUNT(TELE);
+				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 --- */

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)

src/modules/sdlog2/sdlog2_messages.h

@@ -276,18 +276,7 @@ struct log_DIST_s {
 	uint8_t flags;
 };
 
-/* --- TELE - TELEMETRY STATUS --- */
+// ID 22 available
-#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 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"),

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()
 {

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)
 {

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);

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 */

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.   */
 

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;
+		const char *ss = search_string;
-		char *pp = p_name;
+		const char *pp = p_name;
 		bool mismatch = false;
 
 		/* XXX this comparison is only ok for trailing wildcards */

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);
 	}