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 connector for morse simulator
17 : */
18 :
19 : #include "SIM_Morse.h"
20 :
21 : #if AP_SIM_MORSE_ENABLED
22 :
23 : #include <arpa/inet.h>
24 : #include <errno.h>
25 : #include <fcntl.h>
26 : #include <stdio.h>
27 : #include <stdarg.h>
28 : #include <sys/stat.h>
29 : #include <sys/types.h>
30 :
31 : #include <AP_HAL/AP_HAL.h>
32 : #include "pthread.h"
33 : #include <AP_HAL/utility/replace.h>
34 :
35 : extern const AP_HAL::HAL& hal;
36 :
37 : using namespace SITL;
38 :
39 : static const struct {
40 : const char *name;
41 : float value;
42 : bool save;
43 : } sim_defaults[] = {
44 : { "AHRS_EKF_TYPE", 10 },
45 : { "INS_GYR_CAL", 0 },
46 : { "RC1_MIN", 1000, true },
47 : { "RC1_MAX", 2000, true },
48 : { "RC2_MIN", 1000, true },
49 : { "RC2_MAX", 2000, true },
50 : { "RC3_MIN", 1000, true },
51 : { "RC3_MAX", 2000, true },
52 : { "RC4_MIN", 1000, true },
53 : { "RC4_MAX", 2000, true },
54 : { "RC2_REVERSED", 1 }, // interlink has reversed rc2
55 : { "SERVO1_MIN", 1000 },
56 : { "SERVO1_MAX", 2000 },
57 : { "SERVO2_MIN", 1000 },
58 : { "SERVO2_MAX", 2000 },
59 : { "SERVO3_MIN", 1000 },
60 : { "SERVO3_MAX", 2000 },
61 : { "SERVO4_MIN", 1000 },
62 : { "SERVO4_MAX", 2000 },
63 : { "SERVO5_MIN", 1000 },
64 : { "SERVO5_MAX", 2000 },
65 : { "SERVO6_MIN", 1000 },
66 : { "SERVO6_MAX", 2000 },
67 : { "SERVO6_MIN", 1000 },
68 : { "SERVO6_MAX", 2000 },
69 : { "INS_ACC2OFFS_X", 0.001 },
70 : { "INS_ACC2OFFS_Y", 0.001 },
71 : { "INS_ACC2OFFS_Z", 0.001 },
72 : { "INS_ACC2SCAL_X", 1.001 },
73 : { "INS_ACC2SCAL_Y", 1.001 },
74 : { "INS_ACC2SCAL_Z", 1.001 },
75 : { "INS_ACCOFFS_X", 0.001 },
76 : { "INS_ACCOFFS_Y", 0.001 },
77 : { "INS_ACCOFFS_Z", 0.001 },
78 : { "INS_ACCSCAL_X", 1.001 },
79 : { "INS_ACCSCAL_Y", 1.001 },
80 : { "INS_ACCSCAL_Z", 1.001 },
81 : };
82 :
83 :
84 0 : Morse::Morse(const char *frame_str) :
85 0 : Aircraft(frame_str)
86 : {
87 0 : char *saveptr = nullptr;
88 0 : char *s = strdup(frame_str);
89 0 : char *frame_option = strtok_r(s, ":", &saveptr);
90 0 : char *args1 = strtok_r(nullptr, ":", &saveptr);
91 0 : char *args2 = strtok_r(nullptr, ":", &saveptr);
92 0 : char *args3 = strtok_r(nullptr, ":", &saveptr);
93 : /*
94 : allow setting of IP, sensors port and control port
95 : format morse:IPADDRESS:SENSORS_PORT:CONTROL_PORT
96 : */
97 0 : if (args1) {
98 0 : morse_ip = args1;
99 : }
100 0 : if (args2) {
101 0 : morse_sensors_port = atoi(args2);
102 0 : morse_control_port = morse_sensors_port+1;
103 : }
104 0 : if (args3) {
105 0 : morse_control_port = atoi(args3);
106 : }
107 :
108 0 : if (strstr(frame_option, "-rover")) {
109 0 : output_type = OUTPUT_ROVER_REGULAR;
110 0 : } else if (strstr(frame_option, "-skid")) {
111 0 : output_type = OUTPUT_ROVER_SKID;
112 0 : } else if (strstr(frame_option, "-quad")) {
113 0 : output_type = OUTPUT_QUAD;
114 0 : } else if (strstr(frame_option, "-pwm")) {
115 0 : output_type = OUTPUT_PWM;
116 : } else {
117 : // default to rover
118 0 : output_type = OUTPUT_ROVER_REGULAR;
119 : }
120 :
121 0 : for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {
122 0 : AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
123 0 : if (sim_defaults[i].save) {
124 : enum ap_var_type ptype;
125 0 : AP_Param *p = AP_Param::find(sim_defaults[i].name, &ptype);
126 0 : if (!p->configured()) {
127 0 : p->save();
128 : }
129 : }
130 : }
131 0 : printf("Started Morse with %s:%u:%u type %u\n",
132 0 : morse_ip, morse_sensors_port, morse_control_port,
133 0 : (unsigned)output_type);
134 0 : }
135 :
136 : /*
137 : very simple JSON parser for sensor data
138 : called with pointer to one row of sensor data, nul terminated
139 :
140 : This parser does not do any syntax checking, and is not at all
141 : general purpose
142 : */
143 0 : bool Morse::parse_sensors(const char *json)
144 : {
145 : //printf("%s\n", json);
146 0 : for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
147 0 : struct keytable &key = keytable[i];
148 :
149 : /* look for section header */
150 0 : const char *p = strstr(json, key.section);
151 0 : if (!p) {
152 : // we don't have this sensor
153 0 : continue;
154 : }
155 0 : p += strlen(key.section)+1;
156 :
157 : // find key inside section
158 0 : p = strstr(p, key.key);
159 0 : if (!p) {
160 0 : printf("Failed to find key %s/%s\n", key.section, key.key);
161 0 : return false;
162 : }
163 :
164 0 : p += strlen(key.key)+3;
165 0 : switch (key.type) {
166 0 : case DATA_FLOAT:
167 0 : *((float *)key.ptr) = strtof(p, nullptr);
168 0 : break;
169 :
170 0 : case DATA_DOUBLE:
171 0 : *((double *)key.ptr) = atof(p);
172 0 : break;
173 :
174 0 : case DATA_VECTOR3F: {
175 0 : Vector3f *v = (Vector3f *)key.ptr;
176 0 : if (sscanf(p, "[%f, %f, %f]", &v->x, &v->y, &v->z) != 3) {
177 0 : printf("Failed to parse Vector3f for %s/%s\n", key.section, key.key);
178 0 : return false;
179 : }
180 0 : break;
181 : }
182 :
183 0 : case DATA_VECTOR3F_ARRAY: {
184 : // example: [[0.0, 0.0, 0.0], [-8.97607135772705, -8.976069450378418, -8.642673492431641e-07]]
185 0 : if (*p++ != '[') {
186 0 : return false;
187 : }
188 0 : uint16_t n = 0;
189 0 : struct vector3f_array *v = (struct vector3f_array *)key.ptr;
190 : while (true) {
191 0 : if (n >= v->length) {
192 0 : Vector3f *d = (Vector3f *)realloc(v->data, sizeof(Vector3f)*(n+1));
193 0 : if (d == nullptr) {
194 0 : return false;
195 : }
196 0 : v->data = d;
197 0 : v->length = n+1;
198 : }
199 0 : if (sscanf(p, "[%f, %f, %f]", &v->data[n].x, &v->data[n].y, &v->data[n].z) != 3) {
200 0 : printf("Failed to parse Vector3f for %s/%s[%u]\n", key.section, key.key, n);
201 0 : return false;
202 : }
203 0 : n++;
204 0 : p = strchr(p,']');
205 0 : if (!p) {
206 0 : return false;
207 : }
208 0 : p++;
209 0 : if (p[0] != ',') {
210 0 : break;
211 : }
212 0 : if (p[1] != ' ') {
213 0 : return false;
214 : }
215 0 : p += 2;
216 0 : }
217 0 : if (p[0] != ']') {
218 0 : return false;
219 : }
220 0 : v->length = n;
221 0 : break;
222 : }
223 :
224 0 : case DATA_FLOAT_ARRAY: {
225 : // example: [18.0, 12.694079399108887]
226 0 : if (*p++ != '[') {
227 0 : return false;
228 : }
229 0 : uint16_t n = 0;
230 0 : struct float_array *v = (struct float_array *)key.ptr;
231 : while (true) {
232 0 : if (n >= v->length) {
233 0 : float *d = (float *)realloc(v->data, sizeof(float)*(n+1));
234 0 : if (d == nullptr) {
235 0 : return false;
236 : }
237 0 : v->data = d;
238 0 : v->length = n+1;
239 : }
240 0 : v->data[n] = strtof(p, nullptr);
241 0 : n++;
242 0 : p = strchr(p,',');
243 0 : if (!p) {
244 0 : break;
245 : }
246 0 : p++;
247 0 : }
248 0 : v->length = n;
249 0 : break;
250 : }
251 : }
252 : }
253 0 : socket_frame_counter++;
254 0 : return true;
255 : }
256 :
257 : /*
258 : connect to the required sockets
259 : */
260 0 : bool Morse::connect_sockets(void)
261 : {
262 0 : if (!sensors_sock) {
263 0 : sensors_sock = NEW_NOTHROW SocketAPM_native(false);
264 0 : if (!sensors_sock) {
265 0 : AP_HAL::panic("Out of memory for sensors socket");
266 : }
267 0 : if (!sensors_sock->connect(morse_ip, morse_sensors_port)) {
268 0 : usleep(100000);
269 0 : if (connect_counter++ == 20) {
270 0 : printf("Waiting to connect to sensors control on %s:%u\n",
271 0 : morse_ip, morse_sensors_port);
272 0 : connect_counter = 0;
273 : }
274 0 : delete sensors_sock;
275 0 : sensors_sock = nullptr;
276 0 : return false;
277 : }
278 0 : sensors_sock->reuseaddress();
279 0 : printf("Sensors connected\n");
280 : }
281 0 : if (!control_sock) {
282 0 : control_sock = NEW_NOTHROW SocketAPM_native(false);
283 0 : if (!control_sock) {
284 0 : AP_HAL::panic("Out of memory for control socket");
285 : }
286 0 : if (!control_sock->connect(morse_ip, morse_control_port)) {
287 0 : usleep(100000);
288 0 : if (connect_counter++ == 20) {
289 0 : printf("Waiting to connect to control control on %s:%u\n",
290 0 : morse_ip, morse_control_port);
291 0 : connect_counter = 0;
292 : }
293 0 : delete control_sock;
294 0 : control_sock = nullptr;
295 0 : return false;
296 : }
297 0 : control_sock->reuseaddress();
298 0 : printf("Control connected\n");
299 : }
300 0 : return true;
301 : }
302 :
303 : /*
304 : get any new data from the sensors socket
305 : */
306 0 : bool Morse::sensors_receive(void)
307 : {
308 0 : ssize_t ret = sensors_sock->recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, 0);
309 0 : if (ret <= 0) {
310 0 : no_data_counter++;
311 0 : if (no_data_counter == 1000) {
312 0 : no_data_counter = 0;
313 0 : delete sensors_sock;
314 0 : delete control_sock;
315 0 : sensors_sock = nullptr;
316 0 : control_sock = nullptr;
317 : }
318 0 : return false;
319 : }
320 0 : no_data_counter = 0;
321 :
322 : // convert '\n' into nul
323 0 : while (uint8_t *p = (uint8_t *)memchr(&sensor_buffer[sensor_buffer_len], '\n', ret)) {
324 0 : *p = 0;
325 0 : }
326 0 : sensor_buffer_len += ret;
327 :
328 0 : const uint8_t *p2 = (const uint8_t *)memrchr(sensor_buffer, 0, sensor_buffer_len);
329 0 : if (p2 == nullptr || p2 == sensor_buffer) {
330 0 : return false;
331 : }
332 0 : const uint8_t *p1 = (const uint8_t *)memrchr(sensor_buffer, 0, p2 - sensor_buffer);
333 0 : if (p1 == nullptr) {
334 0 : return false;
335 : }
336 :
337 0 : bool parse_ok = parse_sensors((const char *)(p1+1));
338 :
339 0 : memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer));
340 0 : sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer);
341 :
342 0 : return parse_ok;
343 : }
344 :
345 : /*
346 : output control command assuming steering/throttle rover
347 : */
348 0 : void Morse::output_rover_regular(const struct sitl_input &input)
349 : {
350 0 : float throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
351 0 : float ground_steer = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
352 0 : float max_steer = radians(60);
353 0 : float max_speed = 20;
354 0 : float max_accel = 20;
355 :
356 : // speed in m/s in body frame
357 0 : Vector3f velocity_body = dcm.transposed() * velocity_ef;
358 :
359 : // speed along x axis, +ve is forward
360 0 : float speed = velocity_body.x;
361 :
362 : // target speed with current throttle
363 0 : float target_speed = throttle * max_speed;
364 :
365 : // linear acceleration in m/s/s - very crude model
366 0 : float accel = max_accel * (target_speed - speed) / max_speed;
367 :
368 : //force directly proportion to acceleration
369 0 : float force = accel;
370 :
371 0 : float steer = ground_steer * max_steer;
372 :
373 : // construct a JSON packet for steer/force
374 : char buf[60];
375 0 : snprintf(buf, sizeof(buf)-1, "{\"steer\": %.3f, \"force\": %.2f, \"brake\": %.2f}\n",
376 0 : steer, -force, 0.0);
377 0 : buf[sizeof(buf)-1] = 0;
378 :
379 0 : control_sock->send(buf, strlen(buf));
380 0 : }
381 : /*
382 : output control command assuming skid-steering rover
383 : */
384 0 : void Morse::output_rover_skid(const struct sitl_input &input)
385 : {
386 0 : float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
387 0 : float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
388 0 : const float steer_rate_max_dps = 60;
389 0 : const float max_speed = 2;
390 :
391 0 : const float steering_rps = (motor1 - motor2) * radians(steer_rate_max_dps);
392 0 : const float speed_ms = 0.5*(motor1 + motor2) * max_speed;
393 :
394 : // construct a JSON packet for v and w
395 : char buf[60];
396 0 : snprintf(buf, sizeof(buf)-1, "{\"v\": %.3f, \"w\": %.2f}\n",
397 0 : speed_ms, -steering_rps);
398 0 : buf[sizeof(buf)-1] = 0;
399 :
400 0 : control_sock->send(buf, strlen(buf));
401 0 : }
402 :
403 : /*
404 : output control command assuming a 4 channel quad
405 : */
406 0 : void Morse::output_quad(const struct sitl_input &input)
407 : {
408 0 : const float max_thrust = 1500;
409 : float motors[4];
410 0 : for (uint8_t i=0; i<4; i++) {
411 0 : motors[i] = constrain_float(((input.servos[i]-1000)/1000.0f) * max_thrust, 0, max_thrust);
412 : }
413 0 : const float &m_right = motors[0];
414 0 : const float &m_left = motors[1];
415 0 : const float &m_front = motors[2];
416 0 : const float &m_back = motors[3];
417 :
418 : // quad format in Morse is:
419 : // m1: back
420 : // m2: right
421 : // m3: front
422 : // m4: left
423 :
424 : // construct a JSON packet for motors
425 : char buf[60];
426 0 : snprintf(buf, sizeof(buf)-1, "{\"engines\": [%.3f, %.3f, %.3f, %.3f]}\n",
427 0 : m_back, m_right, m_front, m_left);
428 0 : buf[sizeof(buf)-1] = 0;
429 :
430 0 : control_sock->send(buf, strlen(buf));
431 0 : }
432 :
433 : /*
434 : output all 16 channels as PWM values. This allows for general
435 : control of a robot
436 : */
437 0 : void Morse::output_pwm(const struct sitl_input &input)
438 : {
439 : char buf[200];
440 0 : snprintf(buf, sizeof(buf)-1, "{\"pwm\": [%u, %uf, %u, %u, %u, %uf, %u, %u, %u, %uf, %u, %u, %u, %uf, %u, %u]}\n",
441 0 : input.servos[0], input.servos[1], input.servos[2], input.servos[3],
442 0 : input.servos[4], input.servos[5], input.servos[6], input.servos[7],
443 0 : input.servos[8], input.servos[9], input.servos[10], input.servos[11],
444 0 : input.servos[12], input.servos[13], input.servos[14], input.servos[15]);
445 0 : buf[sizeof(buf)-1] = 0;
446 0 : control_sock->send(buf, strlen(buf));
447 0 : }
448 :
449 :
450 : /*
451 : update the Morse simulation by one time step
452 : */
453 0 : void Morse::update(const struct sitl_input &input)
454 : {
455 0 : update_battery();
456 0 : if (!connect_sockets()) {
457 0 : return;
458 : }
459 :
460 0 : last_state = state;
461 :
462 0 : if (sensors_receive()) {
463 : // update average frame time used for extrapolation
464 0 : double dt = constrain_float(state.timestamp - last_state.timestamp, 0.001, 1.0/50);
465 0 : if (average_frame_time_s < 1.0e-6) {
466 0 : average_frame_time_s = dt;
467 : }
468 0 : average_frame_time_s = average_frame_time_s * 0.98 + dt * 0.02;
469 : }
470 :
471 0 : double dt_s = state.timestamp - last_state.timestamp;
472 0 : if (dt_s < 0 || dt_s > 1) {
473 : // cope with restarting while connected
474 0 : initial_time_s = time_now_us * 1.0e-6f;
475 0 : last_time_s = state.timestamp;
476 0 : return;
477 : }
478 :
479 0 : if (dt_s < 0.00001f) {
480 0 : float delta_time = 0.001;
481 : // don't go past the next expected frame
482 0 : if (delta_time + extrapolated_s > average_frame_time_s) {
483 0 : delta_time = average_frame_time_s - extrapolated_s;
484 : }
485 0 : if (delta_time <= 0) {
486 0 : usleep(1000);
487 0 : return;
488 : }
489 0 : time_now_us += delta_time * 1.0e6;
490 0 : extrapolate_sensors(delta_time);
491 0 : update_position();
492 0 : update_mag_field_bf();
493 0 : usleep(delta_time*1.0e6);
494 0 : extrapolated_s += delta_time;
495 0 : report_FPS();
496 0 : return;
497 : }
498 :
499 0 : extrapolated_s = 0;
500 :
501 0 : if (initial_time_s <= 0) {
502 0 : dt_s = 0.001f;
503 0 : initial_time_s = state.timestamp - dt_s;
504 : }
505 :
506 : // convert from state variables to ardupilot conventions
507 0 : dcm.from_euler(state.pose.roll, -state.pose.pitch, -state.pose.yaw);
508 :
509 0 : gyro = Vector3f(state.imu.angular_velocity[0],
510 0 : -state.imu.angular_velocity[1],
511 0 : -state.imu.angular_velocity[2]);
512 :
513 0 : velocity_ef = Vector3f(state.velocity.world_linear_velocity[0],
514 0 : -state.velocity.world_linear_velocity[1],
515 0 : -state.velocity.world_linear_velocity[2]);
516 :
517 0 : position = Vector3d(state.gps.x, -state.gps.y, -state.gps.z);
518 0 : position.xy() += origin.get_distance_NE_double(home);
519 :
520 : // Morse IMU accel is NEU, convert to NED
521 0 : accel_body = Vector3f(state.imu.linear_acceleration[0],
522 0 : -state.imu.linear_acceleration[1],
523 0 : -state.imu.linear_acceleration[2]);
524 :
525 : // limit to 16G to match pixhawk1
526 0 : float a_limit = GRAVITY_MSS*16;
527 0 : accel_body.x = constrain_float(accel_body.x, -a_limit, a_limit);
528 0 : accel_body.y = constrain_float(accel_body.y, -a_limit, a_limit);
529 0 : accel_body.z = constrain_float(accel_body.z, -a_limit, a_limit);
530 :
531 : // fill in laser scanner results, if available
532 0 : scanner.points = state.scanner.points;
533 0 : scanner.ranges = state.scanner.ranges;
534 :
535 0 : update_position();
536 0 : time_advance();
537 0 : uint64_t new_time_us = (state.timestamp - initial_time_s)*1.0e6;
538 0 : if (new_time_us < time_now_us) {
539 0 : uint64_t dt_us = time_now_us - new_time_us;
540 0 : if (dt_us > 500000) {
541 : // time going backwards
542 0 : time_now_us = new_time_us;
543 : }
544 : } else {
545 0 : time_now_us = new_time_us;
546 : }
547 :
548 0 : last_time_s = state.timestamp;
549 :
550 : // update magnetic field
551 0 : update_mag_field_bf();
552 :
553 0 : switch (output_type) {
554 0 : case OUTPUT_ROVER_REGULAR:
555 0 : output_rover_regular(input);
556 0 : break;
557 0 : case OUTPUT_ROVER_SKID:
558 0 : output_rover_skid(input);
559 0 : break;
560 0 : case OUTPUT_QUAD:
561 0 : output_quad(input);
562 0 : break;
563 0 : case OUTPUT_PWM:
564 0 : output_pwm(input);
565 0 : break;
566 : }
567 :
568 0 : report_FPS();
569 :
570 : #if HAL_GCS_ENABLED
571 0 : send_report();
572 : #endif
573 : }
574 :
575 :
576 : /*
577 : report frame rates
578 : */
579 0 : void Morse::report_FPS(void)
580 : {
581 0 : if (frame_counter++ % 1000 == 0) {
582 0 : if (!is_zero(last_frame_count_s)) {
583 0 : uint64_t frames = socket_frame_counter - last_socket_frame_counter;
584 0 : last_socket_frame_counter = socket_frame_counter;
585 0 : double dt = state.timestamp - last_frame_count_s;
586 0 : printf("%.2f/%.2f FPS avg=%.2f\n",
587 0 : frames / dt, 1000 / dt, 1.0/average_frame_time_s);
588 : } else {
589 0 : printf("Initial position %f %f %f\n", position.x, position.y, position.z);
590 : }
591 0 : last_frame_count_s = state.timestamp;
592 : }
593 0 : }
594 :
595 :
596 :
597 : #if HAL_GCS_ENABLED
598 : /*
599 : send a report to the vehicle control code over MAVLink
600 : */
601 0 : void Morse::send_report(void)
602 : {
603 0 : const uint32_t now = AP_HAL::millis();
604 : #if defined(__CYGWIN__) || defined(__CYGWIN64__)
605 : if (now < 10000) {
606 : // don't send lidar reports until 10s after startup. This
607 : // avoids a windows threading issue with non-blocking sockets
608 : // and the initial wait on SERIAL0
609 : return;
610 : }
611 : #endif
612 :
613 : // this is usually loopback
614 0 : if (!mavlink.connected && mav_socket.connect(mavlink_loopback_address, mavlink_loopback_port)) {
615 0 : ::printf("Morse MAVLink loopback connected to %s:%u\n", mavlink_loopback_address, (unsigned)mavlink_loopback_port);
616 0 : mavlink.connected = true;
617 : }
618 0 : if (!mavlink.connected) {
619 0 : return;
620 : }
621 :
622 : // send a OBSTACLE_DISTANCE messages at 15 Hz
623 0 : if (now - send_report_last_ms >= (1000/15) && scanner.points.length == scanner.ranges.length && scanner.points.length > 0) {
624 0 : send_report_last_ms = now;
625 :
626 0 : mavlink_obstacle_distance_t packet {};
627 0 : packet.time_usec = AP_HAL::micros64();
628 0 : packet.min_distance = 1;
629 : // the simulated rangefinder has an imposed 18m limit in
630 : // e.g. rover_scanner.py
631 0 : packet.max_distance = 5000;
632 0 : packet.sensor_type = MAV_DISTANCE_SENSOR_LASER;
633 0 : packet.increment = 0; // use increment_f
634 :
635 0 : packet.angle_offset = 180;
636 0 : packet.increment_f = (-5); // NOTE! This is negative because the distances[] arc is counter-clockwise
637 :
638 0 : for (uint8_t i=0; i<MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN; i++) {
639 :
640 0 : if (i >= scanner.points.length) {
641 0 : packet.distances[i] = 65535;
642 0 : continue;
643 : }
644 :
645 : // convert m to cm and sanity check
646 0 : const Vector2f v = Vector2f(scanner.points.data[i].x, scanner.points.data[i].y);
647 0 : const float distance_cm = v.length()*100;
648 0 : if (distance_cm < packet.min_distance) {
649 0 : packet.distances[i] = packet.max_distance + 1; // "no obstacle"
650 0 : continue;
651 : }
652 :
653 0 : if (distance_cm > packet.max_distance) {
654 0 : packet.distances[i] = packet.max_distance + 1; // "no obstacle"
655 0 : continue;
656 : }
657 :
658 0 : packet.distances[i] = distance_cm;
659 : }
660 :
661 : mavlink_message_t msg;
662 0 : uint16_t len = mavlink_msg_obstacle_distance_encode_status(
663 0 : mavlink_system.sysid,
664 : 13,
665 : &mavlink.status,
666 : &msg, &packet);
667 :
668 0 : uint8_t msgbuf[len];
669 0 : len = mavlink_msg_to_send_buffer(msgbuf, &msg);
670 0 : if (len > 0) {
671 0 : mav_socket.send(msgbuf, len);
672 : }
673 0 : }
674 :
675 : }
676 : #endif // HAL_GCS_ENABLED
677 :
678 : #endif // AP_SIM_MORSE_ENABLED
|
