fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 00:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

px4flow update orb_publish usage to uORB::PublicationMulti<>

Browse Source
This commit is contained in:
Daniel Agar
2019-11-30 15:52:53 -05:00
parent 25acd40ef0
commit 92933d988b
1 changed files with 19 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/optical_flow/px4flow/px4flow.cpp
+19 -36
View File
@@ -50,6 +50,7 @@
#include <px4_platform_common/defines.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/optical_flow.h>
@@ -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<optical_flow_s> _px4flow_topic{ORB_ID(optical_flow)};
uORB::PublicationMulti<distance_sensor_s> _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);