diff --git a/src/drivers/sf1xx/sf1xx.cpp b/src/drivers/sf1xx/sf1xx.cpp index d79ad3871f..2f4d1be6af 100644 --- a/src/drivers/sf1xx/sf1xx.cpp +++ b/src/drivers/sf1xx/sf1xx.cpp @@ -43,6 +43,7 @@ #include #include +#include #include @@ -89,7 +90,8 @@ class SF1XX : public device::I2C { public: - SF1XX(int bus = SF1XX_BUS, int address = SF1XX_BASEADDR); + SF1XX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SF1XX_BUS, + int address = SF1XX_BASEADDR); virtual ~SF1XX(); virtual int init(); @@ -106,6 +108,7 @@ protected: virtual int probe(); private: + uint8_t _rotation; float _min_distance; float _max_distance; int _conversion_interval; @@ -178,8 +181,9 @@ private: */ extern "C" __EXPORT int sf1xx_main(int argc, char *argv[]); -SF1XX::SF1XX(int bus, int address) : +SF1XX::SF1XX(uint8_t rotation, int bus, int address) : I2C("SF1XX", SF1XX_DEVICE_PATH, bus, address, 400000), + _rotation(rotation), _min_distance(-1.0f), _max_distance(-1.0f), _conversion_interval(-1), @@ -563,7 +567,7 @@ SF1XX::collect() struct distance_sensor_s report; report.timestamp = hrt_absolute_time(); 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(); @@ -651,7 +655,7 @@ namespace sf1xx SF1XX *g_dev; -void start(); +void start(uint8_t rotation); void stop(); void test(); void reset(); @@ -661,7 +665,7 @@ void info(); * Start the driver. */ void -start() +start(uint8_t rotation) { int fd = -1; @@ -670,7 +674,7 @@ start() } /* create the driver */ - g_dev = new SF1XX(SF1XX_BUS); + g_dev = new SF1XX(rotation, SF1XX_BUS); if (g_dev == nullptr) { goto fail; @@ -836,38 +840,57 @@ info() int sf1xx_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")) { - sf1xx::start(); + if (!strcmp(argv[myoptind], "start")) { + sf1xx::start(rotation); } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) { + if (!strcmp(argv[myoptind], "stop")) { sf1xx::stop(); } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) { + if (!strcmp(argv[myoptind], "test")) { sf1xx::test(); } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) { + if (!strcmp(argv[myoptind], "reset")) { sf1xx::reset(); } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { sf1xx::info(); }