mavlink: Only send messages when we have updates for them.

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -46,11 +46,15 @@
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr)
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
+	_fd(orb_subscribe(_topic)),
+	_published(false),
+	_topic(topic),
+	_last_check(0),
+	next(nullptr)
 {
 	_data = malloc(topic->o_size);
 	memset(_data, 0, topic->o_size);
-	_fd = orb_subscribe(_topic);
 }
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
@@ -87,3 +91,16 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
 
 	return false;
 }
+
+bool
+MavlinkOrbSubscription::is_published()
+{
+	bool updated;
+	orb_check(_fd, &updated);
+
+	if (updated) {
+		_published = true;
+	}
+
+	return _published;
+}

src/modules/mavlink/mavlink_orb_subscription.h

@@ -54,12 +54,21 @@ public:
 	~MavlinkOrbSubscription();
 
 	bool update(const hrt_abstime t);
+
+	/**
+	 * Check if the topic has been published.
+	 *
+	 * This call will return true if the topic was ever published.
+	 * @param true if the topic has been published at least once.
+	 */
+	bool is_published();
 	void *get_data();
 	const orb_id_t get_topic();
 
 private:
 	const orb_id_t _topic;
 	int _fd;
+	bool _published;
 	void *_data;
 	hrt_abstime _last_check;
 };

src/modules/mavlink/mavlink_receiver.cpp

@@ -586,7 +586,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.gyro_rad_s[0] = imu.xgyro;
 		hil_sensors.gyro_rad_s[1] = imu.ygyro;
 		hil_sensors.gyro_rad_s[2] = imu.zgyro;
-		hil_sensors.gyro_counter = _hil_counter;
 
 		hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
 		hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
@@ -596,7 +595,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.accelerometer_m_s2[2] = imu.zacc;
 		hil_sensors.accelerometer_mode = 0; // TODO what is this?
 		hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
-		hil_sensors.accelerometer_counter = _hil_counter;
+		hil_sensors.accelerometer_timestamp = timestamp;
 
 		hil_sensors.adc_voltage_v[0] = 0.0f;
 		hil_sensors.adc_voltage_v[1] = 0.0f;
@@ -611,15 +610,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.magnetometer_range_ga = 32.7f; // int16
 		hil_sensors.magnetometer_mode = 0; // TODO what is this
 		hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
-		hil_sensors.magnetometer_counter = _hil_counter;
+		hil_sensors.magnetometer_timestamp = timestamp;
 
 		hil_sensors.baro_pres_mbar = imu.abs_pressure;
 		hil_sensors.baro_alt_meter = imu.pressure_alt;
 		hil_sensors.baro_temp_celcius = imu.temperature;
-		hil_sensors.baro_counter = _hil_counter;
+		hil_sensors.baro_timestamp = timestamp;
 
 		hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
-		hil_sensors.differential_pressure_counter = _hil_counter;
+		hil_sensors.differential_pressure_timestamp = timestamp;
 
 		/* publish combined sensor topic */
 		if (_sensors_pub > 0) {