fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 01:31:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

px4flow: publish sonar distance to distance_sensor topic as well

Browse Source
This commit is contained in:
Ban Siesta
2015-05-24 17:31:16 +01:00
parent 2fb999674b
commit 517acd4586
1 changed files with 26 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4flow/px4flow.cpp
+26
View File
@@ -34,6 +34,7 @@
/** /**
* @file px4flow.cpp * @file px4flow.cpp
* @author Dominik Honegger * @author Dominik Honegger
* @author Ban Siesta <bansiesta@gmail.com>
* *
* Driver for the PX4FLOW module connected via I2C. * Driver for the PX4FLOW module connected via I2C.
*/ */
@@ -71,6 +72,7 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h> #include <uORB/topics/subsystem_info.h>
#include <uORB/topics/optical_flow.h> #include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h> #include <board_config.h>
@@ -83,6 +85,9 @@
#define PX4FLOW_CONVERSION_INTERVAL 100000 ///< in microseconds! 20000 = 50 Hz 100000 = 10Hz #define PX4FLOW_CONVERSION_INTERVAL 100000 ///< in microseconds! 20000 = 50 Hz 100000 = 10Hz
#define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed #define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed
#define PX4FLOW_MAX_DISTANCE 5.0f
#define PX4FLOW_MIN_DISTANCE 0.3f
/* oddly, ERROR is not defined for c++ */ /* oddly, ERROR is not defined for c++ */
#ifdef ERROR #ifdef ERROR
# undef ERROR # undef ERROR
@@ -125,6 +130,7 @@ private:
int _measure_ticks; int _measure_ticks;
bool _collect_phase; bool _collect_phase;
orb_advert_t _px4flow_topic; orb_advert_t _px4flow_topic;
orb_advert_t _distance_sensor_topic;
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _comms_errors; perf_counter_t _comms_errors;
@@ -183,6 +189,7 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) :
_measure_ticks(0), _measure_ticks(0),
_collect_phase(false), _collect_phase(false),
_px4flow_topic(-1), _px4flow_topic(-1),
_distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
_comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")), _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")),
@@ -499,6 +506,25 @@ PX4FLOW::collect()
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
} }
/* publish to the distance_sensor topic as well */
struct 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.covariance = 0.0f;
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 = 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);
}
/* post a report to the ring */ /* post a report to the ring */
if (_reports->force(&report)) { if (_reports->force(&report)) {
perf_count(_buffer_overflows); perf_count(_buffer_overflows);