From fa834497bfd61d9d23d14aa501013f22f064cd93 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 6 Dec 2016 12:19:13 -0500 Subject: [PATCH] Setup sf0x driver to handle all lightware lidars. (#5957) --- ROMFS/px4fmu_common/init.d/rcS | 12 +++++- src/drivers/sf0x/sf0x.cpp | 67 +++++++++++++++++++++++------ src/modules/sensors/sensor_params.c | 14 ++++-- 3 files changed, 75 insertions(+), 18 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c94329ee11..534a2ca804 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -639,12 +639,20 @@ then fi fi - # sf0x lidar sensor - if param compare SENS_EN_SF0X 1 + # lightware serial lidar sensor + if param compare SENS_EN_SF0X 0 then + else sf0x start fi + # lightware i2c lidar sensor + if param compare SENS_EN_SF1XX 0 + then + else + sf1xx start + fi + # mb12xx sonar sensor if param compare SENS_EN_MB12XX 1 then diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index edc713b255..3f4a00d079 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -81,10 +81,7 @@ # error This requires CONFIG_SCHED_WORKQUEUE. #endif -#define SF0X_CONVERSION_INTERVAL 83334 #define SF0X_TAKE_RANGE_REG 'd' -#define SF02F_MIN_DISTANCE 0.3f -#define SF02F_MAX_DISTANCE 40.0f // designated SERIAL4/5 on Pixhawk #define SF0X_DEFAULT_PORT "/dev/ttyS6" @@ -112,6 +109,7 @@ private: char _port[20]; float _min_distance; float _max_distance; + int _conversion_interval; work_s _work; ringbuffer::RingBuffer *_reports; bool _sensor_ok; @@ -182,8 +180,9 @@ extern "C" __EXPORT int sf0x_main(int argc, char *argv[]); SF0X::SF0X(const char *port) : CDev("SF0X", RANGE_FINDER0_DEVICE_PATH), - _min_distance(SF02F_MIN_DISTANCE), - _max_distance(SF02F_MAX_DISTANCE), + _min_distance(0.30f), + _max_distance(40.0f), + _conversion_interval(83334), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), @@ -269,6 +268,50 @@ SF0X::~SF0X() int SF0X::init() { + int hw_model; + param_get(param_find("SENS_EN_SF0X"), &hw_model); + + switch (hw_model) { + case 0: + DEVICE_LOG("disabled."); + return 0; + + case 1: /* SF02 (40m, 12 Hz)*/ + _min_distance = 0.3f; + _max_distance = 40.0f; + _conversion_interval = 83334; + break; + + case 2: /* SF10/a (25m 32Hz) */ + _min_distance = 0.01f; + _max_distance = 25.0f; + _conversion_interval = 31250; + break; + + case 3: /* SF10/b (50m 32Hz) */ + _min_distance = 0.01f; + _max_distance = 50.0f; + _conversion_interval = 31250; + break; + + case 4: /* SF10/c (100m 16Hz) */ + _min_distance = 0.01f; + _max_distance = 100.0f; + _conversion_interval = 62500; + break; + + case 5: + /* 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 -1; + } + /* status */ int ret = 0; @@ -367,7 +410,7 @@ SF0X::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(SF0X_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ if (want_start) { @@ -387,7 +430,7 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(SF0X_CONVERSION_INTERVAL)) { + if (ticks < USEC2TICK(_conversion_interval)) { return -EINVAL; } @@ -496,7 +539,7 @@ SF0X::read(struct file *filp, char *buffer, size_t buflen) } /* wait for it to complete */ - usleep(SF0X_CONVERSION_INTERVAL); + usleep(_conversion_interval); /* run the collection phase */ if (OK != collect()) { @@ -559,7 +602,7 @@ SF0X::collect() perf_end(_sample_perf); /* only throw an error if we time out */ - if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { + if (read_elapsed > (_conversion_interval * 2)) { return ret; } else { @@ -705,14 +748,14 @@ SF0X::cycle() /* * Is there a collect->measure gap? */ - if (_measure_ticks > USEC2TICK(SF0X_CONVERSION_INTERVAL)) { + if (_measure_ticks > USEC2TICK(_conversion_interval)) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, - _measure_ticks - USEC2TICK(SF0X_CONVERSION_INTERVAL)); + _measure_ticks - USEC2TICK(_conversion_interval)); return; } @@ -731,7 +774,7 @@ SF0X::cycle() &_work, (worker_t)&SF0X::cycle_trampoline, this, - USEC2TICK(SF0X_CONVERSION_INTERVAL)); + USEC2TICK(_conversion_interval)); } void diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index ebb3ef2943..1c101b0578 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -3090,12 +3090,18 @@ PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000); PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0); /** - * Lightware SF0x laser rangefinder + * Lightware laser rangefinder (serial) * * @reboot_required true - * - * @boolean + * @min 0 + * @max 4 * @group Sensor Enable + * @value 0 Disabled + * @value 1 SF02 + * @value 2 SF10/a + * @value 3 SF10/b + * @value 4 SF10/c + * @value 5 SF11/c */ PARAM_DEFINE_INT32(SENS_EN_SF0X, 0); @@ -3120,7 +3126,7 @@ PARAM_DEFINE_INT32(SENS_EN_MB12XX, 0); PARAM_DEFINE_INT32(SENS_EN_TRONE, 0); /** - * Lightware SF1xx laser rangefinder + * Lightware SF1xx laser rangefinder (i2c) * * @reboot_required true * @min 0