diff --git a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp index e354176f8c..5c0a3a7f03 100644 --- a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp +++ b/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); } diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp index 1daf21f416..c09b84e555 100644 --- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp +++ b/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. */