Merge pull request #1379 from PX4/sf0x_test

Sf0x test
This commit is contained in:
Thomas Gubler 2014-10-07 14:53:29 +02:00
commit 91b4d85b46
7 changed files with 301 additions and 88 deletions

View File

@ -1,4 +1,5 @@
./obj/* ./obj/*
mixer_test mixer_test
sf0x_test
sbus2_test sbus2_test
autodeclination_test autodeclination_test

View File

@ -3,7 +3,7 @@ CC=g++
CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \
-I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm -I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm
all: mixer_test sbus2_test autodeclination_test all: mixer_test sbus2_test autodeclination_test sf0x_test
MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \
../../src/systemcmds/tests/test_conv.cpp \ ../../src/systemcmds/tests/test_conv.cpp \
@ -20,7 +20,12 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
hrt.cpp \ hrt.cpp \
sbus2_test.cpp sbus2_test.cpp
AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ SF0X_FILES= \
hrt.cpp \
sf0x_test.cpp \
../../src/drivers/sf0x/sf0x_parser.cpp
AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \
hrt.cpp \ hrt.cpp \
autodeclination_test.cpp autodeclination_test.cpp
@ -30,10 +35,13 @@ mixer_test: $(MIXER_FILES)
sbus2_test: $(SBUS2_FILES) sbus2_test: $(SBUS2_FILES)
$(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)
sf0x_test: $(SF0X_FILES)
$(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS)
autodeclination_test: $(SBUS2_FILES) autodeclination_test: $(SBUS2_FILES)
$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS) $(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)
.PHONY: clean .PHONY: clean
clean: clean:
rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test sf0x_test

View File

@ -0,0 +1,65 @@
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/sf0x/sf0x_parser.h>
int main(int argc, char *argv[])
{
warnx("SF0X test started");
int ret = 0;
const char LINE_MAX = 20;
char _linebuf[LINE_MAX];
_linebuf[0] = '\0';
const char *lines[] = {"0.01\r\n",
"0.02\r\n",
"0.03\r\n",
"0.04\r\n",
"0",
".",
"0",
"5",
"\r",
"\n",
"0",
"3\r",
"\n"
"\r\n",
"0.06",
"\r\n"
};
enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC;
float dist_m;
char _parserbuf[LINE_MAX];
unsigned _parsebuf_index = 0;
for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
printf("\n%s", _linebuf);
int parse_ret;
for (int i = 0; i < strlen(lines[l]); i++) {
parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
if (parse_ret == 0) {
printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
}
}
printf("%s", lines[l]);
}
warnx("test finished");
return ret;
}

View File

@ -37,6 +37,7 @@
MODULE_COMMAND = sf0x MODULE_COMMAND = sf0x
SRCS = sf0x.cpp SRCS = sf0x.cpp \
sf0x_parser.cpp
MAXOPTIMIZATION = -Os MAXOPTIMIZATION = -Os

View File

@ -72,6 +72,8 @@
#include <board_config.h> #include <board_config.h>
#include "sf0x_parser.h"
/* Configuration Constants */ /* Configuration Constants */
/* oddly, ERROR is not defined for c++ */ /* oddly, ERROR is not defined for c++ */
@ -120,6 +122,7 @@ private:
int _fd; int _fd;
char _linebuf[10]; char _linebuf[10];
unsigned _linebuf_index; unsigned _linebuf_index;
enum SF0X_PARSE_STATE _parse_state;
hrt_abstime _last_read; hrt_abstime _last_read;
orb_advert_t _range_finder_topic; orb_advert_t _range_finder_topic;
@ -186,6 +189,7 @@ SF0X::SF0X(const char *port) :
_collect_phase(false), _collect_phase(false),
_fd(-1), _fd(-1),
_linebuf_index(0), _linebuf_index(0),
_parse_state(SF0X_PARSE_STATE0_UNSYNC),
_last_read(0), _last_read(0),
_range_finder_topic(-1), _range_finder_topic(-1),
_consecutive_fail_count(0), _consecutive_fail_count(0),
@ -200,12 +204,6 @@ SF0X::SF0X(const char *port) :
warnx("FAIL: laser fd"); 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; struct termios uart_config;
int termios_state; int termios_state;
@ -520,20 +518,15 @@ SF0X::collect()
/* clear buffer if last read was too long ago */ /* clear buffer if last read was too long ago */
uint64_t read_elapsed = hrt_elapsed_time(&_last_read); uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* timed out - retry */
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
_linebuf_index = 0;
}
/* the buffer for read chars is buflen minus null termination */ /* the buffer for read chars is buflen minus null termination */
unsigned readlen = sizeof(_linebuf) - 1; char readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf) - 1;
/* read from the sensor (uart buffer) */ /* read from the sensor (uart buffer) */
ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index); ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) { if (ret < 0) {
_linebuf[sizeof(_linebuf) - 1] = '\0'; debug("read err: %d", ret);
debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
perf_count(_comms_errors); perf_count(_comms_errors);
perf_end(_sample_perf); perf_end(_sample_perf);
@ -548,84 +541,23 @@ SF0X::collect()
return -EAGAIN; return -EAGAIN;
} }
/* let the write pointer point to the next free entry */
_linebuf_index += ret;
_last_read = hrt_absolute_time(); _last_read = hrt_absolute_time();
/* require a reasonable amount of minimum bytes */
if (_linebuf_index < 6) {
/* we need at this format: x.xx\r\n */
return -EAGAIN;
} else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
if (_linebuf_index == readlen) {
/* we have a full buffer, but no line ending - abort */
_linebuf_index = 0;
perf_count(_comms_errors);
return -ENOMEM;
} else {
/* incomplete read, reschedule ourselves */
return -EAGAIN;
}
}
char *end;
float si_units; float si_units;
bool valid; bool valid = false;
/* enforce line ending */ for (unsigned i = 0; i < ret; i++) {
_linebuf[_linebuf_index] = '\0'; if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
si_units = -1.0f;
valid = false;
} else {
/* we need to find a dot in the string, as we're missing the meters part else */
valid = false;
/* wipe out partially read content from last cycle(s), check for dot */
for (unsigned i = 0; i < (_linebuf_index - 2); i++) {
if (_linebuf[i] == '\n') {
/* wipe out any partial measurements */
for (unsigned j = 0; j <= i; j++) {
_linebuf[j] = ' ';
}
}
/* we need a digit before the dot and a dot for a valid number */
if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) {
valid = true; valid = true;
} }
} }
if (valid) { if (!valid) {
si_units = strtod(_linebuf, &end); return -EAGAIN;
/* we require at least four characters for a valid number */
if (end > _linebuf + 3) {
valid = true;
} else {
si_units = -1.0f;
valid = false;
}
}
} }
debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
/* done with this chunk, resetting - even if invalid */
_linebuf_index = 0;
/* if its invalid, there is no reason to forward the value */
if (!valid) {
perf_count(_comms_errors);
return -EINVAL;
}
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 */

View File

@ -0,0 +1,155 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sf0x_parser.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Driver for the Lightware SF0x laser rangefinder series
*/
#include "sf0x_parser.h"
#include <string.h>
#include <stdlib.h>
//#define SF0X_DEBUG
#ifdef SF0X_DEBUG
#include <stdio.h>
const char *parser_state[] = {
"0_UNSYNC",
"1_SYNC",
"2_GOT_DIGIT0",
"3_GOT_DOT",
"4_GOT_DIGIT1",
"5_GOT_DIGIT2",
"6_GOT_CARRIAGE_RETURN"
};
#endif
int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
{
int ret = -1;
char *end;
switch (*state) {
case SF0X_PARSE_STATE0_UNSYNC:
if (c == '\n') {
*state = SF0X_PARSE_STATE1_SYNC;
(*parserbuf_index) = 0;
}
break;
case SF0X_PARSE_STATE1_SYNC:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
}
break;
case SF0X_PARSE_STATE2_GOT_DIGIT0:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else if (c == '.') {
*state = SF0X_PARSE_STATE3_GOT_DOT;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE3_GOT_DOT:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE4_GOT_DIGIT1;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE4_GOT_DIGIT1:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE5_GOT_DIGIT2;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE5_GOT_DIGIT2:
if (c == '\r') {
*state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN:
if (c == '\n') {
parserbuf[*parserbuf_index] = '\0';
*dist = strtod(parserbuf, &end);
*state = SF0X_PARSE_STATE1_SYNC;
*parserbuf_index = 0;
ret = 0;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
}
#ifdef SF0X_DEBUG
printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]);
#endif
return ret;
}

View File

@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sf0x_parser.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Declarations of parser for the Lightware SF0x laser rangefinder series
*/
enum SF0X_PARSE_STATE {
SF0X_PARSE_STATE0_UNSYNC = 0,
SF0X_PARSE_STATE1_SYNC,
SF0X_PARSE_STATE2_GOT_DIGIT0,
SF0X_PARSE_STATE3_GOT_DOT,
SF0X_PARSE_STATE4_GOT_DIGIT1,
SF0X_PARSE_STATE5_GOT_DIGIT2,
SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN
};
int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist);