diff --git a/src/drivers/teraranger/teraranger.cpp b/src/drivers/teraranger/teraranger.cpp index a64476b2e0..412118c5d5 100644 --- a/src/drivers/teraranger/teraranger.cpp +++ b/src/drivers/teraranger/teraranger.cpp @@ -40,6 +40,7 @@ #include #include +#include #include @@ -101,7 +102,8 @@ class TERARANGER : public device::I2C { public: - TERARANGER(int bus = TERARANGER_BUS, int address = TRONE_BASEADDR); + TERARANGER(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, + int bus = TERARANGER_BUS, int address = TRONE_BASEADDR); virtual ~TERARANGER(); virtual int init(); @@ -118,6 +120,7 @@ protected: virtual int probe(); private: + uint8_t _rotation; float _min_distance; float _max_distance; work_s _work; @@ -227,8 +230,9 @@ static uint8_t crc8(uint8_t *p, uint8_t len) */ extern "C" __EXPORT int teraranger_main(int argc, char *argv[]); -TERARANGER::TERARANGER(int bus, int address) : +TERARANGER::TERARANGER(uint8_t rotation, int bus, int address) : I2C("TERARANGER", TERARANGER_DEVICE_PATH, bus, address, 100000), + _rotation(rotation), _min_distance(-1.0f), _max_distance(-1.0f), _reports(nullptr), @@ -631,7 +635,7 @@ TERARANGER::collect() report.timestamp = hrt_absolute_time(); /* there is no enum item for a combined LASER and ULTRASOUND which it should be */ report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - report.orientation = 8; + report.orientation = _rotation; report.current_distance = distance_m; report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); @@ -765,7 +769,7 @@ namespace teraranger TERARANGER *g_dev; -void start(); +void start(uint8_t rotation); void stop(); void test(); void reset(); @@ -775,7 +779,7 @@ void info(); * Start the driver. */ void -start() +start(uint8_t rotation) { int fd; @@ -784,7 +788,7 @@ start() } /* create the driver */ - g_dev = new TERARANGER(TERARANGER_BUS); + g_dev = new TERARANGER(rotation, TERARANGER_BUS); if (g_dev == nullptr) { @@ -945,38 +949,55 @@ info() int teraranger_main(int argc, char *argv[]) { + 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 sensor orientation to %d", (int)rotation); + break; + + default: + PX4_WARN("Unknown option!"); + } + } + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) { - teraranger::start(); + if (!strcmp(argv[myoptind], "start")) { + teraranger::start(rotation); } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) { + if (!strcmp(argv[myoptind], "stop")) { teraranger::stop(); } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) { + if (!strcmp(argv[myoptind], "test")) { teraranger::test(); } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) { + if (!strcmp(argv[myoptind], "reset")) { teraranger::reset(); } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) { teraranger::info(); }