diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp index e73fc48a18..ced30e4b23 100644 --- a/src/drivers/srf02/srf02.cpp +++ b/src/drivers/srf02/srf02.cpp @@ -39,6 +39,7 @@ #include #include +#include #include @@ -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(); }