diff --git a/src/drivers/distance_sensor/airlango/isl2950.cpp b/src/drivers/distance_sensor/airlango/isl2950.cpp deleted file mode 100644 index 559a33f072..0000000000 --- a/src/drivers/distance_sensor/airlango/isl2950.cpp +++ /dev/null @@ -1,824 +0,0 @@ -/**************************************************************************** - * - * 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 landbao15L2950.cpp - * @author Claudio Micheli - * - * Driver for the Lanbao ISL2950 - */ - - #include - #include - #include - - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - #include - - #include - - #include - #include - #include - #include - - #include - #include - - #include "isl2950_parser.h" - - /* Configuration Constants */ - -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -#define ISL2950_TAKE_RANGE_REG 'd' - -// designated serial port on Pixhawk - #define ISL2950_DEFAULT_PORT "/dev/ttyS1" // Its baudrate is 115200 - - // normal conversion wait time - #define ISL2950_CONVERSION_INTERVAL 100*1000UL/* 100ms */ - - - class ISL2950 : public cdev::CDev - { - public: - // Constructor - ISL2950(const char *port = ISL2950_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); - // Virtual destructor - virtual ~ISL2950(); - - virtual int init(); - //virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - - private: - char _port[20]; - uint8_t _rotation; - float _min_distance; - float _max_distance; - int _conversion_interval; - work_s _work{}; - ringbuffer::RingBuffer *_reports; - int _measure_ticks; - bool _collect_phase; - int _fd; - uint8_t _linebuf[20]; - unsigned _linebuf_index; - - enum ISL2950_PARSE_STATE _parse_state; - unsigned char _frame_data[4]; - uint16_t _crc16; - - hrt_abstime _last_read; - int _class_instance; - int _orb_class_instance; - - orb_advert_t _distance_sensor_topic; - - unsigned _consecutive_fail_count; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - - /** - * 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 SF0X_MIN_DISTANCE - * and SF0X_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(); - /** - * 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); - - - }; - - - /* - * Driver 'main' command. - */ - extern "C" __EXPORT int isl2950_main(int argc, char *argv[]); - -/** -* Method : Constructor -* -* @note This method initializes the class variables -*/ - - ISL2950::ISL2950(const char *port, uint8_t rotation) : - CDev(RANGE_FINDER0_DEVICE_PATH), - _rotation(rotation), - _min_distance(0.10f), - _max_distance(40.0f), - _conversion_interval(ISL2950_CONVERSION_INTERVAL), - _reports(nullptr), - _measure_ticks(0), - _collect_phase(false), - _fd(-1), - _linebuf_index(0), - _parse_state(TFS_NOT_STARTED), - _frame_data{TOF_SFD1, TOF_SFD2, 0, 0}, - _crc16(0), - _last_read(0), - _class_instance(-1), - _orb_class_instance(-1), - _distance_sensor_topic(nullptr), - _consecutive_fail_count(0), - _sample_perf(perf_alloc(PC_ELAPSED, "isl2950_read")), - _comms_errors(perf_alloc(PC_COUNT, "isl2950_com_err")) - { - /* store port name */ - strncpy(_port, port, sizeof(_port)); - /* enforce null termination */ - _port[sizeof(_port) - 1] = '\0'; - -} - -// Destructor -ISL2950::~ISL2950() -{ - /* 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); - } - - perf_free(_sample_perf); - perf_free(_comms_errors); -} - -/** -* Method : init() -* -* This method setup the general driver for a range finder sensor. -*/ - -int -ISL2950::init() -{ - /* status */ -int ret = 0; - -do { /* create a scope to handle exit conditions using break */ - - /* do regular cdev init */ - ret = CDev::init(); - - if (ret != OK) { break; } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); - - if (_reports == nullptr) { - PX4_ERR("alloc failed"); - ret = -1; - break; - } - _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - - /* 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 == nullptr) { - PX4_ERR("failed to create distance_sensor object"); - } - - } while (0); - - return ret; -} - -void -ISL2950::set_minimum_distance(float min) -{ - _min_distance = min; -} - -void -ISL2950::set_maximum_distance(float max) -{ - _max_distance = max; -} - -float -ISL2950::get_minimum_distance() -{ - return _min_distance; -} - -float -ISL2950::get_maximum_distance() -{ - return _max_distance; -} - -int -ISL2950::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - 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(_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 */ - int ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(_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; - } - } - } - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } -} - -/* -ssize_t -ISL2950::read(device::file_t *filp, char *buffer, size_t buflen) -{ - // SOME STUFFS - -}*/ - -int -ISL2950::measure() -{ - int ret; - - /* - * Send the command to begin a measurement. - */ - char cmd = ISL2950_TAKE_RANGE_REG; - ret = ::write(_fd, &cmd, 1); - - if (ret != sizeof(cmd)) { - perf_count(_comms_errors); - printf("write fail %d", ret); - return ret; - } - - ret = OK; - - return ret; -} - -int - -/* - * Method: collect() - * - * This method reads data from serial UART and places it into a buffer -*/ -ISL2950::collect() -{ - int bytes_read = 0; - - int distance_mm = -1.0f; - bool full_frame = false; - - - perf_begin(_sample_perf); - - - /* the buffer for read chars is buflen minus null termination */ - uint8_t readbuf[sizeof(_linebuf)]; - unsigned readlen = sizeof(readbuf); - - - /* read from the sensor (uart buffer) */ - bytes_read = ::read(_fd, &readbuf[0], readlen); - - if (bytes_read < 0) { - stop_serial_read = true; - PX4_DEBUG("read err: %d \n", bytes_read); - perf_count(_comms_errors); - perf_end(_sample_perf); - - } else if (bytes_read > 0){ - _last_read = hrt_absolute_time(); - - for (int i = 0; i < bytes_read; i++) { - if (OK == isl2950_parser(readbuf[i],_frame_data, &_parse_state,&_crc16, &distance_mm)){ - - full_frame = true; - } - } - - } - - - if (!full_frame) { - return -EAGAIN; - } - - printf("val (int): %d, raw: 0x%08X, valid: %s \n", distance_mm, _frame_data, ((full_frame) ? "OK" : "NO")); - - struct distance_sensor_s report; - - report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - report.orientation = _rotation; - report.current_distance = distance_mm/1000.0f; - report.min_distance = get_minimum_distance(); - report.max_distance = get_maximum_distance(); - report.covariance = 0.0f; - report.signal_quality = -1; - /* TODO: set proper ID */ - report.id = 0; - - /* publish it */ - orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); - - _reports->force(&report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - bytes_read = OK; - - perf_end(_sample_perf); - return bytes_read; - -} - -void -ISL2950::start() -{ - PX4_INFO("ISL2950::start() - launch the work queue"); - /* reset the report ring and state machine */ - _collect_phase = false; - _reports->flush(); - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&ISL2950::cycle_trampoline, this, 1); - -} - -void -ISL2950::stop() -{ - work_cancel(HPWORK, &_work); -} - -void -ISL2950::cycle_trampoline(void *arg) -{ - ISL2950 *dev = static_cast(arg); - - dev->cycle(); -} - -void -ISL2950::cycle() -{ - PX4_DEBUG("ISL2950::cycle() - in the cycle"); - /* fds initialized? */ - if (_fd < 0) { - /* open fd */ - _fd = ::open(_port,O_RDWR); - - if (_fd < 0) { - PX4_ERR("ISL2950::cycle() - open failed (%i)", errno); - return; - } - - struct termios uart_config; - - int termios_state; - - /* fill the struct for the new configuration */ - tcgetattr(_fd, &uart_config); - - /* clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* no parity, one stop bit */ - uart_config.c_cflag &= ~(CSTOPB | PARENB); - - unsigned speed = B115200; - - /* set baud rate */ - if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - PX4_ERR("CFG: %d ISPD", termios_state); - } - - if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - PX4_ERR("CFG: %d OSPD", termios_state); - } - - if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { - PX4_ERR("baud %d ATTR", termios_state); - } - } - - /* perform collection */ - int collect_ret = collect(); - - if (collect_ret == -EAGAIN) { - - /* We are missing bytes to complete the packet, re-cycle at 1ms */ - work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(1000LL)); - return; - } - - - /* schedule a fresh cycle call when a complete packet has been received */ - work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(_conversion_interval)); -} - -void -ISL2950::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("poll interval: %d ticks\n", _measure_ticks); - _reports->print_info("report queue"); -} - -/** - * Local functions in support of the shell command. - */ -namespace isl2950 -{ - -ISL2950 *g_dev; - -int start(const char *port, uint8_t rotation); -int stop(); -int test(); -int reset(); -int info(); - -/** - * Start the driver. - */ -int -start(const char *port, uint8_t rotation) -{ - int fd; - - if (g_dev != nullptr) { - PX4_WARN("already started"); - return -1; - } - - /* create the driver */ - g_dev = new ISL2950(port, rotation); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = open(RANGE_FINDER0_DEVICE_PATH, 0); - - if (fd < 0) { - PX4_ERR("device open fail (%i)", errno); - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("failed to set baudrate %d", B115200); - goto fail; - } - PX4_DEBUG("isl2950::start() succeeded"); - return 0; - -fail: - PX4_DEBUG("isl2950::start() failed"); - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - return -1; -} - -/** - * Stop the driver - */ -int stop() -{ - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - - } else { - return -1; - } - - return 0; -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -int -test() -{ - struct distance_sensor_s report; - ssize_t sz; - - int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("%s open failed (try 'isl2950 start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH); - return -1; - } - - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("immediate read failed"); - return -1; - } - - print_message(report); - - /* start the sensor polling at 2 Hz rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - PX4_ERR("failed to set 2Hz poll rate"); - return -1; - } - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - int ret = poll(&fds, 1, 2000); - - if (ret != 1) { - PX4_ERR("timed out"); - break; - } - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("read failed: got %zi vs exp. %zu", sz, sizeof(report)); - break; - } - - print_message(report); - } - - /* reset the sensor polling to the default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - PX4_ERR("ioctl SENSORIOCSPOLLRATE failed"); - return -1; - } - - return 0; -} - -/** - * Reset the driver. - */ -int -reset() -{ - int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("open failed (%i)", errno); - return -1; - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - return -1; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - return -1; - } - - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return -1; - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - return 0; -} - -} // namespace - -int -isl2950_main(int argc, char *argv[]) -{ - uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - const char *device_path = ISL2950_DEFAULT_PORT; - int ch; - int myoptind = 1; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'R': - rotation = (uint8_t)atoi(myoptarg); - break; - - case 'd': - device_path = myoptarg; - break; - - default: - PX4_WARN("Unknown option!"); - return -1; - } - } - - if (myoptind >= argc) { - goto out_error; - } - - /* - * Start/load the driver. - */ - if (!strcmp(argv[myoptind], "start")) { - return isl2950::start(device_path, rotation); - } - - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { - return isl2950::stop(); - } - - /* - * Test the driver/device. - */ - if (!strcmp(argv[myoptind], "test")) { - return isl2950::test(); - } - - /* - * Reset the driver. - */ - if (!strcmp(argv[myoptind], "reset")) { - return isl2950::reset(); - } - - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { - return isl2950::info(); - } - -out_error: - PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); - return -1; -} diff --git a/src/drivers/distance_sensor/airlango/isl2950_parser.cpp b/src/drivers/distance_sensor/airlango/isl2950_parser.cpp deleted file mode 100644 index 468e02299a..0000000000 --- a/src/drivers/distance_sensor/airlango/isl2950_parser.cpp +++ /dev/null @@ -1,205 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 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 isl2950_parser.cpp - * @author Claudio Micheli - * claudio@auterion.com - * - */ - -#include "isl2950_parser.h" -#include -#include -#include - -#include "isl2950_parser.h" -#include -#include - -typedef unsigned char UCHAR; -typedef unsigned short USHORT; - -// Note : No clue what those static variables are -static const UCHAR aucCRCHi[] = { - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40 -}; - -static const UCHAR aucCRCLo[] = { - 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, - 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, - 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, - 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, - 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, - 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, - 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, - 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, - 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, - 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, - 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, - 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, - 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, - 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, - 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, - 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, - 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, - 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, - 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, - 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, - 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, - 0x41, 0x81, 0x80, 0x40 -}; - - -// TOF frame format -// -// 1B 1B 1B 1B 2B -// | 0xA5 | 0x5A | distance-MSB | distance-LSB | crc-16 | -// -// Frame data saved for CRC calculation -const static int TOF_DISTANCE_MSB_POS = 2; -const static int TOF_DISTANCE_LSB_POS = 3; -const static int TOF_CRC_CALC_DATA_LEN = 4; - - -USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) { - UCHAR ucCRCHi = 0xFF; - UCHAR ucCRCLo = 0xFF; - int iIndex; - while (usLen--) { - iIndex = ucCRCLo ^ *(pucFrame++); - ucCRCLo = (UCHAR)(ucCRCHi ^ aucCRCHi[iIndex]); - ucCRCHi = aucCRCLo[iIndex]; - } - return (USHORT)(ucCRCHi << 8 | ucCRCLo); -} - -int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, uint16_t *crc16, int *dist) -{ - int ret = -1; -// int bytes_processed = 0; - - -// uint8_t b = buffer[bytes_processed++]; // Can be removed -// printf("parse byte 0x%02X \n", b); - - switch (*state) { - case TFS_NOT_STARTED: - if (c == TOF_SFD1) { - *state = TFS_GOT_SFD1; - //printf("Got SFD1 \n"); - } - break; - - case TFS_GOT_SFD1: - if (c == TOF_SFD2) { - *state = TFS_GOT_SFD2; - //printf("Got SFD2 \n"); - } - // @NOTE (claudio@auterion.com): Strange thing, if second byte is wrong we skip all the frame !! - else if (c == TOF_SFD1) { - *state = TFS_GOT_SFD1; -// printf("Discard previous SFD1, Got new SFD1 \n"); - } else { - *state = TFS_NOT_STARTED; - } - break; - - case TFS_GOT_SFD2: - *state = TFS_GOT_DATA1; - parserbuf[TOF_DISTANCE_MSB_POS] = c; // MSB Data - //printf("Got DATA1 0x%02X \n", c); - break; - - case TFS_GOT_DATA1: - *state = TFS_GOT_DATA2; - parserbuf[TOF_DISTANCE_LSB_POS] = c; // LSB Data - //printf("Got DATA2 0x%02X \n", c); - // do crc calculation - *crc16 = usMBCRC16(parserbuf, TOF_CRC_CALC_DATA_LEN); - // convert endian - *crc16 = (*crc16 >> 8) | (*crc16 << 8); - break; - - case TFS_GOT_DATA2: - if (c == (*crc16 >> 8)) { - *state = TFS_GOT_CHECKSUM1; - } else { - printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",c, *crc16); - //*state = TFS_NOT_STARTED; - *state = TFS_GOT_CHECKSUM1; // Forcing to print the value anyway - } - break; - - case TFS_GOT_CHECKSUM1: - // Here, reset state to `NOT-STARTED` no matter crc ok or not - *state = TFS_NOT_STARTED; - /*if (c == (*crc16 & 0xFF)) { - //printf("Checksum verified \n"); - *dist = (parserbuf[TOF_DISTANCE_MSB_POS] << 8) | parserbuf[TOF_DISTANCE_LSB_POS]; - return OK; - }*/ - *dist = (parserbuf[TOF_DISTANCE_MSB_POS] << 8) | parserbuf[TOF_DISTANCE_LSB_POS]; - return OK; - - break; - - default: - printf("This should never happen. \n"); - break; - } - - // SOME STUFFS - return ret; -} diff --git a/src/drivers/distance_sensor/isl2950/isl2950.cpp b/src/drivers/distance_sensor/isl2950/isl2950.cpp index 30dbe88308..fed521bf87 100644 --- a/src/drivers/distance_sensor/isl2950/isl2950.cpp +++ b/src/drivers/distance_sensor/isl2950/isl2950.cpp @@ -35,7 +35,7 @@ * @file landbao15L2950.cpp * @author Claudio Micheli * - * Driver for the Lanbao ISL2950 + * Driver for the ISL2950 */ #include @@ -399,46 +399,39 @@ int ISL2950::collect() { int bytes_read = 0; - int bytes_available = 0; int distance_mm = -1.0f; bool full_frame = false; - bool stop_serial_read = false; + perf_begin(_sample_perf); - /* clear buffer if last read was too long ago */ - int64_t read_elapsed = hrt_absolute_time(); - read_elapsed = read_elapsed - _last_read; /* the buffer for read chars is buflen minus null termination */ uint8_t readbuf[sizeof(_linebuf)]; unsigned readlen = sizeof(readbuf); - while ((!stop_serial_read)) { + /* read from the sensor (uart buffer) */ bytes_read = ::read(_fd, &readbuf[0], readlen); if (bytes_read < 0) { - stop_serial_read = true; PX4_DEBUG("read err: %d \n", bytes_read); perf_count(_comms_errors); perf_end(_sample_perf); } else if (bytes_read > 0){ _last_read = hrt_absolute_time(); - bytes_available += bytes_read; - //printf("Got a buffer with %d bytes,read %d \n", bytes_available,bytes_read); for (int i = 0; i < bytes_read; i++) { if (OK == isl2950_parser(readbuf[i],_frame_data, &_parse_state,&_crc16, &distance_mm)){ - stop_serial_read = true; + full_frame = true; } } } - } + if (!full_frame) { return -EAGAIN; @@ -472,86 +465,6 @@ ISL2950::collect() perf_end(_sample_perf); return bytes_read; - - - // ----------------------- LANBAO SPECIFIC --------------------------- -/* - uint8_t buffer[50]; - int bytes_available = 0; - int bytes_processed = 0; - int bytes_read = 0; - - int distance_mm = -1.0f; - bytes_read = ::read(_fd, buffer + bytes_available, 50 - bytes_available); - //printf("read() returns %02X %02X %02X %02X \n", buffer[0], buffer[1],buffer[2],buffer[3] ); - - //-------------------------------------------------------------------- - if (bytes_read < 0) { - PX4_ERR("isl2950 - read() error: %d \n", bytes_read); - perf_count(_comms_errors); - perf_end(_sample_perf); - - // only throw an error if we time out - if (read_elapsed > (_conversion_interval * 2)) { - printf("read elapsed %d , conversion interval %d",read_elapsed,_conversion_interval * 2); - return bytes_read; - - } else { - printf("EAGAIN",read_elapsed,_conversion_interval * 2); - return -EAGAIN; - } - - } else if (bytes_read == 0) { - return OK; // If we dont read any bites we simply exit from collecting - } - - _last_read = hrt_absolute_time(); - - bytes_available += bytes_read; - - // parse the buffer data - full_frame = false; - - bytes_processed = isl2950_parser(buffer, bytes_available, &full_frame,&distance_mm); - tempo = tempo - hrt_absolute_time(); - //printf("isl2950_parser() processed %d bytes, full_frame %d \n", bytes_processed, full_frame); - - // discard the processed bytes and move the buffer content to the head - bytes_available -= bytes_processed; - memcpy(buffer, buffer + bytes_processed, bytes_available); - - if (full_frame) { - printf("Measured Distance %d mm\n",distance_mm); - } - - else if (!full_frame) { // If the frame is not valid we avoid publishing it - return OK; - } - - struct distance_sensor_s report; - - report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - report.orientation = _rotation; - report.current_distance = distance_mm/1000; // To meters - report.min_distance = get_minimum_distance(); - report.max_distance = get_maximum_distance(); - report.covariance = 0.0f; - report.signal_quality = -1; - // TODO: set proper ID - report.id = 0; - - // publish it - orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); - - _reports->force(&report); - - // notify anyone waiting for data - poll_notify(POLLIN); - - printf("tempo %d \n",tempo); - perf_end(_sample_perf); - return OK; */ } void @@ -623,44 +536,20 @@ ISL2950::cycle() PX4_ERR("baud %d ATTR", termios_state); } } - //printf("COLLECT \n"); + /* perform collection */ int collect_ret = collect(); if (collect_ret == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ - work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(1042 * 8)); + + /* We are missing bytes to complete the packet, re-cycle at 1ms */ + work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(500LL)); return; } - // ------------------- DISABLED CHECKING OF CONSECUTIVE FAIL - // if (OK != collect_ret) { - - /* we know the sensor needs about four seconds to initialize */ - // if (hrt_absolute_time() > 1 * 1000 * 1000LL && _consecutive_fail_count < 5) { - // PX4_ERR("collection error #%u", _consecutive_fail_count); - // } - - // _consecutive_fail_count++; - - /* restart the measurement state machine */ - // start(); - // return; - - // } else { - /* apparently success */ - // _consecutive_fail_count = 0; - // } - // ------------------- DISABLED CHECKING OF CONSECUTIVE FAIL - - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&ISL2950::cycle_trampoline, - this, - USEC2TICK(_conversion_interval)); + /* schedule a fresh cycle call when a complete packet has been received */ + work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(_conversion_interval)); } void