diff --git a/src/drivers/distance_sensor/srf02/CMakeLists.txt b/src/drivers/distance_sensor/srf02/CMakeLists.txt index dd412dcdfc39..5f80b27be718 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 9bae3d750db9..b2e653962249 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 @@ class SRF02 : public device::I2C, public px4::ScheduledWorkItem 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: - 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; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + int collect(); - 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 */ + 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); + * 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(); + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void Run() override; /** - * Stop the automatic measurement state machine. - */ - void stop(); + * 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(); /** - * 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(); + * Stop the automatic measurement state machine. + */ + void stop(); - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; - int measure(); - int collect(); + 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 _max_distance{SRF02_MAX_DISTANCE}; + float _min_distance{SRF02_MIN_DISTANCE}; + + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "srf02_com_err")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "srf02_read")}; + + orb_advert_t _distance_sensor_topic{nullptr}; + + ringbuffer::RingBuffer *_reports{nullptr}; }; /* @@ -175,32 +171,18 @@ class SRF02 : public device::I2C, public px4::ScheduledWorkItem 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,188 +470,124 @@ SRF02::read(device::file_t *filp, char *buffer, size_t buflen) return ret; } -int -SRF02::measure() -{ - - int ret; - - /* - * Send the command to begin a measurement. - */ - - uint8_t cmd[2]; - cmd[0] = 0x00; - cmd[1] = SRF02_TAKE_RANGE_REG; - ret = transfer(cmd, 2, nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - PX4_DEBUG("i2c::transfer returned %d", ret); - return ret; - } - - ret = OK; - - 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; - } - - uint16_t distance_cm = val[0] << 8 | val[1]; - float distance_m = float(distance_cm) * 1e-2f; - - 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; -} - -void -SRF02::start() -{ - - /* reset the report ring and state machine */ - _collect_phase = false; - _reports->flush(); - - /* schedule a cycle to start things */ - ScheduleDelayed(5); -} - -void -SRF02::stop() -{ - ScheduleClear(); -} - void SRF02::Run() { if (_collect_phase) { - _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ + _index_counter = addr_ind[_cycle_counter]; // Sonar from previous iteration collect is now read out. set_device_address(_index_counter); - /* perform collection */ + // Perform collection. if (OK != collect()) { PX4_DEBUG("collection error"); - /* if error restart the measurement state machine */ + // If error restart the measurement state machine. start(); return; } - /* next phase is measurement */ + // Next phase is measurement. _collect_phase = false; - /* change i2c adress to next sonar */ + // 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 */ - + // 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 */ + // 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 */ + // Measurement (firing) phase - Ensure sonar i2c adress is still correct. _index_counter = addr_ind[_cycle_counter]; set_device_address(_index_counter); - /* Perform measurement */ + // Perform measurement. if (OK != measure()) { PX4_DEBUG("measure error sonar adress %d", _index_counter); } - /* next phase is collection */ + // Next phase is collection. _collect_phase = true; - /* schedule a fresh cycle call when the measurement is done */ + // Schedule a fresh cycle call when the measurement is done. ScheduleDelayed(_cycling_rate); } void -SRF02::print_info() +SRF02::start() { - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("poll interval: %u\n", _measure_interval); - _reports->print_info("report queue"); + // Reset the report ring and state machine. + _collect_phase = false; + _reports->flush(); + + // Schedule a cycle to start things. + ScheduleDelayed(5); +} + +void +SRF02::stop() +{ + ScheduleClear(); } + /** * Local functions in support of the shell command. */ 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(); +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. + */ +int +reset() +{ + int fd = px4_open(SRF02_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + PX4_ERR("failed"); + return PX4_ERROR; + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("driver reset failed"); + return PX4_ERROR; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("driver poll restart failed"); + return PX4_ERROR; + } + + px4_close(fd); + return PX4_OK; +} /** - * * 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) @@ -649,50 +615,61 @@ start(uint8_t rotation) 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 */ + // Create the driver. g_dev = new SRF02(rotation, i2c_bus); if (g_dev == nullptr) { - goto fail; + PX4_ERR("failed to instantiate the device"); + return PX4_ERROR; } if (OK != g_dev->init()) { - goto fail; + 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 */ - fd = px4_open(SRF02_DEVICE_PATH, O_RDONLY); + // Set the poll rate to default, starts automatic data collection. + int fd = px4_open(SRF02_DEVICE_PATH, O_RDONLY); if (fd < 0) { - goto fail; + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; + delete g_dev; + g_dev = nullptr; + px4_close(fd); + return PX4_ERROR; } px4_close(fd); return PX4_OK; +} -fail: - - if (fd >= 0) { - px4_close(fd); +/** + * Print the driver status. + */ +int +status() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return PX4_ERROR; } - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } + printf("state @ %p\n", g_dev); + g_dev->print_info(); - return PX4_ERROR; + return PX4_OK; } /** @@ -721,10 +698,6 @@ stop() int test() { - struct distance_sensor_s report; - ssize_t sz; - int ret; - int fd = px4_open(SRF02_DEVICE_PATH, O_RDONLY); if (fd < 0) { @@ -732,8 +705,10 @@ test() return PX4_ERROR; } - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); + 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"); @@ -742,27 +717,27 @@ test() print_message(report); - /* start the sensor polling at 2Hz */ + // 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 */ + // Read the sensor 5x and report each value. for (unsigned i = 0; i < 5; i++) { struct pollfd fds; - /* wait for data to be ready */ + // Wait for data to be ready. fds.fd = fd; fds.events = POLLIN; - ret = poll(&fds, 1, 2000); + int ret = poll(&fds, 1, 2000); if (ret != 1) { PX4_ERR("timed out waiting for sensor data"); return PX4_ERROR; } - /* now go get it */ + // Now go get it. sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { @@ -773,7 +748,7 @@ test() print_message(report); } - /* reset the sensor polling to default rate */ + // 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; @@ -783,55 +758,8 @@ test() return PX4_OK; } -/** - * Reset the driver. - */ -int -reset() -{ - int fd = px4_open(SRF02_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("failed"); - return PX4_ERROR; - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - return PX4_ERROR; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - return PX4_ERROR; - } - - px4_close(fd); - return PX4_OK; -} - -/** - * Print a little info about the driver. - */ int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - return PX4_OK; -} - -} /* namespace */ - - -static void -srf02_usage() +usage() { PX4_INFO("usage: srf02 command [options]"); PX4_INFO("options:"); @@ -840,18 +768,26 @@ 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; + int i2c_bus = SRF02_BUS_DEFAULT; + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - bool start_all = false; - int i2c_bus = SRF02_BUS_DEFAULT; + bool start_all = false; while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { @@ -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(); }