Merge branch 'master' into offboard2_merge

Conflicts:
	src/modules/commander/commander.cpp

21 files changed, 205 insertions, 133 deletions

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