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

distance_sensor: adopt message in both range drivers and on ekf_att_pos_estimator

This commit is contained in:
TSC21 2015-05-23 17:49:52 +01:00
parent 93f8d7c4e8
commit 43668cc8c8
9 changed files with 97 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/drivers/drv_range_finder.h
View File

@ -48,14 +48,10 @@
#define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0"
#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected
#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

55
src/drivers/ll40ls/ll40ls.cpp
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>
@ -144,7 +145,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;
@ -224,7 +225,7 @@ LL40LS::LL40LS(int bus, const char *path, 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, "ll40ls_read")),
_comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")),
@ -278,7 +279,7 @@ LL40LS::init()
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(range_finder_report));
_reports = new RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
goto out;
@ -288,12 +289,12 @@ LL40LS::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?");
}
}
@ -497,8 +498,8 @@ LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
LL40LS::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 */
@ -664,8 +665,8 @@ LL40LS::collect()
}
uint16_t distance = (val[0] << 8) | val[1];
float si_units = distance * 0.01f; /* cm to m */
struct range_finder_report report;
float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
struct distance_sensor_s report;
if (distance == 0) {
_zero_counter++;
@ -688,22 +689,18 @@ LL40LS::collect()
_last_distance = distance;
/* 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();
if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
report.valid = 1;
}
else {
report.valid = 0;
}
report.time_boot_ms = hrt_absolute_time();
report.id = 0;
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)) {
@ -958,7 +955,7 @@ void stop(int bus)
void
test(int bus)
{
struct range_finder_report report;
struct distance_sensor_s report;
ssize_t sz;
int ret;
const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
@ -977,8 +974,8 @@ test(int bus)
}
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)) {
@ -1006,8 +1003,8 @@ test(int bus)
}
warnx("periodic read %u", i);
warnx("measurement: %0.3f", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("measurement: %0.3f m", (double)report.current_distance);
warnx("time: %d", report.time_boot_ms);
}
/* reset the sensor polling to default rate */

83
src/drivers/mb12xx/mb12xx.cpp
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(2, sizeof(range_finder_report));
_reports = new 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 = 0;
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 */

4
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -58,13 +58,13 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/distance_sensor.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_range_finder.h>
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
@ -162,7 +162,7 @@ private:
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */
struct wind_estimate_s _wind; /**< wind estimate */
struct range_finder_report _distance; /**< distance estimate */
struct distance_sensor_s _distance; /**< distance estimate */
struct vehicle_land_detected_s _landDetector;
struct actuator_armed_s _armed;

12
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -501,7 +501,7 @@ void AttitudePositionEstimatorEKF::task_main()
/*
* do subscriptions
*/
_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
_distance_sub = orb_subscribe(ORB_ID(distance_sensor));
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@ -1471,11 +1471,11 @@ void AttitudePositionEstimatorEKF::pollData()
orb_check(_distance_sub, &_newRangeData);
if (_newRangeData) {
orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
if (_distance.valid) {
_ekf->rngMea = _distance.distance;
_distance_last_valid = _distance.timestamp;
orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance);
if ((_distance.current_distance > _distance.min_distance)
&& (_distance.current_distance < _distance.max_distance)) {
_ekf->rngMea = _distance.current_distance;
_distance_last_valid = _distance.time_boot_ms;
} else {
_newRangeData = false;

1
src/modules/mavlink/mavlink_messages.cpp
View File

@ -71,7 +71,6 @@
#include <uORB/topics/distance_sensor.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_range_finder.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>

52
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -56,7 +56,6 @@
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_range_finder.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
@ -109,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_battery_pub(-1),
_cmd_pub(-1),
_flow_pub(-1),
_range_pub(-1),
_distance_sensor_pub(-1),
_offboard_control_mode_pub(-1),
_actuator_controls_pub(-1),
_global_vel_sp_pub(-1),
@ -134,8 +133,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_att_sp{},
_rates_sp{},
_time_offset_avg_alpha(0.6),
_time_offset(0),
_distance_sensor_pub(-1)
_time_offset(0)
{
}
@ -417,6 +415,25 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
}
/* Use distance value for distance sensor topic */
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
d.time_boot_ms = hrt_absolute_time();
d.min_distance = 3.0f;
d.max_distance = 50.0f;
d.current_distance = flow.distance;
d.type = 1;
d.id = 0;
d.orientation = 8;
d.covariance = 0.0;
if (_distance_sensor_pub < 0) {
_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
} else {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
}
}
void
@ -449,22 +466,23 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
}
/* Use distance value for range finder report */
struct range_finder_report r;
memset(&r, 0, sizeof(r));
/* Use distance value for distance sensor topic */
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
r.timestamp = hrt_absolute_time();
r.error_count = 0;
r.type = RANGE_FINDER_TYPE_LASER;
r.distance = flow.distance;
r.minimum_distance = 0.0f;
r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
d.time_boot_ms = hrt_absolute_time();
d.min_distance = 3.0f;
d.max_distance = 50.0f;
d.current_distance = flow.distance;
d.type = 1;
d.id = 0;
d.orientation = 8;
d.covariance = 0.0;
if (_range_pub < 0) {
_range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r);
if (_distance_sensor_pub < 0) {
_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
} else {
orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r);
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
}
}

3
src/modules/mavlink/mavlink_receiver.h
View File

@ -166,7 +166,7 @@ private:
orb_advert_t _battery_pub;
orb_advert_t _cmd_pub;
orb_advert_t _flow_pub;
orb_advert_t _range_pub;
orb_advert_t _distance_sensor_pub;
orb_advert_t _offboard_control_mode_pub;
orb_advert_t _actuator_controls_pub;
orb_advert_t _global_vel_sp_pub;
@ -192,7 +192,6 @@ private:
struct vehicle_rates_setpoint_s _rates_sp;
double _time_offset_avg_alpha;
uint64_t _time_offset;
orb_advert_t _distance_sensor_pub;
/* do not allow copying this class */
MavlinkReceiver(const MavlinkReceiver &);

3
src/modules/uORB/objects_common.cpp
View File

@ -57,9 +57,6 @@ ORB_DEFINE(sensor_gyro, struct gyro_report);
#include <drivers/drv_baro.h>
ORB_DEFINE(sensor_baro, struct baro_report);
#include <drivers/drv_range_finder.h>
ORB_DEFINE(sensor_range_finder, struct range_finder_report);
#include <drivers/drv_pwm_output.h>
ORB_DEFINE(output_pwm, struct pwm_output_values);