zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Updated and fixed parser for SF02/F laser sensor, test harness runs clean

sbg
Lorenz Meier 10 years ago
parent
8d187cc2fa
commit
72fbd76c84
6 changed files with 222 additions and 115 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      Tools/tests-host/.gitignore
  2. 5
      Tools/tests-host/Makefile
  3. BIN
      Tools/tests-host/sf0x_test
  4. 137
      Tools/tests-host/sf0x_test.cpp
  5. 143
      src/drivers/sf0x/sf0x_parser.cpp
  6. 51
      src/drivers/sf0x/sf0x_parser.h

1
Tools/tests-host/.gitignore vendored
Unescape View File

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

5
Tools/tests-host/Makefile
Unescape View File

@ -22,9 +22,10 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ @@ -22,9 +22,10 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
SF0X_FILES= \
hrt.cpp \
sf0x_test.cpp
sf0x_test.cpp \
../../src/drivers/sf0x/sf0x_parser.cpp
AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \
AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \
hrt.cpp \
autodeclination_test.cpp

BIN
Tools/tests-host/sf0x_test
View File

Binary file not shown.

137
Tools/tests-host/sf0x_test.cpp
Unescape View File

@ -1,152 +1,63 @@ @@ -1,152 +1,63 @@
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
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)
{
int ret = -1;
unsigned len = strlen(parserbuf);
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_STATE4_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(linebuf, &end);
*state = SF0X_PARSE_STATE0_SYNC;
*parserbuf_index = 0;
ret = 0;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
}
return ret;
}
#include <drivers/sf0x/sf0x_parser.h>
int main(int argc, char *argv[]) {
warnx("SF0X test started");
if (argc < 2)
errx(1, "Need a filename for the input file");
warnx("loading data from: %s", argv[1]);
FILE *fp;
fp = fopen(argv[1],"rt");
if (!fp)
errx(1, "failed opening file");
int ret = 0;
const char LINE_MAX = 20;
char _linebuf[LINE_MAX];
_linebuf[0] = '\0';
char *end;
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;
while (fgets(_linebuf, LINE_MAX, fp) != NULL) {
for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
printf("\n%s", _linebuf);
int parse_ret;
for (int i = 0; i < strlen(_linebuf); i++)
for (int i = 0; i < strlen(lines[l]); i++)
{
printf("%0x ", _linebuf[i]);
parse_ret = sf0x_parser(_linebuf[i], _parserbuf, &_parsebuf_index, &state, &dist_m);
parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
if (parse_ret == 0) {
printf("PARSED!");
printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
}
}
printf("%s", lines[l]);
printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
}
// Init the parser
if (ret == EOF) {
warnx("Test finished, reached end of file");
} else {

143
src/drivers/sf0x/sf0x_parser.cpp
Unescape View File

@ -0,0 +1,143 @@ @@ -0,0 +1,143 @@
/****************************************************************************
*
* 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;
unsigned len = strlen(parserbuf);
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;
}

51
src/drivers/sf0x/sf0x_parser.h
Unescape View File

@ -0,0 +1,51 @@ @@ -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);
Write Preview
Loading…
Cancel
Save