From 9b7ef7141f5149e83fb21940e7f077cecf655d9d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 11 Sep 2019 16:34:35 -0400 Subject: [PATCH] tfmini cleanup and use PX4Rangefinder --- src/drivers/distance_sensor/tfmini/tfmini.cpp | 527 +++--------------- .../distance_sensor/tfmini/tfmini_parser.cpp | 50 +- .../distance_sensor/tfmini/tfmini_parser.h | 26 +- 3 files changed, 109 insertions(+), 494 deletions(-) diff --git a/src/drivers/distance_sensor/tfmini/tfmini.cpp b/src/drivers/distance_sensor/tfmini/tfmini.cpp index c1f48d86a2..50efb06b56 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini.cpp +++ b/src/drivers/distance_sensor/tfmini/tfmini.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019 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 @@ -42,149 +42,78 @@ * Driver for the Benewake TFmini laser rangefinder series */ -#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 -#include +#include #include -#ifdef __PX4_CYGWIN -#include -#endif - #include "tfmini_parser.h" #define TFMINI_DEFAULT_PORT "/dev/ttyS3" -/* Configuration Constants */ +using namespace time_literals; -class TFMINI : public cdev::CDev, public px4::ScheduledWorkItem +class TFMINI : public px4::ScheduledWorkItem { public: TFMINI(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); virtual ~TFMINI(); - virtual int init() override; + int init(); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; - - /** - * Diagnostics - print some basic information about the driver. - */ void print_info(); private: int collect(); - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ void Run() override; - /** - * Initialise the automatic measurement state machine and start it. - */ 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 TFMINI_MIN_DISTANCE - * and TFMINI_MAX_DISTANCE - */ - void set_minimum_distance(float min); - void set_maximum_distance(float max); - float get_minimum_distance(); - float get_maximum_distance(); + PX4Rangefinder _px4_rangefinder; - enum TFMINI_PARSE_STATE _parse_state {TFMINI_PARSE_STATE0_UNSYNC}; + TFMINI_PARSE_STATE _parse_state {TFMINI_PARSE_STATE::STATE0_UNSYNC}; - char _linebuf[10]; - char _port[20]; + char _linebuf[10] {}; + char _port[20] {}; - bool _collect_phase{false}; + static constexpr int kCONVERSIONINTERVAL{9_ms}; - int _class_instance{-1}; - int _conversion_interval{9000}; int _fd{-1}; - int _measure_interval{0}; - int _orb_class_instance{-1}; unsigned int _linebuf_index{0}; - uint8_t _rotation{0}; - - float _max_distance{12.0f}; - float _min_distance{0.30f}; - hrt_abstime _last_read{0}; - orb_advert_t _distance_sensor_topic{nullptr}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; - perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "tfmini_com_err")}; - perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "tfmini_read")}; - - ringbuffer::RingBuffer *_reports{nullptr}; }; - TFMINI::TFMINI(const char *port, uint8_t rotation) : - CDev(RANGE_FINDER0_DEVICE_PATH), ScheduledWorkItem(px4::wq_configurations::hp_default), - _rotation(rotation) + _px4_rangefinder(0 /* TODO: device id */, ORB_PRIO_DEFAULT, rotation) { - /* store port name */ + // store port name strncpy(_port, port, sizeof(_port) - 1); - /* enforce null termination */ + // enforce null termination _port[sizeof(_port) - 1] = '\0'; } TFMINI::~TFMINI() { - /* make sure we are truly inactive */ + // 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); } @@ -195,10 +124,11 @@ TFMINI::init() int32_t hw_model = 1; // only one model so far... switch (hw_model) { - case 1: /* TFMINI (12m, 100 Hz)*/ - _min_distance = 0.3f; - _max_distance = 12.0f; - _conversion_interval = 9000; + case 1: // TFMINI (12m, 100 Hz) + _px4_rangefinder.set_min_distance(0.3f); + _px4_rangefinder.set_max_distance(12.0f); + _px4_rangefinder.set_fov(math::radians(1.15)); + break; default: @@ -206,12 +136,12 @@ TFMINI::init() return -1; } - /* status */ + // status int ret = 0; - do { /* create a scope to handle exit conditions using break */ + do { // create a scope to handle exit conditions using break - /* open fd */ + // open fd _fd = ::open(_port, O_RDWR | O_NOCTTY); if (_fd < 0) { @@ -219,19 +149,17 @@ TFMINI::init() return -1; } - /*baudrate 115200, 8 bits, no parity, 1 stop bit */ + // baudrate 115200, 8 bits, no parity, 1 stop bit unsigned speed = B115200; - - struct termios uart_config; - - int termios_state; + termios uart_config{}; + int termios_state{}; tcgetattr(_fd, &uart_config); - /* clear ONLCR flag (which appends a CR for every LF) */ + // clear ONLCR flag (which appends a CR for every LF) uart_config.c_oflag &= ~ONLCR; - /* set baud rate */ + // set baud rate if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { PX4_ERR("CFG: %d ISPD", termios_state); ret = -1; @@ -250,19 +178,19 @@ TFMINI::init() break; } - uart_config.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ + uart_config.c_cflag |= (CLOCAL | CREAD); // ignore modem controls uart_config.c_cflag &= ~CSIZE; - uart_config.c_cflag |= CS8; /* 8-bit characters */ - uart_config.c_cflag &= ~PARENB; /* no parity bit */ - uart_config.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ - uart_config.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ + uart_config.c_cflag |= CS8; // 8-bit characters + uart_config.c_cflag &= ~PARENB; // no parity bit + uart_config.c_cflag &= ~CSTOPB; // only need 1 stop bit + uart_config.c_cflag &= ~CRTSCTS; // no hardware flowcontrol - /* setup for non-canonical mode */ + // setup for non-canonical mode uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); uart_config.c_oflag &= ~OPOST; - /* fetch bytes as they become available */ + // fetch bytes as they become available uart_config.c_cc[VMIN] = 1; uart_config.c_cc[VTIME] = 1; @@ -271,177 +199,15 @@ TFMINI::init() ret = -1; 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("mem err"); - 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. Did you start uOrb?"); - } - } while (0); - /* close the fd */ + // close the fd ::close(_fd); _fd = -1; - return ret; -} - -void -TFMINI::set_minimum_distance(float min) -{ - _min_distance = min; -} - -void -TFMINI::set_maximum_distance(float max) -{ - _max_distance = max; -} - -float -TFMINI::get_minimum_distance() -{ - return _min_distance; -} - -float -TFMINI::get_maximum_distance() -{ - return _max_distance; -} - -int -TFMINI::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_interval == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_interval = (_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_interval == 0); - - /* convert hz to tick interval via microseconds */ - int interval = (1000000 / arg); - - /* check against maximum rate */ - if (interval < _conversion_interval) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_interval = interval; - - /* 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); + if (ret == PX4_OK) { + start(); } -} - -ssize_t -TFMINI::read(device::file_t *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 (_measure_interval > 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(); - - /* wait for it to complete */ - px4_usleep(_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; } @@ -451,27 +217,30 @@ TFMINI::collect() { perf_begin(_sample_perf); - /* clear buffer if last read was too long ago */ + // clear buffer if last read was too long ago int64_t read_elapsed = hrt_elapsed_time(&_last_read); - /* the buffer for read chars is buflen minus null termination */ + // the buffer for read chars is buflen minus null termination char readbuf[sizeof(_linebuf)] {}; unsigned readlen = sizeof(readbuf) - 1; int ret = 0; float distance_m = -1.0f; - /* Check the number of bytes available in the buffer*/ + // Check the number of bytes available in the buffer int bytes_available = 0; ::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available); if (!bytes_available) { + perf_end(_sample_perf); return -EAGAIN; } - /* parse entire buffer */ + // parse entire buffer + const hrt_abstime timestamp_sample = hrt_absolute_time(); + do { - /* read from the sensor (uart buffer) */ + // read from the sensor (uart buffer) ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { @@ -479,8 +248,8 @@ TFMINI::collect() perf_count(_comms_errors); perf_end(_sample_perf); - /* only throw an error if we time out */ - if (read_elapsed > (_conversion_interval * 2)) { + // only throw an error if we time out + if (read_elapsed > (kCONVERSIONINTERVAL * 2)) { /* flush anything in RX buffer */ tcflush(_fd, TCIFLUSH); return ret; @@ -492,40 +261,25 @@ TFMINI::collect() _last_read = hrt_absolute_time(); - /* parse buffer */ + // parse buffer for (int i = 0; i < ret; i++) { tfmini_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m); } - /* bytes left to parse */ + // bytes left to parse bytes_available -= ret; } while (bytes_available > 0); - /* no valid measurement after parsing buffer */ + // no valid measurement after parsing buffer if (distance_m < 0.0f) { + perf_end(_sample_perf); return -EAGAIN; } - /* publish most recent valid measurement from buffer */ - distance_sensor_s report {}; - report.current_distance = distance_m; - report.id = 0; // TODO: set proper ID. - report.min_distance = get_minimum_distance(); - report.max_distance = get_maximum_distance(); - report.orientation = _rotation; - report.signal_quality = -1; - report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - report.variance = 0.0f; + // publish most recent valid measurement from buffer + _px4_rangefinder.update(timestamp_sample, distance_m); - /* publish it */ - orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); - - _reports->force(&report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); perf_end(_sample_perf); return PX4_OK; @@ -534,12 +288,8 @@ TFMINI::collect() void TFMINI::start() { - /* reset the report ring and state machine */ - _collect_phase = false; - _reports->flush(); - - /* schedule a cycle to start things */ - ScheduleNow(); + // schedule a cycle to start things + ScheduleOnInterval(100_us); } void @@ -551,41 +301,19 @@ TFMINI::stop() void TFMINI::Run() { - /* fds initialized? */ + // fds initialized? if (_fd < 0) { - /* open fd */ + // open fd _fd = ::open(_port, O_RDWR | O_NOCTTY); } - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - if (collect() == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps */ - ScheduleDelayed(87 * 9); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_interval > (_conversion_interval)) { - /* schedule a fresh cycle call when - * we are ready to measure again */ - ScheduleDelayed(_measure_interval - _conversion_interval); - return; - } + // perform collection + if (collect() == -EAGAIN) { + // reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps + ScheduleClear(); + ScheduleOnInterval(100_us, 87 * 9); + return; } - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(_conversion_interval); } void @@ -594,8 +322,8 @@ TFMINI::print_info() printf("Using port '%s'\n", _port); perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); - printf("poll interval: %d \n", _measure_interval); - _reports->print_info("report queue"); + + _px4_rangefinder.print_status(); } /** @@ -604,17 +332,13 @@ TFMINI::print_info() namespace tfmini { -TFMINI *g_dev; +TFMINI *g_dev{nullptr}; int start(const char *port, uint8_t rotation); int status(); int stop(); -int test(); int usage(); -/** - * Start the driver. - */ int start(const char *port, uint8_t rotation) { @@ -638,28 +362,9 @@ start(const char *port, uint8_t rotation) return PX4_ERROR; } - // Set the poll rate to default, starts automatic data collection. - int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("Opening device '%s' failed", port); - delete g_dev; - g_dev = nullptr; - return PX4_ERROR; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - delete g_dev; - g_dev = nullptr; - return PX4_ERROR; - } - return PX4_OK; } -/** - * Print the driver status. - */ int status() { @@ -674,9 +379,6 @@ status() return 0; } -/** - * Stop the driver. - */ int stop() { if (g_dev != nullptr) { @@ -693,77 +395,6 @@ int stop() return PX4_OK; } -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -int -test() -{ - int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("%s open failed (try 'tfmini start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH); - return PX4_ERROR; - } - - /* do a simple demand read */ - distance_sensor_s report; - ssize_t sz = px4_read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("immediate read failed"); - close(fd); - return PX4_ERROR; - } - - print_message(report); - - /* start the sensor polling at 2 Hz rate */ - if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - PX4_ERR("failed to set 2Hz poll rate"); - return PX4_ERROR; - } - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - px4_pollfd_struct_t fds{}; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - int ret = px4_poll(&fds, 1, 2000); - - if (ret != 1) { - PX4_ERR("timed out"); - break; - } - - /* now go get it */ - sz = px4_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 != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - PX4_ERR("failed to set default poll rate"); - return PX4_ERROR; - } - - PX4_INFO("PASS"); - return PX4_OK; -} - -/** - * Print a little info on how to use the driver. - */ int usage() { @@ -799,13 +430,9 @@ $ tfmini stop } // namespace - -/** - * Driver 'main' command. - */ extern "C" __EXPORT int tfmini_main(int argc, char *argv[]) { - int ch; + int ch = 0; uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; const char *device_path = TFMINI_DEFAULT_PORT; int myoptind = 1; @@ -832,7 +459,6 @@ extern "C" __EXPORT int tfmini_main(int argc, char *argv[]) return tfmini::usage(); } - // Start/load the driver. if (!strcmp(argv[myoptind], "start")) { if (strcmp(device_path, "") != 0) { return tfmini::start(device_path, rotation); @@ -841,20 +467,9 @@ extern "C" __EXPORT int tfmini_main(int argc, char *argv[]) PX4_WARN("Please specify device path!"); return tfmini::usage(); } - } - - // Stop the driver - if (!strcmp(argv[myoptind], "stop")) { + } else if (!strcmp(argv[myoptind], "stop")) { return tfmini::stop(); - } - - // Test the driver/device. - if (!strcmp(argv[myoptind], "test")) { - return tfmini::test(); - } - - // Print driver information. - if (!strcmp(argv[myoptind], "status")) { + } else if (!strcmp(argv[myoptind], "status")) { return tfmini::status(); } diff --git a/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp b/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp index e6a4ac4b38..ab65a0c583 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp +++ b/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019 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 @@ -63,89 +63,89 @@ const char *parser_state[] = { }; #endif -int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TFMINI_PARSE_STATE *state, float *dist) +int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, TFMINI_PARSE_STATE *state, float *dist) { int ret = -1; //char *end; switch (*state) { - case TFMINI_PARSE_STATE6_GOT_CHECKSUM: + case TFMINI_PARSE_STATE::STATE6_GOT_CHECKSUM: if (c == 'Y') { - *state = TFMINI_PARSE_STATE1_SYNC_1; + *state = TFMINI_PARSE_STATE::STATE1_SYNC_1; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; } else { - *state = TFMINI_PARSE_STATE0_UNSYNC; + *state = TFMINI_PARSE_STATE::STATE0_UNSYNC; } break; - case TFMINI_PARSE_STATE0_UNSYNC: + case TFMINI_PARSE_STATE::STATE0_UNSYNC: if (c == 'Y') { - *state = TFMINI_PARSE_STATE1_SYNC_1; + *state = TFMINI_PARSE_STATE::STATE1_SYNC_1; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; } break; - case TFMINI_PARSE_STATE1_SYNC_1: + case TFMINI_PARSE_STATE::STATE1_SYNC_1: if (c == 'Y') { - *state = TFMINI_PARSE_STATE1_SYNC_2; + *state = TFMINI_PARSE_STATE::STATE1_SYNC_2; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; } else { - *state = TFMINI_PARSE_STATE0_UNSYNC; + *state = TFMINI_PARSE_STATE::STATE0_UNSYNC; *parserbuf_index = 0; } break; - case TFMINI_PARSE_STATE1_SYNC_2: - *state = TFMINI_PARSE_STATE2_GOT_DIST_L; + case TFMINI_PARSE_STATE::STATE1_SYNC_2: + *state = TFMINI_PARSE_STATE::STATE2_GOT_DIST_L; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; break; - case TFMINI_PARSE_STATE2_GOT_DIST_L: - *state = TFMINI_PARSE_STATE2_GOT_DIST_H; + case TFMINI_PARSE_STATE::STATE2_GOT_DIST_L: + *state = TFMINI_PARSE_STATE::STATE2_GOT_DIST_H; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; break; - case TFMINI_PARSE_STATE2_GOT_DIST_H: - *state = TFMINI_PARSE_STATE3_GOT_STRENGTH_L; + case TFMINI_PARSE_STATE::STATE2_GOT_DIST_H: + *state = TFMINI_PARSE_STATE::STATE3_GOT_STRENGTH_L; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; break; - case TFMINI_PARSE_STATE3_GOT_STRENGTH_L: - *state = TFMINI_PARSE_STATE3_GOT_STRENGTH_H; + case TFMINI_PARSE_STATE::STATE3_GOT_STRENGTH_L: + *state = TFMINI_PARSE_STATE::STATE3_GOT_STRENGTH_H; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; break; - case TFMINI_PARSE_STATE3_GOT_STRENGTH_H: - *state = TFMINI_PARSE_STATE4_GOT_RESERVED; + case TFMINI_PARSE_STATE::STATE3_GOT_STRENGTH_H: + *state = TFMINI_PARSE_STATE::STATE4_GOT_RESERVED; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; break; - case TFMINI_PARSE_STATE4_GOT_RESERVED: - *state = TFMINI_PARSE_STATE5_GOT_QUALITY; + case TFMINI_PARSE_STATE::STATE4_GOT_RESERVED: + *state = TFMINI_PARSE_STATE::STATE5_GOT_QUALITY; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; break; - case TFMINI_PARSE_STATE5_GOT_QUALITY: + case TFMINI_PARSE_STATE::STATE5_GOT_QUALITY: // Find the checksum unsigned char cksm = 0; @@ -160,12 +160,12 @@ int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TFMINI t2 <<= 8; t2 += t1; *dist = ((float)t2) / 100; - *state = TFMINI_PARSE_STATE6_GOT_CHECKSUM; + *state = TFMINI_PARSE_STATE::STATE6_GOT_CHECKSUM; *parserbuf_index = 0; ret = 0; } else { - *state = TFMINI_PARSE_STATE0_UNSYNC; + *state = TFMINI_PARSE_STATE::STATE0_UNSYNC; *parserbuf_index = 0; } diff --git a/src/drivers/distance_sensor/tfmini/tfmini_parser.h b/src/drivers/distance_sensor/tfmini/tfmini_parser.h index 740a9378fe..dbf40a5f5d 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini_parser.h +++ b/src/drivers/distance_sensor/tfmini/tfmini_parser.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019 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 @@ -56,17 +56,17 @@ // 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though -enum TFMINI_PARSE_STATE { - TFMINI_PARSE_STATE0_UNSYNC = 0, - TFMINI_PARSE_STATE1_SYNC_1, - TFMINI_PARSE_STATE1_SYNC_2, - TFMINI_PARSE_STATE2_GOT_DIST_L, - TFMINI_PARSE_STATE2_GOT_DIST_H, - TFMINI_PARSE_STATE3_GOT_STRENGTH_L, - TFMINI_PARSE_STATE3_GOT_STRENGTH_H, - TFMINI_PARSE_STATE4_GOT_RESERVED, - TFMINI_PARSE_STATE5_GOT_QUALITY, - TFMINI_PARSE_STATE6_GOT_CHECKSUM +enum class TFMINI_PARSE_STATE { + STATE0_UNSYNC = 0, + STATE1_SYNC_1, + STATE1_SYNC_2, + STATE2_GOT_DIST_L, + STATE2_GOT_DIST_H, + STATE3_GOT_STRENGTH_L, + STATE3_GOT_STRENGTH_H, + STATE4_GOT_RESERVED, + STATE5_GOT_QUALITY, + STATE6_GOT_CHECKSUM }; -int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TFMINI_PARSE_STATE *state, float *dist); +int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, TFMINI_PARSE_STATE *state, float *dist);