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 : #include "AP_RangeFinder_LightWareI2C.h"
16 :
17 : #if AP_RANGEFINDER_LWI2C_ENABLED
18 :
19 : #include <AP_HAL/AP_HAL.h>
20 : #include <AP_HAL/utility/sparse-endian.h>
21 :
22 : extern const AP_HAL::HAL& hal;
23 :
24 : #define LIGHTWARE_DISTANCE_READ_REG 0
25 : #define LIGHTWARE_LOST_SIGNAL_TIMEOUT_READ_REG 22
26 : #define LIGHTWARE_LOST_SIGNAL_TIMEOUT_WRITE_REG 23
27 : #define LIGHTWARE_TIMEOUT_REG_DESIRED_VALUE 20 // number of lost signal confirmations for legacy protocol only
28 :
29 : #define LIGHTWARE_OUT_OF_RANGE_ADD 1.00 // metres
30 :
31 : static const size_t lx20_max_reply_len_bytes = 32;
32 : static const size_t lx20_max_expected_stream_reply_len_bytes = 14;
33 :
34 : #define stream_the_median_distance_to_the_first_return "ldf,0"
35 : #define stream_the_raw_distance_to_the_first_return "ldf,1"
36 : #define stream_the_signal_strength_first_return "lhf"
37 : #define stream_the_raw_distance_to_the_last_return "ldl,1"
38 : #define stream_the_signal_strength_last_return "lhl"
39 : #define stream_the_level_of_background_noise "ln"
40 :
41 : /* Data streams from the LiDAR can include any sf20 LiDAR measurement.
42 : * A request to stream the desired measurement is made on a 20Hz basis and
43 : * on the next 20Hz service 50ms later, the result is read and a streaming
44 : * request is made for the next desired measurement in the sequence.
45 : * Results are generally available from the LiDAR within 10mS of request.
46 : */
47 : #define STREAM1_VAL stream_the_raw_distance_to_the_last_return
48 : #define STREAM2_VAL stream_the_signal_strength_last_return
49 : #define STREAM3_VAL stream_the_raw_distance_to_the_first_return
50 : #define STREAM4_VAL stream_the_signal_strength_first_return
51 : #define STREAM5_VAL stream_the_level_of_background_noise
52 : static const char *parse_stream_id[NUM_SF20_DATA_STREAMS] = { // List of stream identifier strings. Must match init_stream_id[].
53 : STREAM1_VAL ":"
54 : };
55 :
56 : static const char *init_stream_id[NUM_SF20_DATA_STREAMS] = {// List of stream initialization strings. Must match parse_stream_id[].
57 : "$1" STREAM1_VAL "\r\n"
58 : };
59 :
60 : static const uint8_t streamSequence[] = { 0 }; // List of 0 based stream Ids that determine the LiDAR values collected.
61 :
62 : static const uint8_t numStreamSequenceIndexes = sizeof(streamSequence)/sizeof(streamSequence[0]);
63 :
64 : /**
65 : * Wrapper function over #transfer() to write a sequence of bytes to
66 : * device. No values are read.
67 : */
68 4 : bool AP_RangeFinder_LightWareI2C::write_bytes(uint8_t *write_buf_u8, uint32_t len_u8)
69 : {
70 4 : return dev.transfer(write_buf_u8, len_u8, NULL, 0);
71 : }
72 :
73 : /**
74 : * Disables "address tagging" in the sf20 response packets.
75 : */
76 1 : void AP_RangeFinder_LightWareI2C::sf20_disable_address_tagging()
77 : {
78 1 : sf20_send_and_expect("#CT,0\r\n", "ct:0");
79 1 : }
80 :
81 : /*
82 : send a native command and check for an expected reply
83 : */
84 3 : bool AP_RangeFinder_LightWareI2C::sf20_send_and_expect(const char* send_msg, const char* expected_reply)
85 : {
86 3 : const size_t expected_reply_len = strlen(expected_reply);
87 3 : uint8_t rx_bytes[expected_reply_len + 1];
88 3 : memset(rx_bytes, 0, sizeof(rx_bytes));
89 :
90 3 : if ((expected_reply_len > lx20_max_reply_len_bytes) ||
91 : (expected_reply_len < 2)) {
92 0 : return false;
93 : }
94 :
95 3 : if (!write_bytes((uint8_t*)send_msg,
96 3 : strlen(send_msg))) {
97 3 : return false;
98 : }
99 :
100 0 : if (!sf20_wait_on_reply(rx_bytes)) {
101 0 : return false;
102 : }
103 :
104 0 : if ((rx_bytes[0] != expected_reply[0]) ||
105 0 : (rx_bytes[1] != expected_reply[1]) ) {
106 0 : return false;
107 : }
108 :
109 0 : for (uint8_t i=0; i<10; i++) {
110 0 : if (dev.read(rx_bytes, expected_reply_len)) {
111 0 : break;
112 : }
113 : // give a bit of time for the remaining bytes to be available
114 0 : hal.scheduler->delay(1);
115 : }
116 :
117 0 : return memcmp(rx_bytes, expected_reply, expected_reply_len) == 0;
118 3 : }
119 :
120 : /*
121 : send a native command and fill a reply into a buffer. Used for
122 : version string
123 : */
124 1 : void AP_RangeFinder_LightWareI2C::sf20_get_version(const char* send_msg, const char *reply_prefix, char *reply, uint8_t reply_len)
125 : {
126 1 : const size_t expected_reply_len = 16;
127 : uint8_t rx_bytes[expected_reply_len + 1];
128 1 : memset(rx_bytes, 0, sizeof(rx_bytes));
129 :
130 1 : if (!write_bytes((uint8_t*)send_msg, strlen(send_msg))) {
131 1 : return;
132 : }
133 :
134 0 : if (!sf20_wait_on_reply(rx_bytes)) {
135 0 : return;
136 : }
137 :
138 0 : if ((rx_bytes[0] != uint8_t(reply_prefix[0])) ||
139 0 : (rx_bytes[1] != uint8_t(reply_prefix[1])) ) {
140 0 : return;
141 : }
142 :
143 0 : for (uint8_t i=0; i<10; i++) {
144 0 : if (dev.read(rx_bytes, expected_reply_len)) {
145 0 : break;
146 : }
147 : // give a bit of time for the remaining bytes to be available
148 0 : hal.scheduler->delay(1);
149 : }
150 0 : memcpy(reply, &rx_bytes[2], reply_len);
151 : }
152 :
153 : /* Driver first attempts to initialize the sf20.
154 : * If for any reason this fails, the driver attempts to initialize the legacy LightWare LiDAR.
155 : * If this fails, the driver returns false indicating no LightWare LiDAR is present.
156 : */
157 1 : bool AP_RangeFinder_LightWareI2C::init()
158 : {
159 1 : WITH_SEMAPHORE(dev.get_semaphore());
160 :
161 1 : if (sf20_init()) {
162 0 : DEV_PRINTF("Found SF20 native Lidar\n");
163 0 : return true;
164 : }
165 1 : if (legacy_init()) {
166 1 : DEV_PRINTF("Found SF20 legacy Lidar\n");
167 1 : return true;
168 : }
169 0 : DEV_PRINTF("SF20 not found\n");
170 0 : return false;
171 1 : }
172 :
173 : /*
174 : initialise lidar using legacy 16 bit protocol
175 : */
176 1 : bool AP_RangeFinder_LightWareI2C::legacy_init()
177 : {
178 : union {
179 : be16_t be16_val;
180 : uint8_t bytes[2];
181 : } timeout;
182 :
183 : // Retrieve lost signal timeout register
184 1 : const uint8_t read_reg = LIGHTWARE_LOST_SIGNAL_TIMEOUT_READ_REG;
185 1 : if (!dev.transfer(&read_reg, 1, timeout.bytes, 2)) {
186 0 : return false;
187 : }
188 :
189 : // Check lost signal timeout register against desired value and write it if it does not match
190 1 : if (be16toh(timeout.be16_val) != LIGHTWARE_TIMEOUT_REG_DESIRED_VALUE) {
191 1 : timeout.be16_val = htobe16(LIGHTWARE_TIMEOUT_REG_DESIRED_VALUE);
192 1 : const uint8_t send_buf[3] = {LIGHTWARE_LOST_SIGNAL_TIMEOUT_WRITE_REG, timeout.bytes[0], timeout.bytes[1]};
193 1 : if (!dev.transfer(send_buf, sizeof(send_buf), nullptr, 0)) {
194 0 : return false;
195 : }
196 : }
197 :
198 : // call timer() at 20Hz
199 1 : dev.register_periodic_callback(50000,
200 : FUNCTOR_BIND_MEMBER(&AP_RangeFinder_LightWareI2C::legacy_timer, void));
201 :
202 1 : return true;
203 : }
204 :
205 : /*
206 : initialise using newer text based protocol
207 : */
208 1 : bool AP_RangeFinder_LightWareI2C::sf20_init()
209 : {
210 : // version strings for reporting
211 1 : char version[15] {};
212 :
213 : // -1 here preserves null termination on the version string:
214 1 : sf20_get_version("?P\r\n", "p:", version, ARRAY_SIZE(version)-1);
215 :
216 1 : if (version[0]) {
217 0 : DEV_PRINTF("SF20 Lidar version %s\n", version);
218 : }
219 :
220 : // Makes sure that "address tagging" is turned off.
221 : // Address tagging starts every response with "0x66".
222 : // Turns off Address Tagging just in case it was previously left on in the non-volatile configuration.
223 1 : sf20_disable_address_tagging();
224 : // Disconnect the servo (if applicable)
225 1 : sf20_send_and_expect("#SC,0\r\n", "sc:0");
226 :
227 : // Change the power consumption:
228 : // 0 = power off
229 : // 1 = power on
230 : // As of 7/10/17 sw and fw version 1.0 the "#E,1" command does not seem to be supported.
231 : // When it is supported the expected response would be "e:1".
232 :
233 : // Changes the number of lost signal confirmations: 1 [1..250].
234 1 : if (!sf20_send_and_expect("#LC,20\r\n", "lc:20")) {
235 1 : return false;
236 : }
237 :
238 : #if 0
239 : // This location in the code may be uncommented to do a one time change of the devices address.
240 : // It should be commented out again and immediately reloaded to the pixhawk after the device has
241 : // been modified by this initialization process.
242 : // Address change to 0x65 = 101
243 : write_bytes((uint8_t*)"#CI,0x65\r\n",10);
244 : dev.set_address(0x65);
245 : uint8_t rx_bytes[lx20_max_reply_len_bytes + 1];
246 : sf20_wait_on_reply(rx_bytes);
247 : // Save the comm settings
248 : if (!sf20_send_and_expect("%C\r\n", "%c:")) {
249 : return false;
250 : }
251 : #endif
252 :
253 : #if 0
254 : /*
255 : this can be used to change the laser encoding pattern
256 : Changes the laser encoding pattern: 3 (Random A) [0..4].
257 : */
258 : if (!sf20_send_and_expect("#LE,3\r\n", "le:3")) {
259 : return false;
260 : }
261 : #endif
262 :
263 : // Sets datum offset [-10.00 ... 10.00].
264 0 : if (!sf20_send_and_expect("#LO,0.00\r\n", "lo:0.00")) {
265 0 : return false;
266 : }
267 :
268 : // Changes to a new measuring mode (update rate):
269 : // 1 = 388 readings per second
270 : // 2 = 194 readings per second
271 : // 3 = 129 readings per second
272 : // 4 = 97 readings per second
273 : // 5 = 78 readings per second
274 : // 6 = 65 readings per second
275 : // 7 = 55 readings per second
276 : // 8 = 48 readings per second
277 0 : if (!sf20_send_and_expect("#LM,7\r\n", "lm:7")) {
278 0 : return false;
279 : }
280 :
281 : // Changes the gain boost value:
282 : // Adjustment range = -20.00 ... 5.00
283 0 : if (!sf20_send_and_expect("#LB,0.00\r\n", "lb:0.00")) {
284 0 : return false;
285 : }
286 :
287 : // Switches distance streaming on or off:
288 : // 0 = off
289 : // 1 = on
290 0 : if (!sf20_send_and_expect("#SU,1\r\n", "su:1")) {
291 0 : return false;
292 : }
293 :
294 : // Changes the laser state:
295 : // 0 = laser is off
296 : // 1 = laser is running
297 0 : if (!sf20_send_and_expect("#LF,1\r\n", "lf:1")) {
298 0 : return false;
299 : }
300 :
301 : // Requests the measurement specified in the first stream.
302 0 : write_bytes((uint8_t*)init_stream_id[0], strlen(init_stream_id[0]));
303 :
304 : // call timer() at 20Hz
305 0 : dev.register_periodic_callback(50000,
306 : FUNCTOR_BIND_MEMBER(&AP_RangeFinder_LightWareI2C::sf20_timer, void));
307 :
308 0 : return true;
309 : }
310 :
311 : // read - return last value measured by sensor
312 1398 : bool AP_RangeFinder_LightWareI2C::legacy_get_reading(float &reading_m)
313 : {
314 : be16_t val;
315 :
316 1398 : const uint8_t read_reg = LIGHTWARE_DISTANCE_READ_REG;
317 :
318 : // read the high and low byte distance registers
319 1398 : if (dev.transfer(&read_reg, 1, (uint8_t *)&val, sizeof(val))) {
320 1398 : int16_t signed_val = int16_t(be16toh(val));
321 1398 : if (signed_val < 0) {
322 : // some lidar firmwares will return 65436 for out of range
323 0 : reading_m = max_distance() + LIGHTWARE_OUT_OF_RANGE_ADD;
324 : } else {
325 1398 : reading_m = uint16_t(signed_val) * 0.01f;
326 : }
327 1398 : return true;
328 : }
329 0 : return false;
330 : }
331 :
332 : // read - return last value measured by sf20 sensor
333 0 : bool AP_RangeFinder_LightWareI2C::sf20_get_reading(float &reading_m)
334 : {
335 : // Parses up to 5 ASCII streams for LiDAR data.
336 : // If a parse fails, the stream measurement is not updated until it is successfully read in the future.
337 : uint8_t stream[lx20_max_expected_stream_reply_len_bytes+1]; // Maximum response length for a stream ie "ldf,0:40.99" is 11 characters
338 :
339 0 : uint8_t i = streamSequence[currentStreamSequenceIndex];
340 0 : size_t num_processed_chars = 0;
341 :
342 : /* Reads the LiDAR value requested during the last interrupt. */
343 0 : if (!dev.read(stream, sizeof(stream))) {
344 0 : return false;
345 : }
346 0 : stream[lx20_max_expected_stream_reply_len_bytes] = 0;
347 :
348 0 : if (!sf20_parse_stream(stream, &num_processed_chars, parse_stream_id[i], sf20_stream_val[i])) {
349 0 : return false;
350 : }
351 :
352 0 : if (i==0) {
353 0 : reading_m = sf20_stream_val[0];
354 : }
355 :
356 : // Increment the stream sequence
357 0 : currentStreamSequenceIndex++;
358 0 : if (currentStreamSequenceIndex >= numStreamSequenceIndexes) {
359 0 : currentStreamSequenceIndex = 0;
360 : }
361 0 : i = streamSequence[currentStreamSequenceIndex];
362 :
363 : // Request the next stream in the sequence from the SF20
364 0 : write_bytes((uint8_t*)init_stream_id[i], strlen(init_stream_id[i]));
365 :
366 0 : return true;
367 : }
368 :
369 0 : bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf,
370 : size_t *p_num_processed_chars,
371 : const char *string_identifier,
372 : float &val)
373 : {
374 0 : size_t string_identifier_len = strlen(string_identifier);
375 0 : for (uint32_t i = 0 ; i < string_identifier_len ; i++) {
376 0 : if (stream_buf[*p_num_processed_chars] != string_identifier[i]) {
377 0 : return false;
378 : }
379 0 : (*p_num_processed_chars)++;
380 : }
381 :
382 : /*
383 : special case for being beyond maximum range, we receive a message like this:
384 : ldl,1:-1.00
385 : we will return max distance
386 : */
387 0 : if (strncmp((const char *)&stream_buf[*p_num_processed_chars], "-1.00", 5) == 0) {
388 0 : val = max_distance() + LIGHTWARE_OUT_OF_RANGE_ADD;
389 0 : (*p_num_processed_chars) += 5;
390 0 : return true;
391 : }
392 :
393 : /* Number is always returned in hundredths. So 6.33 is returned as 633. 6.3 is returned as 630.
394 : * 6 is returned as 600.
395 : * Extract number in format 6.33 or 123.99 (meters to be converted to centimeters).
396 : * Percentages such as 100 (percent), are returned as 10000.
397 : */
398 0 : uint32_t final_multiplier = 100;
399 0 : bool decrement_multiplier = false;
400 0 : bool number_found = false;
401 0 : uint16_t accumulator = 0;
402 0 : uint16_t digit_u16 = (uint16_t)stream_buf[*p_num_processed_chars];
403 0 : while ((((digit_u16 <= '9') &&
404 0 : (digit_u16 >= '0')) ||
405 0 : (digit_u16 == '.')) &&
406 0 : (*p_num_processed_chars < lx20_max_reply_len_bytes)) {
407 0 : (*p_num_processed_chars)++;
408 0 : if (decrement_multiplier) {
409 0 : final_multiplier /=10;
410 : }
411 0 : if (digit_u16 == '.') {
412 0 : decrement_multiplier = true;
413 0 : digit_u16 = (uint16_t)stream_buf[*p_num_processed_chars];
414 0 : continue;
415 : }
416 0 : number_found = true;
417 0 : accumulator *= 10;
418 0 : accumulator += digit_u16 - '0';
419 0 : digit_u16 = (uint16_t)stream_buf[*p_num_processed_chars];
420 : }
421 :
422 0 : accumulator *= final_multiplier;
423 0 : val = accumulator * 0.01;
424 0 : return number_found;
425 : }
426 :
427 : /*
428 : update the state of the sensor
429 : */
430 1344 : void AP_RangeFinder_LightWareI2C::update(void)
431 : {
432 : // nothing to do - its all done in the timer()
433 1344 : }
434 :
435 1398 : void AP_RangeFinder_LightWareI2C::legacy_timer(void)
436 : {
437 1398 : if (legacy_get_reading(state.distance_m)) {
438 : // update range_valid state based on distance measured
439 1398 : update_status();
440 1398 : state.last_reading_ms = AP_HAL::millis();
441 : } else {
442 0 : set_status(RangeFinder::Status::NoData);
443 : }
444 1398 : }
445 :
446 0 : void AP_RangeFinder_LightWareI2C::sf20_timer(void)
447 : {
448 0 : if (sf20_get_reading(state.distance_m)) {
449 : // update range_valid state based on distance measured
450 0 : update_status();
451 0 : state.last_reading_ms = AP_HAL::millis();
452 : } else {
453 0 : set_status(RangeFinder::Status::NoData);
454 : }
455 0 : }
456 :
457 : // Only for use during init as this blocks while waiting for the SF20 to be ready.
458 0 : bool AP_RangeFinder_LightWareI2C::sf20_wait_on_reply(uint8_t *rx_two_byte)
459 : {
460 : // Waits for a non-zero first byte while repeatedly reading 16 bits.
461 : // This is used after a read command to allow the sf20 time to provide the result.
462 0 : uint32_t start_time_ms = AP_HAL::millis();
463 0 : const uint32_t max_wait_time_ms = 50;
464 :
465 0 : while (AP_HAL::millis() - start_time_ms < max_wait_time_ms) {
466 0 : if (!dev.read(rx_two_byte, 2)) {
467 0 : hal.scheduler->delay(1);
468 0 : continue;
469 : }
470 0 : if (rx_two_byte[0] != 0) {
471 : // normal exit
472 0 : return true;
473 : }
474 : }
475 0 : return false;
476 : }
477 :
478 : #endif // AP_RANGEFINDER_LWI2C_ENABLED
|
