diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-12 20:42:34 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-12 20:42:34 +0100 |
commit | 4840d5fbcb936147276d639ad628c12c7c0d400e (patch) | |
tree | ef23fc93f3f9aa2b9426c9d3d609b730d0caffdf | |
parent | 89e6b3f60621eb1b60dc58b60ea3c7c01dfb11cc (diff) | |
parent | b7662537df656f8546acecc9cca6ab2ac40714bf (diff) | |
download | px4-firmware-4840d5fbcb936147276d639ad628c12c7c0d400e.tar.gz px4-firmware-4840d5fbcb936147276d639ad628c12c7c0d400e.tar.bz2 px4-firmware-4840d5fbcb936147276d639ad628c12c7c0d400e.zip |
merged master
-rw-r--r-- | src/drivers/mb12xx/mb12xx.cpp | 7 | ||||
-rw-r--r-- | src/drivers/sf0x/sf0x.cpp | 216 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 14 |
3 files changed, 197 insertions, 40 deletions
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 7693df295..5adb8cf69 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"); } diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 5dd1f59de..70cd1ab1e 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 83334 +#define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f #define SF0X_DEFAULT_PORT "/dev/ttyS2" @@ -92,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(); @@ -117,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; @@ -178,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")), @@ -186,8 +193,45 @@ 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"); + } + + /* 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; + + /* 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,15 +252,18 @@ int SF0X::init() { int ret = ERROR; + 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)); if (_reports == nullptr) { + warnx("mem err"); goto out; } @@ -226,12 +273,35 @@ 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 +523,7 @@ SF0X::measure() if (ret != sizeof(cmd)) { perf_count(_comms_errors); - log("serial transfer returned %d", ret); + log("write fail %d", ret); return ret; } @@ -469,21 +539,64 @@ SF0X::collect() perf_begin(_sample_perf); - char buf[16]; + /* 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; + } + /* read from the sensor (uart buffer) */ - ret = ::read(_fd, buf, sizeof(buf)); + ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); - if (ret < 1) { - log("error reading from sensor: %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, 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(_linebuf, &end); + valid = true; } - printf("ret: %d, val (str): %s\n", ret, buf); - char* end; - float si_units = strtod(buf,&end); - printf("val (float): %8.4f\n", si_units); + debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); + + /* done with this chunk, resetting */ + _linebuf_index = 0; struct range_finder_report report; @@ -491,7 +604,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 +632,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,11 +666,29 @@ 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) { /* 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(); @@ -633,7 +764,7 @@ void info(); * Start the driver. */ void -start(const char* port) +start(const char *port) { int fd; @@ -653,9 +784,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; } @@ -749,6 +881,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"); } @@ -802,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); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ad3a7f12c..2514bafee 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -58,6 +58,8 @@ #include <drivers/drv_hrt.h> #include <math.h> +#include <drivers/drv_range_finder.h> + #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/sensor_combined.h> @@ -791,6 +793,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)); @@ -851,6 +854,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; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -874,6 +878,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); thread_running = true; @@ -1227,6 +1232,15 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(TELE); } + /* --- BOTTOM DISTANCE --- */ + if (copy_if_updated(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 */ |