diff --git a/src/drivers/optical_flow/px4flow/px4flow.cpp b/src/drivers/optical_flow/px4flow/px4flow.cpp index 68ab508f2d..2b6e2a6df6 100644 --- a/src/drivers/optical_flow/px4flow/px4flow.cpp +++ b/src/drivers/optical_flow/px4flow/px4flow.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -99,8 +100,9 @@ private: bool _collect_phase{false}; int _class_instance{-1}; int _orb_class_instance{-1}; - orb_advert_t _px4flow_topic{nullptr}; - orb_advert_t _distance_sensor_topic{nullptr}; + + uORB::PublicationMulti _px4flow_topic{ORB_ID(optical_flow)}; + uORB::PublicationMulti _distance_sensor_topic{ORB_ID(distance_sensor)}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -182,21 +184,6 @@ PX4FLOW::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; - - if (_class_instance == CLASS_DEVICE_PRIMARY) { - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_HIGH); - - if (_distance_sensor_topic == nullptr) { - PX4_ERR("failed to create distance_sensor object"); - } - - } else { - DEVICE_LOG("not primary range device, not advertising"); - } - ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -339,28 +326,24 @@ PX4FLOW::collect() rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral); - if (_px4flow_topic == nullptr) { - _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); - - } else { - /* publish it */ - orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); - } + _px4flow_topic.publish(report); /* publish to the distance_sensor topic as well */ - distance_sensor_s distance_report{}; - distance_report.timestamp = report.timestamp; - distance_report.min_distance = PX4FLOW_MIN_DISTANCE; - distance_report.max_distance = PX4FLOW_MAX_DISTANCE; - distance_report.current_distance = report.ground_distance_m; - distance_report.variance = 0.0f; - distance_report.signal_quality = -1; - distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; - /* TODO: the ID needs to be properly set */ - distance_report.id = 0; - distance_report.orientation = _sonar_rotation; + if (_class_instance == CLASS_DEVICE_PRIMARY) { + distance_sensor_s distance_report{}; + distance_report.timestamp = report.timestamp; + distance_report.min_distance = PX4FLOW_MIN_DISTANCE; + distance_report.max_distance = PX4FLOW_MAX_DISTANCE; + distance_report.current_distance = report.ground_distance_m; + distance_report.variance = 0.0f; + distance_report.signal_quality = -1; + distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + /* TODO: the ID needs to be properly set */ + distance_report.id = 0; + distance_report.orientation = _sonar_rotation; - orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report); + _distance_sensor_topic.publish(distance_report); + } perf_end(_sample_perf);