diff --git a/CMakeLists.txt b/CMakeLists.txt index 457a0bfa74..bfd7b662b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,6 +80,7 @@ add_message_files( vehicle_global_velocity_setpoint.msg offboard_control_mode.msg vehicle_force_setpoint.msg + distance_sensor.msg ) ## Generate services in the 'srv' folder diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 0442637941..4c2119aa2f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -9,6 +9,7 @@ mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 +mavlink stream -d /dev/ttyACM0 -s DISTANCE_SENSOR -r 10 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30 diff --git a/makefiles/nuttx/config_px4fmu-v2_default.mk b/makefiles/nuttx/config_px4fmu-v2_default.mk index 7884b94cb0..4c8f60e693 100644 --- a/makefiles/nuttx/config_px4fmu-v2_default.mk +++ b/makefiles/nuttx/config_px4fmu-v2_default.mk @@ -45,6 +45,7 @@ MODULES += drivers/mkblctrl MODULES += drivers/px4flow MODULES += drivers/oreoled MODULES += drivers/gimbal +MODULES += drivers/pwm_input # # System commands diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg new file mode 100644 index 0000000000..ad01ce63e0 --- /dev/null +++ b/msg/distance_sensor.msg @@ -0,0 +1,17 @@ +# DISTANCE_SENSOR message data + +uint64 timestamp # Microseconds since system boot + +float32 min_distance # Minimum distance the sensor can measure (in m) +float32 max_distance # Maximum distance the sensor can measure (in m) +float32 current_distance # Current distance reading (in m) +float32 covariance # Measurement covariance (in m), 0 for unknown / invalid readings + +uint8 type # Type from MAV_DISTANCE_SENSOR enum +uint8 MAV_DISTANCE_SENSOR_LASER = 0 +uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 +uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 + +uint8 id # Onboard ID of the sensor + +uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum diff --git a/msg/range_finder.msg b/msg/range_finder.msg deleted file mode 100644 index d2e67543e5..0000000000 --- a/msg/range_finder.msg +++ /dev/null @@ -1,13 +0,0 @@ -int16 RANGE_FINDER_TYPE_LASER = 0 - -# range finder report structure. Reads from the device must be in multiples of this -# structure. -uint64 timestamp -uint64 error_count -uint16 type # type, following RANGE_FINDER_TYPE enum -float32 distance # in meters -float32 minimum_distance # minimum distance the sensor can measure -float32 maximum_distance # maximum distance the sensor can measure -uint8 valid # 1 == within sensor range, 0 = outside sensor range -float32[12] distance_vector # in meters length should match MB12XX_MAX_RANGEFINDERS -uint8 just_updated # number of the most recent measurement sensor diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 7a6a58746c..b4570e4bbd 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -128,7 +128,7 @@ Airspeed::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(differential_pressure_s)); + _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); if (_reports == nullptr) goto out; diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 3fbf3f9ddf..fad04c9978 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -109,7 +109,7 @@ public: virtual void print_info(); private: - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; perf_counter_t _buffer_overflows; /* this class has pointer data members and should not be copied */ diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 86c026fb04..0332d3fecf 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -161,7 +161,7 @@ __BEGIN_DECLS #define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) #define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) #define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) -#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14) /* Power supply control and monitoring GPIOs */ #define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 6a718fdd24..698258fa45 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -243,6 +243,7 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_VDD_SERVO_VALID); stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); + stm32_configgpio(GPIO_GPIO5_OUTPUT); /* configure the high-resolution time/callout interface */ hrt_init(); diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index a9e964ef94..c206a5037c 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -41,7 +41,8 @@ SRCS = \ cdev.cpp \ i2c_nuttx.cpp \ pio.cpp \ - spi.cpp + spi.cpp \ + ringbuffer.cpp else SRCS = \ device_posix.cpp \ @@ -50,5 +51,5 @@ SRCS = \ vdev_posix.cpp \ i2c_posix.cpp \ sim.cpp \ - ringbuffer.cpp + ringbuffer.cpp endif diff --git a/src/drivers/device/ringbuffer.cpp b/src/drivers/device/ringbuffer.cpp index 1790289338..c30e0a54df 100644 --- a/src/drivers/device/ringbuffer.cpp +++ b/src/drivers/device/ringbuffer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,6 +40,9 @@ #include "ringbuffer.h" #include +namespace ringbuffer +{ + RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : _num_items(num_items), _item_size(item_size), @@ -57,25 +60,25 @@ RingBuffer::~RingBuffer() unsigned RingBuffer::_next(unsigned index) { - return (0 == index) ? _num_items : (index - 1); + return (0 == index) ? _num_items : (index - 1); } bool RingBuffer::empty() { - return _tail == _head; + return _tail == _head; } bool RingBuffer::full() { - return _next(_head) == _tail; + return _next(_head) == _tail; } unsigned RingBuffer::size() { - return (_buf != nullptr) ? _num_items : 0; + return (_buf != nullptr) ? _num_items : 0; } void @@ -86,7 +89,7 @@ RingBuffer::flush() } bool -RingBuffer::put(const void *val, size_t val_size) +RingBuffer::put(const void *val, size_t val_size) { unsigned next = _next(_head); if (next != _tail) { @@ -250,7 +253,7 @@ static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned #define __PX4_SBCAP __sync_bool_compare_and_swap #endif bool -RingBuffer::get(void *val, size_t val_size) +RingBuffer::get(void *val, size_t val_size) { if (_tail != _head) { unsigned candidate; @@ -340,7 +343,7 @@ RingBuffer::get(double &val) } unsigned -RingBuffer::space(void) +RingBuffer::space(void) { unsigned tail, head; @@ -361,7 +364,7 @@ RingBuffer::space(void) } unsigned -RingBuffer::count(void) +RingBuffer::count(void) { /* * Note that due to the conservative nature of space(), this may @@ -371,7 +374,7 @@ RingBuffer::count(void) } bool -RingBuffer::resize(unsigned new_size) +RingBuffer::resize(unsigned new_size) { char *old_buffer; char *new_buffer = new char [(new_size+1) * _item_size]; @@ -388,13 +391,15 @@ RingBuffer::resize(unsigned new_size) } void -RingBuffer::print_info(const char *name) +RingBuffer::print_info(const char *name) { printf("%s %u/%lu (%u/%u @ %p)\n", - name, - _num_items, - (unsigned long)_num_items * _item_size, - _head, - _tail, + name, + _num_items, + (unsigned long)_num_items * _item_size, + _head, + _tail, _buf); } + +} // namespace ringbuffer diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 1c65c02135..4fcdaf47fa 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,6 +43,9 @@ #include #include +namespace ringbuffer __EXPORT +{ + class RingBuffer { public: RingBuffer(unsigned ring_size, size_t entry_size); @@ -161,7 +164,7 @@ public: private: unsigned _num_items; const size_t _item_size; - char *_buf; + char *_buf; volatile unsigned _head; /**< insertion point in _item_size units */ volatile unsigned _tail; /**< removal point in _item_size units */ @@ -172,9 +175,4 @@ private: RingBuffer operator=(const RingBuffer&); }; -#ifdef __PX4_NUTTX -// Not sure why NuttX requires these to be defined in the header file -// but on other targets it causes a problem with multiple definitions -// at link time -#include "ringbuffer.cpp" -#endif +} // namespace ringbuffer diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index bd72971b94..725835cc0a 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -46,16 +46,12 @@ #define RANGE_FINDER_BASE_DEVICE_PATH "/dev/range_finder" #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" -#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected +#define MB12XX_MAX_RANGEFINDERS 12 // Maximum number of Maxbotix sensors on bus -#define range_finder_report range_finder_s -#define __orb_sensor_range_finder __orb_range_finder - -#include - -#ifndef RANGE_FINDER_TYPE_LASER -#define RANGE_FINDER_TYPE_LASER 0 -#endif +/* + * ObjDev tag for distance sensor data. + */ +ORB_DECLARE(distance_sensor); /* * ioctl() definitions diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 92ffa80665..2d3be15833 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -156,7 +156,7 @@ private: work_s _work; unsigned _measure_ticks; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; mag_scale _scale; float _range_scale; float _range_ga; @@ -423,7 +423,7 @@ HMC5883::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(mag_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) goto out; @@ -921,10 +921,10 @@ HMC5883::collect() _temperature_counter = 0; - ret = _interface->read(ADDR_TEMP_OUT_MSB, + ret = _interface->read(ADDR_TEMP_OUT_MSB, raw_temperature, sizeof(raw_temperature)); if (ret == OK) { - int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + + int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + raw_temperature[1]; new_report.temperature = 25 + (temp16 / (16*8.0f)); _temperature_error_count = 0; @@ -1146,8 +1146,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = -EIO; goto out; } - float cal[3] = {fabsf(expected_cal[0] / report.x), - fabsf(expected_cal[1] / report.y), + float cal[3] = {fabsf(expected_cal[0] / report.x), + fabsf(expected_cal[1] / report.y), fabsf(expected_cal[2] / report.z)}; if (cal[0] > 0.7f && cal[0] < 1.35f && @@ -1381,7 +1381,7 @@ HMC5883::print_info() printf("poll interval: %u ticks\n", _measure_ticks); printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z); printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); - printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, (double)(1.0f/_range_scale), (double)_range_ga); printf("temperature %.2f\n", (double)_last_report.temperature); @@ -1451,7 +1451,7 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) bus.dev = NULL; return false; } - + int fd = open(bus.devpath, O_RDONLY); if (fd < 0) { return false; @@ -1505,7 +1505,7 @@ struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid) busid == bus_options[i].busid) && bus_options[i].dev != NULL) { return bus_options[i]; } - } + } errx(1, "bus %u not started", (unsigned)busid); } @@ -1750,7 +1750,7 @@ hmc5883_main(int argc, char *argv[]) } } - const char *verb = argv[optind]; + const char *verb = argv[optind]; /* * Start/load the driver. diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f036987d7d..f276b67f00 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -229,7 +229,7 @@ private: struct hrt_call _call; unsigned _call_interval; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -472,7 +472,7 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(gyro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) goto out; diff --git a/src/drivers/ll40ls/LidarLite.cpp b/src/drivers/ll40ls/LidarLite.cpp new file mode 100644 index 0000000000..6406f9590e --- /dev/null +++ b/src/drivers/ll40ls/LidarLite.cpp @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file LidarLite.h + * @author Johan Jansen + * + * Generic interface driver for the PulsedLight Lidar-Lite range finders. + */ +#include "LidarLite.h" +#include +#include + +LidarLite::LidarLite() : + _min_distance(LL40LS_MIN_DISTANCE), + _max_distance(LL40LS_MAX_DISTANCE), + _measure_ticks(0) +{ +} + +LidarLite::~LidarLite() +{ +} + +void LidarLite::set_minimum_distance(const float min) +{ + _min_distance = min; +} + +void LidarLite::set_maximum_distance(const float max) +{ + _max_distance = max; +} + +float LidarLite::get_minimum_distance() const +{ + return _min_distance; +} + +float LidarLite::get_maximum_distance() const +{ + return _max_distance; +} + +uint32_t LidarLite::getMeasureTicks() const +{ + return _measure_ticks; +} + +int LidarLite::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCRESET: + reset_sensor(); + return OK; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return OK; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return OK; + } + break; + + default: + return -EINVAL; + } +} diff --git a/src/drivers/ll40ls/LidarLite.h b/src/drivers/ll40ls/LidarLite.h new file mode 100644 index 0000000000..72bb22f263 --- /dev/null +++ b/src/drivers/ll40ls/LidarLite.h @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file LidarLite.h + * @author Johan Jansen + * + * Generic interface driver for the PulsedLight Lidar-Lite range finders. + */ +#pragma once + +#include +#include + +/* Device limits */ +#define LL40LS_MIN_DISTANCE (0.00f) +#define LL40LS_MAX_DISTANCE (60.00f) + +// normal conversion wait time +#define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */ + +// maximum time to wait for a conversion to complete. +#define LL40LS_CONVERSION_TIMEOUT 100*1000UL /* 100ms */ + +class LidarLite +{ +public: + LidarLite(); + + virtual ~LidarLite(); + + virtual int init() = 0; + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + virtual void start() = 0; + + virtual void stop() = 0; + + /** + * @brief + * Diagnostics - print some basic information about the driver. + */ + virtual void print_info() = 0; + + /** + * @brief + * print registers to console + */ + virtual void print_registers() = 0; + +protected: + /** + * 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 LL40LS_MIN_DISTANCE + * and LL40LS_MAX_DISTANCE + */ + void set_minimum_distance(const float min); + void set_maximum_distance(const float max); + float get_minimum_distance() const; + float get_maximum_distance() const; + + uint32_t getMeasureTicks() const; + + virtual int measure() = 0; + virtual int collect() = 0; + + virtual int reset_sensor() = 0; + +private: + float _min_distance; + float _max_distance; + uint32_t _measure_ticks; +}; diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp new file mode 100644 index 0000000000..a9626c8c45 --- /dev/null +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -0,0 +1,568 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file LidarLiteI2C.cpp + * @author Allyson Kreft + * + * Driver for the PulsedLight Lidar-Lite range finders connected via I2C. + */ + +#include "LidarLiteI2C.h" +#include +#include +#include +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : + I2C("LL40LS", path, bus, address, 100000), + _work{}, + _reports(nullptr), + _sensor_ok(false), + _collect_phase(false), + _class_instance(-1), + _orb_class_instance(-1), + _distance_sensor_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")), + _comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_i2c_overflows")), + _sensor_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_resets")), + _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_zero_resets")), + _last_distance(0), + _zero_counter(0), + _acquire_time_usec(0), + _pause_measurements(false), + _bus(bus) +{ + // up the retries since the device misses the first measure attempts + _retries = 3; + + // enable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +LidarLiteI2C::~LidarLiteI2C() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + perf_free(_sensor_resets); + perf_free(_sensor_zero_resets); +} + +int LidarLiteI2C::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); + + if (_reports == nullptr) { + goto out; + } + + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report; + measure(); + _reports->get(&ds_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); + + if (_distance_sensor_topic < 0) { + debug("failed to create distance_sensor object. Did you start uOrb?"); + } + } + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val) +{ + return transfer(®, 1, &val, 1); +} + +int LidarLiteI2C::probe() +{ + // cope with both old and new I2C bus address + const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; + + // more retries for detection + _retries = 10; + + for (uint8_t i = 0; i < sizeof(addresses); i++) { + uint8_t who_am_i = 0, max_acq_count = 0; + + // set the I2C bus address + set_address(addresses[i]); + + /* register 2 defaults to 0x80. If this matches it is + almost certainly a ll40ls */ + if (read_reg(LL40LS_MAX_ACQ_COUNT_REG, max_acq_count) == OK && max_acq_count == 0x80) { + // very likely to be a ll40ls. This is the + // default max acquisition counter + goto ok; + } + + if (read_reg(LL40LS_WHO_AM_I_REG, who_am_i) == OK && who_am_i == LL40LS_WHO_AM_I_REG_VAL) { + // it is responding correctly to a + // WHO_AM_I. This works with older sensors (pre-production) + goto ok; + } + + debug("probe failed reg11=0x%02x reg2=0x%02x\n", + (unsigned)who_am_i, + (unsigned)max_acq_count); + } + + // not found on any address + return -EIO; + +ok: + _retries = 3; + + // reset the sensor to ensure it is in a known state with + // correct settings + return reset_sensor(); +} + +int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + default: { + int result = LidarLite::ioctl(filp, cmd, arg); + + if (result == -EINVAL) { + result = I2C::ioctl(filp, cmd, arg); + } + + return result; + } + } +} + +ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (getMeasureTicks() > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(LL40LS_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int LidarLiteI2C::measure() +{ + int ret; + + if (_pause_measurements) { + // we are in print_registers() and need to avoid + // acquisition to keep the I2C peripheral on the + // sensor active + return OK; + } + + /* + * Send the command to begin a measurement. + */ + const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; + ret = transfer(cmd, sizeof(cmd), nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + debug("i2c::transfer returned %d", ret); + + // if we are getting lots of I2C transfer errors try + // resetting the sensor + if (perf_event_count(_comms_errors) % 10 == 0) { + perf_count(_sensor_resets); + reset_sensor(); + } + + return ret; + } + + // remember when we sent the acquire so we can know when the + // acquisition has timed out + _acquire_time_usec = hrt_absolute_time(); + ret = OK; + + return ret; +} + +/* + reset the sensor to power on defaults + */ +int LidarLiteI2C::reset_sensor() +{ + const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET }; + int ret = transfer(cmd, sizeof(cmd), nullptr, 0); + return ret; +} + +/* + dump sensor registers for debugging + */ +void LidarLiteI2C::print_registers() +{ + _pause_measurements = true; + printf("ll40ls registers\n"); + // wait for a while to ensure the lidar is in a ready state + usleep(50000); + + for (uint8_t reg = 0; reg <= 0x67; reg++) { + uint8_t val = 0; + int ret = transfer(®, 1, &val, 1); + + if (ret != OK) { + printf("%02x:XX ", (unsigned)reg); + + } else { + printf("%02x:%02x ", (unsigned)reg, (unsigned)val); + } + + if (reg % 16 == 15) { + printf("\n"); + } + } + + printf("\n"); + _pause_measurements = false; +} + +int LidarLiteI2C::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + // read the high and low byte distance registers + uint8_t distance_reg = LL40LS_DISTHIGH_REG; + ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); + + if (ret < 0) { + if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { + /* + NACKs from the sensor are expected when we + read before it is ready, so only consider it + an error if more than 100ms has elapsed. + */ + debug("error reading from sensor: %d", ret); + perf_count(_comms_errors); + + if (perf_event_count(_comms_errors) % 10 == 0) { + perf_count(_sensor_resets); + reset_sensor(); + } + } + + perf_end(_sample_perf); + // if we are getting lots of I2C transfer errors try + // resetting the sensor + return ret; + } + + uint16_t distance_cm = (val[0] << 8) | val[1]; + float distance_m = float(distance_cm) * 1e-2f; + struct distance_sensor_s report; + + if (distance_cm == 0) { + _zero_counter++; + + if (_zero_counter == 20) { + /* we have had 20 zeros in a row - reset the + sensor. This is a known bad state of the + sensor where it returns 16 bits of zero for + the distance with a trailing NACK, and + keeps doing that even when the target comes + into a valid range. + */ + _zero_counter = 0; + perf_end(_sample_perf); + perf_count(_sensor_zero_resets); + return reset_sensor(); + } + + } else { + _zero_counter = 0; + } + + _last_distance = distance_m; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.current_distance = distance_m; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0f; + /* the sensor is in fact a laser + sonar but there is no enum for this */ + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + report.orientation = 8; + /* TODO: set proper ID */ + report.id = 0; + + /* publish it, if we are the primary */ + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void LidarLiteI2C::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&LidarLiteI2C::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void LidarLiteI2C::stop() +{ + work_cancel(HPWORK, &_work); +} + +void LidarLiteI2C::cycle_trampoline(void *arg) +{ + LidarLiteI2C *dev = (LidarLiteI2C *)arg; + + dev->cycle(); +} + +void LidarLiteI2C::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* try a collection */ + if (OK != collect()) { + debug("collection error"); + + /* if we've been waiting more than 200ms then + send a new acquire */ + if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT * 2) { + _collect_phase = false; + } + + } else { + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&LidarLiteI2C::cycle_trampoline, + this, + getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + + return; + } + } + } + + if (_collect_phase == false) { + /* measurement phase */ + if (OK != measure()) { + debug("measure error"); + + } else { + /* next phase is collection. Don't switch to + collection phase until we have a successful + acquire request I2C transfer */ + _collect_phase = true; + } + } + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&LidarLiteI2C::cycle_trampoline, + this, + USEC2TICK(LL40LS_CONVERSION_INTERVAL)); +} + +void LidarLiteI2C::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + perf_print_counter(_sensor_resets); + perf_print_counter(_sensor_zero_resets); + printf("poll interval: %u ticks\n", getMeasureTicks()); + _reports->print_info("report queue"); + printf("distance: %ucm (0x%04x)\n", + (unsigned)_last_distance, (unsigned)_last_distance); +} diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h new file mode 100644 index 0000000000..f03784fc4d --- /dev/null +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file LidarLiteI2C.h + * @author Allyson Kreft + * + * Driver for the PulsedLight Lidar-Lite range finders connected via I2C. + */ + +#pragma once + +#include "LidarLite.h" + +#include +#include +#include + +#include +#include + +#include +#include +#include + +/* Configuration Constants */ +#define LL40LS_BUS PX4_I2C_BUS_EXPANSION +#define LL40LS_BASEADDR 0x62 /* 7-bit address */ +#define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */ + +/* LL40LS Registers addresses */ + +#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ +#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */ +#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ +#define LL40LS_MAX_ACQ_COUNT_REG 0x02 /* maximum acquisition count register */ +#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ +#define LL40LS_WHO_AM_I_REG 0x11 +#define LL40LS_WHO_AM_I_REG_VAL 0xCA +#define LL40LS_SIGNAL_STRENGTH_REG 0x5b + +class LidarLiteI2C : public LidarLite, public device::I2C +{ +public: + LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR); + virtual ~LidarLiteI2C(); + + int init() override; + + ssize_t read(struct file *filp, char *buffer, size_t buflen); + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info() override; + + /** + * print registers to console + */ + void print_registers() override; + +protected: + int probe(); + int read_reg(uint8_t reg, uint8_t &val); + + int measure() override; + int reset_sensor(); + +private: + work_s _work; + ringbuffer::RingBuffer *_reports; + bool _sensor_ok; + 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; + perf_counter_t _buffer_overflows; + perf_counter_t _sensor_resets; + perf_counter_t _sensor_zero_resets; + uint16_t _last_distance; + uint16_t _zero_counter; + uint64_t _acquire_time_usec; + volatile bool _pause_measurements; + + /**< the bus the device is connected to */ + int _bus; + + /** + * 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(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int collect(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + +private: + LidarLiteI2C(const LidarLiteI2C ©) = delete; + LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete; +}; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp new file mode 100644 index 0000000000..14ce921b4d --- /dev/null +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -0,0 +1,295 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file LidarLitePWM.h + * @author Johan Jansen + * @author Ban Siesta + * + * Driver for the PulsedLight Lidar-Lite range finders connected via PWM. + * + * This driver accesses the pwm_input published by the pwm_input driver. + */ + +#include "LidarLitePWM.h" +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef __cplusplus +static const int ERROR = -1; +#endif + +LidarLitePWM::LidarLitePWM(const char *path) : + CDev("LidarLitePWM", path), + _work{}, + _reports(nullptr), + _class_instance(-1), + _orb_class_instance(-1), + _pwmSub(-1), + _pwm{}, + _distance_sensor_topic(-1), + _range{}, + _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_pwm_read")), + _read_errors(perf_alloc(PC_COUNT, "ll40ls_pwm_read_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_pwm_buffer_overflows")), + _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_pwm_zero_resets")) +{ +} + +LidarLitePWM::~LidarLitePWM() +{ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); + } + + /* free perf counters */ + perf_free(_sample_perf); + perf_free(_buffer_overflows); + perf_free(_sensor_zero_resets); +} + + +int LidarLitePWM::init() +{ + /* do regular cdev init */ + int ret = CDev::init(); + + if (ret != OK) { + return ERROR; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); + + if (_reports == nullptr) { + return ERROR; + } + + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the distance_sensor topic */ + struct distance_sensor_s ds_report; + measure(); + _reports->get(&ds_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); + + if (_distance_sensor_topic < 0) { + debug("failed to create distance_sensor object. Did you start uOrb?"); + } + } + + return OK; +} + +void LidarLitePWM::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_read_errors); + perf_print_counter(_buffer_overflows); + perf_print_counter(_sensor_zero_resets); + warnx("poll interval: %u ticks", getMeasureTicks()); + warnx("distance: %.3fm", (double)_range.current_distance); +} + +void LidarLitePWM::print_registers() +{ + printf("Not supported in PWM mode\n"); +} + +void LidarLitePWM::start() +{ + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&LidarLitePWM::cycle_trampoline, this, 1); +} + +void LidarLitePWM::stop() +{ + work_cancel(HPWORK, &_work); +} + +void LidarLitePWM::cycle_trampoline(void *arg) +{ + LidarLitePWM *dev = (LidarLitePWM *)arg; + + dev->cycle(); +} + +void LidarLitePWM::cycle() +{ + measure(); + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&LidarLitePWM::cycle_trampoline, + this, + getMeasureTicks()); + return; +} + +int LidarLitePWM::measure() +{ + perf_begin(_sample_perf); + + if (OK != collect()) { + debug("collection error"); + perf_count(_read_errors); + perf_end(_sample_perf); + return ERROR; + } + + _range.timestamp = hrt_absolute_time(); + _range.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + _range.max_distance = get_maximum_distance(); + _range.min_distance = get_minimum_distance(); + _range.current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */ + _range.covariance = 0.0f; + _range.orientation = 8; + /* TODO: set proper ID */ + _range.id = 0; + + /* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */ + if (_range.current_distance <= 0.0f) { + perf_count(_sensor_zero_resets); + perf_end(_sample_perf); + return reset_sensor(); + } + + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &_range); + } + + if (_reports->force(&_range)) { + perf_count(_buffer_overflows); + } + + poll_notify(POLLIN); + perf_end(_sample_perf); + return OK; +} + +ssize_t LidarLitePWM::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (getMeasureTicks() > 0) { + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + + } else { + + _reports->flush(); + measure(); + + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + } + + return ret; +} + +int +LidarLitePWM::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + /* no custom ioctls implemented for now */ + default: + /* give it to the superclass */ + return LidarLite::ioctl(filp, cmd, arg); + } +} + +int LidarLitePWM::collect() +{ + int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY); + + if (fd == -1) { + return ERROR; + } + + if (::read(fd, &_pwm, sizeof(_pwm)) == sizeof(_pwm)) { + ::close(fd); + return OK; + } + + ::close(fd); + return EAGAIN; +} + +int LidarLitePWM::reset_sensor() +{ + + int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY); + + if (fd == -1) { + return ERROR; + } + + int ret = ::ioctl(fd, SENSORIOCRESET, 0); + ::close(fd); + return ret; +} diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h new file mode 100644 index 0000000000..8d337e48ba --- /dev/null +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file LidarLitePWM.h + * @author Johan Jansen + * @author Ban Siesta + * + * Driver for the PulsedLight Lidar-Lite range finders connected via PWM. + * + * This driver accesses the pwm_input published by the pwm_input driver. + */ +#pragma once + +#include "LidarLite.h" + +#include +#include + +#include +#include + +#include +#include +#include + + + +class LidarLitePWM : public LidarLite, public device::CDev +{ +public: + LidarLitePWM(const char *path); + virtual ~LidarLitePWM(); + + int init() override; + + ssize_t read(struct file *filp, char *buffer, size_t buflen) override; + int ioctl(struct file *filp, int cmd, unsigned long arg); + + void start() override; + + void stop() override; + + void cycle(); + + /** + * @brief + * Diagnostics - print some basic information about the driver. + */ + void print_info() override; + + /** + * @brief + * print registers to console + */ + void print_registers() override; + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + +protected: + + int measure() override; + + int collect() override; + + int reset_sensor() override; + + void task_main_trampoline(int argc, char *argv[]); + +private: + work_s _work; + ringbuffer::RingBuffer *_reports; + int _class_instance; + int _orb_class_instance; + int _pwmSub; + struct pwm_input_s _pwm; + orb_advert_t _distance_sensor_topic; + struct distance_sensor_s _range; + + perf_counter_t _sample_perf; + perf_counter_t _read_errors; + perf_counter_t _buffer_overflows; + perf_counter_t _sensor_zero_resets; +}; diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index b3368406a1..8984220625 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -34,800 +34,34 @@ /** * @file ll40ls.cpp * @author Allyson Kreft + * @author Johan Jansen + * @author Ban Siesta * - * Driver for the PulsedLight Lidar-Lite range finders connected via I2C. + * Interface for the PulsedLight Lidar-Lite range finders. */ -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include - +#include "LidarLiteI2C.h" +#include "LidarLitePWM.h" #include - -/* Configuration Constants */ -#define LL40LS_BUS PX4_I2C_BUS_EXPANSION -#define LL40LS_BASEADDR 0x62 /* 7-bit address */ -#define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */ -#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" -#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" - -/* LL40LS Registers addresses */ - -#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ -#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */ -#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ -#define LL40LS_MAX_ACQ_COUNT_REG 0x02 /* maximum acquisition count register */ -#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ -#define LL40LS_WHO_AM_I_REG 0x11 -#define LL40LS_WHO_AM_I_REG_VAL 0xCA -#define LL40LS_SIGNAL_STRENGTH_REG 0x5b - -/* Device limits */ -#define LL40LS_MIN_DISTANCE (0.00f) -#define LL40LS_MAX_DISTANCE (60.00f) - -// normal conversion wait time -#define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */ - -// maximum time to wait for a conversion to complete. -#define LL40LS_CONVERSION_TIMEOUT 100*1000UL /* 100ms */ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +#include +#include +#include +#include +#include #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class LL40LS : public device::I2C -{ -public: - LL40LS(int bus, const char *path, int address = LL40LS_BASEADDR); - virtual ~LL40LS(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - - /** - * print registers to console - */ - void print_registers(); - -protected: - virtual int probe(); - virtual int read_reg(uint8_t reg, uint8_t &val); - -private: - float _min_distance; - float _max_distance; - work_s _work; - RingBuffer *_reports; - bool _sensor_ok; - unsigned _measure_ticks; - bool _collect_phase; - int _class_instance; - - orb_advert_t _range_finder_topic; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - perf_counter_t _sensor_resets; - perf_counter_t _sensor_zero_resets; - uint16_t _last_distance; - uint16_t _zero_counter; - uint64_t _acquire_time_usec; - volatile bool _pause_measurements; - - /**< the bus the device is connected to */ - int _bus; - - /** - * 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 LL40LS_MIN_DISTANCE - * and LL40LS_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 cycle(); - int measure(); - int collect(); - int reset_sensor(); - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - - -}; +#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" +#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" +#define LL40LS_DEVICE_PATH_PWM "/dev/ll40ls_pwm" /* * Driver 'main' command. */ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]); -LL40LS::LL40LS(int bus, const char *path, int address) : - I2C("LL40LS", path, bus, address, 100000), - _min_distance(LL40LS_MIN_DISTANCE), - _max_distance(LL40LS_MAX_DISTANCE), - _reports(nullptr), - _sensor_ok(false), - _measure_ticks(0), - _collect_phase(false), - _class_instance(-1), - _range_finder_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), - _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), - _sensor_resets(perf_alloc(PC_COUNT, "ll40ls_resets")), - _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_zero_resets")), - _last_distance(0), - _zero_counter(0), - _pause_measurements(false), - _bus(bus) -{ - // up the retries since the device misses the first measure attempts - _retries = 3; - - // enable debug() calls - _debug_enabled = false; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); -} - -LL40LS::~LL40LS() -{ - /* make sure we are truly inactive */ - stop(); - - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - - if (_class_instance != -1) { - unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); - } - - // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); - perf_free(_buffer_overflows); - perf_free(_sensor_resets); - perf_free(_sensor_zero_resets); -} - -int -LL40LS::init() -{ - int ret = ERROR; - - /* do I2C init (and probe) first */ - if (I2C::init() != OK) { - goto out; - } - - /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); - - if (_reports == nullptr) { - goto out; - } - - _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; - measure(); - _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); - - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); - } - } - - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; -out: - return ret; -} - -int -LL40LS::read_reg(uint8_t reg, uint8_t &val) -{ - return transfer(®, 1, &val, 1); -} - -int -LL40LS::probe() -{ - // cope with both old and new I2C bus address - const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; - - // more retries for detection - _retries = 10; - - for (uint8_t i=0; i 100)) { - return -EINVAL; - } - - irqstate_t flags = irqsave(); - - if (!_reports->resize(arg)) { - irqrestore(flags); - return -ENOMEM; - } - - irqrestore(flags); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _reports->size(); - - case SENSORIOCRESET: - reset_sensor(); - return OK; - - case RANGEFINDERIOCSETMINIUMDISTANCE: { - set_minimum_distance(*(float *)arg); - return 0; - } - break; - - case RANGEFINDERIOCSETMAXIUMDISTANCE: { - set_maximum_distance(*(float *)arg); - return 0; - } - break; - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} - -ssize_t -LL40LS::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(rbuf)) { - ret += sizeof(*rbuf); - rbuf++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - do { - _reports->flush(); - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - usleep(LL40LS_CONVERSION_INTERVAL); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - if (_reports->get(rbuf)) { - ret = sizeof(*rbuf); - } - - } while (0); - - return ret; -} - -int -LL40LS::measure() -{ - int ret; - - if (_pause_measurements) { - // we are in print_registers() and need to avoid - // acquisition to keep the I2C peripheral on the - // sensor active - return OK; - } - - /* - * Send the command to begin a measurement. - */ - const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; - ret = transfer(cmd, sizeof(cmd), nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - debug("i2c::transfer returned %d", ret); - // if we are getting lots of I2C transfer errors try - // resetting the sensor - if (perf_event_count(_comms_errors) % 10 == 0) { - perf_count(_sensor_resets); - reset_sensor(); - } - return ret; - } - - // remember when we sent the acquire so we can know when the - // acquisition has timed out - _acquire_time_usec = hrt_absolute_time(); - ret = OK; - - return ret; -} - -/* - reset the sensor to power on defaults - */ -int -LL40LS::reset_sensor() -{ - const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET }; - int ret = transfer(cmd, sizeof(cmd), nullptr, 0); - return ret; -} - -/* - dump sensor registers for debugging - */ -void -LL40LS::print_registers() -{ - _pause_measurements = true; - printf("ll40ls registers\n"); - // wait for a while to ensure the lidar is in a ready state - usleep(50000); - for (uint8_t reg=0; reg<=0x67; reg++) { - uint8_t val = 0; - int ret = transfer(®, 1, &val, 1); - if (ret != OK) { - printf("%02x:XX ",(unsigned)reg); - } else { - printf("%02x:%02x ",(unsigned)reg, (unsigned)val); - } - if (reg % 16 == 15) { - printf("\n"); - } - } - printf("\n"); - _pause_measurements = false; -} - -int -LL40LS::collect() -{ - int ret = -EIO; - - /* read from the sensor */ - uint8_t val[2] = {0, 0}; - - perf_begin(_sample_perf); - - // read the high and low byte distance registers - uint8_t distance_reg = LL40LS_DISTHIGH_REG; - ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); - - if (ret < 0) { - if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { - /* - NACKs from the sensor are expected when we - read before it is ready, so only consider it - an error if more than 100ms has elapsed. - */ - debug("error reading from sensor: %d", ret); - perf_count(_comms_errors); - if (perf_event_count(_comms_errors) % 10 == 0) { - perf_count(_sensor_resets); - reset_sensor(); - } - } - perf_end(_sample_perf); - // if we are getting lots of I2C transfer errors try - // resetting the sensor - return ret; - } - - uint16_t distance = (val[0] << 8) | val[1]; - float si_units = distance * 0.01f; /* cm to m */ - struct range_finder_report report; - - if (distance == 0) { - _zero_counter++; - if (_zero_counter == 20) { - /* we have had 20 zeros in a row - reset the - sensor. This is a known bad state of the - sensor where it returns 16 bits of zero for - the distance with a trailing NACK, and - keeps doing that even when the target comes - into a valid range. - */ - _zero_counter = 0; - perf_end(_sample_perf); - perf_count(_sensor_zero_resets); - return reset_sensor(); - } - } else { - _zero_counter = 0; - } - - _last_distance = distance; - - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { - report.valid = 1; - } - else { - report.valid = 0; - } - - /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); - } - - if (_reports->force(&report)) { - perf_count(_buffer_overflows); - } - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - ret = OK; - - perf_end(_sample_perf); - return ret; -} - -void -LL40LS::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - _reports->flush(); - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&LL40LS::cycle_trampoline, this, 1); - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_RANGEFINDER - }; - static orb_advert_t pub = -1; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } -} - -void -LL40LS::stop() -{ - work_cancel(HPWORK, &_work); -} - -void -LL40LS::cycle_trampoline(void *arg) -{ - LL40LS *dev = (LL40LS *)arg; - - dev->cycle(); -} - -void -LL40LS::cycle() -{ - /* collection phase? */ - if (_collect_phase) { - - /* try a collection */ - if (OK != collect()) { - debug("collection error"); - /* if we've been waiting more than 200ms then - send a new acquire */ - if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT*2) { - _collect_phase = false; - } - } else { - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&LL40LS::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); - - return; - } - } - } - - if (_collect_phase == false) { - /* measurement phase */ - if (OK != measure()) { - debug("measure error"); - } else { - /* next phase is collection. Don't switch to - collection phase until we have a successful - acquire request I2C transfer */ - _collect_phase = true; - } - } - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&LL40LS::cycle_trampoline, - this, - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); -} - -void -LL40LS::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - perf_print_counter(_sensor_resets); - perf_print_counter(_sensor_zero_resets); - printf("poll interval: %u ticks\n", _measure_ticks); - _reports->print_info("report queue"); - printf("distance: %ucm (0x%04x)\n", - (unsigned)_last_distance, (unsigned)_last_distance); -} /** * Local functions in support of the shell command. @@ -841,90 +75,136 @@ namespace ll40ls #endif const int ERROR = -1; -LL40LS *g_dev_int; -LL40LS *g_dev_ext; +LidarLiteI2C *g_dev_int; +LidarLiteI2C *g_dev_ext; +LidarLitePWM *g_dev_pwm; -void start(int bus); -void stop(int bus); -void test(int bus); -void reset(int bus); -void info(int bus); -void regdump(int bus); +void start(const bool use_i2c, const int bus); +void stop(const bool use_i2c, const int bus); +void test(const bool use_i2c, const int bus); +void reset(const bool use_i2c, const int bus); +void info(const bool use_i2c, const int bus); +void regdump(const bool use_i2c, const int bus); void usage(); /** * Start the driver. */ -void -start(int bus) +void start(const bool use_i2c, const int bus) { - /* create the driver, attempt expansion bus first */ - if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { - if (g_dev_ext != nullptr) - errx(0, "already started external"); - g_dev_ext = new LL40LS(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); - if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { - delete g_dev_ext; - g_dev_ext = nullptr; - if (bus == PX4_I2C_BUS_EXPANSION) { - goto fail; + if (use_i2c) { + /* create the driver, attempt expansion bus first */ + if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { + if (g_dev_ext != nullptr) { + errx(0, "already started external"); + } + + g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); + + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; + + if (bus == PX4_I2C_BUS_EXPANSION) { + goto fail; + } } } - } #ifdef PX4_I2C_BUS_ONBOARD - /* if this failed, attempt onboard sensor */ - if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) - errx(0, "already started internal"); - g_dev_int = new LL40LS(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); - if (g_dev_int != nullptr && OK != g_dev_int->init()) { - /* tear down the failing onboard instance */ - delete g_dev_int; - g_dev_int = nullptr; - if (bus == PX4_I2C_BUS_ONBOARD) { + /* if this failed, attempt onboard sensor */ + if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { + if (g_dev_int != nullptr) { + errx(0, "already started internal"); + } + + g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); + + if (g_dev_int != nullptr && OK != g_dev_int->init()) { + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; + + if (bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } + } + + if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { goto fail; } } - if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { - goto fail; - } - } + #endif - /* set the poll rate to default, starts automatic data collection */ - if (g_dev_int != nullptr) { - int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); - if (fd == -1) { - goto fail; - } - int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); - if (ret < 0) { - goto fail; - } - } + /* set the poll rate to default, starts automatic data collection */ + if (g_dev_int != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); - if (g_dev_ext != nullptr) { - int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); - if (fd == -1) { - goto fail; - } - int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); - if (ret < 0) { - goto fail; + if (fd == -1) { + goto fail; + } + + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + + if (ret < 0) { + goto fail; + } } - } + + if (g_dev_ext != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); + + if (fd == -1) { + goto fail; + } + + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + + if (ret < 0) { + goto fail; + } + } + + } else { + g_dev_pwm = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM); + + if (g_dev_pwm != nullptr && OK != g_dev_pwm->init()) { + delete g_dev_pwm; + g_dev_pwm = nullptr; + warnx("failed to init PWM"); + } + + if (g_dev_pwm != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_PWM, O_RDONLY); + + if (fd == -1) { + warnx("fd nothing"); + goto fail; + } + + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + + if (ret < 0) { + warnx("pollrate fail"); + goto fail; + } + } + } exit(0); fail: + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { delete g_dev_int; g_dev_int = nullptr; } + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { delete g_dev_ext; g_dev_ext = nullptr; @@ -936,9 +216,10 @@ fail: /** * Stop the driver */ -void stop(int bus) +void stop(const bool use_i2c, const int bus) { - LL40LS **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext); + LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD ? &g_dev_int : &g_dev_ext); + if (*g_dev != nullptr) { delete *g_dev; *g_dev = nullptr; @@ -956,17 +237,25 @@ void stop(int bus) * and automatic modes. */ void -test(int bus) +test(const bool use_i2c, const int bus) { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; - const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); + + const char *path; + + if (use_i2c) { + path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + + } else { + path = LL40LS_DEVICE_PATH_PWM; + } int fd = open(path, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'll40ls start' if the driver is not running", path); + err(1, "%s open failed, is the driver running?", path); } /* do a simple demand read */ @@ -977,7 +266,7 @@ test(int bus) } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); + warnx("measurement: %0.2f m", (double)report.current_distance); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ @@ -985,7 +274,7 @@ test(int bus) errx(1, "failed to set 2Hz poll rate"); } - /* read the sensor 5x and report each value */ + /* read the sensor 5 times and report each value */ for (unsigned i = 0; i < 5; i++) { struct pollfd fds; @@ -1006,7 +295,7 @@ test(int bus) } warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); + warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %lld", report.timestamp); } @@ -1022,9 +311,18 @@ test(int bus) * Reset the driver. */ void -reset(int bus) +reset(const bool use_i2c, const int bus) { - const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); + + const char *path; + + if (use_i2c) { + path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + + } else { + path = LL40LS_DEVICE_PATH_PWM; + } + int fd = open(path, O_RDONLY); if (fd < 0) { @@ -1046,11 +344,19 @@ reset(int bus) * Print a little info about the driver. */ void -info(int bus) +info(const bool use_i2c, const int bus) { - LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); - if (g_dev == nullptr) { - errx(1, "driver not running"); + LidarLite *g_dev = nullptr; + + if (use_i2c) { + g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + } else { + g_dev = g_dev_pwm; } printf("state @ %p\n", g_dev); @@ -1063,9 +369,10 @@ info(int bus) * Dump registers */ void -regdump(int bus) +regdump(const bool use_i2c, const int bus) { - LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); + LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + if (g_dev == nullptr) { errx(1, "driver not running"); } @@ -1079,8 +386,8 @@ regdump(int bus) void usage() { - warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump'"); - warnx("options:"); + warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump' [i2c|pwm]"); + warnx("options for I2C:"); warnx(" -X only external bus"); #ifdef PX4_I2C_BUS_ONBOARD warnx(" -I only internal bus"); @@ -1098,62 +405,84 @@ ll40ls_main(int argc, char *argv[]) while ((ch = getopt(argc, argv, "XI")) != EOF) { switch (ch) { #ifdef PX4_I2C_BUS_ONBOARD + case 'I': bus = PX4_I2C_BUS_ONBOARD; break; #endif + case 'X': bus = PX4_I2C_BUS_EXPANSION; break; + default: ll40ls::usage(); exit(0); } } - const char *verb = argv[optind]; + /* default to I2C if no protocol is given */ + bool use_i2c = true; - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - ll40ls::start(bus); + /* determine protocol first because it's needed next */ + if (argc > optind + 1) { + const char *protocol = argv[optind + 1]; + + if (!strcmp(protocol, "pwm")) { + use_i2c = false; + + } else if (!strcmp(protocol, "i2c")) { + use_i2c = true; + + } else { + warnx("unknown protocol, choose pwm or i2c"); + ll40ls::usage(); + exit(0); + } } - /* - * Stop the driver - */ - if (!strcmp(verb, "stop")) { - ll40ls::stop(bus); + /* now determine action */ + if (argc > optind) { + const char *verb = argv[optind]; + + /* Start/load the driver. */ + if (!strcmp(verb, "start")) { + + ll40ls::start(use_i2c, bus); + } + + /* Stop the driver */ + if (!strcmp(verb, "stop")) { + ll40ls::stop(use_i2c, bus); + } + + /* Test the driver/device. */ + else if (!strcmp(verb, "test")) { + ll40ls::test(use_i2c, bus); + } + + /* Reset the driver. */ + else if (!strcmp(verb, "reset")) { + ll40ls::reset(use_i2c, bus); + } + + /* dump registers */ + else if (!strcmp(verb, "regdump")) { + ll40ls::regdump(use_i2c, bus); + } + + /* Print driver information. */ + else if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + ll40ls::info(use_i2c, bus); + } + + else { + ll40ls::usage(); + exit(0); + } } - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - ll40ls::test(bus); - } - - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - ll40ls::reset(bus); - } - - /* - * dump registers - */ - if (!strcmp(verb, "regdump")) { - ll40ls::regdump(bus); - } - - /* - * Print driver information. - */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - ll40ls::info(bus); - } - - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + warnx("unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + ll40ls::usage(); + exit(0); } diff --git a/src/drivers/ll40ls/module.mk b/src/drivers/ll40ls/module.mk index fb627afeea..c2c5e1e22a 100644 --- a/src/drivers/ll40ls/module.mk +++ b/src/drivers/ll40ls/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,6 +37,9 @@ MODULE_COMMAND = ll40ls -SRCS = ll40ls.cpp +SRCS = ll40ls.cpp \ + LidarLite.cpp \ + LidarLiteI2C.cpp \ + LidarLitePWM.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index e0863f46cd..8a77eeb917 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -275,8 +275,8 @@ private: unsigned _call_accel_interval; unsigned _call_mag_interval; - RingBuffer *_accel_reports; - RingBuffer *_mag_reports; + ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -588,7 +588,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _debug_enabled = true; _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D; - + /* Prime _mag with parents devid. */ _mag->_device_id.devid = _device_id.devid; _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; @@ -646,12 +646,12 @@ LSM303D::init() } /* allocate basic report buffers */ - _accel_reports = new RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; - _mag_reports = new RingBuffer(2, sizeof(mag_report)); + _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) goto out; @@ -1399,9 +1399,9 @@ LSM303D::start() _mag_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_accel_call, + hrt_call_every(&_accel_call, 1000, - _call_accel_interval - LSM303D_TIMER_REDUCTION, + _call_accel_interval - LSM303D_TIMER_REDUCTION, (hrt_callout)&LSM303D::measure_trampoline, this); hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); } diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 1a4ea17cf1..afeb8e5545 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -70,6 +70,7 @@ #include #include +#include #include @@ -125,13 +126,14 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; - int _class_instance; + int _class_instance; + int _orb_class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -208,7 +210,8 @@ MB12XX::MB12XX(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _orb_class_instance(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")), @@ -255,7 +258,7 @@ MB12XX::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ @@ -268,12 +271,13 @@ MB12XX::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report = {}; + struct distance_sensor_s ds_report = {}; - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); - if (_range_finder_topic < 0) { - log("failed to create sensor_range_finder object. Did you start uOrb?"); + if (_distance_sensor_topic < 0) { + log("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -469,8 +473,8 @@ ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -569,53 +573,23 @@ MB12XX::collect() return ret; } - uint16_t distance = val[0] << 8 | val[1]; - float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ - struct range_finder_report report; + uint16_t distance_cm = val[0] << 8 | val[1]; + float distance_m = float(distance_cm) * 1e-2f; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + struct distance_sensor_s report; report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - - /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ - if (addr_ind.size() == 1) { - report.distance = si_units; - - for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) { - report.distance_vector[i] = 0; - } - - report.just_updated = 0; - - } else { - /* for multiple sonars connected */ - - /* don't use the orginial single sonar variable */ - report.distance = 0; - - /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */ - _latest_sonar_measurements[_cycle_counter] = si_units; - - for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { - report.distance_vector[i] = _latest_sonar_measurements[i]; - } - - /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */ - report.just_updated = _index_counter; - - /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */ - for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) { - report.distance_vector[addr_ind.size() + i] = 0; - } - } - - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + report.orientation = 8; + report.current_distance = distance_m; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0f; + /* TODO: set proper ID */ + report.id = 0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -840,7 +814,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -858,8 +832,7 @@ test() } warnx("single read"); - warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); - warnx("time: %lld", report.timestamp); + warnx("time: %llu", report.timestamp); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -887,13 +860,7 @@ test() } warnx("periodic read %u", i); - - /* Print the sonar rangefinder report sonar distance vector */ - for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) { - warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1); - } - - warnx("time: %lld", report.timestamp); + warnx("time: %llu", report.timestamp); } /* reset the sensor polling to default rate */ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 9dd7cc7048..7049bd0788 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -242,7 +242,7 @@ private: struct hrt_call _call; unsigned _call_interval; - RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_accel_reports; struct accel_scale _accel_scale; float _accel_range_scale; @@ -251,7 +251,7 @@ private: int _accel_orb_class_instance; int _accel_class_instance; - RingBuffer *_gyro_reports; + ringbuffer::RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -611,11 +611,11 @@ MPU6000::init() } /* allocate basic report buffers */ - _accel_reports = new RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; - _gyro_reports = new RingBuffer(2, sizeof(gyro_report)); + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); if (_gyro_reports == nullptr) goto out; diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index 9d0e436eeb..cf96a7a95b 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -127,7 +127,7 @@ protected: struct work_s _work; unsigned _measure_ticks; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -269,7 +269,7 @@ MS5611::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(baro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { debug("can't get memory for reports"); @@ -896,7 +896,7 @@ start_bus(struct ms5611_bus_option &bus) bus.dev = NULL; return false; } - + int fd = open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ @@ -955,7 +955,7 @@ struct ms5611_bus_option &find_bus(enum MS5611_BUS busid) busid == bus_options[i].busid) && bus_options[i].dev != NULL) { return bus_options[i]; } - } + } errx(1,"bus %u not started", (unsigned)busid); } diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index b92e950010..3918a6575e 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -121,7 +121,7 @@ private: work_s _work; ///< work queue for scheduling reads bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED uint8_t _num_healthy; ///< number of healthy LEDs - RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs + ringbuffer::RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs uint64_t _last_gencall; uint64_t _start_time; ///< system time we first attempt to communicate with battery }; @@ -176,7 +176,7 @@ OREOLED::init() } /* allocate command queue */ - _cmd_queue = new RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); + _cmd_queue = new ringbuffer::RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); if (_cmd_queue == nullptr) { return ENOTTY; diff --git a/src/drivers/pwm_input/module.mk b/src/drivers/pwm_input/module.mk index 04f04d6cbc..a135ce777e 100644 --- a/src/drivers/pwm_input/module.mk +++ b/src/drivers/pwm_input/module.mk @@ -39,3 +39,4 @@ MODULE_COMMAND = pwm_input SRCS = pwm_input.cpp +EXTRACXXFLAGS = -Wno-pmf-conversions diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index c7d58579e8..d06d125e58 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -32,10 +32,13 @@ ****************************************************************************/ /** - * @file pwm_input.cpp + * @file pwm_input.cpp * * PWM input driver based on earlier driver from Evan Slatyer, * which in turn was based on drv_hrt.c + * + * @author: Andrew Tridgell + * @author: Ban Siesta */ #include @@ -58,6 +61,7 @@ #include #include #include +#include #include "chip.h" #include "up_internal.h" @@ -80,6 +84,9 @@ #include #include +/* Reset pin define */ +#define GPIO_VDD_RANGEFINDER_EN GPIO_GPIO5_OUTPUT + #if HRT_TIMER == PWMIN_TIMER #error cannot share timer between HRT and PWMIN #endif @@ -188,34 +195,36 @@ * Specific registers and bits used by HRT sub-functions */ #if PWMIN_TIMER_CHANNEL == 1 - #define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */ - #define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */ - #define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */ - #define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */ - #define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */ - #define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT)) - #define CCMR2_PWMIN 0 - #define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) - #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) - #define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT) - #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */ +#define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */ +#define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */ +#define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */ +#define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */ +#define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT)) +#define CCMR2_PWMIN 0 +#define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) +#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) +#define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT) +#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) #elif PWMIN_TIMER_CHANNEL == 2 - #define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */ - #define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */ - #define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */ - #define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */ - #define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */ - #define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */ - #define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT)) - #define CCMR2_PWMIN 0 - #define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) - #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) - #define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT) - #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */ +#define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */ +#define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */ +#define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */ +#define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */ +#define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */ +#define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT)) +#define CCMR2_PWMIN 0 +#define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) +#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) +#define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT) +#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) #else - #error PWMIN_TIMER_CHANNEL must be either 1 and 2. +#error PWMIN_TIMER_CHANNEL must be either 1 and 2. #endif +#define TIMEOUT_POLL 300000 /* reset after no response over this time in microseconds [0.3s] */ +#define TIMEOUT_READ 200000 /* don't reset if the last read is back more than this time in microseconds [0.2s] */ class PWMIN : device::CDev { @@ -228,22 +237,33 @@ public: virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - void _publish(uint16_t status, uint32_t period, uint32_t pulse_width); - void _print_info(void); + void publish(uint16_t status, uint32_t period, uint32_t pulse_width); + void print_info(void); + void hard_reset(); private: - uint32_t error_count; - uint32_t pulses_captured; - uint32_t last_period; - uint32_t last_width; - RingBuffer *reports; - bool timer_started; + uint32_t _error_count; + uint32_t _pulses_captured; + uint32_t _last_period; + uint32_t _last_width; + hrt_abstime _last_poll_time; + hrt_abstime _last_read_time; + ringbuffer::RingBuffer *_reports; + bool _timer_started; + + hrt_call _hard_reset_call; /* HRT callout for note completion */ + hrt_call _freeze_test_call; /* HRT callout for note completion */ + + void _timer_init(void); + + void _turn_on(); + void _turn_off(); + void _freeze_test(); - void timer_init(void); }; static int pwmin_tim_isr(int irq, void *context); -static void pwmin_start(void); +static void pwmin_start(); static void pwmin_info(void); static void pwmin_test(void); static void pwmin_reset(void); @@ -252,49 +272,53 @@ static PWMIN *g_dev; PWMIN::PWMIN() : CDev("pwmin", PWMIN0_DEVICE_PATH), - error_count(0), - pulses_captured(0), - last_period(0), - last_width(0), - reports(nullptr), - timer_started(false) + _error_count(0), + _pulses_captured(0), + _last_period(0), + _last_width(0), + _reports(nullptr), + _timer_started(false) { } PWMIN::~PWMIN() { - if (reports != nullptr) - delete reports; + if (_reports != nullptr) { + delete _reports; + } } /* - initialise the driver. This doesn't actually start the timer (that - is done on open). We don't start the timer to allow for this driver - to be started in init scripts when the user may be using the input - pin as PWM output + * initialise the driver. This doesn't actually start the timer (that + * is done on open). We don't start the timer to allow for this driver + * to be started in init scripts when the user may be using the input + * pin as PWM output */ int PWMIN::init() { - // we just register the device in /dev, and only actually - // activate the timer when requested to when the device is opened + /* we just register the device in /dev, and only actually + * activate the timer when requested to when the device is opened */ CDev::init(); - reports = new RingBuffer(2, sizeof(struct pwm_input_s)); - if (reports == nullptr) { + _reports = new ringbuffer::RingBuffer(2, sizeof(struct pwm_input_s)); + if (_reports == nullptr) { return -ENOMEM; } + /* Schedule freeze check to invoke periodically */ + hrt_call_every(&_freeze_test_call, 0, TIMEOUT_POLL, reinterpret_cast(&PWMIN::_freeze_test), this); + return OK; } /* * Initialise the timer we are going to use. */ -void PWMIN::timer_init(void) +void PWMIN::_timer_init(void) { - // run with interrupts disabled in case the timer is already - // setup. We don't want it firing while we are doing the setup + /* run with interrupts disabled in case the timer is already + * setup. We don't want it firing while we are doing the setup */ irqstate_t flags = irqsave(); stm32_configgpio(GPIO_PWM_IN); @@ -302,7 +326,7 @@ void PWMIN::timer_init(void) irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr); /* Clear no bits, set timer enable bit.*/ - modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); + modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); /* disable and configure the timer */ rCR1 = 0; @@ -317,13 +341,13 @@ void PWMIN::timer_init(void) rCCER = CCER_PWMIN; rDCR = 0; - // for simplicity scale by the clock in MHz. This gives us - // readings in microseconds which is typically what is needed - // for a PWM input driver - uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL; + /* for simplicity scale by the clock in MHz. This gives us + * readings in microseconds which is typically what is needed + * for a PWM input driver */ + uint32_t prescaler = PWMIN_TIMER_CLOCK / 1000000UL; - /* - * define the clock speed. We want the highest possible clock + /* + * define the clock speed. We want the highest possible clock * speed that avoids overflows. */ rPSC = prescaler - 1; @@ -343,12 +367,41 @@ void PWMIN::timer_init(void) irqrestore(flags); - timer_started = true; + _timer_started = true; +} + +void +PWMIN::_freeze_test() +{ + /* reset if last poll time was way back and a read was recently requested */ + if (hrt_elapsed_time(&_last_poll_time) > TIMEOUT_POLL && hrt_elapsed_time(&_last_read_time) < TIMEOUT_READ) { + warnx("Lidar is down, reseting"); + hard_reset(); + } +} + +void +PWMIN::_turn_on() +{ + stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1); +} + +void +PWMIN::_turn_off() +{ + stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0); +} + +void +PWMIN::hard_reset() +{ + _turn_off(); + hrt_call_after(&_hard_reset_call, 9000, reinterpret_cast(&PWMIN::_turn_on), this); } /* - hook for open of the driver. We start the timer at this point, then - leave it running + * hook for open of the driver. We start the timer at this point, then + * leave it running */ int PWMIN::open(struct file *filp) @@ -356,16 +409,19 @@ PWMIN::open(struct file *filp) if (g_dev == nullptr) { return -EIO; } - int ret = CDev::open(filp); - if (ret == OK && !timer_started) { - g_dev->timer_init(); + + int ret = CDev::open(filp); + + if (ret == OK && !_timer_started) { + g_dev->_timer_init(); } + return ret; } /* - handle ioctl requests + * handle ioctl requests */ int PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) @@ -373,28 +429,32 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 500)) + if ((arg < 1) || (arg > 500)) { return -EINVAL; + } irqstate_t flags = irqsave(); - if (!reports->resize(arg)) { + + if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return reports->size(); + return _reports->size(); case SENSORIOCRESET: /* user has asked for the timer to be reset. This may - be needed if the pin was used for a different - purpose (such as PWM output) - */ - timer_init(); + * be needed if the pin was used for a different + * purpose (such as PWM output) */ + _timer_init(); + /* also reset the sensor */ + hard_reset(); return OK; default: @@ -405,72 +465,73 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) /* - read some samples from the device + * read some samples from the device */ ssize_t PWMIN::read(struct file *filp, char *buffer, size_t buflen) { + _last_read_time = hrt_absolute_time(); + unsigned count = buflen / sizeof(struct pwm_input_s); struct pwm_input_s *buf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } while (count--) { - if (reports->get(buf)) { + if (_reports->get(buf)) { ret += sizeof(struct pwm_input_s); buf++; } } - /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } /* - publish some data from the ISR in the ring buffer + * publish some data from the ISR in the ring buffer */ -void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width) +void PWMIN::publish(uint16_t status, uint32_t period, uint32_t pulse_width) { /* if we missed an edge, we have to give up */ if (status & SR_OVF_PWMIN) { - error_count++; + _error_count++; return; } + _last_poll_time = hrt_absolute_time(); + struct pwm_input_s pwmin_report; - pwmin_report.timestamp = hrt_absolute_time(); - pwmin_report.error_count = error_count; + pwmin_report.timestamp = _last_poll_time; + pwmin_report.error_count = _error_count; pwmin_report.period = period; pwmin_report.pulse_width = pulse_width; - reports->force(&pwmin_report); - - last_period = period; - last_width = pulse_width; - pulses_captured++; + _reports->force(&pwmin_report); } /* - print information on the last captured + * print information on the last captured */ -void PWMIN::_print_info(void) +void PWMIN::print_info(void) { - if (!timer_started) { + if (!_timer_started) { printf("timer not started - try the 'test' command\n"); + } else { printf("count=%u period=%u width=%u\n", - (unsigned)pulses_captured, - (unsigned)last_period, - (unsigned)last_width); + (unsigned)_pulses_captured, + (unsigned)_last_period, + (unsigned)_last_width); } } /* - Handle the interupt, gathering pulse data + * Handle the interupt, gathering pulse data */ static int pwmin_tim_isr(int irq, void *context) { @@ -482,46 +543,52 @@ static int pwmin_tim_isr(int irq, void *context) rSR = 0; if (g_dev != nullptr) { - g_dev->_publish(status, period, pulse_width); + g_dev->publish(status, period, pulse_width); } return OK; } /* - start the driver + * start the driver */ -static void pwmin_start(void) +static void pwmin_start() { if (g_dev != nullptr) { - printf("driver already started\n"); - exit(1); + errx(1, "driver already started"); } + g_dev = new PWMIN(); + if (g_dev == nullptr) { errx(1, "driver allocation failed"); } + if (g_dev->init() != OK) { errx(1, "driver init failed"); } + exit(0); } /* - test the driver + * test the driver */ static void pwmin_test(void) { int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { errx(1, "Failed to open device"); } + uint64_t start_time = hrt_absolute_time(); printf("Showing samples for 5 seconds\n"); - while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) { + while (hrt_absolute_time() < start_time + 5U * 1000UL * 1000UL) { struct pwm_input_s buf; + if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) { printf("period=%u width=%u error_count=%u\n", (unsigned)buf.period, @@ -529,28 +596,33 @@ static void pwmin_test(void) (unsigned)buf.error_count); } } + close(fd); exit(0); } /* - reset the timer + * reset the timer */ static void pwmin_reset(void) { + g_dev->hard_reset(); int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { errx(1, "Failed to open device"); } + if (ioctl(fd, SENSORIOCRESET, 0) != OK) { errx(1, "reset failed"); } + close(fd); exit(0); } /* - show some information on the driver + * show some information on the driver */ static void pwmin_info(void) { @@ -558,15 +630,16 @@ static void pwmin_info(void) printf("driver not started\n"); exit(1); } - g_dev->_print_info(); + + g_dev->print_info(); exit(0); } /* - driver entry point + * driver entry point */ -int pwm_input_main(int argc, char * argv[]) +int pwm_input_main(int argc, char *argv[]) { const char *verb = argv[1]; diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f35957caae..4d87d1cfe1 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -34,6 +34,7 @@ /** * @file px4flow.cpp * @author Dominik Honegger + * @author Ban Siesta * * Driver for the PX4FLOW module connected via I2C. */ @@ -66,11 +67,13 @@ #include #include +#include #include #include #include #include +#include #include @@ -83,6 +86,9 @@ #define PX4FLOW_CONVERSION_INTERVAL 100000 ///< in microseconds! 20000 = 50 Hz 100000 = 10Hz #define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed +#define PX4FLOW_MAX_DISTANCE 5.0f +#define PX4FLOW_MIN_DISTANCE 0.3f + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -120,16 +126,19 @@ protected: private: work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + int _orb_class_instance; orb_advert_t _px4flow_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - + enum Rotation _sensor_rotation; /** @@ -182,13 +191,16 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), + _class_instance(-1), + _orb_class_instance(-1), _px4flow_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")), _sensor_rotation(rotation) { - // enable debug() calls + // disable debug() calls _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... @@ -217,12 +229,26 @@ PX4FLOW::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(optical_flow_s)); + _reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) { goto out; } + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* 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, + &_orb_class_instance, ORB_PRIO_HIGH); + + if (_distance_sensor_topic < 0) { + log("failed to create distance_sensor object. Did you start uOrb?"); + } + } + ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -486,10 +512,10 @@ PX4FLOW::collect() report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; - + /* rotate measurements according to parameter */ float zeroval = 0.0f; - rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); @@ -499,6 +525,20 @@ PX4FLOW::collect() orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); } + /* publish to the distance_sensor topic as well */ + struct distance_sensor_s distance_report; + distance_report.timestamp = report.timestamp; + distance_report.min_distance = PX4FLOW_MIN_DISTANCE; + distance_report.max_distance = PX4FLOW_MAX_DISTANCE; + distance_report.current_distance = report.ground_distance_m; + distance_report.covariance = 0.0f; + distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + /* TODO: the ID needs to be properly set */ + distance_report.id = 0; + distance_report.orientation = 8; + + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report); + /* post a report to the ring */ if (_reports->force(&report)) { perf_count(_buffer_overflows); diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index c45e73fd9a..431184d046 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -69,6 +69,7 @@ #include #include +#include #include @@ -118,7 +119,7 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -128,7 +129,10 @@ private: enum SF0X_PARSE_STATE _parse_state; hrt_abstime _last_read; - orb_advert_t _range_finder_topic; + int _class_instance; + int _orb_class_instance; + + orb_advert_t _distance_sensor_topic; unsigned _consecutive_fail_count; @@ -194,7 +198,9 @@ SF0X::SF0X(const char *port) : _linebuf_index(0), _parse_state(SF0X_PARSE_STATE0_UNSYNC), _last_read(0), - _range_finder_topic(-1), + _class_instance(-1), + _orb_class_instance(-1), + _distance_sensor_topic(-1), _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), @@ -255,6 +261,14 @@ SF0X::~SF0X() if (_reports != nullptr) { delete _reports; } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); + } + + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int @@ -270,21 +284,27 @@ SF0X::init() if (ret != OK) break; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { warnx("mem err"); ret = -1; break; } - /* get a publish handle on the range finder topic */ - struct range_finder_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_range_finder_topic < 0) { - warnx("advert err"); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* 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, + &_orb_class_instance, ORB_PRIO_HIGH); + + if (_distance_sensor_topic < 0) { + log("failed to create distance_sensor object. Did you start uOrb?"); + } } + } while(0); /* close the fd */ @@ -442,8 +462,8 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t SF0X::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -556,11 +576,11 @@ SF0X::collect() _last_read = hrt_absolute_time(); - float si_units; + float distance_m = -1.0f; bool valid = false; for (int i = 0; i < ret; i++) { - if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) { + if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) { valid = true; } } @@ -569,20 +589,22 @@ SF0X::collect() return -EAGAIN; } - debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); + debug("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO")); - struct range_finder_report report; + struct distance_sensor_s report; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + report.orientation = 8; + report.current_distance = distance_m; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0f; + /* TODO: set proper ID */ + report.id = 0; /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); if (_reports->force(&report)) { perf_count(_buffer_overflows); @@ -815,7 +837,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); @@ -832,8 +854,8 @@ test() } warnx("single read"); - warnx("val: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("val: %0.2f m", (double)report.current_distance); + warnx("time: %llu", report.timestamp); /* start the sensor polling at 2 Hz rate */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -863,8 +885,8 @@ test() } warnx("read #%u", i); - warnx("val: %0.3f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("val: %0.3f m", (double)report.current_distance); + warnx("time: %llu", report.timestamp); } /* reset the sensor polling to the default rate */ diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index a3c8633725..23e52547a1 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -68,6 +68,7 @@ #include #include +#include #include @@ -82,7 +83,7 @@ #define TRONE_WHO_AM_I_REG 0x01 /* Who am I test register */ #define TRONE_WHO_AM_I_REG_VAL 0xA1 - + /* Device limits */ #define TRONE_MIN_DISTANCE (0.20f) #define TRONE_MAX_DISTANCE (14.00f) @@ -122,13 +123,14 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; - int _class_instance; + int _class_instance; + int _orb_class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -209,7 +211,7 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; -static uint8_t crc8(uint8_t *p, uint8_t len) { +/* static uint8_t crc8(uint8_t *p, uint8_t len) { uint16_t i; uint16_t crc = 0x0; @@ -219,7 +221,7 @@ static uint8_t crc8(uint8_t *p, uint8_t len) { } return crc & 0xFF; -} +}*/ /* * Driver 'main' command. @@ -235,7 +237,8 @@ TRONE::TRONE(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _orb_class_instance(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "trone_read")), _comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows")) @@ -281,7 +284,7 @@ TRONE::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { goto out; @@ -291,13 +294,15 @@ TRONE::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; + struct distance_sensor_s ds_report; measure(); - _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _reports->get(&ds_report); - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); + + if (_distance_sensor_topic < 0) { + log("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -468,8 +473,8 @@ TRONE::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t TRONE::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -566,22 +571,24 @@ TRONE::collect() return ret; } - uint16_t distance = (val[0] << 8) | val[1]; - float si_units = distance * 0.001f; /* mm to m */ - struct range_finder_report report; + uint16_t distance_mm = (val[0] << 8) | val[1]; + float distance_m = float(distance_mm) * 1e-3f; + struct distance_sensor_s report; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; - + /* there is no enum item for a combined LASER and ULTRASOUND which it should be */ + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + report.orientation = 8; + report.current_distance = distance_m; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0f; + /* TODO: set proper ID */ + report.id = 0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -787,7 +794,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -805,8 +812,8 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.2f m", (double)report.current_distance); + warnx("time: %llu", report.timestamp); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -834,9 +841,8 @@ test() } warnx("periodic read %u", i); - warnx("valid %u", report.valid); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.3f", (double)report.current_distance); + warnx("time: %llu", report.timestamp); } /* reset the sensor polling to default rate */ diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 4d5e56a961..ee6fcb3ac5 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -58,13 +58,13 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include @@ -162,7 +162,7 @@ private: struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ struct wind_estimate_s _wind; /**< wind estimate */ - struct range_finder_report _distance; /**< distance estimate */ + struct distance_sensor_s _distance; /**< distance estimate */ struct vehicle_land_detected_s _landDetector; struct actuator_armed_s _armed; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 1b6e17ac37..c04d79f4e7 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -501,7 +501,7 @@ void AttitudePositionEstimatorEKF::task_main() /* * do subscriptions */ - _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + _distance_sub = orb_subscribe(ORB_ID(distance_sensor)); _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); @@ -1471,10 +1471,10 @@ void AttitudePositionEstimatorEKF::pollData() orb_check(_distance_sub, &_newRangeData); if (_newRangeData) { - orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); - - if (_distance.valid) { - _ekf->rngMea = _distance.distance; + orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance); + if ((_distance.current_distance > _distance.min_distance) + && (_distance.current_distance < _distance.max_distance)) { + _ekf->rngMea = _distance.current_distance; _distance_last_valid = _distance.timestamp; } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 91a30763bb..1b2689e6bb 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -68,9 +68,9 @@ #include #include #include +#include #include #include -#include #include #include @@ -2178,7 +2178,6 @@ protected: } }; - class MavlinkStreamDistanceSensor : public MavlinkStream { public: @@ -2204,12 +2203,12 @@ public: unsigned get_size() { - return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _distance_sensor_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_range_sub; - uint64_t _range_time; + MavlinkOrbSubscription *_distance_sensor_sub; + uint64_t _dist_sensor_time; /* do not allow top copying this class */ MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); @@ -2217,36 +2216,46 @@ private: protected: explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), - _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))), - _range_time(0) + _distance_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(distance_sensor))), + _dist_sensor_time(0) {} void send(const hrt_abstime t) { - struct range_finder_report range; + struct distance_sensor_s dist_sensor; - if (_range_sub->update(&_range_time, &range)) { + if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) { mavlink_distance_sensor_t msg; - msg.time_boot_ms = range.timestamp / 1000; + msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */ - switch (range.type) { - case RANGE_FINDER_TYPE_LASER: + /* TODO: use correct ID here */ + msg.id = 0; + + switch (dist_sensor.type) { + case MAV_DISTANCE_SENSOR_ULTRASOUND: + msg.type = MAV_DISTANCE_SENSOR_ULTRASOUND; + break; + + case MAV_DISTANCE_SENSOR_LASER: msg.type = MAV_DISTANCE_SENSOR_LASER; break; + case MAV_DISTANCE_SENSOR_INFRARED: + msg.type = MAV_DISTANCE_SENSOR_INFRARED; + break; + default: msg.type = MAV_DISTANCE_SENSOR_LASER; break; } - msg.id = 0; - msg.orientation = 0; - msg.min_distance = range.minimum_distance * 100; - msg.max_distance = range.maximum_distance * 100; - msg.current_distance = range.distance * 100; - msg.covariance = 20; + msg.orientation = dist_sensor.orientation; + msg.min_distance = dist_sensor.min_distance * 100.0f; /* m to cm */ + msg.max_distance = dist_sensor.max_distance * 100.0f; /* m to cm */ + msg.current_distance = dist_sensor.current_distance * 100.0f; /* m to cm */ + msg.covariance = dist_sensor.covariance; _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ca31ec9b6c..59dfb45f47 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -56,7 +56,6 @@ #include #include #include -#include #include #include #include @@ -109,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _battery_pub(-1), _cmd_pub(-1), _flow_pub(-1), - _range_pub(-1), + _distance_sensor_pub(-1), _offboard_control_mode_pub(-1), _actuator_controls_pub(-1), _global_vel_sp_pub(-1), @@ -211,6 +210,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_timesync(msg); break; + case MAVLINK_MSG_ID_DISTANCE_SENSOR: + handle_message_distance_sensor(msg); + break; + default: break; } @@ -412,6 +415,25 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) } else { orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } + + /* Use distance value for distance sensor topic */ + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); + + d.timestamp = flow.integration_time_us * 1000; /* ms to us */ + d.min_distance = 0.3f; + d.max_distance = 5.0f; + d.current_distance = flow.distance; /* both are in m */ + d.type = 1; + d.id = 0; + d.orientation = 8; + d.covariance = 0.0; + + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + } else { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); + } } void @@ -444,22 +466,23 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } - /* Use distance value for range finder report */ - struct range_finder_report r; - memset(&r, 0, sizeof(r)); + /* Use distance value for distance sensor topic */ + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); - r.timestamp = hrt_absolute_time(); - r.error_count = 0; - r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.distance; - r.minimum_distance = 0.0f; - r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable - r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + d.timestamp = hrt_absolute_time(); + d.min_distance = 0.3f; + d.max_distance = 5.0f; + d.current_distance = flow.distance; /* both are in m */ + d.type = 1; + d.id = 0; + d.orientation = 8; + d.covariance = 0.0; - if (_range_pub < 0) { - _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); } else { - orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); } } @@ -497,6 +520,35 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) +{ + /* distance sensor */ + mavlink_distance_sensor_t dist_sensor; + mavlink_msg_distance_sensor_decode(msg, &dist_sensor); + + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); + + d.timestamp = dist_sensor.time_boot_ms * 1000; /* ms to us */ + d.min_distance = float(dist_sensor.min_distance) * 1e-2f; /* cm to m */ + d.max_distance = float(dist_sensor.max_distance) * 1e-2f; /* cm to m */ + d.current_distance = float(dist_sensor.current_distance) * 1e-2f; /* cm to m */ + d.type = dist_sensor.type; + d.id = dist_sensor.id; + d.orientation = dist_sensor.orientation; + d.covariance = dist_sensor.covariance; + + /// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum + + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + + } else { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); + } +} + void MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index fe217f3c3b..016305e796 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -73,6 +73,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -134,6 +135,7 @@ private: void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); + void handle_message_distance_sensor(mavlink_message_t *msg); void *receive_thread(void *arg); @@ -164,7 +166,7 @@ private: orb_advert_t _battery_pub; orb_advert_t _cmd_pub; orb_advert_t _flow_pub; - orb_advert_t _range_pub; + orb_advert_t _distance_sensor_pub; orb_advert_t _offboard_control_mode_pub; orb_advert_t _actuator_controls_pub; orb_advert_t _global_vel_sp_pub; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3f1047742e..5ccadde57f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -61,8 +61,6 @@ #include #include -#include - #include #include #include @@ -92,6 +90,7 @@ #include #include #include +#include #include #include #include @@ -1084,7 +1083,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; struct telemetry_status_s telemetry; - struct range_finder_report range_finder; + struct distance_sensor_s distance_sensor; struct estimator_status_report estimator_status; struct tecs_status_s tecs_status; struct system_power_s system_power; @@ -1174,7 +1173,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_vel_sp_sub; int battery_sub; int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; - int range_finder_sub; + int distance_sensor_sub; int estimator_status_sub; int tecs_status_sub; int system_power_sub; @@ -1208,7 +1207,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = -1; subs.global_vel_sp_sub = -1; subs.battery_sub = -1; - subs.range_finder_sub = -1; + subs.distance_sensor_sub = -1; subs.estimator_status_sub = -1; subs.tecs_status_sub = -1; subs.system_power_sub = -1; @@ -1820,12 +1819,14 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- BOTTOM DISTANCE --- */ - if (copy_if_updated(ORB_ID(sensor_range_finder), &subs.range_finder_sub, &buf.range_finder)) { + /* --- DISTANCE SENSOR --- */ + if (copy_if_updated(ORB_ID(distance_sensor), &subs.distance_sensor_sub, &buf.distance_sensor)) { log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.range_finder.distance; - log_msg.body.log_DIST.bottom_rate = 0.0f; - log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); + log_msg.body.log_DIST.id = buf.distance_sensor.id; + log_msg.body.log_DIST.type = buf.distance_sensor.type; + log_msg.body.log_DIST.orientation = buf.distance_sensor.orientation; + log_msg.body.log_DIST.current_distance = buf.distance_sensor.current_distance; + log_msg.body.log_DIST.covariance = buf.distance_sensor.covariance; LOGBUFFER_WRITE_AND_COUNT(DIST); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index abdf518c51..062ab88f4f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -295,12 +295,14 @@ struct log_BATT_s { float discharged; }; -/* --- DIST - DISTANCE TO SURFACE --- */ +/* --- DIST - RANGE SENSOR DISTANCE --- */ #define LOG_DIST_MSG 21 struct log_DIST_s { - float bottom; - float bottom_rate; - uint8_t flags; + uint8_t id; + uint8_t type; + uint8_t orientation; + uint16_t current_distance; + uint8_t covariance; }; /* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ @@ -486,7 +488,6 @@ struct log_PARM_s { }; #pragma pack(pop) - /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ @@ -514,7 +515,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), - LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), + LOG_FORMAT(DIST, "iiff", "Type,Orientation,Distance,Covariance"), LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index eb89b79585..7add7bf06e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -57,15 +57,15 @@ ORB_DEFINE(sensor_gyro, struct gyro_report); #include ORB_DEFINE(sensor_baro, struct baro_report); -#include -ORB_DEFINE(sensor_range_finder, struct range_finder_report); - #include ORB_DEFINE(output_pwm, struct pwm_output_values); #include ORB_DEFINE(input_rc, struct rc_input_values); +#include "topics/pwm_input.h" +ORB_DEFINE(pwm_input, struct pwm_input_s); + #include "topics/vehicle_attitude.h" ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); @@ -256,3 +256,6 @@ ORB_DEFINE(time_offset, struct time_offset_s); #include "topics/mc_att_ctrl_status.h" ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s); + +#include "topics/distance_sensor.h" +ORB_DEFINE(distance_sensor, struct distance_sensor_s); \ No newline at end of file diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 576e758df5..9c3a0aefb4 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -36,7 +36,6 @@ */ #include "baro.hpp" -#include #include const char *const UavcanBarometerBridge::NAME = "baro"; @@ -56,7 +55,7 @@ int UavcanBarometerBridge::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(baro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) return -1; diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 6a39eebfb6..b5da6985a7 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -39,11 +39,10 @@ #include "sensor_bridge.hpp" #include +#include #include -class RingBuffer; - class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase { public: @@ -68,5 +67,5 @@ private: uavcan::Subscriber _sub_air_data; unsigned _msl_pressure = 101325; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; };