Cleanup of heartbeat handling and status printing. Ready to go mainline

4 files changed, 32 insertions, 24 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index e25851c7d..0e8f17c77 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -432,11 +432,9 @@ Mavlink::get_status_all_instances()
 
 	unsigned iterations = 0;
 
-	warnx("waiting for instances to stop");
-
 	while (inst != nullptr) {
 
-		printf("instance #%u:\n", iterations);
+		printf("\ninstance #%u:\n", iterations);
 		inst->display_status();
 
 		/* move on */
@@ -1688,27 +1686,33 @@ Mavlink::start(int argc, char *argv[])
 void
 Mavlink::display_status()
 {
-	warnx("running");
+
+	if (_rstatus.heartbeat_time > 0) {
+		printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
+	}
 
 	if (_rstatus.timestamp > 0) {
-		printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time);
+
+		printf("\ttype:\t\t");
 
 		switch (_rstatus.type) {
 			case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
-			printf("\t3DR RADIO\n");
+			printf("3DR RADIO\n");
 			break;
 			default:
-			printf("\tUNKNOWN RADIO\n");
+			printf("UNKNOWN RADIO\n");
 			break;
 		}
 
-		printf("\trssi:\t%d\t\n", _rstatus.rssi);
-		printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi);
-		printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf);
-		printf("\tnoise:\t%d\tus\n", _rstatus.noise);
-		printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise);
-		printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors);
-		printf("\tfixed:\t%u\tus\n", _rstatus.fixed);
+		printf("\trssi:\t\t%d\n", _rstatus.rssi);
+		printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi);
+		printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf);
+		printf("\tnoise:\t\t%d\n", _rstatus.noise);
+		printf("\tremote noise:\t%u\n", _rstatus.remote_noise);
+		printf("\trx errors:\t%u\n", _rstatus.rxerrors);
+		printf("\tfixed:\t\t%u\n", _rstatus.fixed);
+	} else {
+		printf("\tno telem status.\n");
 	}
 }
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 528c8b72a..454d730d8 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -111,7 +111,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
-	_telemetry_heartbeat_time(0),
 	_radio_status_available(false),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
@@ -403,7 +402,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 		struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
 
 		tstatus.timestamp = hrt_absolute_time();
-		tstatus.heartbeat_time = _telemetry_heartbeat_time;
+		tstatus.telem_time = tstatus.timestamp;
+		/* tstatus.heartbeat_time is set by system heartbeats */
 		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
 		tstatus.rssi = rstatus.rssi;
 		tstatus.remote_rssi = rstatus.remrssi;
@@ -460,16 +460,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 
 		/* 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();
+
+			struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
+
+			hrt_abstime tnow = hrt_absolute_time();
+
+			/* always set heartbeat, publish only if telemetry link not up */
+			tstatus.heartbeat_time = tnow;
 
 			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
 			if (!_radio_status_available) {
 
-				struct telemetry_status_s tstatus;
-				memset(&tstatus, 0, sizeof(tstatus));
-
-				tstatus.timestamp = _telemetry_heartbeat_time;
-				tstatus.heartbeat_time = _telemetry_heartbeat_time;
+				tstatus.timestamp = tnow;
+				/* telem_time indicates the timestamp of a telemetry status packet and we got none */
+				tstatus.telem_time = 0;
 				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 
 				if (_telemetry_status_pub < 0) {
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index b64a060e3..f4d0c52d8 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -148,7 +148,6 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
-	hrt_abstime _telemetry_heartbeat_time;
 	bool _radio_status_available;
 	int _control_mode_sub;
 	int _hil_frames;
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index 3347724a5..1297c1a9d 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -57,7 +57,8 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
 
 struct telemetry_status_s {
 	uint64_t timestamp;
-	uint64_t heartbeat_time;	/**< Time of last received heartbeat from remote system */
+	uint64_t heartbeat_time;		/**< Time of last received heartbeat from remote system */
+	uint64_t telem_time;			/**< Time of last received telemetry status packet, 0 for none */
 	enum TELEMETRY_STATUS_RADIO_TYPE type;	/**< type of the radio hardware     */
 	uint8_t rssi;				/**< local signal strength                      */
 	uint8_t remote_rssi;			/**< remote signal strength                     */