DF driver wrappers: advertise with actual data

src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp

@@ -143,16 +143,6 @@ DfHmc9250Wrapper::~DfHmc9250Wrapper()
 
 int DfHmc9250Wrapper::start()
 {
-	// TODO: don't publish garbage here
-	mag_report mag_report = {};
-	_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
-					 &_mag_orb_class_instance, ORB_PRIO_DEFAULT);
-
-	if (_mag_topic == nullptr) {
-		PX4_ERR("sensor_mag advert fail");
-		return -1;
-	}
-
 	/* Subscribe to param update topic. */
 	if (_param_update_sub < 0) {
 		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -301,7 +291,10 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
 	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {
 
-		if (_mag_topic != nullptr) {
+		if (_mag_topic == nullptr) {
+			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+							 &_mag_orb_class_instance, ORB_PRIO_HIGH);
+		} else {
 			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
 		}
 

src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp

@@ -259,16 +259,7 @@ int DfMpu9250Wrapper::start()
 		return -1;
 	}
 
-	if (_mag_enabled == true) {
-		// TODO: don't publish garbage here
-		mag_report mag_report = {};
-		_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
-						  &_mag_orb_class_instance, ORB_PRIO_DEFAULT);
-
-		if (_mag_topic == nullptr) {
-			PX4_ERR("sensor_mag advert fail");
-			return -1;
-		}
+	if (_mag_enabled) {
 	}
 
 	/* Subscribe to param update topic. */
@@ -707,8 +698,14 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 		}
 
-		if ((_mag_topic != nullptr) && (_mag_enabled == true)) {
-			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+		if (_mag_enabled) {
+
+			if (_mag_topic == nullptr) {
+				_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+								 &_mag_orb_class_instance, ORB_PRIO_LOW);
+			} else {
+				orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+			}
 		}
 
 		/* Notify anyone waiting for data. */