yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Lorenz Meier
2014-10-07 12:46:09 +02:00
parent 8d187cc2fa
commit 72fbd76c84
6 changed files with 222 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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 {