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 : Simulator for the RPLidar proximity sensors
17 : */
18 :
19 : #include "SIM_PS_RPLidar.h"
20 :
21 : #if AP_SIM_PS_RPLIDARA2_ENABLED || AP_SIM_PS_RPLIDARA1_ENABLED || AP_SIM_PS_RPLIDARS2_ENABLED
22 :
23 : #include <GCS_MAVLink/GCS.h>
24 : #include <stdio.h>
25 : #include <errno.h>
26 : #include <math.h>
27 :
28 : using namespace SITL;
29 :
30 0 : uint32_t PS_RPLidar::packet_for_location(const Location &location,
31 : uint8_t *data,
32 : uint8_t buflen)
33 : {
34 0 : return 0;
35 : }
36 :
37 158677 : void PS_RPLidar::move_preamble_in_buffer()
38 : {
39 : uint8_t i;
40 158677 : for (i=0; i<_buflen; i++) {
41 6 : if ((uint8_t)_buffer[i] == PREAMBLE) {
42 6 : break;
43 : }
44 : }
45 158677 : if (i == 0) {
46 158677 : return;
47 : }
48 0 : memmove(_buffer, &_buffer[i], _buflen-i);
49 0 : _buflen = _buflen - i;
50 : }
51 :
52 158677 : void PS_RPLidar::update_input()
53 : {
54 158677 : const ssize_t n = read_from_autopilot(&_buffer[_buflen], ARRAY_SIZE(_buffer) - _buflen - 1);
55 158677 : if (n < 0) {
56 : // TODO: do better here
57 0 : if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
58 0 : AP_HAL::panic("Failed to read from autopilot");
59 : }
60 : } else {
61 158677 : _buflen += n;
62 : }
63 :
64 158677 : switch (_inputstate) {
65 158677 : case InputState::WAITING_FOR_PREAMBLE:
66 158677 : move_preamble_in_buffer();
67 158677 : if (_buflen == 0) {
68 158671 : return;
69 : }
70 6 : set_inputstate(InputState::GOT_PREAMBLE);
71 : // consume the preamble:
72 6 : memmove(_buffer, &_buffer[1], _buflen-1);
73 6 : _buflen--;
74 : FALLTHROUGH;
75 6 : case InputState::GOT_PREAMBLE:
76 6 : if (_buflen == 0) {
77 0 : return;
78 : }
79 6 : switch ((Command)_buffer[0]) {
80 0 : case Command::STOP:
81 : // consume the command:
82 0 : memmove(_buffer, &_buffer[1], _buflen-1);
83 0 : _buflen--;
84 0 : set_inputstate(InputState::WAITING_FOR_PREAMBLE);
85 0 : set_state(State::IDLE);
86 0 : _scan_mode = ScanMode::SCAN;
87 0 : return;
88 2 : case Command::SCAN:
89 : // 5-byte scan mode
90 2 : memmove(_buffer, &_buffer[1], _buflen-1);
91 2 : _buflen--;
92 2 : send_response_descriptor(0x05, SendMode::SRMR, DataType::Unknown81);
93 2 : set_inputstate(InputState::WAITING_FOR_PREAMBLE);
94 2 : set_state(State::SCANNING);
95 2 : _scan_mode = ScanMode::SCAN;
96 2 : last_scan_output_time_ms = 0;
97 2 : last_degrees_bf = 0.0f;
98 2 : return;
99 1 : case Command::EXPRESS_SCAN:
100 : // Dense Express mode (40 samples / packet)
101 : //
102 : // Request is: A5 82 <payload_size=0x05> <5-byte payload> <checksum>
103 : // Descriptor: A5 5A 54 00 00 40 85 (len=0x54=84 bytes, type=0x85)
104 1 : memmove(_buffer, &_buffer[1], _buflen-1);
105 1 : _buflen--;
106 1 : if (_buflen >= 1) {
107 1 : const uint8_t payload_size = (uint8_t)_buffer[0];
108 1 : const uint8_t total_to_discard = 1 + payload_size + 1; // size + payload + checksum
109 1 : if (_buflen >= total_to_discard) {
110 1 : memmove(_buffer, &_buffer[total_to_discard], _buflen - total_to_discard);
111 1 : _buflen -= total_to_discard;
112 : } else {
113 0 : _buflen = 0;
114 : }
115 : } else {
116 0 : _buflen = 0;
117 : }
118 1 : send_response_descriptor(0x54, SendMode::SRMR, DataType::Unknown85);
119 1 : set_inputstate(InputState::WAITING_FOR_PREAMBLE);
120 1 : set_state(State::SCANNING);
121 1 : _scan_mode = ScanMode::EXPRESS_SCAN_DENSE;
122 1 : _express_w_i_deg = 0.0f;
123 1 : last_scan_output_time_ms = 0;
124 1 : return;
125 0 : case Command::GET_HEALTH: {
126 : // consume the command:
127 0 : memmove(_buffer, &_buffer[1], _buflen-1);
128 0 : _buflen--;
129 0 : send_response_descriptor(0x03, SendMode::SRSR, DataType::Unknown06);
130 : // now send the health:
131 0 : const uint8_t health[3] {}; // all zeros fine for now
132 0 : const ssize_t ret = write_to_autopilot((const char*)health, ARRAY_SIZE(health));
133 0 : if (ret != ARRAY_SIZE(health)) {
134 0 : abort();
135 : }
136 0 : set_inputstate(InputState::WAITING_FOR_PREAMBLE);
137 0 : return;
138 : }
139 3 : case Command::GET_DEVICE_INFO: {
140 : // consume the command:
141 3 : memmove(_buffer, &_buffer[1], _buflen-1);
142 3 : _buflen--;
143 3 : send_response_descriptor(0x14, SendMode::SRSR, DataType::Unknown04);
144 : // now send the device info:
145 : struct PACKED _device_info {
146 : uint8_t model;
147 : uint8_t firmware_minor;
148 : uint8_t firmware_major;
149 : uint8_t hardware;
150 : uint8_t serial[16];
151 : } device_info;
152 3 : device_info.model = device_info_model();
153 3 : device_info.firmware_minor = 17;
154 3 : device_info.firmware_major = 42;
155 3 : device_info.hardware = 6;
156 3 : const ssize_t ret = write_to_autopilot((const char*)&device_info, sizeof(device_info));
157 3 : if (ret != sizeof(device_info)) {
158 0 : abort();
159 : }
160 3 : set_inputstate(InputState::WAITING_FOR_PREAMBLE);
161 3 : return;
162 : }
163 0 : case Command::FORCE_SCAN:
164 0 : abort();
165 0 : case Command::RESET:
166 : // consume the command:
167 0 : memmove(_buffer, &_buffer[1], _buflen-1);
168 0 : _buflen--;
169 0 : set_inputstate(InputState::RESETTING_START);
170 0 : return;
171 0 : default:
172 0 : const uint8_t bad = static_cast<uint8_t>(_buffer[0]);
173 0 : ::fprintf(stderr, "SIM_RPLidar: unknown command 0x%02x, ignoring\n", bad);
174 : // consume this byte and resync:
175 0 : memmove(_buffer, &_buffer[1], _buflen-1);
176 0 : _buflen--;
177 0 : set_inputstate(InputState::WAITING_FOR_PREAMBLE);
178 0 : return;
179 : }
180 0 : case InputState::RESETTING_START:
181 0 : _firmware_info_offset = 0;
182 0 : set_inputstate(InputState::RESETTING_SEND_FIRMWARE_INFO);
183 : FALLTHROUGH;
184 0 : case InputState::RESETTING_SEND_FIRMWARE_INFO: {
185 0 : const ssize_t written = write_to_autopilot(&FIRMWARE_INFO[_firmware_info_offset], strlen(FIRMWARE_INFO) - _firmware_info_offset);
186 0 : if (written <= 0) {
187 0 : AP_HAL::panic("Failed to write to autopilot");
188 : }
189 0 : _firmware_info_offset += written;
190 0 : if (_firmware_info_offset < strlen(FIRMWARE_INFO)) {
191 0 : return;
192 : }
193 0 : set_inputstate(InputState::WAITING_FOR_PREAMBLE);
194 0 : return;
195 : }
196 : }
197 : }
198 :
199 99027 : void PS_RPLidar::update_output_scan(const Location &location)
200 : {
201 99027 : const uint32_t now = AP_HAL::millis();
202 99027 : if (last_scan_output_time_ms == 0) {
203 2 : last_scan_output_time_ms = now;
204 2 : return;
205 : }
206 99025 : const uint32_t time_delta = (now - last_scan_output_time_ms);
207 :
208 99025 : const uint32_t samples_per_second = 1000;
209 99025 : const float samples_per_ms = samples_per_second / 1000.0f;
210 99025 : const uint32_t sample_count = time_delta / samples_per_ms;
211 99025 : const float degrees_per_ms = 3600 / 1000.0f;
212 99025 : const float degrees_per_sample = degrees_per_ms / samples_per_ms;
213 :
214 : // ::fprintf(stderr, "Packing %u samples in for %ums interval (%f degrees/sample)\n", sample_count, time_delta, degrees_per_sample);
215 :
216 99025 : last_scan_output_time_ms += sample_count/samples_per_ms;
217 :
218 181513 : for (uint32_t i=0; i<sample_count; i++) {
219 82488 : const float current_degrees_bf = fmodf((last_degrees_bf + degrees_per_sample), 360.0f);
220 82488 : const uint8_t quality = 17; // random number
221 82488 : const uint16_t angle_q6 = current_degrees_bf * 64;
222 82488 : const bool is_start_packet = current_degrees_bf < last_degrees_bf;
223 82488 : last_degrees_bf = current_degrees_bf;
224 :
225 :
226 82488 : float distance = measure_distance_at_angle_bf(location, current_degrees_bf);
227 : // ::fprintf(stderr, "SIM: %f=%fm\n", current_degrees_bf, distance);
228 82488 : if (distance > max_range()) {
229 : // sensor returns zero for out-of-range
230 34416 : distance = 0.0f;
231 : }
232 82488 : const uint16_t distance_q2 = (distance*1000 * 4); // m->mm and *4
233 :
234 : struct PACKED {
235 : uint8_t startbit : 1; ///< on the first revolution 1 else 0
236 : uint8_t not_startbit : 1; ///< complementary to startbit
237 : uint8_t quality : 6; ///< Related the reflected laser pulse strength
238 : uint8_t checkbit : 1; ///< always set to 1
239 : uint16_t angle_q6 : 15; ///< Actual heading = angle_q6/64.0 Degree
240 : uint16_t distance_q2 : 16; ///< Actual Distance = distance_q2/4.0 mm
241 : } send_buffer;
242 82488 : send_buffer.startbit = is_start_packet;
243 82488 : send_buffer.not_startbit = !is_start_packet;
244 82488 : send_buffer.quality = quality;
245 82488 : send_buffer.checkbit = 1;
246 82488 : send_buffer.angle_q6 = angle_q6;
247 82488 : send_buffer.distance_q2 = distance_q2;
248 :
249 : static_assert(sizeof(send_buffer) == 5, "send_buffer correct size");
250 :
251 82488 : const ssize_t ret = write_to_autopilot((const char*)&send_buffer, sizeof(send_buffer));
252 82488 : if (ret != sizeof(send_buffer)) {
253 0 : abort();
254 : }
255 : }
256 : }
257 :
258 48808 : void PS_RPLidar::update_output_express_dense(const Location &location)
259 : {
260 48808 : const uint32_t now = AP_HAL::millis();
261 48808 : if (last_scan_output_time_ms == 0) {
262 1 : last_scan_output_time_ms = now;
263 1 : return;
264 : }
265 :
266 : // Number of dense packets per second (tunable).
267 : // 80 packets/s -> 3200 samples/s (40 samples/packet).
268 48807 : const uint32_t packets_per_second = 80;
269 48807 : const uint32_t packet_period_ms = 1000 / packets_per_second;
270 :
271 48807 : const uint32_t time_delta = (now - last_scan_output_time_ms);
272 48807 : const uint32_t packet_count = time_delta / packet_period_ms;
273 :
274 48807 : if (packet_count == 0) {
275 45419 : return;
276 : }
277 :
278 3388 : last_scan_output_time_ms += packet_count * packet_period_ms;
279 :
280 : // 0.1125 degree per sample -> 4.5 degrees per packet
281 3388 : const float sample_delta_deg = 0.1125f;
282 3388 : const float packet_delta_deg = 40.0f * sample_delta_deg;
283 :
284 6776 : for (uint32_t p = 0; p < packet_count; p++) {
285 3388 : uint8_t packet[84] {};
286 : // Start angle in degrees for this packet
287 3388 : const float w_i_deg = _express_w_i_deg;
288 :
289 : // start_angle_q6 is in 1/64 degrees
290 3388 : const uint16_t start_angle_q6 = (uint16_t)lrintf(w_i_deg * 64.0f);
291 :
292 3388 : packet[2] = start_angle_q6 & 0xFF;
293 3388 : packet[3] = (start_angle_q6 >> 8) & 0xFF;
294 :
295 : // cabins: 40 distances (2 bytes each, little-endian)
296 3388 : uint8_t* cab = &packet[4];
297 138908 : for (uint32_t k = 0; k < 40; k++) {
298 : // angle of sample k
299 135520 : float angle_deg = w_i_deg + sample_delta_deg * k;
300 : // wrap to [0, 360)
301 135520 : if (angle_deg >= 360.0f) {
302 0 : angle_deg = fmodf(angle_deg, 360.0f);
303 : }
304 :
305 135520 : float distance = measure_distance_at_angle_bf(location, angle_deg);
306 135520 : if (distance > max_range()) {
307 22798 : distance = 0.0f;
308 : }
309 :
310 135520 : const uint16_t dist_mm = (uint16_t)lrintf(distance * 1000.0f);
311 135520 : cab[0] = dist_mm & 0xFF;
312 135520 : cab[1] = (dist_mm >> 8) & 0xFF;
313 135520 : cab += 2;
314 : }
315 :
316 : // checksum: XOR of bytes [2..83]
317 3388 : uint8_t checksum = 0;
318 281204 : for (uint32_t i = 2; i < sizeof(packet); i++) {
319 277816 : checksum ^= packet[i];
320 : }
321 :
322 : // sync1/sync2 and checksum nibbles (dense format)
323 3388 : const uint8_t sync1 = 0x0A;
324 3388 : const uint8_t sync2 = 0x05;
325 :
326 3388 : packet[0] = uint8_t((sync1 << 4) | (checksum & 0x0F));
327 3388 : packet[1] = uint8_t((sync2 << 4) | ((checksum >> 4) & 0x0F));
328 :
329 3388 : const ssize_t ret = write_to_autopilot((const char*)packet, sizeof(packet));
330 3388 : if (ret != (ssize_t)sizeof(packet)) {
331 0 : abort();
332 : }
333 :
334 : // advance start angle for next packet
335 3388 : _express_w_i_deg += packet_delta_deg;
336 3388 : if (_express_w_i_deg >= 360.0f) {
337 42 : _express_w_i_deg = fmodf(_express_w_i_deg, 360.0f);
338 : }
339 : }
340 : }
341 :
342 158677 : void PS_RPLidar::update_output(const Location &location)
343 : {
344 158677 : switch (_state) {
345 10842 : case State::IDLE:
346 10842 : return;
347 147835 : case State::SCANNING:
348 147835 : if (_scan_mode == ScanMode::SCAN) {
349 99027 : update_output_scan(location);
350 : } else {
351 48808 : update_output_express_dense(location);
352 : }
353 147835 : return;
354 : }
355 : }
356 :
357 158677 : void PS_RPLidar::update(const Location &location)
358 : {
359 158677 : update_input();
360 158677 : update_output(location);
361 158677 : }
362 :
363 :
364 6 : void PS_RPLidar::send_response_descriptor(uint32_t data_response_length, SendMode sendmode, DataType datatype)
365 : {
366 6 : const uint8_t send_buffer[] = {
367 : 0xA5,
368 : 0x5A,
369 : uint8_t((data_response_length >> 0) & 0xff),
370 6 : uint8_t((data_response_length >> 8) & 0xff),
371 6 : uint8_t((data_response_length >> 16) & 0xff),
372 6 : uint8_t(((data_response_length >> 24) & 0xff) | (uint8_t) sendmode),
373 : (uint8_t)datatype
374 6 : };
375 : static_assert(ARRAY_SIZE(send_buffer) == 7, "send_buffer correct size");
376 :
377 6 : const ssize_t ret = write_to_autopilot((const char*)send_buffer, ARRAY_SIZE(send_buffer));
378 6 : if (ret != ARRAY_SIZE(send_buffer)) {
379 0 : abort();
380 : }
381 6 : }
382 :
383 : #endif // AP_SIM_PS_RPLIDARA2_ENABLED || AP_SIM_PS_RPLIDARA1_ENABLED || AP_SIM_PS_RPLIDARS2_ENABLED
|
