diff --git a/src/drivers/distance_sensor/tfmini/tfmini.cpp b/src/drivers/distance_sensor/tfmini/tfmini.cpp index a346175919..1448fea952 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini.cpp +++ b/src/drivers/distance_sensor/tfmini/tfmini.cpp @@ -118,16 +118,11 @@ private: 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(); @@ -174,7 +169,7 @@ TFMINI::TFMINI(const char *port, uint8_t rotation) : _rotation(rotation), _min_distance(0.30f), _max_distance(12.0f), - _conversion_interval(10000), + _conversion_interval(9000), _reports(nullptr), _measure_ticks(0), _collect_phase(false), @@ -185,7 +180,6 @@ TFMINI::TFMINI(const char *port, uint8_t rotation) : _class_instance(-1), _orb_class_instance(-1), _distance_sensor_topic(nullptr), - _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "tfmini_read")), _comms_errors(perf_alloc(PC_COUNT, "tfmini_com_err")) { @@ -233,7 +227,7 @@ TFMINI::init() case 1: /* TFMINI (12m, 100 Hz)*/ _min_distance = 0.3f; _max_distance = 12.0f; - _conversion_interval = 10000; + _conversion_interval = 9000; break; default: @@ -247,10 +241,10 @@ TFMINI::init() do { /* create a scope to handle exit conditions using break */ /* open fd */ - _fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC); + _fd = ::open(_port, O_RDWR | O_NOCTTY); if (_fd < 0) { - warnx("Error opening fd"); + PX4_ERR("Error opening fd"); return -1; } @@ -268,19 +262,19 @@ TFMINI::init() /* set baud rate */ if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - warnx("ERR CFG: %d ISPD", termios_state); + PX4_ERR("CFG: %d ISPD", termios_state); ret = -1; break; } if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - warnx("ERR CFG: %d OSPD\n", termios_state); + PX4_ERR("CFG: %d OSPD\n", termios_state); ret = -1; break; } if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { - warnx("ERR baud %d ATTR", termios_state); + PX4_ERR("baud %d ATTR", termios_state); ret = -1; break; } @@ -302,7 +296,7 @@ TFMINI::init() uart_config.c_cc[VTIME] = 1; if (_fd < 0) { - warnx("FAIL: laser fd"); + PX4_ERR("FAIL: laser fd"); ret = -1; break; } @@ -316,7 +310,7 @@ TFMINI::init() _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { - warnx("mem err"); + PX4_ERR("mem err"); ret = -1; break; } @@ -538,7 +532,7 @@ TFMINI::collect() ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { - DEVICE_DEBUG("read err: %d", ret); + PX4_ERR("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); @@ -629,7 +623,7 @@ TFMINI::cycle() /* fds initialized? */ if (_fd < 0) { /* open fd */ - _fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC); + _fd = ::open(_port, O_RDWR | O_NOCTTY); } /* collection phase? */ @@ -639,33 +633,15 @@ TFMINI::cycle() int collect_ret = collect(); if (collect_ret == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ + /* reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps */ work_queue(HPWORK, &_work, (worker_t)&TFMINI::cycle_trampoline, this, - USEC2TICK(1042 * 8)); + USEC2TICK(87 * 9)); return; } - if (OK != collect_ret) { - - /* we know the sensor needs about four seconds to initialize */ - if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { - DEVICE_LOG("collection error #%u", _consecutive_fail_count); - } - - _consecutive_fail_count++; - - /* restart the measurement state machine */ - start(); - return; - - } else { - /* apparently success */ - _consecutive_fail_count = 0; - } - /* next phase is measurement */ _collect_phase = false; @@ -717,7 +693,6 @@ TFMINI *g_dev; void start(const char *port, uint8_t rotation); void stop(); void test(); -void reset(); void info(); void usage(); @@ -730,7 +705,8 @@ start(const char *port, uint8_t rotation) int fd; if (g_dev != nullptr) { - errx(1, "already started"); + PX4_ERR("already started"); + exit(1); } /* create the driver */ @@ -748,7 +724,7 @@ start(const char *port, uint8_t rotation) fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); if (fd < 0) { - warnx("Opening device '%s' failed"); + PX4_ERR("Opening device '%s' failed"); goto fail; } @@ -765,7 +741,8 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + PX4_ERR("driver start failed"); + exit(1); } /** @@ -774,13 +751,14 @@ fail: void stop() { if (g_dev != nullptr) { - warnx("stopping driver"); + PX4_INFO("stopping driver"); delete g_dev; g_dev = nullptr; - warnx("driver stopped"); + PX4_INFO("driver stopped"); } else { - errx(1, "driver not running"); + PX4_ERR("driver not running"); + exit(1); } exit(0); @@ -814,7 +792,8 @@ test() /* start the sensor polling at 2 Hz rate */ if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - errx(1, "failed to set 2Hz poll rate"); + PX4_ERR("failed to set 2Hz poll rate"); + exit(1); } /* read the sensor 5x and report each value */ @@ -827,7 +806,7 @@ test() int ret = px4_poll(&fds, 1, 2000); if (ret != 1) { - warnx("timed out"); + PX4_ERR("timed out"); break; } @@ -835,7 +814,7 @@ test() sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warnx("read failed: got %d vs exp. %d", sz, sizeof(report)); + PX4_ERR("read failed: got %d vs exp. %d", sz, sizeof(report)); break; } @@ -844,33 +823,11 @@ test() /* reset the sensor polling to the default rate */ if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - errx(1, "ERR: DEF RATE"); + PX4_ERR("failed to set default poll rate"); + exit(1); } - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - err(1, "failed "); - } - - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { - err(1, "driver reset failed"); - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "driver poll restart failed"); - } - - exit(0); + PX4_INFO("PASS"); } /** @@ -880,7 +837,8 @@ void info() { if (g_dev == nullptr) { - errx(1, "driver not running"); + PX4_ERR("driver not running"); + exit(1); } printf("state @ %p\n", g_dev); @@ -958,13 +916,6 @@ tfmini_main(int argc, char *argv[]) tfmini::test(); } - /* - * Reset the driver. - */ - if (!strcmp(argv[myoptind], "reset")) { - tfmini::reset(); - } - /* * Print driver information. */ @@ -973,6 +924,6 @@ tfmini_main(int argc, char *argv[]) } out_error: - PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); + PX4_ERR("unrecognized command, try 'start', 'test', or 'info'"); return -1; }