dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Complete output parsing, pending testing

This commit is contained in:
Lorenz Meier 2014-03-08 21:06:29 +01:00
parent 008efff42e
commit 7cc33b9ca6
1 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/drivers/sf0x/sf0x.cpp
View File

@ -33,7 +33,8 @@
/** /**
* @file sf0x.cpp * @file sf0x.cpp
* @author Lorenz Meier * @author Lorenz Meier <lm@inf.ethz.ch>
* @author Greg Hulands
* *
* Driver for the Lightware SF0x laser rangefinder series * Driver for the Lightware SF0x laser rangefinder series
*/ */
@ -459,25 +460,26 @@ SF0X::measure()
int int
SF0X::collect() SF0X::collect()
{ {
int ret = -EIO; int ret;
/* read from the sensor */
float val;
perf_begin(_sample_perf); perf_begin(_sample_perf);
char buf[16]; char buf[16];
/* read from the sensor (uart buffer) */
ret = ::read(_fd, buf, sizeof(buf)); ret = ::read(_fd, buf, sizeof(buf));
printf("ret: %d, val: %s", ret, buf);
if (ret < 0) { if (ret < 1) {
log("error reading from sensor: %d", ret); log("error reading from sensor: %d", ret);
perf_count(_comms_errors); perf_count(_comms_errors);
perf_end(_sample_perf); perf_end(_sample_perf);
return ret; return ret;
} }
float si_units = 0.0f; 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);
struct range_finder_report report; struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */ /* this should be fairly close to the end of the measurement, so the best approximation of the time */
@ -538,7 +540,7 @@ SF0X::stop()
void void
SF0X::cycle_trampoline(void *arg) SF0X::cycle_trampoline(void *arg)
{ {
SF0X *dev = (SF0X *)arg; SF0X *dev = static_cast<SF0X *>(arg);
dev->cycle(); dev->cycle();
} }
@ -598,7 +600,7 @@ SF0X::print_info()
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows); perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks); printf("poll interval: %d ticks\n", _measure_ticks);
_reports->print_info("report queue"); _reports->print_info("report queue");
} }
@ -694,7 +696,6 @@ test()
{ {
struct range_finder_report report; struct range_finder_report report;
ssize_t sz; ssize_t sz;
int ret;
int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
@ -725,7 +726,7 @@ test()
/* wait for data to be ready */ /* wait for data to be ready */
fds.fd = fd; fds.fd = fd;
fds.events = POLLIN; fds.events = POLLIN;
ret = poll(&fds, 1, 2000); int ret = poll(&fds, 1, 2000);
if (ret != 1) { if (ret != 1) {
errx(1, "timed out waiting for sensor data"); errx(1, "timed out waiting for sensor data");