From 6689a3d1b1df17d42c76949ec14006888a2d4667 Mon Sep 17 00:00:00 2001 From: Vasily Evseenko Date: Fri, 29 Jul 2016 16:17:02 +0300 Subject: [PATCH] Add SENS_EN_SF1XX param to specify sensor model Fix missing orb_unadvertise on shutdown Fix missing closing of file descriptors --- src/drivers/sf1xx/sf1xx.cpp | 81 +++++++++++++++++++++-------- src/modules/sensors/sensor_params.c | 15 ++++++ 2 files changed, 75 insertions(+), 21 deletions(-) diff --git a/src/drivers/sf1xx/sf1xx.cpp b/src/drivers/sf1xx/sf1xx.cpp index 043f123f9a..1f282b459a 100644 --- a/src/drivers/sf1xx/sf1xx.cpp +++ b/src/drivers/sf1xx/sf1xx.cpp @@ -80,15 +80,6 @@ #define SF1XX_BASEADDR 0x55 #define SF1XX_DEVICE_PATH "/dev/sf1xx" -/* Device limits */ -#define SF1XX_MIN_DISTANCE (0.01f) -#define SF1XX_MAX_DISTANCE (120.0f) - - -/* conversion rates */ - -#define SF1XX_CONVERSION_INTERVAL 50000 // Maximum rate according to datasheet is 20Hz - /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -122,12 +113,13 @@ protected: private: float _min_distance; float _max_distance; + int _conversion_interval; work_s _work; - ringbuffer::RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; - int _measure_ticks; - int _class_instance; - int _orb_class_instance; + int _measure_ticks; + int _class_instance; + int _orb_class_instance; orb_advert_t _distance_sensor_topic; @@ -194,8 +186,9 @@ extern "C" __EXPORT int sf1xx_main(int argc, char *argv[]); SF1XX::SF1XX(int bus, int address) : I2C("SF1XX", SF1XX_DEVICE_PATH, bus, address, 400000), - _min_distance(SF1XX_MIN_DISTANCE), - _max_distance(SF1XX_MAX_DISTANCE), + _min_distance(-1.0f), + _max_distance(-1.0f), + _conversion_interval(-1), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), @@ -224,6 +217,10 @@ SF1XX::~SF1XX() delete _reports; } + if (_distance_sensor_topic != nullptr) { + orb_unadvertise(_distance_sensor_topic); + } + if (_class_instance != -1) { unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); } @@ -238,6 +235,43 @@ int SF1XX::init() { int ret = ERROR; + int hw_model; + param_get(param_find("SENS_EN_SF1XX"), &hw_model); + + switch (hw_model) { + case 0: + DEVICE_LOG("disabled."); + return ret; + + case 1: /* SF10/a (25m 32Hz) */ + _min_distance = 0.01f; + _max_distance = 25.0f; + _conversion_interval = 31250; + break; + + case 2: /* SF10/b (50m 32Hz) */ + _min_distance = 0.01f; + _max_distance = 50.0f; + _conversion_interval = 31250; + break; + + case 3: /* SF10/c (100m 16Hz) */ + _min_distance = 0.01f; + _max_distance = 100.0f; + _conversion_interval = 62500; + break; + + case 4: + /* SF11/c (120m 20Hz) */ + _min_distance = 0.01f; + _max_distance = 120.0f; + _conversion_interval = 50000; + break; + + default: + DEVICE_LOG("invalid HW model %d.", hw_model); + return ret; + } /* do I2C init (and probe) first */ if (I2C::init() != OK) { @@ -271,7 +305,8 @@ SF1XX::init() if (ret2 == 0) { ret = OK; _sensor_ok = true; - DEVICE_LOG("SF1xx with address %d found", SF1XX_BASEADDR); + DEVICE_LOG("(%dm %dHz) with address %d found", (int)_max_distance, + (int)(1e6f / _conversion_interval), SF1XX_BASEADDR); } return ret; @@ -335,7 +370,7 @@ SF1XX::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(SF1XX_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ if (want_start) { @@ -355,7 +390,7 @@ SF1XX::ioctl(struct file *filp, int cmd, unsigned long arg) int ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(SF1XX_CONVERSION_INTERVAL)) { + if (ticks < USEC2TICK(_conversion_interval)) { return -EINVAL; } @@ -464,7 +499,7 @@ SF1XX::read(struct file *filp, char *buffer, size_t buflen) } /* wait for it to complete */ - usleep(SF1XX_CONVERSION_INTERVAL); + usleep(_conversion_interval); /* run the collection phase */ if (OK != collect()) { @@ -598,7 +633,7 @@ SF1XX::cycle() &_work, (worker_t)&SF1XX::cycle_trampoline, this, - USEC2TICK(SF1XX_CONVERSION_INTERVAL)); + USEC2TICK(_conversion_interval)); } @@ -638,7 +673,7 @@ void info(); void start() { - int fd; + int fd = -1; if (g_dev != nullptr) { errx(1, "already started"); @@ -663,9 +698,11 @@ start() } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + ::close(fd); goto fail; } + ::close(fd); exit(0); fail: @@ -760,6 +797,7 @@ test() errx(1, "failed to set default poll rate"); } + ::close(fd); errx(0, "PASS"); } @@ -783,6 +821,7 @@ reset() err(1, "driver poll restart failed"); } + ::close(fd); exit(0); } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 710b4154d2..6d62f81968 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -3064,6 +3064,21 @@ PARAM_DEFINE_INT32(SENS_EN_SF0X, 0); */ PARAM_DEFINE_INT32(SENS_EN_MB12XX, 0); +/** + * Lightware SF1xx laser rangefinder + * + * @reboot_required true + * @min 0 + * @max 4 + * @group Sensor Enable + * @value 0 Disabled + * @value 1 SF10/a (25m) + * @value 2 SF10/b (50m) + * @value 3 SF10/c (100m) + * @value 4 SF11/c (120m) + */ +PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0); + /** * Set the minimum PWM for the MAIN outputs *