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 XPlane
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_XPLANE_ENABLED
22 :
23 : #include "SIM_XPlane.h"
24 :
25 : #include <errno.h>
26 : #include <fcntl.h>
27 : #include <stdio.h>
28 : #include <stdarg.h>
29 : #include <sys/stat.h>
30 : #include <sys/types.h>
31 :
32 : #include <AP_HAL/AP_HAL.h>
33 : #include <AP_Filesystem/AP_Filesystem.h>
34 : #include <SRV_Channel/SRV_Channel.h>
35 : #include <AP_Vehicle/AP_Vehicle_Type.h>
36 :
37 : // ignore cast errors in this case to keep complexity down
38 : #pragma GCC diagnostic ignored "-Wcast-align"
39 :
40 : extern const AP_HAL::HAL& hal;
41 :
42 : #if APM_BUILD_TYPE(APM_BUILD_Heli)
43 : #define XPLANE_JSON "xplane_heli.json"
44 : #else
45 : #define XPLANE_JSON "xplane_plane.json"
46 : #endif
47 :
48 : // DATA@ frame types. Thanks to TauLabs xplanesimulator.h
49 : // (which strangely enough acknowledges APM as a source!)
50 : enum {
51 : FramRate = 0,
52 : Times = 1,
53 : SimStats = 2,
54 : Speed = 3,
55 : Gload = 4,
56 : AtmosphereWeather = 5,
57 : AtmosphereAircraft = 6,
58 : SystemPressures = 7,
59 : Joystick1 = 8,
60 : Joystick2 = 9,
61 : ArtStab = 10,
62 : FlightCon = 11,
63 : WingSweep = 12,
64 : Trim = 13,
65 : Brakes = 14,
66 : AngularMoments = 15,
67 : AngularVelocities = 16,
68 : PitchRollHeading = 17,
69 : AoA = 18,
70 : MagCompass = 19,
71 : LatLonAlt = 20,
72 : LocVelDistTraveled = 21,
73 : ThrottleCommand = 25,
74 : CarbHeat = 30,
75 : EngineRPM = 37,
76 : PropRPM = 38,
77 : PropPitch = 39,
78 : Generator = 58,
79 : JoystickRaw = 136,
80 : };
81 :
82 : enum RREF {
83 : RREF_VERSION = 1,
84 : };
85 :
86 : static const uint8_t required_data[] {
87 : Times, LatLonAlt, Speed, PitchRollHeading,
88 : LocVelDistTraveled, AngularVelocities, Gload,
89 : Trim,
90 : PropPitch, EngineRPM, PropRPM,
91 : JoystickRaw };
92 :
93 : using namespace SITL;
94 :
95 0 : XPlane::XPlane(const char *frame_str) :
96 0 : Aircraft(frame_str)
97 : {
98 0 : use_time_sync = false;
99 0 : const char *colon = strchr(frame_str, ':');
100 0 : if (colon) {
101 0 : xplane_ip = colon+1;
102 : }
103 :
104 0 : socket_in.bind("0.0.0.0", bind_port);
105 0 : printf("Waiting for XPlane data on UDP port %u and sending to port %u\n",
106 0 : (unsigned)bind_port, (unsigned)xplane_port);
107 :
108 : // XPlane sensor data is not good enough for EKF. Use fake EKF by default
109 0 : AP_Param::set_default_by_name("AHRS_EKF_TYPE", 10);
110 0 : AP_Param::set_default_by_name("GPS1_TYPE", 100);
111 0 : AP_Param::set_default_by_name("INS_GYR_CAL", 0);
112 :
113 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
114 : // default flaps to channel 5
115 0 : AP_Param::set_default_by_name("SERVO5_FUNCTION", 3);
116 0 : AP_Param::set_default_by_name("SERVO5_MIN", 1000);
117 0 : AP_Param::set_default_by_name("SERVO5_MAX", 2000);
118 : #endif
119 :
120 0 : if (!load_dref_map(XPLANE_JSON)) {
121 0 : AP_HAL::panic("%s failed to load", XPLANE_JSON);
122 : }
123 0 : }
124 :
125 : /*
126 : add one DRef to list
127 : */
128 0 : void XPlane::add_dref(const char *name, DRefType type, const AP_JSON::value &dref)
129 : {
130 0 : struct DRef *d = NEW_NOTHROW struct DRef;
131 0 : if (d == nullptr) {
132 0 : AP_HAL::panic("out of memory for DRef %s", name);
133 : }
134 0 : d->name = strdup(name);
135 0 : d->type = type;
136 0 : if (d->name == nullptr) {
137 0 : AP_HAL::panic("out of memory for DRef %s", name);
138 : }
139 0 : if (d->type == DRefType::FIXED) {
140 0 : d->fixed_value = dref.get("value").get<double>();
141 : } else {
142 0 : d->range = dref.get("range").get<double>();
143 0 : d->channel = dref.get("channel").get<double>();
144 : }
145 : // add to linked list
146 0 : d->next = drefs;
147 0 : drefs = d;
148 0 : }
149 :
150 : /*
151 : add one joystick axis to list
152 : */
153 0 : void XPlane::add_joyinput(const char *label, JoyType type, const AP_JSON::value &d)
154 : {
155 0 : if (strncmp(label, "axis", 4) == 0) {
156 0 : struct JoyInput *j = NEW_NOTHROW struct JoyInput;
157 0 : if (j == nullptr) {
158 0 : AP_HAL::panic("out of memory for JoyInput %s", label);
159 : }
160 0 : j->axis = atoi(label+4);
161 0 : j->type = JoyType::AXIS;
162 0 : j->channel = d.get("channel").get<double>();
163 0 : j->input_min = d.get("input_min").get<double>();
164 0 : j->input_max = d.get("input_max").get<double>();
165 0 : j->next = joyinputs;
166 0 : joyinputs = j;
167 : }
168 0 : if (strncmp(label, "button", 6) == 0) {
169 0 : struct JoyInput *j = NEW_NOTHROW struct JoyInput;
170 0 : if (j == nullptr) {
171 0 : AP_HAL::panic("out of memory for JoyInput %s", label);
172 : }
173 0 : j->type = JoyType::BUTTON;
174 0 : j->channel = d.get("channel").get<double>();
175 0 : j->mask = d.get("mask").get<double>();
176 0 : j->next = joyinputs;
177 0 : joyinputs = j;
178 : }
179 0 : }
180 :
181 : /*
182 : handle a setting
183 : */
184 0 : void XPlane::handle_setting(const AP_JSON::value &d)
185 : {
186 0 : if (d.contains("debug")) {
187 0 : dref_debug = d.get("debug").get<double>();
188 : }
189 0 : }
190 :
191 :
192 : /*
193 : load mapping of channels to datarefs from a json file
194 : */
195 0 : bool XPlane::load_dref_map(const char *map_json)
196 : {
197 0 : char *fname = nullptr;
198 0 : if (AP::FS().stat(map_json, &map_st) == 0) {
199 0 : fname = strdup(map_json);
200 : } else {
201 0 : IGNORE_RETURN(asprintf(&fname, "@ROMFS/models/%s", map_json));
202 0 : if (AP::FS().stat(fname, &map_st) != 0) {
203 0 : return false;
204 : }
205 : }
206 0 : if (fname == nullptr) {
207 0 : return false;
208 : }
209 0 : AP_JSON::value *obj = AP_JSON::load_json(fname);
210 0 : if (obj == nullptr) {
211 0 : free((void*)fname);
212 0 : return false;
213 : }
214 :
215 0 : free(map_filename);
216 0 : map_filename = fname;
217 :
218 : // free old drefs
219 0 : while (drefs) {
220 0 : auto *d = drefs->next;
221 0 : free(drefs->name);
222 0 : delete drefs;
223 0 : drefs = d;
224 : }
225 :
226 : // free old joystick
227 0 : while (joyinputs) {
228 0 : auto *j = joyinputs->next;
229 0 : delete joyinputs;
230 0 : joyinputs = j;
231 : }
232 :
233 0 : uint32_t count = 0;
234 : // obtain a const reference to the map, and print the contents
235 0 : const AP_JSON::value::object& o = obj->get<AP_JSON::value::object>();
236 0 : for (AP_JSON::value::object::const_iterator i = o.begin();
237 0 : i != o.end();
238 0 : ++i) {
239 0 : const char *label = i->first.c_str();
240 0 : const auto &d = i->second;
241 0 : if (strchr(label, '/') != nullptr) {
242 0 : const auto str = d.get("type").to_str();
243 0 : const char *type_s = str.c_str();
244 0 : if (strcmp(type_s, "angle") == 0) {
245 0 : add_dref(label, DRefType::ANGLE, d);
246 0 : } else if (strcmp(type_s, "range") == 0) {
247 0 : add_dref(label, DRefType::RANGE, d);
248 0 : } else if (strcmp(type_s, "fixed") == 0) {
249 0 : add_dref(label, DRefType::FIXED, d);
250 : } else {
251 0 : ::printf("Invalid dref type %s for %s in %s", type_s, label, map_filename);
252 : }
253 0 : } else if (strcmp(label, "settings") == 0) {
254 0 : handle_setting(d);
255 0 : } else if (strncmp(label, "axis", 4) == 0) {
256 0 : add_joyinput(label, JoyType::AXIS, d);
257 0 : } else if (strncmp(label, "button", 6) == 0) {
258 0 : add_joyinput(label, JoyType::BUTTON, d);
259 : } else {
260 0 : ::printf("Invalid json type %s in %s", label, map_json);
261 0 : continue;
262 : }
263 0 : count++;
264 : }
265 0 : delete obj;
266 :
267 0 : ::printf("Loaded %u DRefs from %s\n", unsigned(count), map_filename);
268 0 : return true;
269 : }
270 :
271 : /*
272 : load mapping of channels to datarefs from a json file
273 : */
274 0 : void XPlane::check_reload_dref(void)
275 : {
276 0 : if (!hal.util->get_soft_armed()) {
277 : struct stat st;
278 0 : if (AP::FS().stat(map_filename, &st) == 0 && st.st_mtime != map_st.st_mtime) {
279 0 : load_dref_map(map_filename);
280 : }
281 : }
282 0 : }
283 :
284 0 : int8_t XPlane::find_data_index(uint8_t code)
285 : {
286 0 : for (uint8_t i = 0; i<ARRAY_SIZE(required_data); i++) {
287 0 : if (required_data[i] == code) {
288 0 : return i;
289 : }
290 : }
291 0 : return -1;
292 : }
293 :
294 : /*
295 : change what data is requested from XPlane. This saves the user from
296 : having to setup the data screen correctly
297 : */
298 0 : void XPlane::select_data(void)
299 : {
300 0 : const uint64_t all_mask = (1U<<ARRAY_SIZE(required_data))-1;
301 0 : if ((seen_mask & all_mask) == all_mask) {
302 : // got it all
303 0 : return;
304 : }
305 : struct PACKED {
306 : uint8_t marker[5] { 'D', 'S', 'E', 'L', '0' };
307 : uint32_t data[8] {};
308 0 : } dsel;
309 0 : uint8_t count = 0;
310 0 : for (uint8_t i=0; i<ARRAY_SIZE(required_data); i++) {
311 0 : if (seen_mask & (1U<<i)) {
312 : // got this one
313 0 : continue;
314 : }
315 0 : dsel.data[count++] = required_data[i];
316 : }
317 0 : if (count != 0) {
318 0 : socket_out.send(&dsel, sizeof(dsel));
319 0 : printf("Selecting %u data types\n", (unsigned)count);
320 : }
321 : }
322 :
323 0 : void XPlane::deselect_code(uint8_t code)
324 : {
325 : struct PACKED {
326 : uint8_t marker[5] { 'U', 'S', 'E', 'L', '0' };
327 : uint32_t data[8] {};
328 0 : } usel;
329 0 : usel.data[0] = code;
330 0 : socket_out.send(&usel, sizeof(usel));
331 0 : printf("De-selecting code %u\n", code);
332 0 : }
333 :
334 : /*
335 : receive data from X-Plane via UDP
336 : return true if we get a gyro frame
337 : */
338 0 : bool XPlane::receive_data(void)
339 : {
340 : uint8_t pkt[10000];
341 0 : uint8_t *p = &pkt[5];
342 0 : const uint8_t pkt_len = 36;
343 0 : Location loc {};
344 0 : Vector3d pos;
345 0 : uint32_t wait_time_ms = 1;
346 0 : uint32_t now = AP_HAL::millis();
347 0 : bool ret = false;
348 :
349 : // if we are about to get another frame from X-Plane then wait longer
350 0 : if (xplane_frame_time > wait_time_ms &&
351 0 : now+1 >= last_data_time_ms + xplane_frame_time) {
352 0 : wait_time_ms = 10;
353 : }
354 0 : ssize_t len = socket_in.recv(pkt, sizeof(pkt), wait_time_ms);
355 :
356 0 : if (len < 5) {
357 : // bad packet
358 0 : goto failed;
359 : }
360 :
361 0 : if (memcmp(pkt, "RREF", 4) == 0) {
362 0 : handle_rref(pkt, len);
363 0 : return false;
364 : }
365 :
366 0 : if (memcmp(pkt, "DATA", 4) != 0) {
367 : // not a data packet we understand
368 0 : ::printf("PACKET: %4.4s\n", (const char *)pkt);
369 0 : goto failed;
370 : }
371 0 : len -= 5;
372 :
373 0 : if (len < pkt_len) {
374 : // bad packet
375 0 : goto failed;
376 : }
377 :
378 :
379 0 : if (!connected) {
380 : // we now know the IP X-Plane is using
381 : uint16_t port;
382 0 : socket_in.last_recv_address(xplane_ip, port);
383 0 : socket_out.connect(xplane_ip, xplane_port);
384 0 : connected = true;
385 0 : printf("Connected to %s:%u\n", xplane_ip, (unsigned)xplane_port);
386 : }
387 :
388 0 : while (len >= pkt_len) {
389 0 : const float *data = (const float *)p;
390 0 : uint8_t code = p[0];
391 0 : int8_t idx = find_data_index(code);
392 0 : if (idx == -1) {
393 0 : deselect_code(code);
394 0 : len -= pkt_len;
395 0 : p += pkt_len;
396 0 : continue;
397 : }
398 0 : seen_mask |= (1U<<idx);
399 :
400 0 : switch (code) {
401 0 : case Times: {
402 0 : uint64_t tus = data[3] * 1.0e6f;
403 0 : if (tus + time_base_us <= time_now_us) {
404 0 : uint64_t tdiff = time_now_us - (tus + time_base_us);
405 0 : if (tdiff > 1e6f) {
406 0 : printf("X-Plane time reset %lu\n", (unsigned long)tdiff);
407 : }
408 0 : time_base_us = time_now_us - tus;
409 : }
410 0 : uint64_t tnew = time_base_us + tus;
411 : //uint64_t dt = tnew - time_now_us;
412 : //printf("dt %u\n", (unsigned)dt);
413 0 : time_now_us = tnew;
414 0 : break;
415 : }
416 :
417 0 : case LatLonAlt: {
418 0 : loc.lat = data[1] * 1e7;
419 0 : loc.lng = data[2] * 1e7;
420 0 : loc.alt = data[3] * FEET_TO_METERS * 100.0f;
421 0 : const float altitude_above_ground = data[4] * FEET_TO_METERS;
422 0 : ground_level = loc.alt * 0.01f - altitude_above_ground;
423 0 : break;
424 : }
425 :
426 0 : case Speed:
427 0 : airspeed = data[2] * KNOTS_TO_METERS_PER_SECOND;
428 0 : airspeed_pitot = airspeed;
429 0 : break;
430 :
431 0 : case AoA:
432 : // ignored
433 0 : break;
434 :
435 0 : case PitchRollHeading: {
436 : float roll, pitch, yaw;
437 0 : pitch = radians(data[1]);
438 0 : roll = radians(data[2]);
439 0 : yaw = radians(data[3]);
440 0 : dcm.from_euler(roll, pitch, yaw);
441 0 : break;
442 : }
443 :
444 0 : case AtmosphereWeather:
445 : // ignored
446 0 : break;
447 :
448 0 : case LocVelDistTraveled:
449 0 : pos.y = data[1];
450 0 : pos.z = -data[2];
451 0 : pos.x = -data[3];
452 0 : velocity_ef.y = data[4];
453 0 : velocity_ef.z = -data[5];
454 0 : velocity_ef.x = -data[6];
455 0 : break;
456 :
457 0 : case AngularVelocities:
458 0 : if (is_xplane12()) {
459 0 : gyro.x = radians(data[1]);
460 0 : gyro.y = radians(data[2]);
461 0 : gyro.z = radians(data[3]);
462 : } else {
463 : // xplane 11
464 0 : gyro.x = data[2];
465 0 : gyro.y = data[1];
466 0 : gyro.z = data[3];
467 : }
468 : // we only count gyro data towards data counts
469 0 : ret = true;
470 0 : break;
471 :
472 0 : case Gload:
473 0 : accel_body.z = -data[5] * GRAVITY_MSS;
474 0 : accel_body.x = data[6] * GRAVITY_MSS;
475 0 : accel_body.y = data[7] * GRAVITY_MSS;
476 0 : break;
477 :
478 0 : case PropPitch: {
479 0 : break;
480 : }
481 :
482 0 : case EngineRPM:
483 0 : rpm[0] = data[1];
484 0 : motor_mask |= 1;
485 0 : break;
486 :
487 0 : case PropRPM:
488 0 : rpm[1] = data[1];
489 0 : motor_mask |= 2;
490 0 : break;
491 :
492 0 : case JoystickRaw: {
493 0 : for (auto *j = joyinputs; j; j=j->next) {
494 0 : switch (j->type) {
495 0 : case JoyType::AXIS: {
496 0 : if (j->axis >= 1 && j->axis <= 6) {
497 0 : float v = (data[j->axis] - j->input_min) / (j->input_max - j->input_min);
498 0 : rcin[j->channel-1] = v;
499 0 : rcin_chan_count = MAX(rcin_chan_count, j->channel);
500 : }
501 0 : break;
502 : }
503 0 : case JoyType::BUTTON: {
504 0 : uint32_t m = uint32_t(data[7]) & j->mask;
505 0 : float v = 0;
506 0 : if (m == 0) {
507 0 : v = 0;
508 0 : } else if (1U<<(__builtin_ffs(j->mask)-1) != m) {
509 0 : v = 0.5;
510 : } else {
511 0 : v = 1;
512 : }
513 0 : rcin[j->channel-1] = v;
514 0 : rcin_chan_count = MAX(rcin_chan_count, j->channel);
515 0 : break;
516 : }
517 : }
518 : }
519 : }
520 : }
521 0 : len -= pkt_len;
522 0 : p += pkt_len;
523 : }
524 :
525 : // update data selection
526 0 : select_data();
527 :
528 0 : position = pos + position_zero;
529 0 : position.xy() += origin.get_distance_NE_double(home);
530 0 : update_position();
531 0 : time_advance();
532 :
533 0 : accel_earth = dcm * accel_body;
534 0 : accel_earth.z += GRAVITY_MSS;
535 :
536 : // the position may slowly deviate due to float accuracy and longitude scaling
537 0 : if (loc.get_distance(location) > 4 || abs(loc.alt - location.alt)*0.01f > 2.0f) {
538 0 : printf("X-Plane home reset dist=%f alt=%.1f/%.1f\n",
539 0 : loc.get_distance(location), loc.alt*0.01f, location.alt*0.01f);
540 : // reset home location
541 0 : position_zero = {-pos.x, -pos.y, -pos.z};
542 0 : home.lat = loc.lat;
543 0 : home.lng = loc.lng;
544 0 : home.alt = loc.alt;
545 0 : origin = home;
546 0 : position.x = 0;
547 0 : position.y = 0;
548 0 : position.z = 0;
549 0 : update_position();
550 0 : time_advance();
551 : }
552 :
553 0 : update_mag_field_bf();
554 :
555 0 : if (now > last_data_time_ms && now - last_data_time_ms < 100) {
556 0 : xplane_frame_time = now - last_data_time_ms;
557 : }
558 0 : last_data_time_ms = AP_HAL::millis();
559 :
560 0 : if (ret) {
561 0 : report.data_count++;
562 0 : report.frame_count++;
563 : }
564 :
565 0 : return ret;
566 :
567 0 : failed:
568 0 : if (AP_HAL::millis() - last_data_time_ms > 200) {
569 : // don't extrapolate beyond 0.2s
570 0 : return false;
571 : }
572 :
573 : // advance time by 1ms
574 0 : frame_time_us = 1000;
575 0 : float delta_time = frame_time_us * 1e-6f;
576 :
577 0 : time_now_us += frame_time_us;
578 :
579 0 : extrapolate_sensors(delta_time);
580 :
581 0 : update_position();
582 0 : time_advance();
583 0 : update_mag_field_bf();
584 0 : report.frame_count++;
585 0 : return false;
586 : }
587 :
588 : /*
589 : receive RREF replies
590 : */
591 0 : void XPlane::handle_rref(const uint8_t *pkt, uint32_t len)
592 : {
593 0 : const uint8_t *p = &pkt[5];
594 : const struct PACKED RRefPacket {
595 : uint32_t code;
596 : union PACKED {
597 : float value_f;
598 : double value_d;
599 : };
600 0 : } *ref = (const struct RRefPacket *)p;
601 0 : switch (ref->code) {
602 0 : case RREF_VERSION:
603 0 : if (xplane_version == 0) {
604 0 : ::printf("XPlane version %.0f\n", ref->value_f);
605 : }
606 0 : xplane_version = uint32_t(ref->value_f);
607 0 : break;
608 : }
609 0 : }
610 :
611 :
612 : /*
613 : send DRef data to X-Plane via UDP
614 : */
615 0 : void XPlane::send_drefs(const struct sitl_input &input)
616 : {
617 0 : for (const auto *d = drefs; d; d=d->next) {
618 0 : switch (d->type) {
619 :
620 0 : case DRefType::ANGLE: {
621 0 : float v = d->range * (input.servos[d->channel-1]-1500)/500.0;
622 0 : send_dref(d->name, v);
623 0 : break;
624 : }
625 :
626 0 : case DRefType::RANGE: {
627 0 : float v = d->range * (input.servos[d->channel-1]-1000)/1000.0;
628 0 : send_dref(d->name, v);
629 0 : break;
630 : }
631 :
632 0 : case DRefType::FIXED: {
633 0 : send_dref(d->name, d->fixed_value);
634 0 : break;
635 : }
636 : }
637 : }
638 0 : }
639 :
640 :
641 : /*
642 : send DREF to X-Plane via UDP
643 : */
644 0 : void XPlane::send_dref(const char *name, float value)
645 : {
646 : struct PACKED {
647 : uint8_t marker[5] { 'D', 'R', 'E', 'F', '0' };
648 : float value;
649 : char name[500];
650 0 : } d {};
651 0 : d.value = value;
652 0 : strcpy(d.name, name);
653 0 : socket_out.send(&d, sizeof(d));
654 0 : if (dref_debug > 0) {
655 0 : ::printf("-> %s : %.3f\n", name, value);
656 : }
657 0 : }
658 :
659 : /*
660 : request a dref
661 : */
662 0 : void XPlane::request_dref(const char *name, uint8_t code, uint32_t rate)
663 : {
664 : struct PACKED {
665 : uint8_t marker[5] { 'R', 'R', 'E', 'F', '0' };
666 : uint32_t rate_hz;
667 : uint32_t code;
668 : char name[400];
669 0 : } d {};
670 0 : d.rate_hz = rate;
671 0 : d.code = code; // given back in responses
672 0 : strcpy(d.name, name);
673 0 : socket_in.sendto(&d, sizeof(d), xplane_ip, xplane_port);
674 0 : }
675 :
676 0 : void XPlane::request_drefs(void)
677 : {
678 0 : request_dref("sim/version/xplane_internal_version", RREF_VERSION, 1);
679 0 : }
680 :
681 : /*
682 : update the XPlane simulation by one time step
683 : */
684 0 : void XPlane::update(const struct sitl_input &input)
685 : {
686 0 : if (receive_data()) {
687 0 : send_drefs(input);
688 : }
689 :
690 0 : update_battery();
691 :
692 0 : uint32_t now = AP_HAL::millis();
693 0 : if (report.last_report_ms == 0) {
694 0 : report.last_report_ms = now;
695 0 : request_drefs();
696 : }
697 0 : if (now - report.last_report_ms > 5000) {
698 0 : float dt = (now - report.last_report_ms) * 1.0e-3f;
699 0 : printf("Data rate: %.1f FPS Frame rate: %.1f FPS\n",
700 0 : report.data_count/dt, report.frame_count/dt);
701 0 : report.last_report_ms = now;
702 0 : report.data_count = 0;
703 0 : report.frame_count = 0;
704 0 : request_drefs();
705 : }
706 0 : check_reload_dref();
707 0 : }
708 :
709 : #endif // AP_SIM_XPLANE_ENABLED
|
