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

srf02: update for new orientation convention 

add possibility to specify orientation and adapt to new defaults and use px4_getopt
This commit is contained in:
ChristophTobler 2017-09-19 14:14:35 +02:00 committed by ChristophTobler
parent d5ddc2b489
commit 018aa8e535
1 changed files with 35 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/drivers/srf02/srf02.cpp
View File

@ -39,6 +39,7 @@
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_getopt.h>
#include <drivers/device/i2c.h>
@ -100,7 +101,8 @@
class SRF02 : public device::I2C
{
public:
SRF02(int bus = SRF02_BUS, int address = SRF02_BASEADDR);
SRF02(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SRF02_BUS,
int address = SRF02_BASEADDR);
virtual ~SRF02();
virtual int init();
@ -117,6 +119,7 @@ protected:
virtual int probe();
private:
uint8_t _rotation;
float _min_distance;
float _max_distance;
work_s _work;
@ -195,8 +198,9 @@ private:
*/
extern "C" __EXPORT int srf02_main(int argc, char *argv[]);
SRF02::SRF02(int bus, int address) :
SRF02::SRF02(uint8_t rotation, int bus, int address) :
I2C("MB12xx", SRF02_DEVICE_PATH, bus, address, 100000),
_rotation(rotation),
_min_distance(SRF02_MIN_DISTANCE),
_max_distance(SRF02_MAX_DISTANCE),
_reports(nullptr),
@ -572,7 +576,7 @@ SRF02::collect()
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
report.orientation = 8;
report.orientation = _rotation;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
@ -722,7 +726,7 @@ namespace srf02
SRF02 *g_dev;
void start();
void start(uint8_t rotation);
void stop();
void test();
void reset();
@ -732,7 +736,7 @@ void info();
* Start the driver.
*/
void
start()
start(uint8_t rotation)
{
int fd;
@ -741,7 +745,7 @@ start()
}
/* create the driver */
g_dev = new SRF02(SRF02_BUS);
g_dev = new SRF02(rotation, SRF02_BUS);
if (g_dev == nullptr) {
goto fail;
@ -903,38 +907,57 @@ info()
int
srf02_main(int argc, char *argv[])
{
// check for optional arguments
int ch;
int myoptind = 1;
const char *myoptarg = NULL;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
break;
default:
PX4_WARN("Unknown option!");
}
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
srf02::start();
if (!strcmp(argv[myoptind], "start")) {
srf02::start(rotation);
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
if (!strcmp(argv[myoptind], "stop")) {
srf02::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
if (!strcmp(argv[myoptind], "test")) {
srf02::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
if (!strcmp(argv[myoptind], "reset")) {
srf02::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
srf02::info();
}