forked from ArduPilot/ardupilot
/
AP_RangeFinder_Benewake.cpp
176 lines (161 loc) · 6.81 KB
/
AP_RangeFinder_Benewake.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_RangeFinder_Benewake.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <ctype.h>
#include <AP_HAL/utility/sparse-endian.h>
extern const AP_HAL::HAL& hal;
#define BENEWAKE_FRAME_HEADER 0x59
#define BENEWAKE_FRAME_LENGTH 9
#define BENEWAKE_DIST_MAX_CM 32768
#define BENEWAKE_TFMINI_OUT_OF_RANGE_CM 1200
#define BENEWAKE_TF02_OUT_OF_RANGE_CM 2200
#define BENEWAKE_OUT_OF_RANGE_ADD_CM 100
// format of serial packets received from benewake lidar
//
// Data Bit Definition Description
// ------------------------------------------------
// byte 0 Frame header 0x59
// byte 1 Frame header 0x59
// byte 2 DIST_L Distance (in cm) low 8 bits
// byte 3 DIST_H Distance (in cm) high 8 bits
// byte 4 STRENGTH_L Strength low 8 bits
// byte 5 STRENGTH_H Strength high 8 bits
// byte 6 (TF02) SIG Reliability in 8 levels, 7 & 8 means reliable
// byte 6 (TFmini) Distance Mode 0x02 for short distance (mm), 0x07 for long distance (cm)
// byte 7 (TF02 only) TIME Exposure time in two levels 0x03 and 0x06
// byte 8 Checksum Checksum byte, sum of bytes 0 to bytes 7
/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_Benewake::AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance,
benewake_model_type model) :
AP_RangeFinder_Backend(_state, _params),
model_type(model)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance));
}
}
/*
detect if a Benewake rangefinder is connected. We'll detect by
trying to take a reading on Serial. If we get a result the sensor is
there.
*/
bool AP_RangeFinder_Benewake::detect(AP_SerialManager &serial_manager, uint8_t serial_instance)
{
return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr;
}
// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
{
if (uart == nullptr) {
return false;
}
float sum_cm = 0;
uint16_t count = 0;
uint16_t count_out_of_range = 0;
// read any available lines from the lidar
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
int16_t r = uart->read();
if (r < 0) {
continue;
}
uint8_t c = (uint8_t)r;
// if buffer is empty and this byte is 0x59, add to buffer
if (linebuf_len == 0) {
if (c == BENEWAKE_FRAME_HEADER) {
linebuf[linebuf_len++] = c;
}
} else if (linebuf_len == 1) {
// if buffer has 1 element and this byte is 0x59, add it to buffer
// if not clear the buffer
if (c == BENEWAKE_FRAME_HEADER) {
linebuf[linebuf_len++] = c;
} else {
linebuf_len = 0;
}
} else {
// add character to buffer
linebuf[linebuf_len++] = c;
// if buffer now has 9 items try to decode it
if (linebuf_len == BENEWAKE_FRAME_LENGTH) {
// calculate checksum
uint8_t checksum = 0;
for (uint8_t i=0; i<BENEWAKE_FRAME_LENGTH-1; i++) {
checksum += linebuf[i];
}
// if checksum matches extract contents
if (checksum == linebuf[BENEWAKE_FRAME_LENGTH-1]) {
// calculate distance
uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2];
if (dist >= BENEWAKE_DIST_MAX_CM) {
// this reading is out of range
count_out_of_range++;
} else if (model_type == BENEWAKE_TFmini) {
// no signal byte from TFmini so add distance to sum
sum_cm += dist;
count++;
} else {
// TF02 provides signal reliability (good = 7 or 8)
if (linebuf[6] >= 7) {
// add distance to sum
sum_cm += dist;
count++;
} else {
// this reading is out of range
count_out_of_range++;
}
}
}
// clear buffer
linebuf_len = 0;
}
}
}
if (count > 0) {
// return average distance of readings
reading_cm = sum_cm / count;
return true;
}
if (count_out_of_range > 0) {
// if only out of range readings return larger of
// driver defined maximum range for the model and user defined max range + 1m
float model_dist_max_cm = (model_type == BENEWAKE_TFmini) ? BENEWAKE_TFMINI_OUT_OF_RANGE_CM : BENEWAKE_TF02_OUT_OF_RANGE_CM;
reading_cm = MAX(model_dist_max_cm, max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM);
return true;
}
// no readings so return false
return false;
}
/*
update the state of the sensor
*/
void AP_RangeFinder_Benewake::update(void)
{
if (get_reading(state.distance_cm)) {
// update range_valid state based on distance measured
state.last_reading_ms = AP_HAL::millis();
update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData);
}
}