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 JSON based interfaces
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_JSON_ENABLED
22 :
23 : #include "SIM_JSON.h"
24 :
25 : #include <stdio.h>
26 : #include <arpa/inet.h>
27 : #include <errno.h>
28 :
29 : #include <AP_HAL/AP_HAL.h>
30 : #include <AP_Logger/AP_Logger.h>
31 : #include <AP_HAL/utility/replace.h>
32 : #include <SRV_Channel/SRV_Channel.h>
33 : #include <AP_Filesystem/AP_Filesystem.h>
34 :
35 : #define UDP_TIMEOUT_MS 100
36 :
37 : extern const AP_HAL::HAL& hal;
38 :
39 : using namespace SITL;
40 :
41 : static const struct {
42 : const char *name;
43 : float value;
44 : bool save;
45 : } sim_defaults[] = {
46 : { "BRD_OPTIONS", 0},
47 : { "INS_GYR_CAL", 0 },
48 : { "INS_ACC2OFFS_X", 0.001 },
49 : { "INS_ACC2OFFS_Y", 0.001 },
50 : { "INS_ACC2OFFS_Z", 0.001 },
51 : { "INS_ACC2SCAL_X", 1.001 },
52 : { "INS_ACC2SCAL_Y", 1.001 },
53 : { "INS_ACC2SCAL_Z", 1.001 },
54 : { "INS_ACCOFFS_X", 0.001 },
55 : { "INS_ACCOFFS_Y", 0.001 },
56 : { "INS_ACCOFFS_Z", 0.001 },
57 : { "INS_ACCSCAL_X", 1.001 },
58 : { "INS_ACCSCAL_Y", 1.001 },
59 : { "INS_ACCSCAL_Z", 1.001 },
60 : };
61 :
62 :
63 0 : JSON::JSON(const char *frame_str) :
64 : Aircraft(frame_str),
65 0 : sock(true)
66 : {
67 0 : printf("Starting SITL: JSON\n");
68 :
69 0 : const char *colon = strchr(frame_str, ':');
70 0 : if (colon) {
71 0 : target_ip = colon+1;
72 : }
73 :
74 0 : for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {
75 0 : AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
76 0 : if (sim_defaults[i].save) {
77 : enum ap_var_type ptype;
78 0 : AP_Param *p = AP_Param::find(sim_defaults[i].name, &ptype);
79 0 : if (!p->configured()) {
80 0 : p->save();
81 : }
82 : }
83 : }
84 0 : }
85 :
86 : /*
87 : Create & set in/out socket
88 : */
89 0 : void JSON::set_interface_ports(const char* address, const int port_in, const int port_out)
90 : {
91 0 : sock.set_blocking(false);
92 0 : sock.reuseaddress();
93 :
94 0 : if (strcmp("127.0.0.1",address) != 0) {
95 0 : target_ip = address;
96 : }
97 0 : control_port = port_out;
98 :
99 0 : printf("JSON control interface set to %s:%u\n", target_ip, control_port);
100 0 : }
101 :
102 : /*
103 : Decode and send servos
104 : */
105 0 : void JSON::output_servos(const struct sitl_input &input)
106 : {
107 0 : size_t pkt_size = 0;
108 0 : ssize_t send_ret = -1;
109 0 : if (SRV_Channels::have_32_channels()) {
110 0 : servo_packet_32 pkt;
111 0 : pkt.frame_rate = rate_hz;
112 0 : pkt.frame_count = frame_counter;
113 0 : for (uint8_t i=0; i<32; i++) {
114 0 : pkt.pwm[i] = input.servos[i];
115 : }
116 0 : pkt_size = sizeof(pkt);
117 0 : send_ret = sock.sendto(&pkt, pkt_size, target_ip, control_port);
118 : } else {
119 0 : servo_packet_16 pkt;
120 0 : pkt.frame_rate = rate_hz;
121 0 : pkt.frame_count = frame_counter;
122 0 : for (uint8_t i=0; i<16; i++) {
123 0 : pkt.pwm[i] = input.servos[i];
124 : }
125 0 : pkt_size = sizeof(pkt);
126 0 : send_ret = sock.sendto(&pkt, pkt_size, target_ip, control_port);
127 : }
128 :
129 0 : if ((size_t)send_ret != pkt_size) {
130 0 : if (send_ret <= 0) {
131 0 : printf("Unable to send servo output to %s:%u - Error: %s, Return value: %ld\n",
132 0 : target_ip, control_port, strerror(errno), (long)send_ret);
133 : } else {
134 0 : printf("Sent %ld bytes instead of %lu bytes\n", (long)send_ret, (unsigned long)pkt_size);
135 : }
136 : }
137 0 : }
138 :
139 :
140 : /*
141 : very simple JSON parser for sensor data
142 : called with pointer to one row of sensor data, nul terminated
143 :
144 : This parser does not do any syntax checking, and is not at all
145 : general purpose
146 : */
147 :
148 : template <typename T>
149 0 : bool parse_array(const char *str, T &arr, int count) {
150 0 : const char *p = str;
151 :
152 0 : for (int i = 0; i < count; i++) {
153 : // Skip to start of number
154 0 : while (*p && *p != '-' && (*p < '0' || *p > '9'))
155 0 : p++;
156 :
157 0 : if (!*p) {
158 : // End of string before we got all numbers
159 0 : return false;
160 : }
161 :
162 0 : arr[i] = strtod(p, nullptr);
163 :
164 : // Move past the number
165 0 : while (*p && *p != ',' && *p != ']')
166 0 : p++;
167 :
168 0 : if (i < count - 1) { // expect comma between numbers
169 0 : if (*p != ',') return false;
170 0 : p++; // skip comma
171 : }
172 : }
173 :
174 0 : return true;
175 : }
176 :
177 0 : uint64_t JSON::parse_sensors(const char *json)
178 : {
179 0 : uint64_t received_bitmask = 0;
180 :
181 : #if SITL_JSON_DEBUG && AP_FILESYSTEM_FILE_WRITING_ENABLED
182 : // it is useful in some environments to be able to get a copy of the raw
183 : // JSON data
184 : const uint32_t now_ms = AP_HAL::millis();
185 : if (now_ms - last_debug_ms >= 1000) {
186 : // dump received JSON at 1Hz for easy debugging
187 : last_debug_ms = now_ms;
188 : auto &fs = AP::FS();
189 : int fd = fs.open("json_debug.txt", O_WRONLY|O_CREAT|O_TRUNC);
190 : if (fd != -1) {
191 : fs.write(fd, json, strlen(json));
192 : fs.close(fd);
193 : }
194 : }
195 : #endif
196 :
197 : //printf("%s\n", json);
198 0 : for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
199 0 : struct keytable &key = keytable[i];
200 :
201 : /* look for section header */
202 0 : const char *p = strstr(json, key.section);
203 0 : if (!p) {
204 : // we don't have this sensor
205 0 : if (key.required) {
206 0 : printf("Failed to find %s\n", key.section);
207 0 : return 0;
208 : }
209 0 : continue;
210 : }
211 0 : p += strlen(key.section)+1;
212 :
213 : // find key inside section
214 0 : p = strstr(p, key.key);
215 0 : if (!p) {
216 0 : if (key.required) {
217 0 : printf("Failed to find key %s/%s\n", key.section, key.key);
218 0 : return 0;
219 : }
220 0 : continue;
221 : }
222 :
223 : // record the keys that are found
224 0 : received_bitmask |= 1ULL << i;
225 :
226 0 : p += strlen(key.key)+2;
227 0 : switch (key.type) {
228 0 : case DATA_UINT64:
229 0 : *((uint64_t *)key.ptr) = strtoull(p, nullptr, 10);
230 : //printf("%s/%s = %lu\n", key.section, key.key, *((uint64_t *)key.ptr));
231 0 : break;
232 :
233 0 : case DATA_FLOAT:
234 0 : *((float *)key.ptr) = strtof(p, nullptr);
235 : //printf("%s/%s = %f\n", key.section, key.key, *((float *)key.ptr));
236 0 : break;
237 :
238 0 : case DATA_DOUBLE:
239 0 : *((double *)key.ptr) = atof(p);
240 : //printf("%s/%s = %f\n", key.section, key.key, *((double *)key.ptr));
241 0 : break;
242 :
243 0 : case DATA_VECTOR3F: {
244 0 : Vector3<float> v;
245 0 : if (!parse_array(p, v, ARRAY_SIZE(v))) {
246 0 : printf("Failed to parse Vector3f for %s/%s\n", key.section, key.key);
247 0 : return received_bitmask;
248 : }
249 0 : Vector3f *tv = (Vector3<float> *)key.ptr;
250 0 : *tv = v;
251 : //printf("%s/%s = %f, %f, %f\n", key.section, key.key, v->x, v->y, v->z);
252 0 : break;
253 : }
254 :
255 0 : case DATA_VECTOR3D: {
256 0 : Vector3<double> v;
257 0 : if (!parse_array(p, v, ARRAY_SIZE(v))) {
258 0 : printf("Failed to parse Vector3d for %s/%s\n", key.section, key.key);
259 0 : return received_bitmask;
260 : }
261 0 : Vector3d *tv = (Vector3d *)key.ptr;
262 0 : *tv = v;
263 : //printf("%s/%s = %f, %f, %f\n", key.section, key.key, v->x, v->y, v->z);
264 0 : break;
265 : }
266 :
267 0 : case QUATERNION: {
268 0 : VectorN<float, 4> v;
269 0 : if (!parse_array(p, v, ARRAY_SIZE(v))) {
270 0 : printf("Failed to parse Vector4f for %s/%s\n", key.section, key.key);
271 0 : return received_bitmask;
272 : }
273 0 : Quaternion *tv = static_cast<Quaternion*>(key.ptr);
274 0 : tv->q1 = v[0];
275 0 : tv->q2 = v[1];
276 0 : tv->q3 = v[2];
277 0 : tv->q4 = v[3];
278 0 : break;
279 : }
280 :
281 0 : case BOOLEAN: {
282 0 : bool *b = (bool *)key.ptr;
283 0 : if (strncasecmp(p, "true", 4) == 0) {
284 0 : *b = true;
285 0 : } else if (strncasecmp(p, "false", 5) == 0) {
286 0 : *b = false;
287 : } else {
288 0 : *b = strtoull(p, nullptr, 10) != 0;
289 : }
290 : //printf("%s/%s = %i\n", key.section, key.key, *((unit8_t *)key.ptr));
291 0 : break;
292 : }
293 : }
294 : }
295 :
296 0 : return received_bitmask;
297 : }
298 :
299 : /*
300 : Receive new sensor data from simulator
301 : This is a blocking function
302 : */
303 0 : void JSON::recv_fdm(const struct sitl_input &input)
304 : {
305 : // Receive sensor packet
306 0 : ssize_t ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS);
307 0 : uint32_t wait_ms = UDP_TIMEOUT_MS;
308 :
309 0 : if (state.no_lockstep && ret <= 0) {
310 : // in no_lockstep mode do not block waiting for data; advance time using SITL loop rate
311 0 : if (sitl != nullptr && sitl->loop_rate_hz > 0) {
312 0 : frame_time_us = (uint32_t)(1000000.0f / sitl->loop_rate_hz);
313 : } else {
314 0 : frame_time_us = 10000; // fallback to 10ms
315 : }
316 0 : time_now_us += frame_time_us;
317 0 : time_advance();
318 0 : return;
319 : }
320 :
321 0 : while (ret <= 0) {
322 : //printf("No JSON sensor message received - %s\n", strerror(errno));
323 0 : ret = sock.recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, UDP_TIMEOUT_MS);
324 0 : wait_ms += UDP_TIMEOUT_MS;
325 : // if no sensor message is received after 10 second resend servos, this help cope with SITL and the physics getting out of sync
326 0 : if (wait_ms > 1000) {
327 0 : wait_ms = 0;
328 0 : printf("No JSON sensor message received, resending servos\n");
329 0 : output_servos(input);
330 : }
331 : }
332 :
333 : // convert '\n' into nul
334 0 : while (uint8_t *p = (uint8_t *)memchr(&sensor_buffer[sensor_buffer_len], '\n', ret)) {
335 0 : *p = 0;
336 0 : }
337 0 : sensor_buffer_len += ret;
338 :
339 0 : const uint8_t *p2 = (const uint8_t *)memrchr(sensor_buffer, 0, sensor_buffer_len);
340 0 : if (p2 == nullptr || p2 == sensor_buffer) {
341 0 : return;
342 : }
343 :
344 0 : const uint8_t *p1 = (const uint8_t *)memrchr(sensor_buffer, 0, p2 - sensor_buffer);
345 0 : if (p1 == nullptr) {
346 0 : return;
347 : }
348 :
349 0 : const uint64_t received_bitmask = parse_sensors((const char *)(p1+1));
350 0 : if (received_bitmask == 0) {
351 : // did not receive one of the mandatory fields
352 0 : printf("Did not contain all mandatory fields\n");
353 0 : return;
354 : }
355 :
356 : // Must get either attitude or quaternion fields
357 0 : if ((received_bitmask & (EULER_ATT | QUAT_ATT)) == 0) {
358 0 : printf("Did not receive attitude or quaternion\n");
359 0 : return;
360 : }
361 :
362 0 : if (received_bitmask != last_received_bitmask) {
363 : // some change in the message we have received, print what we got
364 0 : printf("\nJSON received:\n");
365 0 : for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
366 0 : struct keytable &key = keytable[i];
367 0 : if ((received_bitmask & 1ULL << i) == 0) {
368 0 : continue;
369 : }
370 0 : if (strcmp(key.section, "") == 0) {
371 0 : printf("\t%s\n",key.key);
372 : } else {
373 0 : printf("\t%s: %s\n",key.section,key.key);
374 : }
375 : }
376 0 : printf("\n");
377 : }
378 0 : last_received_bitmask = received_bitmask;
379 :
380 0 : memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer));
381 0 : sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer);
382 :
383 0 : accel_body = state.imu.accel_body;
384 0 : gyro = state.imu.gyro;
385 0 : velocity_ef = state.velocity;
386 :
387 0 : if ((received_bitmask & (LATITUDE | LONGITUDE | ALTITUDE)) == (LATITUDE | LONGITUDE | ALTITUDE)) {
388 0 : Location new_loc;
389 0 : new_loc.lat = state.latitude * 1.0e7;
390 0 : new_loc.lng = state.longitude * 1.0e7;
391 0 : new_loc.alt = state.altitude * 100.0;
392 0 : new_loc.relative_alt = false;
393 0 : new_loc.origin_alt = false;
394 0 : new_loc.terrain_alt = false;
395 :
396 0 : if (!home_is_set) {
397 0 : set_start_location(new_loc, 0.0f);
398 : }
399 :
400 0 : position = origin.get_distance_NED_double(new_loc);
401 : } else {
402 0 : position = state.position;
403 0 : position.xy() += origin.get_distance_NE_double(home);
404 : }
405 :
406 0 : if (received_bitmask & TIME_SYNC) {
407 0 : if (use_time_sync != !state.no_time_sync) {
408 0 : use_time_sync = !state.no_time_sync;
409 0 : printf("Forcing use_time_sync=%d\n", int(use_time_sync));
410 0 : if (!use_time_sync) {
411 : // if not using time sync then default EKF type to 10, as
412 : // otherwise EKF is likely to diverge
413 0 : AP_Param::set_default_by_name("AHRS_EKF_TYPE", 10);
414 : }
415 : }
416 : }
417 :
418 : // Handle lockstep setting from JSON
419 0 : if (received_bitmask & LOCKSTEP) {
420 : // state.no_lockstep is true when we DON'T want lockstep
421 : // So we need to check if it changed and act accordingly
422 0 : if (state.no_lockstep != last_no_lockstep) {
423 0 : last_no_lockstep = state.no_lockstep;
424 0 : printf("Lockstep mode changed: no_lockstep=%d (lockstep %s)\n",
425 0 : int(state.no_lockstep), state.no_lockstep ? "DISABLED" : "ENABLED");
426 : }
427 : }
428 :
429 : // deal with euler or quaternion attitude
430 0 : if ((received_bitmask & QUAT_ATT) != 0) {
431 : // if we have a quaternion attitude use it rather than euler
432 0 : state.quaternion.rotation_matrix(dcm);
433 : } else {
434 0 : dcm.from_euler(state.attitude[0], state.attitude[1], state.attitude[2]);
435 : }
436 :
437 0 : if ((received_bitmask & AIRSPEED)) {
438 : // received airspeed directly
439 0 : airspeed = state.airspeed;
440 :
441 0 : airspeed_pitot = state.airspeed;
442 : } else {
443 :
444 : // wind is not supported yet for JSON sim, assume zero for now
445 0 : wind_ef.zero();
446 :
447 : // velocity relative to airmass in Earth's frame
448 0 : velocity_air_ef = velocity_ef - wind_ef;
449 :
450 : // velocity relative to airmass in body frame
451 0 : velocity_air_bf = dcm.transposed() * velocity_air_ef;
452 :
453 : // airspeed fix for eas2tas
454 0 : update_eas_airspeed();
455 : }
456 :
457 0 : if ((received_bitmask & WIND_VEL) != 0) {
458 0 : wind_ef = state.velocity_wind;
459 : }
460 :
461 : // Convert from a meters from origin physics to a lat long alt
462 0 : update_position();
463 :
464 : // update range finder distances
465 0 : for (uint8_t i=0; i<6; i++) {
466 0 : if ((received_bitmask & (RNG_1 << i)) == 0) {
467 0 : continue;
468 : }
469 0 : rangefinder_m[i] = state.rng[i];
470 : }
471 :
472 : // update wind vane
473 0 : if ((received_bitmask & WIND_DIR) != 0) {
474 0 : wind_vane_apparent.direction = state.wind_vane_apparent.direction;
475 : }
476 0 : if ((received_bitmask & WIND_SPD) != 0) {
477 0 : wind_vane_apparent.speed = state.wind_vane_apparent.speed;
478 : }
479 :
480 : // update RC input
481 : static_assert(ARRAY_SIZE(state.rc) <= ARRAY_SIZE(rcin), "JSON rc in size mismatch");
482 0 : uint8_t rc_chan_count = 0;
483 0 : for (uint8_t i=0; i<ARRAY_SIZE(state.rc); i++) {
484 0 : if ((received_bitmask & (RC_1 << i)) != 0) {
485 0 : rcin[i] = (state.rc[i] - 1000.0f) / 1000.0f;
486 0 : rc_chan_count = i+1;
487 : }
488 : }
489 0 : rcin_chan_count = rc_chan_count;
490 :
491 : // update battery state
492 0 : if ((received_bitmask & BAT_VOLT) != 0) {
493 0 : battery_voltage = state.bat_volt;
494 : } else {
495 0 : battery_voltage = sitl->batt_voltage;
496 : }
497 0 : if ((received_bitmask & BAT_AMP) != 0) {
498 0 : battery_current = state.bat_amp;
499 : } else {
500 0 : battery_current = 0.0f;
501 : }
502 : // (temperature is not part of the protocol, just set it explicitly here)
503 0 : battery_temperature_degC = 0.0f;
504 :
505 : double deltat;
506 0 : if (state.timestamp_s < last_timestamp_s) {
507 : // Physics time has gone backwards, don't reset AP
508 0 : printf("Detected physics reset\n");
509 0 : deltat = 0;
510 0 : last_received_bitmask = 0;
511 : } else {
512 0 : deltat = state.timestamp_s - last_timestamp_s;
513 : }
514 0 : time_now_us += deltat * 1.0e6;
515 :
516 0 : if (is_positive(deltat) && deltat < 0.1) {
517 : // Only adjust frame time if we want lockstep
518 : // When no_lockstep is true, skip adjust_frame_time to let SITL run free
519 0 : if (use_time_sync && !state.no_lockstep) {
520 : // time in us to hz
521 0 : adjust_frame_time(1.0 / deltat);
522 : }
523 : // match actual frame rate with desired speedup
524 0 : time_advance();
525 : }
526 0 : last_timestamp_s = state.timestamp_s;
527 0 : frame_counter++;
528 :
529 : #if 0
530 :
531 : float roll, pitch, yaw;
532 : if ((received_bitmask & QUAT_ATT) != 0) {
533 : dcm.to_euler(&roll, &pitch, &yaw);
534 : } else {
535 : roll = state.attitude[0];
536 : pitch = state.attitude[1];
537 : yaw = state.attitude[2];
538 : }
539 :
540 : // @LoggerMessage: JSN1
541 : // @Description: Log data received from JSON simulator
542 : // @Field: TimeUS: Time since system startup (us)
543 : // @Field: TStamp: Simulation's timestamp (s)
544 : // @Field: R: Simulation's roll (rad)
545 : // @Field: P: Simulation's pitch (rad)
546 : // @Field: Y: Simulation's yaw (rad)
547 : // @Field: GX: Simulated gyroscope, X-axis (rad/sec)
548 : // @Field: GY: Simulated gyroscope, Y-axis (rad/sec)
549 : // @Field: GZ: Simulated gyroscope, Z-axis (rad/sec)
550 : AP::logger().WriteStreaming("JSN1", "TimeUS,TStamp,R,P,Y,GX,GY,GZ",
551 : "ssrrrEEE",
552 : "F???????",
553 : "Qfffffff",
554 : AP_HAL::micros64(),
555 : state.timestamp_s,
556 : roll,
557 : pitch,
558 : yaw,
559 : gyro.x,
560 : gyro.y,
561 : gyro.z);
562 :
563 : Vector3f accel_ef = dcm.transposed() * accel_body;
564 :
565 : // @LoggerMessage: JSN2
566 : // @Description: Log data received from JSON simulator
567 : // @Field: TimeUS: Time since system startup (us)
568 : // @Field: VN: simulation's velocity, North-axis (m/s)
569 : // @Field: VE: simulation's velocity, East-axis (m/s)
570 : // @Field: VD: simulation's velocity, Down-axis (m/s)
571 : // @Field: AX: simulation's acceleration, X-axis (m/s^2)
572 : // @Field: AY: simulation's acceleration, Y-axis (m/s^2)
573 : // @Field: AZ: simulation's acceleration, Z-axis (m/s^2)
574 : // @Field: AN: simulation's acceleration, North (m/s^2)
575 : // @Field: AE: simulation's acceleration, East (m/s^2)
576 : // @Field: AD: simulation's acceleration, Down (m/s^2)
577 : AP::logger().WriteStreaming("JSN2", "TimeUS,VN,VE,VD,AX,AY,AZ,AN,AE,AD",
578 : "snnnoooooo",
579 : "F?????????",
580 : "Qfffffffff",
581 : AP_HAL::micros64(),
582 : velocity_ef.x,
583 : velocity_ef.y,
584 : velocity_ef.z,
585 : accel_body.x,
586 : accel_body.y,
587 : accel_body.z,
588 : accel_ef.x,
589 : accel_ef.y,
590 : accel_ef.z);
591 : #endif
592 :
593 : }
594 :
595 : /*
596 : update the JSON simulation by one time step
597 : */
598 0 : void JSON::update(const struct sitl_input &input)
599 : {
600 : // send to JSON model
601 0 : output_servos(input);
602 :
603 : // receive from JSON model
604 0 : recv_fdm(input);
605 :
606 : // update magnetic field
607 : // as the model does not provide mag feild we calculate it from position and attitude
608 0 : update_mag_field_bf();
609 :
610 : // allow for changes in physics step
611 : // Only adjust frame time if lockstep is enabled (no_lockstep is false)
612 0 : if (use_time_sync && !state.no_lockstep) {
613 0 : adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1));
614 : }
615 :
616 : #if 0
617 : // report frame rate
618 : if (frame_counter % 1000 == 0) {
619 : printf("FPS %.2f\n", rate_hz); // this is instantaneous rather than any clever average
620 : }
621 : #endif
622 0 : }
623 :
624 : #endif // AP_SIM_JSON_ENABLED
|
