From d2905b8d8eba3a3b1e4258343e7a384071c55073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Mar 2014 11:18:02 +0100 Subject: [PATCH 1/6] SF02/F operational, but not cleaned up / optimized - good enough for first logging --- src/drivers/sf0x/sf0x.cpp | 133 +++++++++++++++++++++++++++++--------- 1 file changed, 104 insertions(+), 29 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 5dd1f59dea..7b4b49813d 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -83,8 +84,8 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -#define SF0X_CONVERSION_INTERVAL 84000 -#define SF0X_TAKE_RANGE_REG 'D' +#define SF0X_CONVERSION_INTERVAL 90000 +#define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f #define SF0X_DEFAULT_PORT "/dev/ttyS2" @@ -186,8 +187,39 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); - // enable debug() calls - _debug_enabled = true; + if (_fd < 0) { + warnx("FAIL: laser fd"); + } + + 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 = B9600; + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERR CFG: %d ISPD", termios_state); + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERR CFG: %d OSPD\n", termios_state); + } + + if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR baud %d ATTR", termios_state); + } + + // disable debug() calls + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -208,6 +240,7 @@ int SF0X::init() { int ret = ERROR; + unsigned i = 0; /* do regular cdev init */ if (CDev::init() != OK) @@ -217,6 +250,7 @@ SF0X::init() _reports = new RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) { + warnx("mem err"); goto out; } @@ -226,12 +260,33 @@ SF0X::init() _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + warnx("advert err"); } - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; + /* attempt to get a measurement 5 times */ + while (ret != OK && i < 5) { + + if (measure()) { + ret = ERROR; + _sensor_ok = false; + } + + usleep(100000); + if (collect()) { + ret = ERROR; + _sensor_ok = false; + } else { + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; + } + + i++; + } + + /* close the fd */ + ::close(_fd); + _fd = -1; out: return ret; } @@ -453,7 +508,7 @@ SF0X::measure() if (ret != sizeof(cmd)) { perf_count(_comms_errors); - log("serial transfer returned %d", ret); + log("write fail %d", ret); return ret; } @@ -473,17 +528,30 @@ SF0X::collect() /* read from the sensor (uart buffer) */ ret = ::read(_fd, buf, sizeof(buf)); - if (ret < 1) { - log("error reading from sensor: %d", ret); + if (ret < 3) { + log("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - printf("ret: %d, val (str): %s\n", ret, buf); + if (buf[ret - 2] != '\r' || buf[ret - 1] != '\n') { + /* incomplete read */ + return -1; + } + char* end; - float si_units = strtod(buf,&end); - printf("val (float): %8.4f\n", si_units); + float si_units; + bool valid; + + if (buf[0] == '-' && buf[1] == '-' && buf[2] == '.') { + si_units = -1.0f; + valid = false; + } else { + si_units = strtod(buf,&end); + valid = true; + } + debug("val (float): %8.4f, raw: %s\n", si_units, buf); struct range_finder_report report; @@ -491,7 +559,7 @@ SF0X::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; - report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); @@ -519,21 +587,21 @@ SF0X::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&SF0X::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; + // /* 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); + // if (pub > 0) { + // orb_publish(ORB_ID(subsystem_info), pub, &info); - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } + // } else { + // pub = orb_advertise(ORB_ID(subsystem_info), &info); + // } } void @@ -553,6 +621,12 @@ SF0X::cycle_trampoline(void *arg) void SF0X::cycle() { + /* fds initialized? */ + if (_fd < 0) { + /* open fd */ + _fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK); + } + /* collection phase? */ if (_collect_phase) { @@ -653,9 +727,10 @@ start(const char* port) } /* set the poll rate to default, starts automatic data collection */ - fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + fd = open(RANGE_FINDER_DEVICE_PATH, 0); if (fd < 0) { + warnx("device open fail"); goto fail; } From 1c488a95da934817999a0a40ccb87e07bf5e0307 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Mar 2014 18:46:22 +0100 Subject: [PATCH 2/6] Added laser range finder logging, needs testing --- src/drivers/sf0x/sf0x.cpp | 32 +++++++++++++++++++++++++------- 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 7b4b49813d..685b96646c 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -118,6 +118,9 @@ private: int _measure_ticks; bool _collect_phase; int _fd; + char _linebuf[10]; + unsigned _linebuf_index; + hrt_abstime _last_read; orb_advert_t _range_finder_topic; @@ -179,6 +182,9 @@ SF0X::SF0X(const char *port) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), + _fd(-1), + _linebuf_index(0), + _last_read(0), _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), @@ -187,6 +193,10 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); + /* tell it to stop auto-triggering */ + char stop_auto = ' '; + ::write(_fd, &stop_auto, 1); + if (_fd < 0) { warnx("FAIL: laser fd"); } @@ -219,7 +229,7 @@ SF0X::SF0X(const char *port) : } // disable debug() calls - _debug_enabled = false; + _debug_enabled = true; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -524,18 +534,26 @@ SF0X::collect() perf_begin(_sample_perf); - char buf[16]; - /* read from the sensor (uart buffer) */ - ret = ::read(_fd, buf, sizeof(buf)); + /* clear buffer if last read was too long ago */ + if (hrt_elapsed(&_last_read) > (SF0X_CONVERSION_INTERVAL * 2)) { + _linebuf_index = 0; + } - if (ret < 3) { + /* read from the sensor (uart buffer) */ + ret = ::read(_fd, _linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); + + if (ret < 1) { log("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - if (buf[ret - 2] != '\r' || buf[ret - 1] != '\n') { + _linebuf_index += ret; + + _last_read = hrt_absolute_time(); + + if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { /* incomplete read */ return -1; } @@ -544,7 +562,7 @@ SF0X::collect() float si_units; bool valid; - if (buf[0] == '-' && buf[1] == '-' && buf[2] == '.') { + if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; } else { From 3f3abebf688c97f87fd6d6016aac5d234d76893f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Mar 2014 18:47:31 +0100 Subject: [PATCH 3/6] Add range finder to logging --- src/modules/sdlog2/sdlog2.c | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1365d9e705..53f07651fe 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -58,6 +58,8 @@ #include #include +#include + #include #include #include @@ -759,6 +761,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; } buf; memset(&buf, 0, sizeof(buf)); @@ -785,6 +788,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_vel_sp_sub; int battery_sub; int telemetry_sub; + int range_finder_sub; } subs; /* log message buffer: header + body */ @@ -955,6 +959,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- RANGE FINDER --- */ + subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + fds[fdsc_count].fd = subs.range_finder_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1370,6 +1380,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(TELE); } + /* --- BOTTOM DISTANCE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder); + 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); + LOGBUFFER_WRITE_AND_COUNT(DIST); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ From 290b07920c94df09415ccc1c44850c57d0750fc3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 10:52:26 +0100 Subject: [PATCH 4/6] Fixed missing reset of poll rate after test exit in ultrasound driver --- src/drivers/mb12xx/mb12xx.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 7693df295c..5adb8cf698 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -200,7 +200,7 @@ MB12XX::MB12XX(int bus, int address) : _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -762,6 +762,11 @@ test() warnx("time: %lld", report.timestamp); } + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + errx(0, "PASS"); } From 7851472ae9f19341a1a8905eed63c2c907f16385 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 10:52:53 +0100 Subject: [PATCH 5/6] Laser range finder ready to roll, logging tested --- src/drivers/sf0x/sf0x.cpp | 68 ++++++++++++++++++++++++++++++--------- 1 file changed, 52 insertions(+), 16 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 685b96646c..dace02cf8d 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -84,7 +84,7 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -#define SF0X_CONVERSION_INTERVAL 90000 +#define SF0X_CONVERSION_INTERVAL 83334 #define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f @@ -193,14 +193,16 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); - /* tell it to stop auto-triggering */ - char stop_auto = ' '; - ::write(_fd, &stop_auto, 1); - if (_fd < 0) { warnx("FAIL: laser fd"); } + /* tell it to stop auto-triggering */ + char stop_auto = ' '; + (void)::write(_fd, &stop_auto, 1); + usleep(100); + (void)::write(_fd, &stop_auto, 1); + struct termios uart_config; int termios_state; @@ -229,7 +231,7 @@ SF0X::SF0X(const char *port) : } // disable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -535,41 +537,58 @@ SF0X::collect() perf_begin(_sample_perf); /* clear buffer if last read was too long ago */ - if (hrt_elapsed(&_last_read) > (SF0X_CONVERSION_INTERVAL * 2)) { + uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; } /* read from the sensor (uart buffer) */ - ret = ::read(_fd, _linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); + ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); - if (ret < 1) { - log("read err: %d", ret); + if (ret < 0) { + _linebuf[sizeof(_linebuf) - 1] = '\0'; + debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); perf_count(_comms_errors); perf_end(_sample_perf); - return ret; + + /* only throw an error if we time out */ + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { + return ret; + } else { + return -EAGAIN; + } } _linebuf_index += ret; + if (_linebuf_index >= sizeof(_linebuf)) { + _linebuf_index = 0; + } _last_read = hrt_absolute_time(); if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - /* incomplete read */ - return -1; + /* incomplete read, reschedule ourselves */ + return -EAGAIN; } char* end; float si_units; bool valid; + /* enforce line ending */ + _linebuf[sizeof(_linebuf) - 1] = '\0'; + if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; } else { - si_units = strtod(buf,&end); + si_units = strtod(_linebuf,&end); valid = true; } - debug("val (float): %8.4f, raw: %s\n", si_units, buf); + debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); + + /* done with this chunk, resetting */ + _linebuf_index = 0; struct range_finder_report report; @@ -649,7 +668,19 @@ SF0X::cycle() if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + int collect_ret = collect(); + + if (collect_ret == -EAGAIN) { + /* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + USEC2TICK(1100)); + return; + } + + if (OK != collect_ret) { log("collection error"); /* restart the measurement state machine */ start(); @@ -842,6 +873,11 @@ test() warnx("time: %lld", report.timestamp); } + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + errx(0, "PASS"); } From b7662537df656f8546acecc9cca6ab2ac40714bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 10:54:48 +0100 Subject: [PATCH 6/6] Fix code style of laser rangefinder --- src/drivers/sf0x/sf0x.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index dace02cf8d..70cd1ab1e5 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -93,7 +93,7 @@ static const int ERROR = -1; class SF0X : public device::CDev { public: - SF0X(const char* port=SF0X_DEFAULT_PORT); + SF0X(const char *port = SF0X_DEFAULT_PORT); virtual ~SF0X(); virtual int init(); @@ -255,8 +255,9 @@ SF0X::init() unsigned i = 0; /* do regular cdev init */ - if (CDev::init() != OK) + if (CDev::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); @@ -284,9 +285,11 @@ SF0X::init() } usleep(100000); + if (collect()) { ret = ERROR; _sensor_ok = false; + } else { ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -538,6 +541,7 @@ SF0X::collect() /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; } @@ -550,16 +554,18 @@ SF0X::collect() debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); perf_count(_comms_errors); perf_end(_sample_perf); - + /* only throw an error if we time out */ if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { return ret; + } else { return -EAGAIN; } } _linebuf_index += ret; + if (_linebuf_index >= sizeof(_linebuf)) { _linebuf_index = 0; } @@ -571,7 +577,7 @@ SF0X::collect() return -EAGAIN; } - char* end; + char *end; float si_units; bool valid; @@ -581,10 +587,12 @@ SF0X::collect() if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; + } else { - si_units = strtod(_linebuf,&end); + si_units = strtod(_linebuf, &end); valid = true; } + debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); /* done with this chunk, resetting */ @@ -756,7 +764,7 @@ void info(); * Start the driver. */ void -start(const char* port) +start(const char *port) { int fd; @@ -931,6 +939,7 @@ sf0x_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (argc > 2) { sf0x::start(argv[2]); + } else { sf0x::start(SF0X_DEFAULT_PORT); }