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

Merge remote-tracking branch 'px4/pr/2200' into lidarlite_merge

Browse Source
This commit is contained in:
Ban Siesta
2015-05-24 13:59:18 +01:00
parent 2c4c0ecb32 5672df8eda
commit a0011d2163
16 changed files with 233 additions and 197 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/drv_range_finder.h
+5 -9
View File
@@ -46,16 +46,12 @@
#define RANGE_FINDER_BASE_DEVICE_PATH "/dev/range_finder"
#define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0"
#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected
#define MB12XX_MAX_RANGEFINDERS 12 // Maximum number of Maxbotix sensors on bus
#define range_finder_report range_finder_s
#define __orb_sensor_range_finder __orb_range_finder
#include <uORB/topics/range_finder.h>
#ifndef RANGE_FINDER_TYPE_LASER
#define RANGE_FINDER_TYPE_LASER 0
#endif
/*
* ObjDev tag for px4flow data.
*/
ORB_DECLARE(distance_sensor);
/*
* ioctl() definitions
src/drivers/mb12xx/mb12xx.cpp
+23 -60
View File
@@ -70,6 +70,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@@ -131,7 +132,7 @@ private:
bool _collect_phase;
int _class_instance;
orb_advert_t _range_finder_topic;
orb_advert_t _distance_sensor_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
@@ -208,7 +209,7 @@ MB12XX::MB12XX(int bus, int address) :
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_range_finder_topic(-1),
_distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")),
@@ -255,7 +256,7 @@ MB12XX::init()
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report));
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
_index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */
set_address(_index_counter); /* set I2c port to temp sonar i2c adress */
@@ -268,11 +269,11 @@ MB12XX::init()
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct range_finder_report rf_report = {};
struct distance_sensor_s rf_report = {};
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report);
if (_range_finder_topic < 0) {
if (_distance_sensor_topic < 0) {
log("failed to create sensor_range_finder object. Did you start uOrb?");
}
}
@@ -469,8 +470,8 @@ ssize_t
MB12XX::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
@@ -571,51 +572,20 @@ MB12XX::collect()
uint16_t distance = val[0] << 8 | val[1];
float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
struct range_finder_report report;
struct distance_sensor_s report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
/* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */
if (addr_ind.size() == 1) {
report.distance = si_units;
for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) {
report.distance_vector[i] = 0;
}
report.just_updated = 0;
} else {
/* for multiple sonars connected */
/* don't use the orginial single sonar variable */
report.distance = 0;
/* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */
_latest_sonar_measurements[_cycle_counter] = si_units;
for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) {
report.distance_vector[i] = _latest_sonar_measurements[i];
}
/* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */
report.just_updated = _index_counter;
/* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */
for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) {
report.distance_vector[addr_ind.size() + i] = 0;
}
}
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
report.time_boot_ms = hrt_absolute_time();
report.id = 1;
report.type = 1;
report.orientation = 8;
report.current_distance = si_units;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0;
/* publish it, if we are the primary */
if (_range_finder_topic >= 0) {
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
if (_distance_sensor_topic >= 0) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
}
if (_reports->force(&report)) {
@@ -840,7 +810,7 @@ void stop()
void
test()
{
struct range_finder_report report;
struct distance_sensor_s report;
ssize_t sz;
int ret;
@@ -858,8 +828,7 @@ test()
}
warnx("single read");
warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated);
warnx("time: %lld", report.timestamp);
warnx("time: %d", report.time_boot_ms);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -887,13 +856,7 @@ test()
}
warnx("periodic read %u", i);
/* Print the sonar rangefinder report sonar distance vector */
for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) {
warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1);
}
warnx("time: %lld", report.timestamp);
warnx("time: %d", report.time_boot_ms);
}
/* reset the sensor polling to default rate */
src/drivers/sf0x/sf0x.cpp
+24 -22
View File
@@ -69,6 +69,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@@ -128,7 +129,7 @@ private:
enum SF0X_PARSE_STATE _parse_state;
hrt_abstime _last_read;
orb_advert_t _range_finder_topic;
orb_advert_t _distance_sensor_topic;
unsigned _consecutive_fail_count;
@@ -194,7 +195,7 @@ SF0X::SF0X(const char *port) :
_linebuf_index(0),
_parse_state(SF0X_PARSE_STATE0_UNSYNC),
_last_read(0),
_range_finder_topic(-1),
_distance_sensor_topic(-1),
_consecutive_fail_count(0),
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
@@ -270,7 +271,7 @@ SF0X::init()
if (ret != OK) break;
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report));
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
warnx("mem err");
ret = -1;
@@ -278,11 +279,11 @@ SF0X::init()
}
/* get a publish handle on the range finder topic */
struct range_finder_report zero_report;
struct distance_sensor_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &zero_report);
if (_range_finder_topic < 0) {
if (_distance_sensor_topic < 0) {
warnx("advert err");
}
} while(0);
@@ -442,8 +443,8 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
SF0X::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
@@ -571,18 +572,19 @@ SF0X::collect()
debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
struct range_finder_report report;
struct distance_sensor_s report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
report.time_boot_ms = hrt_absolute_time();
report.id = 2;
report.type = 0;
report.orientation = 8;
report.current_distance = si_units;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0;
/* publish it */
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
@@ -815,7 +817,7 @@ void stop()
void
test()
{
struct range_finder_report report;
struct distance_sensor_s report;
ssize_t sz;
int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
@@ -832,8 +834,8 @@ test()
}
warnx("single read");
warnx("val: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("val: %0.2f m", (double)report.current_distance);
warnx("time: %d", report.time_boot_ms);
/* start the sensor polling at 2 Hz rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -863,8 +865,8 @@ test()
}
warnx("read #%u", i);
warnx("val: %0.3f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("val: %0.3f m", (double)report.current_distance);
warnx("time: %d", report.time_boot_ms);
}
/* reset the sensor polling to the default rate */
src/drivers/trone/trone.cpp
+27 -27
View File
@@ -68,6 +68,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@@ -128,7 +129,7 @@ private:
bool _collect_phase;
int _class_instance;
orb_advert_t _range_finder_topic;
orb_advert_t _distance_sensor_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
@@ -209,7 +210,7 @@ static const uint8_t crc_table[] = {
0xfa, 0xfd, 0xf4, 0xf3
};
static uint8_t crc8(uint8_t *p, uint8_t len) {
/* static uint8_t crc8(uint8_t *p, uint8_t len) {
uint16_t i;
uint16_t crc = 0x0;
@@ -219,7 +220,7 @@ static uint8_t crc8(uint8_t *p, uint8_t len) {
}
return crc & 0xFF;
}
}*/
/*
* Driver 'main' command.
@@ -235,7 +236,7 @@ TRONE::TRONE(int bus, int address) :
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_range_finder_topic(-1),
_distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows"))
@@ -281,7 +282,7 @@ TRONE::init()
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report));
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
goto out;
@@ -291,12 +292,12 @@ TRONE::init()
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct range_finder_report rf_report;
struct distance_sensor_s rf_report;
measure();
_reports->get(&rf_report);
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report);
if (_range_finder_topic < 0) {
if (_distance_sensor_topic < 0) {
debug("failed to create sensor_range_finder object. Did you start uOrb?");
}
}
@@ -468,8 +469,8 @@ TRONE::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
TRONE::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
@@ -568,20 +569,20 @@ TRONE::collect()
uint16_t distance = (val[0] << 8) | val[1];
float si_units = distance * 0.001f; /* mm to m */
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
struct distance_sensor_s report;
report.time_boot_ms = hrt_absolute_time();
report.id = 3;
report.type = 0;
report.orientation = 8;
report.current_distance = si_units;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0;
/* publish it, if we are the primary */
if (_range_finder_topic >= 0) {
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
if (_distance_sensor_topic >= 0) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
}
if (_reports->force(&report)) {
@@ -787,7 +788,7 @@ void stop()
void
test()
{
struct range_finder_report report;
struct distance_sensor_s report;
ssize_t sz;
int ret;
@@ -805,8 +806,8 @@ test()
}
warnx("single read");
warnx("measurement: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("measurement: %0.2f m", (double)report.current_distance);
warnx("time: %d", report.time_boot_ms);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -834,9 +835,8 @@ test()
}
warnx("periodic read %u", i);
warnx("valid %u", report.valid);
warnx("measurement: %0.3f", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("measurement: %0.3f", (double)report.current_distance);
warnx("time: %d", report.time_boot_ms);
}
/* reset the sensor polling to default rate */