PX4 Firmware
PX4 Autopilot Software http://px4.io
sf0x_parser.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2014 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file sf0x_parser.cpp
36  * @author Lorenz Meier <lm@inf.ethz.ch>
37  *
38  * Driver for the Lightware SF0x laser rangefinder series
39  */
40 
41 #include "sf0x_parser.h"
42 #include <string.h>
43 #include <stdlib.h>
44 
45 //#define SF0X_DEBUG
46 
47 #ifdef SF0X_DEBUG
48 #include <stdio.h>
49 
50 const char *parser_state[] = {
51  "0_UNSYNC",
52  "1_SYNC",
53  "2_GOT_DIGIT0",
54  "3_GOT_DOT",
55  "4_GOT_DIGIT1",
56  "5_GOT_DIGIT2",
57  "6_GOT_CARRIAGE_RETURN"
58 };
59 #endif
60 
61 int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
62 {
63  int ret = -1;
64  char *end;
65 
66  switch (*state) {
68  if (c == '\n') {
69  *state = SF0X_PARSE_STATE1_SYNC;
70  (*parserbuf_index) = 0;
71  }
72 
73  break;
74 
76  if (c >= '0' && c <= '9') {
78  parserbuf[*parserbuf_index] = c;
79  (*parserbuf_index)++;
80  }
81 
82  break;
83 
85  if (c >= '0' && c <= '9') {
87  parserbuf[*parserbuf_index] = c;
88  (*parserbuf_index)++;
89 
90  } else if (c == '.') {
92  parserbuf[*parserbuf_index] = c;
93  (*parserbuf_index)++;
94 
95  } else {
96  *state = SF0X_PARSE_STATE0_UNSYNC;
97  }
98 
99  break;
100 
102  if (c >= '0' && c <= '9') {
104  parserbuf[*parserbuf_index] = c;
105  (*parserbuf_index)++;
106 
107  } else {
108  *state = SF0X_PARSE_STATE0_UNSYNC;
109  }
110 
111  break;
112 
114  if (c >= '0' && c <= '9') {
116  parserbuf[*parserbuf_index] = c;
117  (*parserbuf_index)++;
118 
119  } else {
120  *state = SF0X_PARSE_STATE0_UNSYNC;
121  }
122 
123  break;
124 
126  if (c == '\r') {
128 
129  } else {
130  *state = SF0X_PARSE_STATE0_UNSYNC;
131  }
132 
133  break;
134 
136  if (c == '\n') {
137  parserbuf[*parserbuf_index] = '\0';
138  *dist = strtod(parserbuf, &end);
139  *state = SF0X_PARSE_STATE1_SYNC;
140  *parserbuf_index = 0;
141  ret = 0;
142 
143  } else {
144  *state = SF0X_PARSE_STATE0_UNSYNC;
145  }
146 
147  break;
148  }
149 
150 #ifdef SF0X_DEBUG
151  printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]);
152 #endif
153 
154  return ret;
155 }
static enum @74 state
int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
Definition: sf0x_parser.cpp:61
static crsf_parser_state_t parser_state
Definition: crsf.cpp:134
SF0X_PARSE_STATE
Definition: sf0x_parser.h:43