diff --git a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp index 4bc4fdf531..1abade2e8f 100644 --- a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp +++ b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp @@ -117,10 +117,6 @@ DfTROneWrapper::~DfTROneWrapper() int DfTROneWrapper::start() { - struct distance_sensor_s d; - _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_DEFAULT); - int ret; /* Init device and start sensor. */ @@ -156,10 +152,6 @@ int DfTROneWrapper::stop() int DfTROneWrapper::_publish(struct range_sensor_data &data) { - if (!_range_topic) { - return 1; - } - struct distance_sensor_s d; memset(&d, 0, sizeof(d)); @@ -180,7 +172,12 @@ int DfTROneWrapper::_publish(struct range_sensor_data &data) d.covariance = 0.0f; - orb_publish(ORB_ID(distance_sensor), _range_topic, &d); + if (_range_topic == nullptr) { + _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d, + &_orb_class_instance, ORB_PRIO_DEFAULT); + } else { + orb_publish(ORB_ID(distance_sensor), _range_topic, &d); + } /* Notify anyone waiting for data. */ DevMgr::updateNotify(*this);