From 4e5974f1ca27df11b6f23dd8c6a556454ba81479 Mon Sep 17 00:00:00 2001 From: Mark Sauder Date: Wed, 10 Jul 2019 21:05:02 -0600 Subject: [PATCH] srf02 driver: Move member variable initialization to declarations, standardize against other drivers and format. (#11891) * Migrate variable initialization from constructor list to declarations, standardize whitespace formatting, and alphabetize/organize order of methods and variables in srf02.cpp. * Increase stack allocation size for the srf02 driver main. --- .../distance_sensor/srf02/CMakeLists.txt | 1 + src/drivers/distance_sensor/srf02/srf02.cpp | 935 ++++++++---------- 2 files changed, 431 insertions(+), 505 deletions(-) diff --git a/src/drivers/distance_sensor/srf02/CMakeLists.txt b/src/drivers/distance_sensor/srf02/CMakeLists.txt index dd412dcdfc..5f80b27be7 100644 --- a/src/drivers/distance_sensor/srf02/CMakeLists.txt +++ b/src/drivers/distance_sensor/srf02/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE drivers__srf02 MAIN srf02 + STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/distance_sensor/srf02/srf02.cpp b/src/drivers/distance_sensor/srf02/srf02.cpp index 9bae3d750d..b2e6539622 100644 --- a/src/drivers/distance_sensor/srf02/srf02.cpp +++ b/src/drivers/distance_sensor/srf02/srf02.cpp @@ -34,7 +34,7 @@ /** * @file srf02.cpp * - * Driver for the SRF02 sonar range finder adapted from the Maxbotix sonar range finder driver (mb12xx). + * Driver for the SRF02 sonar range finder adapted from the Maxbotix sonar range finder driver (srf02). */ #include @@ -69,23 +69,22 @@ #include /* Configuration Constants */ -#define SRF02_BUS_DEFAULT PX4_I2C_BUS_EXPANSION -#define SRF02_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ -#define SRF02_DEVICE_PATH "/dev/srf02" +#define SRF02_BASEADDR 0x70 // 7-bit address. 8-bit address is 0xE0. +#define SRF02_BUS_DEFAULT PX4_I2C_BUS_EXPANSION +#define SRF02_DEVICE_PATH "/dev/srf02" -/* MB12xx Registers addresses */ - -#define SRF02_TAKE_RANGE_REG 0x51 /* Measure range Register */ -#define SRF02_SET_ADDRESS_0 0xA0 /* Change address 0 Register */ -#define SRF02_SET_ADDRESS_1 0xAA /* Change address 1 Register */ -#define SRF02_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ +/* SRF02 Registers addresses */ +#define SRF02_TAKE_RANGE_REG 0x51 // Measure range Register. +#define SRF02_SET_ADDRESS_0 0xA0 // Change address 0 Register. +#define SRF02_SET_ADDRESS_1 0xAA // Change address 1 Register. +#define SRF02_SET_ADDRESS_2 0xA5 // Change address 2 Register. /* Device limits */ -#define SRF02_MIN_DISTANCE (0.20f) -#define SRF02_MAX_DISTANCE (7.65f) +#define SRF02_MIN_DISTANCE (0.20f) +#define SRF02_MAX_DISTANCE (7.65f) -#define SRF02_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ -#define SRF02_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ +#define SRF02_CONVERSION_INTERVAL 100000 // 60ms for one sonar. +#define SRF02_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100000 // 30ms between each sonar measurement (watch out for interference!). class SRF02 : public device::I2C, public px4::ScheduledWorkItem { @@ -94,79 +93,76 @@ public: int address = SRF02_BASEADDR); virtual ~SRF02(); - virtual int init() override; + int init() override; - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; + int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; protected: - virtual int probe() override; + int probe() override; private: + + int collect(); + + int measure(); + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void Run() override; + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + px4::Array addr_ind; // Temp sonar i2c address vector. + + bool _sensor_ok{false}; + bool _collect_phase{false}; + + int _class_instance{-1}; + int _cycling_rate{0}; // Initialize cycling rate to zero, (can differ depending on one sonar or multiple). + int _measure_interval{0}; + int _orb_class_instance{-1}; + + uint8_t _cycle_counter{0}; // Initialize counter to zero - used to change i2c adresses for multiple devices. + uint8_t _index_counter{0}; // Initialize temp sonar i2c address to zero. uint8_t _rotation; - float _min_distance; - float _max_distance; - ringbuffer::RingBuffer *_reports; - bool _sensor_ok; - int _measure_interval; - bool _collect_phase; - int _class_instance; - int _orb_class_instance; - orb_advert_t _distance_sensor_topic; + float _max_distance{SRF02_MAX_DISTANCE}; + float _min_distance{SRF02_MIN_DISTANCE}; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "srf02_com_err")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "srf02_read")}; - uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ - int _cycling_rate; /* */ - uint8_t _index_counter; /* temporary sonar i2c address */ - px4::Array addr_ind; /* temp sonar i2c address vector */ + orb_advert_t _distance_sensor_topic{nullptr}; - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); - - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * Set the min and max distance thresholds if you want the end points of the sensors - * range to be brought in at all, otherwise it will use the defaults SRF02_MIN_DISTANCE - * and SRF02_MAX_DISTANCE - */ - void set_minimum_distance(float min); - void set_maximum_distance(float max); - float get_minimum_distance(); - float get_maximum_distance(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; - int measure(); - int collect(); + ringbuffer::RingBuffer *_reports{nullptr}; }; /* @@ -175,32 +171,18 @@ private: extern "C" __EXPORT int srf02_main(int argc, char *argv[]); SRF02::SRF02(uint8_t rotation, int bus, int address) : - I2C("MB12xx", SRF02_DEVICE_PATH, bus, address, 100000), + I2C("SRF02", SRF02_DEVICE_PATH, bus, address, 100000), ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), - _rotation(rotation), - _min_distance(SRF02_MIN_DISTANCE), - _max_distance(SRF02_MAX_DISTANCE), - _reports(nullptr), - _sensor_ok(false), - _measure_interval(0), - _collect_phase(false), - _class_instance(-1), - _orb_class_instance(-1), - _distance_sensor_topic(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "srf02_read")), - _comms_errors(perf_alloc(PC_COUNT, "srf02_com_err")), - _cycle_counter(0), /* initialising counter for cycling function to zero */ - _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ - _index_counter(0) /* initialising temp sonar i2c address to zero */ + _rotation(rotation) { } SRF02::~SRF02() { - /* make sure we are truly inactive */ + // Ensure we are truly inactive. stop(); - /* free any existing reports */ + // Free any existing reports. if (_reports != nullptr) { delete _reports; } @@ -209,34 +191,77 @@ SRF02::~SRF02() unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); } - /* free perf counters */ + // Free perf counters. perf_free(_sample_perf); perf_free(_comms_errors); } int -SRF02::init() +SRF02::collect() { - int ret = PX4_ERROR; + // Read from the sensor. + uint8_t val[2] = {0, 0}; + uint8_t cmd = 0x02; + perf_begin(_sample_perf); - /* do I2C init (and probe) first */ - if (I2C::init() != OK) { + int ret = transfer(&cmd, 1, nullptr, 0); + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) { + PX4_DEBUG("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } - /* allocate basic report buffers */ + uint16_t distance_cm = val[0] << 8 | val[1]; + float distance_m = float(distance_cm) * 1e-2f; + + struct distance_sensor_s report; + report.current_distance = distance_m; + report.id = 0; // TODO: set proper ID. + report.max_distance = _max_distance; + report.min_distance = _min_distance; + report.orientation = _rotation; + report.signal_quality = -1; + report.variance = 0.0f; + report.timestamp = hrt_absolute_time(); + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + + // Publish it, if we are the primary. + if (_distance_sensor_topic != nullptr) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); + } + + _reports->force(&report); + + // Notify anyone waiting for data. + poll_notify(POLLIN); + perf_end(_sample_perf); + return PX4_OK; +} + +int +SRF02::init() +{ + // I2C init (and probe) first. + if (I2C::init() != OK) { + return PX4_ERROR; + } + + // Allocate basic report buffers. _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); - _index_counter = SRF02_BASEADDR; /* set temp sonar i2c address to base adress */ - set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + _index_counter = SRF02_BASEADDR; // Set temp sonar i2c address to base adress. + set_device_address(_index_counter); // Set I2c port to temp sonar i2c adress. if (_reports == nullptr) { - return ret; + return PX4_ERROR; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - /* get a publish handle on the range finder topic */ + // Get a publish handle on the range finder topic. struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, @@ -249,24 +274,26 @@ SRF02::init() // XXX we should find out why we need to wait 200 ms here px4_usleep(200000); - /* check for connected rangefinders on each i2c port: - We start from i2c base address (0x70 = 112) and count downwards - So second iteration it uses i2c address 111, third iteration 110 and so on*/ + /* Check for connected rangefinders on each i2c port: + * We start from i2c base address (0x70 = 112) and count downwards, + * so the second iteration it uses i2c address 111, third iteration 110, and so on. */ for (unsigned counter = 0; counter <= RANGE_FINDER_MAX_SENSORS; counter++) { - _index_counter = SRF02_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ - set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ - int ret2 = measure(); + _index_counter = SRF02_BASEADDR - counter; // Set temp sonar i2c address to base adress - counter. + set_device_address(_index_counter); // Set I2c port to temp sonar i2c adress. - if (ret2 == 0) { /* sonar is present -> store address_index in array */ + int ret = measure(); + + if (ret == 0) { + // Sonar is present -> store address_index in array. addr_ind.push_back(_index_counter); PX4_DEBUG("sonar added"); } } _index_counter = SRF02_BASEADDR; - set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */ + set_device_address(_index_counter); // Set i2c port back to base adress for rest of driver. - /* if only one sonar detected, no special timing is required between firing, so use default */ + // If only one sonar detected, no special timing is required between firing, so use default. if (addr_ind.size() == 1) { _cycling_rate = SRF02_CONVERSION_INTERVAL; @@ -274,48 +301,17 @@ SRF02::init() _cycling_rate = SRF02_INTERVAL_BETWEEN_SUCCESIVE_FIRES; } - /* show the connected sonars in terminal */ + // Show the connected sonars in terminal. for (unsigned i = 0; i < addr_ind.size(); i++) { PX4_DEBUG("sonar %d with address %d added", (i + 1), addr_ind[i]); } PX4_DEBUG("Number of sonars connected: %zu", addr_ind.size()); - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ + // Sensor is ok, but we don't really know if it is within range. _sensor_ok = true; - return ret; -} - -int -SRF02::probe() -{ - return measure(); -} - -void -SRF02::set_minimum_distance(float min) -{ - _min_distance = min; -} - -void -SRF02::set_maximum_distance(float max) -{ - _max_distance = max; -} - -float -SRF02::get_minimum_distance() -{ - return _min_distance; -} - -float -SRF02::get_maximum_distance() -{ - return _max_distance; + return PX4_OK; } int @@ -326,19 +322,19 @@ SRF02::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* zero would be bad */ + // Zero would be bad. case 0: return -EINVAL; - /* set default polling rate */ + // Set default polling rate. case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ + // Do we need to start internal polling?. bool want_start = (_measure_interval == 0); - /* set interval for next measurement to minimum legal value */ + // Set interval for next measurement to minimum legal value. _measure_interval = _cycling_rate; - /* if we need to start the poll state machine, do it */ + // If we need to start the poll state machine, do it. if (want_start) { start(); @@ -347,23 +343,23 @@ SRF02::ioctl(device::file_t *filp, int cmd, unsigned long arg) return OK; } - /* adjust to a legal polling interval in Hz */ + // Adjust to a legal polling interval in Hz. default: { - /* do we need to start internal polling? */ + // Do we need to start internal polling?. bool want_start = (_measure_interval == 0); - /* convert hz to tick interval via microseconds */ + // Convert hz to tick interval via microseconds. int interval = (1000000 / arg); - /* check against maximum rate */ + // check against maximum rate. if (interval < _cycling_rate) { return -EINVAL; } - /* update interval for next measurement */ + // Update interval for next measurement. _measure_interval = interval; - /* if we need to start the poll state machine, do it */ + // If we need to start the poll state machine, do it. if (want_start) { start(); } @@ -374,11 +370,45 @@ SRF02::ioctl(device::file_t *filp, int cmd, unsigned long arg) } default: - /* give it to the superclass */ + // Give it to the superclass. return I2C::ioctl(filp, cmd, arg); } } +int +SRF02::measure() +{ + uint8_t cmd[2]; + cmd[0] = 0x00; + cmd[1] = SRF02_TAKE_RANGE_REG; + + // Send the command to begin a measurement. + int ret = transfer(cmd, 2, nullptr, 0); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + return ret; + } + + return PX4_OK; +} + +void +SRF02::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + printf("poll interval: %u\n", _measure_interval); + _reports->print_info("report queue"); +} + +int +SRF02::probe() +{ + return measure(); +} + ssize_t SRF02::read(device::file_t *filp, char *buffer, size_t buflen) { @@ -387,12 +417,12 @@ SRF02::read(device::file_t *filp, char *buffer, size_t buflen) struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; - /* buffer must be large enough */ + // Buffer must be large enough. if (count < 1) { return -ENOSPC; } - /* if automatic measurement is enabled */ + // If automatic measurement is enabled. if (_measure_interval > 0) { /* @@ -407,30 +437,30 @@ SRF02::read(device::file_t *filp, char *buffer, size_t buflen) } } - /* if there was no data, warn the caller */ + // If there was no data, warn the caller. return ret ? ret : -EAGAIN; } - /* manual measurement - run one conversion */ + // Manual measurement - run one conversion. do { _reports->flush(); - /* trigger a measurement */ + // Trigger a measurement. if (OK != measure()) { ret = -EIO; break; } - /* wait for it to complete */ + // Wait for it to complete. px4_usleep(_cycling_rate * 2); - /* run the collection phase */ + // Run the collection phase. if (OK != collect()) { ret = -EIO; break; } - /* state machine will have generated a report, copy it out */ + // State machine will have generated a report, copy it out. if (_reports->get(rbuf)) { ret = sizeof(*rbuf); } @@ -440,92 +470,65 @@ SRF02::read(device::file_t *filp, char *buffer, size_t buflen) return ret; } -int -SRF02::measure() +void +SRF02::Run() { + if (_collect_phase) { + _index_counter = addr_ind[_cycle_counter]; // Sonar from previous iteration collect is now read out. + set_device_address(_index_counter); - int ret; + // Perform collection. + if (OK != collect()) { + PX4_DEBUG("collection error"); + // If error restart the measurement state machine. + start(); + return; + } - /* - * Send the command to begin a measurement. - */ + // Next phase is measurement. + _collect_phase = false; - uint8_t cmd[2]; - cmd[0] = 0x00; - cmd[1] = SRF02_TAKE_RANGE_REG; - ret = transfer(cmd, 2, nullptr, 0); + // Change i2c adress to next sonar. + _cycle_counter = _cycle_counter + 1; - if (OK != ret) { - perf_count(_comms_errors); - PX4_DEBUG("i2c::transfer returned %d", ret); - return ret; + if (_cycle_counter >= addr_ind.size()) { + _cycle_counter = 0; + } + + // Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate + // Otherwise the next sonar would fire without the first one having received its reflected sonar pulse. + if (_measure_interval > _cycling_rate) { + + // schedule a fresh cycle call when we are ready to measure again. + ScheduleDelayed(_measure_interval - _cycling_rate); + return; + } } - ret = OK; + // Measurement (firing) phase - Ensure sonar i2c adress is still correct. + _index_counter = addr_ind[_cycle_counter]; + set_device_address(_index_counter); - return ret; -} - -int -SRF02::collect() -{ - int ret = -EIO; - - /* read from the sensor */ - uint8_t val[2] = {0, 0}; - uint8_t cmd = 0x02; - perf_begin(_sample_perf); - - ret = transfer(&cmd, 1, nullptr, 0); - ret = transfer(nullptr, 0, &val[0], 2); - - if (ret < 0) { - PX4_DEBUG("error reading from sensor: %d", ret); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; + // Perform measurement. + if (OK != measure()) { + PX4_DEBUG("measure error sonar adress %d", _index_counter); } - uint16_t distance_cm = val[0] << 8 | val[1]; - float distance_m = float(distance_cm) * 1e-2f; + // Next phase is collection. + _collect_phase = true; - struct distance_sensor_s report; - report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; - report.orientation = _rotation; - report.current_distance = distance_m; - report.min_distance = get_minimum_distance(); - report.max_distance = get_maximum_distance(); - report.variance = 0.0f; - report.signal_quality = -1; - /* TODO: set proper ID */ - report.id = 0; - - /* publish it, if we are the primary */ - if (_distance_sensor_topic != nullptr) { - orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); - } - - _reports->force(&report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - ret = OK; - - perf_end(_sample_perf); - return ret; + // Schedule a fresh cycle call when the measurement is done. + ScheduleDelayed(_cycling_rate); } void SRF02::start() { - - /* reset the report ring and state machine */ + // Reset the report ring and state machine. _collect_phase = false; _reports->flush(); - /* schedule a cycle to start things */ + // Schedule a cycle to start things. ScheduleDelayed(5); } @@ -535,69 +538,6 @@ SRF02::stop() ScheduleClear(); } -void -SRF02::Run() -{ - if (_collect_phase) { - _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ - set_device_address(_index_counter); - - /* perform collection */ - if (OK != collect()) { - PX4_DEBUG("collection error"); - /* if error restart the measurement state machine */ - start(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* change i2c adress to next sonar */ - _cycle_counter = _cycle_counter + 1; - - if (_cycle_counter >= addr_ind.size()) { - _cycle_counter = 0; - } - - /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate - Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */ - - if (_measure_interval > _cycling_rate) { - - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - _cycling_rate); - - return; - } - } - - /* Measurement (firing) phase */ - - /* ensure sonar i2c adress is still correct */ - _index_counter = addr_ind[_cycle_counter]; - set_device_address(_index_counter); - - /* Perform measurement */ - if (OK != measure()) { - PX4_DEBUG("measure error sonar adress %d", _index_counter); - } - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(_cycling_rate); -} - -void -SRF02::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("poll interval: %u\n", _measure_interval); - _reports->print_info("report queue"); -} /** * Local functions in support of the shell command. @@ -605,183 +545,15 @@ SRF02::print_info() namespace srf02 { -SRF02 *g_dev; +SRF02 *g_dev; -int start(uint8_t rotation); -int start_bus(uint8_t rotation, int i2c_bus); -int stop(); -int test(); -int reset(); -int info(); - -/** - * - * Attempt to start driver on all available I2C busses. - * - * This function will return as soon as the first sensor - * is detected on one of the available busses or if no - * sensors are detected. - * - */ -int -start(uint8_t rotation) -{ - if (g_dev != nullptr) { - PX4_ERR("already started"); - return PX4_ERROR; - } - - for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { - if (start_bus(rotation, i2c_bus_options[i]) == PX4_OK) { - return PX4_OK; - } - } - - return PX4_ERROR; -} - -/** - * Start the driver on a specific bus. - * - * This function only returns if the sensor is up and running - * or could not be detected successfully. - */ -int -start_bus(uint8_t rotation, int i2c_bus) -{ - int fd = -1; - - if (g_dev != nullptr) { - PX4_ERR("already started"); - return PX4_ERROR; - } - - /* create the driver */ - g_dev = new SRF02(rotation, i2c_bus); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(SRF02_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - px4_close(fd); - return PX4_OK; - -fail: - - if (fd >= 0) { - px4_close(fd); - } - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - return PX4_ERROR; -} - -/** - * Stop the driver - */ -int -stop() -{ - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - - } else { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - return PX4_OK; -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -int -test() -{ - struct distance_sensor_s report; - ssize_t sz; - int ret; - - int fd = px4_open(SRF02_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("%s open failed (try 'srf02 start' if the driver is not running)", SRF02_DEVICE_PATH); - return PX4_ERROR; - } - - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("immediate read failed"); - return PX4_ERROR; - } - - print_message(report); - - /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - PX4_ERR("failed to set 2Hz poll rate"); - return PX4_ERROR; - } - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - if (ret != 1) { - PX4_ERR("timed out waiting for sensor data"); - return PX4_ERROR; - } - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("periodic read failed"); - return PX4_ERROR; - } - - print_message(report); - } - - /* reset the sensor polling to default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - PX4_ERR("failed to set default poll rate"); - return PX4_ERROR; - } - - PX4_INFO("PASS"); - return PX4_OK; -} +int reset(); +int start(uint8_t rotation); +int start_bus(uint8_t rotation, int i2c_bus); +int status(); +int stop(); +int test(); +int usage(); /** * Reset the driver. @@ -811,10 +583,83 @@ reset() } /** - * Print a little info about the driver. + * Attempt to start driver on all available I2C busses. + * + * This function will return as soon as the first sensor + * is detected on one of the available busses or if no + * sensors are detected. */ int -info() +start(uint8_t rotation) +{ + if (g_dev != nullptr) { + PX4_ERR("already started"); + return PX4_ERROR; + } + + for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { + if (start_bus(rotation, i2c_bus_options[i]) == PX4_OK) { + return PX4_OK; + } + } + + return PX4_ERROR; +} + +/** + * Start the driver on a specific bus. + * + * This function only returns if the sensor is up and running + * or could not be detected successfully. + */ +int +start_bus(uint8_t rotation, int i2c_bus) +{ + if (g_dev != nullptr) { + PX4_ERR("already started"); + return PX4_ERROR; + } + + // Create the driver. + g_dev = new SRF02(rotation, i2c_bus); + + if (g_dev == nullptr) { + PX4_ERR("failed to instantiate the device"); + return PX4_ERROR; + } + + if (OK != g_dev->init()) { + PX4_ERR("failed to initialize the device"); + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; + } + + // Set the poll rate to default, starts automatic data collection. + int fd = px4_open(SRF02_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + delete g_dev; + g_dev = nullptr; + px4_close(fd); + return PX4_ERROR; + } + + px4_close(fd); + return PX4_OK; +} + +/** + * Print the driver status. + */ +int +status() { if (g_dev == nullptr) { PX4_ERR("driver not running"); @@ -827,11 +672,94 @@ info() return PX4_OK; } -} /* namespace */ +/** + * Stop the driver + */ +int +stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } else { + PX4_ERR("driver not running"); + return PX4_ERROR; + } -static void -srf02_usage() + return PX4_OK; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +int +test() +{ + int fd = px4_open(SRF02_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + PX4_ERR("%s open failed (try 'srf02 start' if the driver is not running)", SRF02_DEVICE_PATH); + return PX4_ERROR; + } + + struct distance_sensor_s report; + + // Perform a simple demand read. + ssize_t sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_ERR("immediate read failed"); + return PX4_ERROR; + } + + print_message(report); + + // Start the sensor polling at 2Hz. + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + PX4_ERR("failed to set 2Hz poll rate"); + return PX4_ERROR; + } + + // Read the sensor 5x and report each value. + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + // Wait for data to be ready. + fds.fd = fd; + fds.events = POLLIN; + int ret = poll(&fds, 1, 2000); + + if (ret != 1) { + PX4_ERR("timed out waiting for sensor data"); + return PX4_ERROR; + } + + // Now go get it. + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_ERR("periodic read failed"); + return PX4_ERROR; + } + + print_message(report); + } + + // Reset the sensor polling to default rate. + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + PX4_ERR("failed to set default poll rate"); + return PX4_ERROR; + } + + PX4_INFO("PASS"); + return PX4_OK; +} + +int +usage() { PX4_INFO("usage: srf02 command [options]"); PX4_INFO("options:"); @@ -840,19 +768,27 @@ srf02_usage() PX4_INFO("\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING); PX4_INFO("command:"); PX4_INFO("\tstart|stop|test|reset|info"); + return PX4_OK; } -int -srf02_main(int argc, char *argv[]) +} // namespace srf02 + + +/** + * Driver 'main' command. + */ +extern "C" __EXPORT int srf02_main(int argc, char *argv[]) { + const char *myoptarg = nullptr; + int ch; int myoptind = 1; - const char *myoptarg = nullptr; - uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - bool start_all = false; - int i2c_bus = SRF02_BUS_DEFAULT; + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + + bool start_all = false; + while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'R': @@ -869,17 +805,15 @@ srf02_main(int argc, char *argv[]) default: PX4_WARN("Unknown option!"); - goto out_error; + return srf02::usage(); } } if (myoptind >= argc) { - goto out_error; + return srf02::usage(); } - /* - * Start/load the driver. - */ + // Start/load the driver. if (!strcmp(argv[myoptind], "start")) { if (start_all) { return srf02::start(rotation); @@ -889,35 +823,26 @@ srf02_main(int argc, char *argv[]) } } - /* - * Stop the driver - */ + // Stop the driver. if (!strcmp(argv[myoptind], "stop")) { return srf02::stop(); } - /* - * Test the driver/device. - */ + // Test the driver/device. if (!strcmp(argv[myoptind], "test")) { return srf02::test(); } - /* - * Reset the driver. - */ + // Reset the driver. if (!strcmp(argv[myoptind], "reset")) { return srf02::reset(); } - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { - return srf02::info(); + // Print driver information. + if (!strcmp(argv[myoptind], "info") || + !strcmp(argv[myoptind], "status")) { + return srf02::status(); } -out_error: - srf02_usage(); - return PX4_ERROR; + return srf02::usage(); }