dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

distance_sensor: changed all distance sensors to orb_advertise_multi

This commit is contained in:
Ban Siesta 2015-05-24 19:20:25 +01:00
parent 517acd4586
commit 69cbbd9b5e
8 changed files with 74 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/drivers/ll40ls/LidarLiteI2C.cpp
View File

@ -61,6 +61,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) :
_sensor_ok(false),
_collect_phase(false),
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")),
_comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")),
@ -128,7 +129,8 @@ int LidarLiteI2C::init()
struct distance_sensor_s ds_report;
measure();
_reports->get(&ds_report);
_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic < 0) {
debug("failed to create distance_sensor object. Did you start uOrb?");

1
src/drivers/ll40ls/LidarLiteI2C.h
View File

@ -103,6 +103,7 @@ private:
bool _sensor_ok;
bool _collect_phase;
int _class_instance;
int _orb_class_instance;
orb_advert_t _distance_sensor_topic;

4
src/drivers/ll40ls/LidarLitePWM.cpp
View File

@ -58,6 +58,7 @@ LidarLitePWM::LidarLitePWM(const char *path) :
_work{},
_reports(nullptr),
_class_instance(-1),
_orb_class_instance(-1),
_pwmSub(-1),
_pwm{},
_distance_sensor_topic(-1),
@ -112,7 +113,8 @@ int LidarLitePWM::init()
struct distance_sensor_s ds_report;
measure();
_reports->get(&ds_report);
_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic < 0) {
debug("failed to create distance_sensor object. Did you start uOrb?");

1
src/drivers/ll40ls/LidarLitePWM.h
View File

@ -108,6 +108,7 @@ private:
work_s _work;
ringbuffer::RingBuffer *_reports;
int _class_instance;
int _orb_class_instance;
int _pwmSub;
struct pwm_input_s _pwm;
orb_advert_t _distance_sensor_topic;

9
src/drivers/mb12xx/mb12xx.cpp
View File

@ -130,7 +130,8 @@ private:
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
int _class_instance;
int _orb_class_instance;
orb_advert_t _distance_sensor_topic;
@ -209,6 +210,7 @@ MB12XX::MB12XX(int bus, int address) :
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
@ -269,9 +271,10 @@ MB12XX::init()
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s rf_report = {};
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic < 0) {
log("failed to create sensor_range_finder object. Did you start uOrb?");

32
src/drivers/px4flow/px4flow.cpp
View File

@ -67,6 +67,7 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_px4flow.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
@ -125,10 +126,12 @@ protected:
private:
work_s _work;
ringbuffer::RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
int _orb_class_instance;
orb_advert_t _px4flow_topic;
orb_advert_t _distance_sensor_topic;
@ -188,6 +191,8 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) :
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_orb_class_instance(-1),
_px4flow_topic(-1),
_distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
@ -195,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) :
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")),
_sensor_rotation(rotation)
{
// enable debug() calls
// disable debug() calls
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
@ -230,6 +235,20 @@ PX4FLOW::init()
goto out;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic < 0) {
log("failed to create sensor_range_finder object. Did you start uOrb?");
}
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
@ -518,12 +537,7 @@ PX4FLOW::collect()
distance_report.id = 0;
distance_report.orientation = 8;
if (_distance_sensor_topic < 0) {
_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &distance_report);
} else {
/* publish it */
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report);
}
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report);
/* post a report to the ring */
if (_reports->force(&report)) {

35
src/drivers/sf0x/sf0x.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -119,7 +119,7 @@ private:
float _min_distance;
float _max_distance;
work_s _work;
ringbuffer::RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
@ -129,6 +129,9 @@ private:
enum SF0X_PARSE_STATE _parse_state;
hrt_abstime _last_read;
int _class_instance;
int _orb_class_instance;
orb_advert_t _distance_sensor_topic;
unsigned _consecutive_fail_count;
@ -195,6 +198,8 @@ SF0X::SF0X(const char *port) :
_linebuf_index(0),
_parse_state(SF0X_PARSE_STATE0_UNSYNC),
_last_read(0),
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(-1),
_consecutive_fail_count(0),
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
@ -256,6 +261,14 @@ SF0X::~SF0X()
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
}
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
}
int
@ -278,14 +291,20 @@ SF0X::init()
break;
}
/* get a publish handle on the range finder topic */
struct distance_sensor_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &zero_report);
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_distance_sensor_topic < 0) {
warnx("advert err");
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic < 0) {
log("failed to create sensor_range_finder object. Did you start uOrb?");
}
}
} while(0);
/* close the fd */

16
src/drivers/trone/trone.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -127,7 +127,8 @@ private:
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
int _class_instance;
int _orb_class_instance;
orb_advert_t _distance_sensor_topic;
@ -236,6 +237,7 @@ TRONE::TRONE(int bus, int address) :
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
@ -292,13 +294,15 @@ TRONE::init()
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s rf_report;
struct distance_sensor_s ds_report;
measure();
_reports->get(&rf_report);
_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report);
_reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic < 0) {
debug("failed to create sensor_range_finder object. Did you start uOrb?");
log("failed to create sensor_range_finder object. Did you start uOrb?");
}
}