diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-09 11:18:02 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-09 11:18:02 +0100 |
commit | d2905b8d8eba3a3b1e4258343e7a384071c55073 (patch) | |
tree | 5b33813cefeaf3223819088859af7b428e874525 /src | |
parent | d69d895f0240a0dda5fda2f8f046179b069439a4 (diff) | |
download | px4-firmware-d2905b8d8eba3a3b1e4258343e7a384071c55073.tar.gz px4-firmware-d2905b8d8eba3a3b1e4258343e7a384071c55073.tar.bz2 px4-firmware-d2905b8d8eba3a3b1e4258343e7a384071c55073.zip |
SF02/F operational, but not cleaned up / optimized - good enough for first logging
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/sf0x/sf0x.cpp | 137 |
1 files changed, 106 insertions, 31 deletions
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 5dd1f59de..7b4b49813 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -53,6 +53,7 @@ #include <stdio.h> #include <math.h> #include <unistd.h> +#include <termios.h> #include <nuttx/arch.h> #include <nuttx/wqueue.h> @@ -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; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } + // /* 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 @@ -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; } |