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 FlightAxis
17 : */
18 :
19 : #include "SIM_FlightAxis.h"
20 :
21 : #if AP_SIM_FLIGHTAXIS_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 : #include <sys/time.h>
31 :
32 : #include <AP_HAL/AP_HAL.h>
33 : #include <AP_Logger/AP_Logger.h>
34 :
35 : extern const AP_HAL::HAL& hal;
36 :
37 : using namespace SITL;
38 :
39 : const AP_Param::GroupInfo FlightAxis::var_info[] = {
40 : // @Param: OPTS
41 : // @DisplayName: FlightAxis options
42 : // @Description: Bitmask of FlightAxis options
43 : // @Bitmask: 0: Reset position on startup
44 : // @Bitmask: 1: Swap first 4 and last 4 servos (for quadplane testing)
45 : // @Bitmask: 2: Demix heli servos and send roll/pitch/collective/yaw
46 : // @Bitmask: 3: Don't print frame rate stats
47 : // @Bitmask: 4: Don't log Dt stats
48 : // @User: Advanced
49 : AP_GROUPINFO("OPTS", 1, FlightAxis, _options, uint32_t(Option::ResetPosition)),
50 :
51 : // @Param: SAMPLEHZ
52 : // @DisplayName: FlightAxis IMU synthetic sample rate
53 : // @Description: FlightAxis IMU synthetic sample rate
54 : // @User: Advanced
55 : AP_GROUPINFO("SAMPLEHZ", 2, FlightAxis, _samplehz, 2000),
56 : AP_GROUPEND
57 : };
58 :
59 : /*
60 : we use a thread for socket creation to reduce the impact of socket
61 : creation latency. These condition variables are used to synchronise
62 : the thread
63 : */
64 : static HAL_BinarySemaphore sockcond1;
65 : static HAL_BinarySemaphore sockcond2;
66 :
67 : // the asprintf() calls are not worth checking for SITL
68 : #pragma GCC diagnostic ignored "-Wunused-result"
69 :
70 : static const struct {
71 : const char *name;
72 : float value;
73 : bool save;
74 : } sim_defaults[] = {
75 : { "BRD_OPTIONS", 0},
76 : { "AHRS_EKF_TYPE", 10 },
77 : { "INS_GYR_CAL", 0 },
78 : { "BATT_MONITOR", 4 },
79 : { "RC1_MIN", 1000, true },
80 : { "RC1_MAX", 2000, true },
81 : { "RC2_MIN", 1000, true },
82 : { "RC2_MAX", 2000, true },
83 : { "RC3_MIN", 1000, true },
84 : { "RC3_MAX", 2000, true },
85 : { "RC4_MIN", 1000, true },
86 : { "RC4_MAX", 2000, true },
87 : { "RC2_REVERSED", 1 }, // interlink has reversed rc2
88 : { "SERVO1_MIN", 1000 },
89 : { "SERVO1_MAX", 2000 },
90 : { "SERVO2_MIN", 1000 },
91 : { "SERVO2_MAX", 2000 },
92 : { "SERVO3_MIN", 1000 },
93 : { "SERVO3_MAX", 2000 },
94 : { "SERVO4_MIN", 1000 },
95 : { "SERVO4_MAX", 2000 },
96 : { "SERVO5_MIN", 1000 },
97 : { "SERVO5_MAX", 2000 },
98 : { "SERVO6_MIN", 1000 },
99 : { "SERVO6_MAX", 2000 },
100 : { "SERVO6_MIN", 1000 },
101 : { "SERVO6_MAX", 2000 },
102 : { "INS_ACC2OFFS_X", 0.001 },
103 : { "INS_ACC2OFFS_Y", 0.001 },
104 : { "INS_ACC2OFFS_Z", 0.001 },
105 : { "INS_ACC2SCAL_X", 1.001 },
106 : { "INS_ACC2SCAL_Y", 1.001 },
107 : { "INS_ACC2SCAL_Z", 1.001 },
108 : { "INS_ACCOFFS_X", 0.001 },
109 : { "INS_ACCOFFS_Y", 0.001 },
110 : { "INS_ACCOFFS_Z", 0.001 },
111 : { "INS_ACCSCAL_X", 1.001 },
112 : { "INS_ACCSCAL_Y", 1.001 },
113 : { "INS_ACCSCAL_Z", 1.001 },
114 : { "RPM1_TYPE", 10 },
115 : };
116 :
117 : /*
118 : get system timestamp in seconds
119 : */
120 0 : static double timestamp_sec()
121 : {
122 : struct timeval tval;
123 0 : gettimeofday(&tval,NULL);
124 0 : return tval.tv_sec + (tval.tv_usec*1.0e-6);
125 : }
126 :
127 : #if defined(CYGWIN_BUILD)
128 : #include <windows.h>
129 :
130 : /*
131 : usleep on cygwin is broken for small times: https://stackoverflow.com/questions/6254703/thread-sleep-for-less-than-1-millisecond
132 : the solution is to define our own busy wait sleep
133 : */
134 : static void us_wait(int64_t microseconds) {
135 : LARGE_INTEGER frequency;
136 : LARGE_INTEGER start_time, current_time;
137 :
138 : QueryPerformanceFrequency(&frequency); // Get the frequency of the performance counter
139 : QueryPerformanceCounter(&start_time); // Get the current time
140 :
141 : // Calculate the number of ticks required for the specified microseconds
142 : int64_t ticks_to_wait = (int64_t)microseconds * frequency.QuadPart / 1000000;
143 : int64_t end_ticks = start_time.QuadPart + ticks_to_wait;
144 :
145 : // Busy-wait until the desired time has elapsed
146 : do {
147 : QueryPerformanceCounter(¤t_time);
148 : } while (current_time.QuadPart < end_ticks);
149 : }
150 : #else
151 : #define us_wait(x) usleep(x)
152 : #endif
153 :
154 0 : FlightAxis::FlightAxis(const char *frame_str) :
155 0 : Aircraft(frame_str)
156 : {
157 0 : AP::sitl()->models.flightaxis_ptr = this;
158 0 : AP_Param::setup_object_defaults(this, var_info);
159 :
160 0 : use_time_sync = false;
161 0 : flightaxis_sync_imus_to_frames = true; // tell the IMUs to advance on each frame that is processed
162 0 : rate_hz = 250 / target_speedup;
163 0 : if(strstr(frame_str, "helidemix") != nullptr) {
164 0 : _options.set(_options | uint32_t(Option::HeliDemix));
165 : }
166 0 : if(strstr(frame_str, "rev4") != nullptr) {
167 0 : _options.set(_options | uint32_t(Option::Rev4Servos));
168 : }
169 :
170 0 : const char *colon = strchr(frame_str, ':');
171 0 : if (colon) {
172 0 : controller_ip = colon+1;
173 : }
174 0 : for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {
175 0 : AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
176 0 : if (sim_defaults[i].save) {
177 : enum ap_var_type ptype;
178 0 : AP_Param *p = AP_Param::find(sim_defaults[i].name, &ptype);
179 0 : if (!p->configured()) {
180 0 : p->save();
181 : }
182 : }
183 : }
184 :
185 0 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&FlightAxis::socket_creator, void), "SocketCreator", 8192,
186 : AP_HAL::Scheduler::PRIORITY_BOOST, 0)) {
187 0 : printf("Failed to create socket_creator thread\n");
188 : }
189 0 : }
190 :
191 : /*
192 : extremely primitive SOAP parser that assumes the format used by FlightAxis
193 : */
194 0 : void FlightAxis::parse_reply(const char *reply)
195 : {
196 0 : const char *reply0 = reply;
197 0 : for (uint16_t i=0; i<num_keys; i++) {
198 0 : const char *p = strstr(reply, keytable[i].key);
199 0 : if (p == nullptr) {
200 0 : p = strstr(reply0, keytable[i].key);
201 : }
202 0 : if (p == nullptr) {
203 0 : printf("Failed to find key %s\n", keytable[i].key);
204 0 : controller_started = false;
205 0 : break;
206 : }
207 0 : p += strlen(keytable[i].key) + 1;
208 : double v;
209 0 : if (strncmp(p, "true", 4) == 0) {
210 0 : v = 1;
211 0 : } else if (strncmp(p, "false", 5) == 0) {
212 0 : v = 0;
213 : } else {
214 0 : v = atof(p);
215 : }
216 0 : keytable[i].ref = v;
217 : // this assumes key order and allows us to decode arrays
218 0 : p = strchr(p, '>');
219 0 : if (p != nullptr) {
220 0 : reply = p;
221 : }
222 : }
223 0 : }
224 :
225 :
226 : /*
227 : make a SOAP request, returning body of reply
228 : */
229 0 : bool FlightAxis::soap_request_start(const char *action, const char *fmt, ...)
230 : {
231 : va_list ap;
232 : char *req1;
233 :
234 0 : if (sock) {
235 0 : delete sock;
236 0 : sock = nullptr;
237 : }
238 :
239 0 : va_start(ap, fmt);
240 0 : vasprintf(&req1, fmt, ap);
241 0 : va_end(ap);
242 :
243 : // consumer/producer pattern
244 0 : while (sock == nullptr) {
245 0 : sock_outsem.wait_blocking();
246 0 : sock = socknext;
247 0 : socknext = nullptr;
248 0 : sock_insem.signal();
249 : }
250 :
251 : char *req;
252 0 : asprintf(&req, R"(POST / HTTP/1.1
253 : soapaction: '%s'
254 : content-length: %u
255 : content-type: text/xml;charset='UTF-8'
256 : Connection: Keep-Alive
257 :
258 : %s)",
259 : action,
260 0 : (unsigned)strlen(req1), req1);
261 0 : sock->send(req, strlen(req));
262 0 : free(req1);
263 0 : free(req);
264 0 : return true;
265 : }
266 :
267 0 : char *FlightAxis::soap_request_end(uint32_t timeout_ms)
268 : {
269 0 : if (!sock) {
270 0 : return nullptr;
271 : }
272 0 : if (!sock->pollin(timeout_ms)) {
273 0 : return nullptr;
274 : }
275 0 : sock->set_blocking(true);
276 0 : ssize_t ret = sock->recv(replybuf, sizeof(replybuf)-1, 1000);
277 0 : if (ret <= 0) {
278 0 : return nullptr;
279 : }
280 0 : replybuf[ret] = 0;
281 :
282 0 : char *p = strstr(replybuf, "Content-Length: ");
283 0 : if (p == nullptr) {
284 0 : delete sock;
285 0 : sock = nullptr;
286 0 : printf("No Content-Length\n");
287 0 : return nullptr;
288 : }
289 :
290 : // get the content length
291 0 : uint32_t content_length = strtoul(p+16, nullptr, 10);
292 0 : char *body = strstr(p, "\r\n\r\n");
293 0 : if (body == nullptr) {
294 0 : printf("No body\n");
295 0 : delete sock;
296 0 : sock = nullptr;
297 0 : return nullptr;
298 : }
299 0 : body += 4;
300 :
301 : // get the rest of the body
302 0 : int32_t expected_length = content_length + (body - replybuf);
303 0 : if (expected_length >= (int32_t)sizeof(replybuf)) {
304 0 : printf("Reply too large %i\n", expected_length);
305 0 : delete sock;
306 0 : sock = nullptr;
307 0 : return nullptr;
308 : }
309 0 : while (ret < expected_length) {
310 0 : ssize_t ret2 = sock->recv(&replybuf[ret], sizeof(replybuf)-(1+ret), 1000);
311 0 : if (ret2 <= 0) {
312 0 : delete sock;
313 0 : sock = nullptr;
314 0 : return nullptr;
315 : }
316 : // nul terminate
317 0 : replybuf[ret+ret2] = 0;
318 0 : ret += ret2;
319 : }
320 0 : delete sock;
321 0 : sock = nullptr;
322 :
323 0 : return strdup(replybuf);
324 : }
325 :
326 0 : bool FlightAxis::exchange_data(const struct sitl_input &input)
327 : {
328 0 : if (!sock &&
329 0 : (!controller_started ||
330 0 : is_zero(next_state.m_flightAxisControllerIsActive) ||
331 0 : !is_zero(next_state.m_resetButtonHasBeenPressed))) {
332 0 : start_controller();
333 : }
334 :
335 0 : if (!sock) {
336 0 : send_request_message(input);
337 : }
338 0 : if (sock) {
339 0 : bool ret = process_reply_message();
340 0 : if (ret) {
341 0 : us_wait(250);
342 0 : send_request_message(input);
343 : }
344 0 : return ret;
345 : }
346 0 : return false;
347 : }
348 :
349 0 : void FlightAxis::start_controller()
350 : {
351 0 : printf("Starting controller at %s\n", controller_ip);
352 : // call a restore first. This allows us to connect after the aircraft is changed in RealFlight
353 0 : soap_request_start("RestoreOriginalControllerDevice", R"(<?xml version='1.0' encoding='UTF-8'?>
354 : <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>
355 : <soap:Body>
356 : <RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>
357 : </soap:Body>
358 : </soap:Envelope>)");
359 0 : free(soap_request_end(1000UL));
360 0 : if(option_is_set(Option::ResetPosition)) {
361 0 : soap_request_start("ResetAircraft", R"(<?xml version='1.0' encoding='UTF-8'?>
362 : <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>
363 : <soap:Body>
364 : <ResetAircraft><a>1</a><b>2</b></ResetAircraft>
365 : </soap:Body>
366 : </soap:Envelope>)");
367 0 : free(soap_request_end(1000UL));
368 : }
369 0 : soap_request_start("InjectUAVControllerInterface", R"(<?xml version='1.0' encoding='UTF-8'?>
370 : <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>
371 : <soap:Body>
372 : <InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>
373 : </soap:Body>
374 : </soap:Envelope>)");
375 0 : free(soap_request_end(1000UL));
376 0 : activation_frame_counter = frame_counter;
377 0 : controller_started = true;
378 0 : }
379 :
380 0 : void FlightAxis::send_request_message(const struct sitl_input &input)
381 : {
382 : // maximum number of servos to send is 12 with new FlightAxis
383 0 : float scaled_servos[12] {};
384 0 : uint16_t valid_channels = 0;
385 0 : for (uint8_t i=0; i<ARRAY_SIZE(scaled_servos); i++) {
386 0 : if (input.servos[i] == 0) {
387 0 : continue;
388 : }
389 0 : scaled_servos[i] = (input.servos[i] - 1000) / 1000.0f;
390 0 : valid_channels |= 1U << i;
391 : }
392 :
393 0 : if (option_is_set(Option::Rev4Servos)) {
394 : // swap first 4 and second 4 servos, for quadplane testing
395 : float saved[4];
396 0 : memcpy(saved, &scaled_servos[0], sizeof(saved));
397 0 : memcpy(&scaled_servos[0], &scaled_servos[4], sizeof(saved));
398 0 : memcpy(&scaled_servos[4], saved, sizeof(saved));
399 : // and the validity flags:
400 0 : uint16_t new_channels = 0;
401 0 : new_channels |= (valid_channels & 0b1111111100000000);
402 0 : new_channels |= (valid_channels & 0b11110000) >> 4;
403 0 : new_channels |= (valid_channels & 0b00001111) << 4;
404 0 : valid_channels = new_channels;
405 : }
406 :
407 0 : if (option_is_set(Option::HeliDemix)) {
408 : // FlightAxis expects "roll/pitch/collective/yaw" input
409 0 : float swash1 = scaled_servos[0];
410 0 : float swash2 = scaled_servos[1];
411 0 : float swash3 = scaled_servos[2];
412 :
413 0 : float roll_rate = swash1 - swash2;
414 0 : float pitch_rate = ((swash1+swash2) / 2.0f - swash3);
415 0 : float col = (swash1 + swash2 + swash3) / 3.0;
416 :
417 0 : scaled_servos[0] = constrain_float(roll_rate + 0.5, 0, 1);
418 0 : scaled_servos[1] = constrain_float(pitch_rate + 0.5, 0, 1);
419 0 : scaled_servos[2] = constrain_float(col, 0, 1);
420 : }
421 :
422 0 : const uint16_t channels = hal.scheduler->is_system_initialized()?4095:0;
423 0 : soap_request_start("ExchangeData", R"(<?xml version='1.0' encoding='UTF-8'?><soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>
424 : <soap:Body>
425 : <ExchangeData>
426 : <pControlInputs>
427 : <m-selectedChannels>%u</m-selectedChannels>
428 : <m-channelValues-0to1>
429 : <item>%.4f</item>
430 : <item>%.4f</item>
431 : <item>%.4f</item>
432 : <item>%.4f</item>
433 : <item>%.4f</item>
434 : <item>%.4f</item>
435 : <item>%.4f</item>
436 : <item>%.4f</item>
437 : <item>%.4f</item>
438 : <item>%.4f</item>
439 : <item>%.4f</item>
440 : <item>%.4f</item>
441 : </m-channelValues-0to1>
442 : </pControlInputs>
443 : </ExchangeData>
444 : </soap:Body>
445 : </soap:Envelope>)",
446 : channels,
447 0 : scaled_servos[0],
448 0 : scaled_servos[1],
449 0 : scaled_servos[2],
450 0 : scaled_servos[3],
451 0 : scaled_servos[4],
452 0 : scaled_servos[5],
453 0 : scaled_servos[6],
454 0 : scaled_servos[7],
455 0 : scaled_servos[8],
456 0 : scaled_servos[9],
457 0 : scaled_servos[10],
458 0 : scaled_servos[11]);
459 0 : }
460 :
461 0 : bool FlightAxis::process_reply_message()
462 : {
463 0 : char *reply = nullptr;
464 0 : reply = soap_request_end(1);
465 0 : if (reply == nullptr) {
466 0 : sock_error_count++;
467 0 : if (sock_error_count >= 10000 && timestamp_sec() - last_recv_sec > 1) {
468 0 : printf("socket timeout\n");
469 0 : delete sock;
470 0 : sock = nullptr;
471 0 : sock_error_count = 0;
472 0 : last_recv_sec = timestamp_sec();
473 : }
474 : }
475 :
476 0 : if (reply) {
477 0 : sock_error_count = 0;
478 0 : last_recv_sec = timestamp_sec();
479 0 : parse_reply(reply);
480 0 : free(reply);
481 0 : return true;
482 : }
483 0 : return false;
484 : }
485 :
486 0 : bool FlightAxis::wait_for_sample(const struct sitl_input &input)
487 : {
488 0 : double dt_seconds = 0;
489 0 : const float SAMPLE_INTERVAL_S = 1.0f / _samplehz.get();
490 0 : const float SAMPLE_INTERVAL_MIN_S = (SAMPLE_INTERVAL_S/2); // smallest interval before moving on to the next
491 :
492 0 : double lastt_s = state.m_currentPhysicsTime_SEC;
493 :
494 0 : if (is_zero(prev_state.m_currentPhysicsTime_SEC)) {
495 0 : if (exchange_data(input)) {
496 0 : state = prev_state = next_state;
497 0 : sample_interval_s = SAMPLE_INTERVAL_S;
498 : }
499 0 : return false;
500 : }
501 :
502 : // get a new frame if we have processed all the whole samples in the previous one
503 0 : if (state.m_currentPhysicsTime_SEC + SAMPLE_INTERVAL_MIN_S >= next_state.m_currentPhysicsTime_SEC) {
504 0 : prev_state = next_state;
505 : do {
506 0 : if (exchange_data(input)) { // updates next_state
507 0 : state = prev_state; // ensure state is at the beginning of the data range
508 :
509 0 : if (is_zero(last_time_s)) {
510 0 : last_time_s = state.m_currentPhysicsTime_SEC - SAMPLE_INTERVAL_S;
511 : }
512 0 : dt_seconds = next_state.m_currentPhysicsTime_SEC - last_dt_sample_s;
513 0 : last_delta_time_s = next_state.m_currentPhysicsTime_SEC - last_time_s;
514 0 : last_dt_sample_s = next_state.m_currentPhysicsTime_SEC;
515 : }
516 0 : } while (is_zero(dt_seconds));
517 : // adjust the sample interval so that the number of samples in a frame is an integer
518 : // this prevents very small dt's which can cause havoc with flight control
519 0 : sample_interval_s = dt_seconds / roundf(dt_seconds / SAMPLE_INTERVAL_S);
520 : }
521 :
522 0 : double new_time = MIN(next_state.m_currentPhysicsTime_SEC, state.m_currentPhysicsTime_SEC + sample_interval_s);
523 0 : state = interpolate_frame(next_state, prev_state, new_time);
524 :
525 0 : socket_frame_counter++;
526 :
527 0 : double dt = state.m_currentPhysicsTime_SEC - lastt_s;
528 0 : if (dt > 0 && dt < 0.1) {
529 0 : if (average_frame_time_s < 1.0e-6) {
530 0 : average_frame_time_s = dt;
531 : }
532 0 : average_frame_time_s = average_frame_time_s * 0.98 + dt * 0.02;
533 : }
534 : #if HAL_LOGGING_ENABLED
535 0 : if (!(option_is_set(Option::NoDtLog))) {
536 0 : uint64_t time_now = uint64_t(state.m_currentPhysicsTime_SEC * 1.0e6);
537 : // @LoggerMessage: RF
538 : // @Description: RealFlight mode messages
539 : // @Field: TimeUS: Time since system startup
540 : // @Field: Dt: delta time between this frame and the previous frame
541 : // @Field: Fps: frames-per-second implied by the current delta time
542 0 : AP::logger().WriteStreaming("RF", "TimeUS,Dt,Fps", "QdI", time_now, dt, uint32_t(roundf(1/dt)));
543 : }
544 : #endif
545 0 : if (last_time_s > 0) {
546 0 : if (dt_seconds > 0 && dt_seconds < 0.1) {
547 0 : if (is_zero(average_delta_time_s)) {
548 0 : average_delta_time_s = dt_seconds;
549 : }
550 0 : average_delta_time_s = average_delta_time_s * 0.98 + dt_seconds * 0.02;
551 : }
552 : }
553 :
554 0 : if (is_equal(last_time_s, state.m_currentPhysicsTime_SEC)) {
555 0 : AP_HAL::panic("Time did not move");
556 : }
557 :
558 0 : return true;
559 : }
560 :
561 : /*
562 : update the FlightAxis simulation by one time step
563 : */
564 0 : void FlightAxis::update(const struct sitl_input &input)
565 : {
566 0 : if (!wait_for_sample(input)) {
567 0 : return;
568 : }
569 :
570 0 : double dt_seconds = state.m_currentPhysicsTime_SEC - last_time_s;
571 0 : if (dt_seconds < 0) {
572 : // cope with restarting RealFlight while connected
573 0 : initial_time_s = time_now_us * 1.0e-6f;
574 0 : last_time_s = state.m_currentPhysicsTime_SEC;
575 0 : position_offset.zero();
576 0 : return;
577 : }
578 :
579 : // initialize timer
580 0 : if (initial_time_s <= 0) {
581 0 : time_now_us = 1; // prevent time going backwards
582 0 : dt_seconds = 0.001f;
583 0 : initial_time_s = state.m_currentPhysicsTime_SEC - dt_seconds;
584 : }
585 :
586 : /*
587 : the quaternion convention in realflight seems to have Z negative
588 : */
589 0 : Quaternion quat(state.m_orientationQuaternion_W,
590 0 : state.m_orientationQuaternion_Y,
591 0 : state.m_orientationQuaternion_X,
592 0 : -state.m_orientationQuaternion_Z);
593 0 : quat.rotation_matrix(dcm);
594 :
595 0 : gyro = Vector3f(radians(constrain_float(state.m_rollRate_DEGpSEC, -2000, 2000)),
596 0 : radians(constrain_float(state.m_pitchRate_DEGpSEC, -2000, 2000)),
597 0 : -radians(constrain_float(state.m_yawRate_DEGpSEC, -2000, 2000))) * target_speedup;
598 :
599 0 : velocity_ef = Vector3f(state.m_velocityWorldU_MPS,
600 0 : state.m_velocityWorldV_MPS,
601 0 : state.m_velocityWorldW_MPS);
602 0 : position = Vector3d(state.m_aircraftPositionY_MTR,
603 : state.m_aircraftPositionX_MTR,
604 0 : -state.m_altitudeASL_MTR - home.alt*0.01);
605 0 : position.xy() += origin.get_distance_NE_double(home);
606 :
607 0 : accel_body = {
608 0 : float(state.m_accelerationBodyAX_MPS2),
609 0 : float(state.m_accelerationBodyAY_MPS2),
610 0 : float(state.m_accelerationBodyAZ_MPS2)
611 : };
612 :
613 : // accel on the ground is nasty in realflight, and prevents helicopter disarm
614 0 : if (!is_zero(state.m_isTouchingGround)) {
615 0 : Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds;
616 0 : accel_ef.z -= GRAVITY_MSS;
617 0 : accel_body = dcm.transposed() * accel_ef;
618 : }
619 :
620 : // limit to 16G to match pixhawk
621 0 : float a_limit = GRAVITY_MSS*16;
622 0 : accel_body.x = constrain_float(accel_body.x, -a_limit, a_limit);
623 0 : accel_body.y = constrain_float(accel_body.y, -a_limit, a_limit);
624 0 : accel_body.z = constrain_float(accel_body.z, -a_limit, a_limit);
625 :
626 : // offset based on first position to account for offset in RF world
627 0 : if (position_offset.is_zero() || !is_zero(state.m_resetButtonHasBeenPressed)) {
628 0 : position_offset = position;
629 : }
630 0 : position -= position_offset;
631 :
632 0 : airspeed = state.m_airspeed_MPS;
633 :
634 : /* for pitot airspeed we need the airspeed along the X axis. We
635 : can't get that from m_airspeed_MPS, so instead we calculate it
636 : from wind vector and ground speed
637 : */
638 0 : wind_ef = Vector3f(state.m_windY_MPS,state.m_windX_MPS,state.m_windZ_MPS);
639 0 : Vector3f airspeed_3d_ef = velocity_ef - wind_ef;
640 0 : Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef);
641 :
642 0 : if (last_imu_rotation != ROTATION_NONE) {
643 0 : airspeed3d = sitl->ahrs_rotation * airspeed3d;
644 : }
645 0 : airspeed_pitot = MAX(airspeed3d.x,0);
646 :
647 : #if 0
648 : printf("WIND: %.1f %.1f %.1f AS3D %.1f %.1f %.1f\n",
649 : state.m_windX_MPS,
650 : state.m_windY_MPS,
651 : state.m_windZ_MPS,
652 : airspeed3d.x,
653 : airspeed3d.y,
654 : airspeed3d.z);
655 : #endif
656 :
657 0 : battery_voltage = MAX(state.m_batteryVoltage_VOLTS, 0);
658 0 : battery_current = MAX(state.m_batteryCurrentDraw_AMPS, 0);
659 : // (temperature is not part of the protocol, so it is explicitly set constant here)
660 0 : battery_temperature_degC = 0.0f;
661 0 : rpm[0] = state.m_heliMainRotorRPM;
662 0 : rpm[1] = state.m_propRPM;
663 0 : motor_mask = 3;
664 :
665 : /*
666 : the interlink interface supports 12 input channels
667 : */
668 0 : rcin_chan_count = 12;
669 0 : for (uint8_t i=0; i<rcin_chan_count; i++) {
670 0 : rcin[i] = state.rcin[i];
671 : }
672 :
673 0 : update_position();
674 0 : time_advance();
675 0 : uint64_t new_time_us = (state.m_currentPhysicsTime_SEC - initial_time_s)*1.0e6;
676 0 : if (new_time_us < time_now_us) {
677 : //printf("Time going backwards by %u\n", uint32_t(time_now_us - new_time_us));
678 0 : uint64_t dt_us = time_now_us - new_time_us;
679 0 : if (dt_us > 500000) {
680 : // time going backwards
681 0 : time_now_us = new_time_us;
682 : }
683 : } else {
684 0 : uint64_t dt_us = new_time_us - time_now_us;
685 0 : const uint64_t glitch_threshold_us = 50000;
686 0 : const uint64_t glitch_max_us = 2000000;
687 0 : if (dt_us > glitch_threshold_us && dt_us < glitch_max_us) {
688 : // we've had a network glitch, compensate by advancing initial time
689 0 : float adjustment_s = (dt_us-glitch_threshold_us)*1.0e-6;
690 0 : initial_time_s += adjustment_s;
691 0 : printf("glitch %.2fs\n", adjustment_s);
692 0 : dt_us = glitch_threshold_us;
693 0 : glitch_count++;
694 : }
695 0 : if (dt_us) { // adjust the frame time to match the dt which will vary over time
696 0 : adjust_frame_time(1.0e6/dt_us);
697 : }
698 0 : time_now_us += dt_us;
699 : }
700 :
701 0 : last_time_s = state.m_currentPhysicsTime_SEC;
702 0 : last_velocity_ef = velocity_ef;
703 :
704 : // update magnetic field
705 0 : update_mag_field_bf();
706 :
707 : // one rangefinder
708 0 : if (is_positive(dcm.c.z)) {
709 0 : rangefinder_m[0] = state.m_altitudeAGL_MTR / dcm.c.z;
710 : } else {
711 0 : rangefinder_m[0] = nanf("");
712 : }
713 :
714 0 : report_FPS();
715 : }
716 :
717 0 : struct FlightAxis::state FlightAxis::interpolate_frame(struct state& new_state, struct state& old_state, double new_time)
718 : {
719 0 : struct state intermediate_state = old_state;
720 0 : double dt = new_state.m_currentPhysicsTime_SEC - old_state.m_currentPhysicsTime_SEC;
721 0 : if (!is_positive(dt)) {
722 0 : return new_state;
723 : }
724 :
725 0 : double interval = new_time - old_state.m_currentPhysicsTime_SEC;
726 :
727 : #define INTERPOLATE(name) (intermediate_state.name = (old_state.name + interval * (new_state.name - old_state.name) / dt))
728 0 : INTERPOLATE(m_airspeed_MPS);
729 0 : INTERPOLATE(m_altitudeAGL_MTR);
730 0 : INTERPOLATE(m_pitchRate_DEGpSEC);
731 0 : INTERPOLATE(m_rollRate_DEGpSEC);
732 0 : INTERPOLATE(m_yawRate_DEGpSEC);
733 0 : INTERPOLATE(m_aircraftPositionX_MTR);
734 0 : INTERPOLATE(m_aircraftPositionY_MTR);
735 0 : INTERPOLATE(m_velocityWorldU_MPS);
736 0 : INTERPOLATE(m_velocityWorldV_MPS);
737 0 : INTERPOLATE(m_velocityWorldW_MPS);
738 0 : INTERPOLATE(m_accelerationBodyAX_MPS2);
739 0 : INTERPOLATE(m_accelerationBodyAY_MPS2);
740 0 : INTERPOLATE(m_accelerationBodyAZ_MPS2);
741 0 : INTERPOLATE(m_windX_MPS);
742 0 : INTERPOLATE(m_windY_MPS);
743 0 : INTERPOLATE(m_windZ_MPS);
744 0 : INTERPOLATE(m_propRPM);
745 0 : INTERPOLATE(m_heliMainRotorRPM);
746 0 : INTERPOLATE(m_batteryVoltage_VOLTS);
747 0 : INTERPOLATE(m_batteryCurrentDraw_AMPS);
748 0 : INTERPOLATE(m_orientationQuaternion_X);
749 0 : INTERPOLATE(m_orientationQuaternion_Y);
750 0 : INTERPOLATE(m_orientationQuaternion_Z);
751 0 : INTERPOLATE(m_orientationQuaternion_W);
752 0 : intermediate_state.m_currentPhysicsTime_SEC = new_time;
753 :
754 0 : return intermediate_state;
755 : }
756 :
757 : /*
758 : report frame rates
759 : */
760 0 : void FlightAxis::report_FPS(void)
761 : {
762 0 : if (frame_counter++ % _samplehz == 0) {
763 0 : if (!is_zero(last_frame_count_s)) {
764 0 : uint64_t frames = socket_frame_counter - last_socket_frame_counter;
765 0 : last_socket_frame_counter = socket_frame_counter;
766 0 : double dt = state.m_currentPhysicsTime_SEC - last_frame_count_s;
767 0 : if(!option_is_set(Option::SilenceFPS)) {
768 0 : printf("%.2f/%.2f FPS avg=%.2f FPS net=%.2f glitches=%u\n",
769 0 : frames / dt, _samplehz / dt, 1.0/average_frame_time_s, 1.0/average_delta_time_s, unsigned(glitch_count));
770 : }
771 : } else {
772 0 : printf("Initial position %f %f %f\n", position.x, position.y, position.z);
773 : }
774 0 : last_frame_count_s = state.m_currentPhysicsTime_SEC;
775 : }
776 0 : }
777 :
778 : #include <errno.h>
779 :
780 0 : void FlightAxis::socket_creator(void)
781 : {
782 0 : socket_pid = getpid();
783 : while (true) {
784 0 : while (socknext != nullptr) {
785 0 : sock_insem.wait_blocking();
786 : }
787 0 : auto *sck = NEW_NOTHROW SocketAPM_native(false);
788 0 : if (sck == nullptr) {
789 0 : us_wait(500);
790 0 : continue;
791 : }
792 : /*
793 : don't let the connection take more than 10ms (100Hz). Longer
794 : than this and we are better off trying for a new socket
795 : */
796 0 : if (!sck->connect_timeout(controller_ip, controller_port, 10)) {
797 0 : ::printf("connect failed\n");
798 0 : delete sck;
799 0 : us_wait(500);
800 0 : continue;
801 : }
802 0 : sck->set_blocking(false);
803 0 : socknext = sck;
804 0 : sock_outsem.signal();
805 0 : }
806 : }
807 :
808 : #endif // AP_SIM_FLIGHTAXIS_ENABLED
|
