Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_RangeFinder_Wasp.h"
17 :
18 : #if AP_RANGEFINDER_WASP_ENABLED
19 :
20 : #include <AP_HAL/AP_HAL.h>
21 : #include <ctype.h>
22 :
23 : extern const AP_HAL::HAL& hal;
24 :
25 : const AP_Param::GroupInfo AP_RangeFinder_Wasp::var_info[] = {
26 : // @Param: WSP_MAVG
27 : // @DisplayName: Moving Average Range
28 : // @Description: Sets the number of historic range results to use for calculating the current range result. When MAVG is greater than 1, the current range result will be the current measured value averaged with the N-1 previous results
29 : // @Range: 0 255
30 : // @User: Advanced
31 : AP_GROUPINFO("WSP_MAVG", 1, AP_RangeFinder_Wasp, mavg, 4),
32 :
33 : // @Param: WSP_MEDF
34 : // @DisplayName: Moving Median Filter
35 : // @Description: Sets the window size for the real-time median filter. When MEDF is greater than 0 the median filter is active
36 : // @Range: 0 255
37 : // @User: Advanced
38 : AP_GROUPINFO("WSP_MEDF", 2, AP_RangeFinder_Wasp, medf, 4),
39 :
40 : // @Param: WSP_FRQ
41 : // @DisplayName: Frequency
42 : // @Description: Sets the repetition frequency of the ranging operation in Hertz. Upon entering the desired frequency the system will calculate the nearest frequency that it can handle according to the resolution of internal timers.
43 : // @Range: 0 10000
44 : // @User: Advanced
45 : AP_GROUPINFO("WSP_FRQ", 3, AP_RangeFinder_Wasp, frq, 20),
46 :
47 : // @Param: WSP_AVG
48 : // @DisplayName: Multi-pulse averages
49 : // @Description: Sets the number of pulses to be used in multi-pulse averaging mode. In this mode, a sequence of rapid fire ranges are taken and then averaged to improve the accuracy of the measurement
50 : // @Range: 0 255
51 : // @User: Advanced
52 : AP_GROUPINFO("WSP_AVG", 4, AP_RangeFinder_Wasp, avg, 2),
53 :
54 : // @Param: WSP_THR
55 : // @DisplayName: Sensitivity threshold
56 : // @Description: Sets the system sensitivity. Larger values of THR represent higher sensitivity. The system may limit the maximum value of THR to prevent excessive false alarm rates based on settings made at the factory. Set to -1 for automatic threshold adjustments
57 : // @Range: -1 255
58 : // @User: Advanced
59 : AP_GROUPINFO("WSP_THR", 5, AP_RangeFinder_Wasp, thr, -1),
60 :
61 : // @Param: WSP_BAUD
62 : // @DisplayName: Baud rate
63 : // @Description: Desired baud rate
64 : // @Values: 0:Low Speed,1:High Speed
65 : // @User: Advanced
66 : AP_GROUPINFO("WSP_BAUD", 6, AP_RangeFinder_Wasp, baud, 0),
67 :
68 : AP_GROUPEND
69 : };
70 :
71 1 : AP_RangeFinder_Wasp::AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state,
72 1 : AP_RangeFinder_Params &_params) :
73 1 : AP_RangeFinder_Backend_Serial(_state, _params)
74 : {
75 1 : AP_Param::setup_object_defaults(this, var_info);
76 :
77 1 : state.var_info = var_info;
78 1 : }
79 :
80 : // read - return last value measured by sensor
81 1353 : bool AP_RangeFinder_Wasp::get_reading(float &reading_m) {
82 1353 : if (uart == nullptr) {
83 0 : return false;
84 : }
85 :
86 : // read any available lines from the lidar
87 1353 : float sum = 0;
88 1353 : uint16_t count = 0;
89 13805 : for (auto i=0; i<8192; i++) {
90 : uint8_t c;
91 13805 : if (!uart->read(c)) {
92 1353 : break;
93 : }
94 12452 : if (c == '\n') {
95 1386 : linebuf[linebuf_len] = 0;
96 1386 : linebuf_len = 0;
97 1386 : state.last_reading_ms = AP_HAL::millis();
98 1386 : if (isalpha(linebuf[0])) {
99 8 : parse_response();
100 : } else {
101 1378 : float read_value = strtof(linebuf, NULL);
102 1378 : if (read_value > 0) {
103 1378 : sum += read_value;
104 1378 : count++;
105 : }
106 : }
107 11066 : } else if (isalnum(c) || c == '.' || c == '-') {
108 11059 : linebuf[linebuf_len++] = c;
109 : }
110 :
111 : // discard excessively long buffers
112 12452 : if (linebuf_len == sizeof(linebuf)) {
113 0 : linebuf_len = 0;
114 : }
115 : }
116 :
117 1353 : if (configuration_state == WASP_CFG_RATE && uart->tx_pending() == 0) {
118 0 : configuration_state = WASP_CFG_ENCODING;
119 : }
120 :
121 1353 : if (count == 0) {
122 0 : return false;
123 : }
124 :
125 1353 : reading_m = sum / count;
126 1353 : set_status(RangeFinder::Status::Good);
127 :
128 1353 : return true;
129 : }
130 :
131 : #define COMMAND_BUFFER_LEN 15
132 :
133 1353 : void AP_RangeFinder_Wasp::update(void) {
134 1353 : if (!get_reading(state.distance_m)) {
135 0 : set_status(RangeFinder::Status::NoData);
136 : }
137 :
138 1353 : if (AP_HAL::millis() - state.last_reading_ms > 500) {
139 : // attempt to reconfigure on the assumption this was a bad baud setting
140 0 : configuration_state = WASP_CFG_RATE;
141 : }
142 :
143 1353 : char command[COMMAND_BUFFER_LEN] = {};
144 :
145 1353 : switch (configuration_state) {
146 0 : case WASP_CFG_RATE:
147 0 : hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">BAUD %s\n", baud > 0 ? "HIGH" : "LOW");
148 0 : break;
149 0 : case WASP_CFG_ENCODING:
150 0 : uart->end();
151 0 : uart->begin(baud > 0 ? 921600 : 115200);
152 0 : hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">LBE LITTLE\n");
153 0 : break;
154 1 : case WASP_CFG_PROTOCOL:
155 1 : hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">FMT ASCII\n");
156 1 : break;
157 1 : case WASP_CFG_FRQ:
158 1 : hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">FRQ %d\n", constrain_int16(frq, 20, baud > 0 ? 10000 : 1440));
159 1 : break;
160 1 : case WASP_CFG_GO:
161 1 : hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">GO\n");
162 1 : break;
163 1 : case WASP_CFG_AUT:
164 1 : hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">AUT %d\n", thr >= 0 ? 0 : 1);
165 1 : break;
166 1 : case WASP_CFG_THR:
167 1 : if (thr >= 0) {
168 0 : hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">THR %d\n", constrain_int16(thr, 0,255));
169 : } else {
170 1 : configuration_state = WASP_CFG_MAVG;
171 : }
172 1 : break;
173 1 : case WASP_CFG_MAVG:
174 1 : hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">MAVG %d\n", constrain_int16(mavg, 0, 255));
175 1 : break;
176 1 : case WASP_CFG_MEDF:
177 1 : hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">MEDF %d\n", constrain_int16(medf, 0, 255));
178 1 : break;
179 1 : case WASP_CFG_AVG:
180 1 : hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">AVG %d\n", constrain_int16(avg, 0, 255));
181 1 : break;
182 1 : case WASP_CFG_AUV:
183 1 : hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">AUV 1\n");
184 1 : break;
185 1344 : case WASP_CFG_DONE:
186 1344 : return;
187 : }
188 :
189 9 : if (command[0] != 0) {
190 8 : uart->write((uint8_t *)command, strlen(command));
191 : }
192 : }
193 :
194 8 : void AP_RangeFinder_Wasp::parse_response(void) {
195 8 : switch (configuration_state) {
196 0 : case WASP_CFG_RATE:
197 0 : configuration_state = WASP_CFG_ENCODING;
198 0 : break;
199 0 : case WASP_CFG_ENCODING:
200 0 : if (strncmp(linebuf, "LBE", 3) == 0) {
201 0 : configuration_state = WASP_CFG_PROTOCOL;
202 : }
203 0 : break;
204 1 : case WASP_CFG_PROTOCOL:
205 1 : if (strncmp(linebuf, "FMT", 3) == 0) {
206 1 : configuration_state = WASP_CFG_FRQ;
207 : }
208 1 : break;
209 1 : case WASP_CFG_FRQ:
210 1 : if (strncmp(linebuf, "FRQ", 3) == 0) {
211 1 : configuration_state = WASP_CFG_GO;
212 : }
213 1 : break;
214 1 : case WASP_CFG_GO:
215 1 : if (strncmp(linebuf, "GO", 2) == 0) {
216 1 : configuration_state = WASP_CFG_AUT;
217 : }
218 1 : break;
219 1 : case WASP_CFG_AUT:
220 1 : if (strncmp(linebuf, "AUT", 3) == 0) {
221 1 : configuration_state = WASP_CFG_THR;
222 : }
223 1 : break;
224 0 : case WASP_CFG_THR:
225 0 : if (strncmp(linebuf, "THR", 3) == 0) {
226 0 : configuration_state = WASP_CFG_MAVG;
227 : }
228 0 : break;
229 1 : case WASP_CFG_MAVG:
230 1 : if (strncmp(linebuf, "MAVG", 4) == 0) {
231 1 : configuration_state = WASP_CFG_MEDF;
232 : }
233 1 : break;
234 1 : case WASP_CFG_MEDF:
235 1 : if (strncmp(linebuf, "MEDF", 4) == 0) {
236 1 : configuration_state = WASP_CFG_AVG;
237 : }
238 1 : break;
239 1 : case WASP_CFG_AVG:
240 1 : if (strncmp(linebuf, "AVG", 3) == 0) {
241 1 : configuration_state = WASP_CFG_AUV;
242 : }
243 1 : break;
244 1 : case WASP_CFG_AUV:
245 1 : if (strncmp(linebuf, "AUV", 3) == 0) {
246 1 : configuration_state = WASP_CFG_DONE;
247 : }
248 1 : break;
249 0 : case WASP_CFG_DONE:
250 0 : return;
251 : }
252 : }
253 :
254 : #endif // AP_RANGEFINDER_WASP_ENABLED
|
