Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
4 :
5 : #include "AP_HAL_SITL.h"
6 : #include "AP_HAL_SITL_Namespace.h"
7 : #include "HAL_SITL_Class.h"
8 : #include "UARTDriver.h"
9 : #include "Scheduler.h"
10 : #include "CANSocketIface.h"
11 : #include "SITL_Multicast.h"
12 :
13 : #include <stdio.h>
14 : #include <signal.h>
15 : #include <unistd.h>
16 : #include <stdlib.h>
17 : #include <string.h>
18 : #include <errno.h>
19 : #include <sys/types.h>
20 : #include <sys/select.h>
21 : #include <sys/socket.h>
22 : #include <netinet/in.h>
23 : #include <arpa/inet.h>
24 : #include <fcntl.h>
25 : #include <poll.h>
26 : #include <time.h>
27 :
28 : #include <AP_Param/AP_Param.h>
29 : #include <SITL/SIM_JSBSim.h>
30 : #include <AP_HAL/utility/Socket_native.h>
31 :
32 : #include <AP_HAL/SIMState.h>
33 :
34 : extern const AP_HAL::HAL& hal;
35 :
36 : using namespace HALSITL;
37 :
38 : /*
39 : setup for SITL handling
40 : */
41 2094 : void SITL_State::_sitl_setup()
42 : {
43 : #if !defined(__CYGWIN__) && !defined(__CYGWIN64__)
44 2094 : _parent_pid = getppid();
45 : #endif
46 :
47 2094 : fprintf(stdout, "Starting SITL input\n");
48 :
49 2094 : _sitl = AP::sitl();
50 :
51 2094 : if (_sitl != nullptr) {
52 : // setup some initial values
53 2094 : _update_airspeed(0);
54 : #if AP_SIM_SOLOGIMBAL_ENABLED
55 2094 : if (enable_gimbal) {
56 : // the gimbal connects back to the vehicle's SERIAL1 MAVLink endpoint
57 1 : const char *serial1_path = _serial_path[1];
58 1 : if (strncmp(serial1_path, "uds:", 4) == 0) {
59 0 : gimbal = NEW_NOTHROW SITL::SoloGimbal(serial1_path + 4);
60 : } else {
61 1 : gimbal = NEW_NOTHROW SITL::SoloGimbal(base_port() + 2);
62 : }
63 : }
64 : #endif
65 :
66 : #if AP_SIM_PRECLAND_ENABLED
67 : // seed the precland simulator's beacon location from home. This
68 : // is done before parameters are loaded from storage so that the
69 : // location-is-zero check inside set_default_location sees the
70 : // unloaded (zero) values; parameters present in storage then
71 : // overwrite the seed, while any absent ones keep it
72 2094 : const Location &home = sitl_model->get_home();
73 2094 : _sitl->precland_sim.set_default_location(home.lat * 1.0e-7f, home.lng * 1.0e-7f, static_cast<int16_t>(sitl_model->get_home_yaw()));
74 : #endif
75 :
76 2094 : if (_use_fg_view) {
77 0 : fprintf(stdout, "FGView: %s:%u\n", _fg_address, _fg_view_port);
78 0 : fg_socket.connect(_fg_address, _fg_view_port);
79 : }
80 :
81 2094 : fprintf(stdout, "Using Irlock at port : %d\n", _irlock_port);
82 2094 : _sitl->irlock_port = _irlock_port;
83 :
84 2094 : _sitl->rcin_port = _rcin_port;
85 2094 : _sitl->rcin_path = _rcin_path;
86 :
87 2094 : fprintf(stdout, "Using \\clock topic for DDS timing: %s\n", _use_dds_sim_time ? "enabled" : "disabled");
88 2094 : _sitl->use_dds_sim_time = _use_dds_sim_time;
89 : }
90 :
91 : // start with non-zero clock
92 2094 : hal.scheduler->stop_clock(1);
93 2094 : }
94 :
95 :
96 : /*
97 : step the FDM by one time step
98 : */
99 187275482 : void SITL_State::_fdm_input_step(void)
100 : {
101 187275482 : _fdm_input_local();
102 :
103 : /* make sure we die if our parent dies */
104 187275482 : if (kill(_parent_pid, 0) != 0) {
105 0 : exit(1);
106 : }
107 :
108 187275482 : if (_scheduler->interrupts_are_blocked() || _sitl == nullptr) {
109 7157 : return;
110 : }
111 :
112 187268325 : _scheduler->sitl_begin_atomic();
113 :
114 187268325 : if (_update_count == 0 && _sitl != nullptr) {
115 0 : HALSITL::Scheduler::timer_event();
116 0 : _scheduler->sitl_end_atomic();
117 0 : return;
118 : }
119 :
120 187268325 : if (_sitl != nullptr) {
121 187268325 : _update_airspeed(_sitl->state.airspeed);
122 187268325 : _update_rangefinder();
123 : }
124 :
125 : // trigger all APM timers.
126 187268325 : HALSITL::Scheduler::timer_event();
127 187268324 : _scheduler->sitl_end_atomic();
128 : }
129 :
130 :
131 148004951 : void SITL_State::wait_clock(uint64_t wait_time_usec)
132 : {
133 148004951 : float speedup = sitl_model->get_speedup();
134 148004296 : if (speedup < 1) {
135 : // for purposes of sleeps treat low speedups as 1
136 0 : speedup = 1.0;
137 : }
138 444700889 : while (AP_HAL::micros64() < wait_time_usec) {
139 475250627 : if (hal.scheduler->in_main_thread() ||
140 9167338 : Scheduler::from(hal.scheduler)->semaphore_wait_hack_required()) {
141 187275482 : _fdm_input_step();
142 : } else {
143 : #ifdef CYGWIN_BUILD
144 : if (speedup > 2 && hal.util->get_soft_armed()) {
145 : const char *current_thread = Scheduler::from(hal.scheduler)->get_current_thread_name();
146 : if (current_thread && strcmp(current_thread, "Scripting") == 0) {
147 : // this effectively does a yield of the CPU. The
148 : // granularity of sleeps on cygwin is very high,
149 : // so this is needed for good thread performance
150 : // in scripting. We don't do this at low speedups
151 : // as it causes the cpu to run hot
152 : // We also don't do it while disarmed, as lua performance is less
153 : // critical while disarmed
154 : usleep(0);
155 : continue;
156 : }
157 : }
158 : #endif
159 : // most devices can't sleep for 10us - so this is also
160 : // essentially a yield. At 30x speedup a 10us wall-clock
161 : // sleep here can equate to your thread sleeping for 300us
162 : // of simulated time
163 9167281 : usleep(10);
164 : }
165 : }
166 : // check the outbound TCP queue size. If it is too long then
167 : // MAVProxy/pymavlink take too long to process packets and it ends
168 : // up seeing traffic well into our past and hits time-out
169 : // conditions.
170 148029003 : if (speedup > 1 && hal.scheduler->in_main_thread()) {
171 : while (true) {
172 87641061 : HALSITL::UARTDriver *uart = (HALSITL::UARTDriver*)hal.serial(0);
173 87641061 : const int queue_length = uart->get_system_outqueue_length();
174 : // ::fprintf(stderr, "queue_length=%d\n", (signed)queue_length);
175 87641061 : if (queue_length < uart->get_system_outqueue_limit()) {
176 86775524 : break;
177 : }
178 865537 : _serial_0_outqueue_full_count++;
179 865537 : uart->handle_reading_from_device_to_readbuffer();
180 865537 : usleep(1000);
181 865537 : }
182 : }
183 148026118 : }
184 :
185 : /*
186 : output current state to flightgear
187 : */
188 0 : void SITL_State::_output_to_flightgear(void)
189 : {
190 0 : SITL::FGNetFDM fdm {};
191 0 : const SITL::sitl_fdm &sfdm = _sitl->state;
192 :
193 0 : fdm.version = 0x18;
194 0 : fdm.padding = 0;
195 0 : fdm.longitude = DEG_TO_RAD_DOUBLE*sfdm.longitude;
196 0 : fdm.latitude = DEG_TO_RAD_DOUBLE*sfdm.latitude;
197 0 : fdm.altitude = sfdm.altitude;
198 0 : fdm.agl = sfdm.altitude;
199 0 : fdm.phi = radians(sfdm.rollDeg);
200 0 : fdm.theta = radians(sfdm.pitchDeg);
201 0 : fdm.psi = radians(sfdm.yawDeg);
202 0 : fdm.vcas = sfdm.velocity_air_bf.length()/0.3048;
203 0 : if (_vehicle == ArduCopter) {
204 0 : fdm.num_engines = 4;
205 0 : if (_model_str != nullptr && strstr(_model_str, "heliquad") != nullptr) {
206 : // copter variable-pitch quad (heli-quad). The packet has no
207 : // field for blade collective, so it rides in an unused
208 : // per-engine field which only the heliquad aircraft model XML
209 : // reads:
210 : // rpm[i] - rotor speed, from the shared RSC output
211 : // fuel_flow[i] - blade collective, -1..1 about trim
212 : // collective servos are SERVO1-4, RSC is SERVO8 (copter-heli convention)
213 0 : const float rsc = constrain_float((pwm_output[7]-1000)*0.001f, 0, 1);
214 0 : for (uint8_t i=0; i<4; i++) {
215 0 : fdm.rpm[i] = rsc * 1500; // nominal head speed, rev/min
216 0 : fdm.fuel_flow[i] = constrain_float((pwm_output[i]-1500)*0.002f, -1, 1);
217 : }
218 0 : } else {
219 : // normal direct-drive fixed-pitch quadcopter
220 0 : for (uint8_t i=0; i<4; i++) {
221 0 : fdm.rpm[i] = constrain_float((pwm_output[i]-1000), 0, 1000);
222 : }
223 : }
224 : } else {
225 0 : fdm.num_engines = 4;
226 0 : fdm.rpm[0] = constrain_float((pwm_output[2]-1000)*3, 0, 3000);
227 : // for quadplane
228 0 : fdm.rpm[1] = constrain_float((pwm_output[5]-1000)*12, 0, 12000);
229 0 : fdm.rpm[2] = constrain_float((pwm_output[6]-1000)*12, 0, 12000);
230 0 : fdm.rpm[3] = constrain_float((pwm_output[7]-1000)*12, 0, 12000);
231 : }
232 0 : fdm.ByteSwap();
233 :
234 0 : fg_socket.send(&fdm, sizeof(fdm));
235 0 : }
236 :
237 : /*
238 : get FDM input from a local model
239 : */
240 187275482 : void SITL_State::_fdm_input_local(void)
241 : {
242 187275482 : if (_sitl == nullptr) {
243 0 : return;
244 : }
245 : struct sitl_input input;
246 :
247 : // construct servos structure for FDM
248 187275482 : _simulator_servos(input);
249 :
250 : #if AP_SIM_JSON_MASTER_ENABLED
251 : // read servo inputs from ride along flight controllers
252 187275482 : ride_along.receive(input);
253 : #endif // AP_SIM_JSON_MASTER_ENABLED
254 :
255 : // replace outputs from multicast
256 187275482 : multicast_servo_update(input);
257 :
258 : // update the model
259 187275482 : sitl_model->update_home();
260 187275482 : sitl_model->update_model(input);
261 :
262 : // get FDM output from the model
263 187275482 : sitl_model->fill_fdm(_sitl->state);
264 :
265 : #if HAL_NUM_CAN_IFACES
266 187275482 : if (CANIface::num_interfaces() > 0) {
267 609376 : multicast_state_send();
268 : }
269 : #endif
270 :
271 : #if AP_SIM_JSON_MASTER_ENABLED
272 : // output JSON state to ride along flight controllers
273 187275482 : ride_along.send(_sitl->state,sitl_model->get_position_relhome());
274 : #endif // AP_SIM_JSON_MASTER_ENABLED
275 :
276 187275482 : sim_update();
277 :
278 187275482 : if (_use_fg_view) {
279 0 : _output_to_flightgear();
280 : }
281 :
282 : // update simulation time
283 187275482 : hal.scheduler->stop_clock(_sitl->state.timestamp_us);
284 :
285 187275482 : set_height_agl();
286 :
287 187275482 : _update_count++;
288 : }
289 :
290 : /*
291 : create sitl_input structure for sending to FDM
292 : */
293 187275482 : void SITL_State::_simulator_servos(struct sitl_input &input)
294 : {
295 187275482 : if (_sitl == nullptr) {
296 0 : return;
297 : }
298 :
299 : #if AP_SIM_WIND_SIMULATION_ENABLED
300 187275482 : hal.simstate->update_simulated_wind(input);
301 : #endif // AP_SIM_WIND_SIMULATION_ENABLED
302 :
303 6180090906 : for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
304 5992815424 : if (pwm_output[i] == 0xFFFF) {
305 0 : input.servos[i] = 0;
306 : } else {
307 5992815424 : input.servos[i] = pwm_output[i];
308 : }
309 : }
310 :
311 : // FETtec ESC simulation support. Input signals of 1000-2000
312 : // are positive thrust, 0 to 1000 are negative thrust. Deeper
313 : // changes required to support negative thrust - potentially
314 : // adding a field to input.
315 187275482 : if (_sitl->fetteconewireesc_sim.enabled()) {
316 657815 : _sitl->fetteconewireesc_sim.update_sitl_input_pwm(input);
317 21707895 : for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
318 21050080 : if (input.servos[i] != 0 && input.servos[i] < 1000) {
319 0 : AP_HAL::panic("Bad input servo value (%u)", input.servos[i]);
320 : }
321 : }
322 : }
323 :
324 : #if AP_SIM_VOLZ_ENABLED
325 : // update simulation input based on data received via "serial" to
326 : // Volz servos:
327 187275482 : if (_sitl->volz_sim.enabled()) {
328 217236 : _sitl->volz_sim.update_sitl_input_pwm(input);
329 7168788 : for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
330 6951552 : if (input.servos[i] != 0 && input.servos[i] < 1000) {
331 0 : AP_HAL::panic("Bad input servo value (%u)", input.servos[i]);
332 : }
333 : }
334 : }
335 : #endif
336 :
337 187275482 : const float engine_mul = _sitl->engine_mul.get();
338 187275482 : const uint32_t engine_fail = _sitl->engine_fail.get();
339 :
340 : // apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter
341 6180090906 : for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
342 5992815424 : if (engine_fail & (1<<i)) {
343 325288 : if (_vehicle != Rover) {
344 325288 : input.servos[i] = ((input.servos[i]-1000) * engine_mul) + 1000;
345 : } else {
346 0 : input.servos[i] = static_cast<uint16_t>(((input.servos[i] - 1500) * engine_mul) + 1500);
347 : }
348 : }
349 : }
350 :
351 187275482 : float throttle = 0.0f; // 0 is 'no throttle', 1.0 is 'full' throttle
352 187275482 : if (_vehicle == ArduPlane) {
353 69973942 : float forward_throttle = constrain_float((input.servos[2] - 1000) / 1000.0f, 0.0f, 1.0f);
354 : // do a little quadplane dance
355 69973942 : float hover_throttle = 0.0f;
356 69973942 : uint8_t running_motors = 0;
357 69973942 : uint32_t mask = _sitl->state.motor_mask;
358 : uint8_t bit;
359 240755986 : while ((bit = __builtin_ffs(mask)) != 0) {
360 170782044 : uint8_t motor = bit-1;
361 170782044 : mask &= ~(1U<<motor);
362 170782044 : float motor_throttle = constrain_float((input.servos[motor] - 1000) / 1000.0f, 0.0f, 1.0f);
363 : // update motor_on flag
364 170782044 : if (!is_zero(motor_throttle)) {
365 78698701 : hover_throttle += motor_throttle;
366 78698701 : running_motors++;
367 : }
368 : }
369 69973942 : if (running_motors > 0) {
370 43678122 : hover_throttle /= running_motors;
371 : }
372 69973942 : if (!is_zero(forward_throttle)) {
373 36272712 : throttle = forward_throttle;
374 : } else {
375 33701230 : throttle = hover_throttle;
376 : }
377 117301540 : } else if (_vehicle == Rover) {
378 15986929 : if (input.servos[2] != 0) {
379 15133367 : const uint16_t servo2 = static_cast<uint16_t>(constrain_int16(input.servos[2], 1000, 2000));
380 15133367 : throttle = fabsf((servo2 - 1500) / 500.0f);
381 : } else {
382 853562 : throttle = 0;
383 : }
384 : } else {
385 : // run checks on each motor
386 101314611 : uint8_t running_motors = 0;
387 101314611 : uint32_t mask = _sitl->state.motor_mask;
388 : uint8_t bit;
389 453296149 : while ((bit = __builtin_ffs(mask)) != 0) {
390 351981538 : const uint8_t motor = bit-1;
391 351981538 : mask &= ~(1U<<motor);
392 351981538 : float motor_throttle = constrain_float((input.servos[motor] - 1000) / 1000.0f, 0.0f, 1.0f);
393 : // update motor_on flag
394 351981538 : if (!is_zero(motor_throttle)) {
395 224490616 : throttle += motor_throttle;
396 224490616 : running_motors++;
397 : }
398 : }
399 101314611 : if (running_motors > 0) {
400 60502818 : throttle /= running_motors;
401 : }
402 : }
403 187275482 : _sitl->throttle = throttle;
404 :
405 187275482 : set_voltage_current_pins(sitl_model->get_battery_voltage(), sitl_model->get_battery_current());
406 : }
407 :
408 2094 : void SITL_State::init(int argc, char * const argv[])
409 : {
410 2094 : _scheduler = Scheduler::from(hal.scheduler);
411 2094 : _parse_command_line(argc, argv);
412 2094 : }
413 :
414 : /*
415 : set height above the ground in meters
416 : */
417 187275482 : void SITL_State::set_height_agl(void)
418 : {
419 : static float home_alt = -1;
420 :
421 187275482 : if (!_sitl) {
422 : // in example program
423 0 : return;
424 : }
425 :
426 187275482 : if (is_equal(home_alt, -1.0f) && _sitl->state.altitude > 0) {
427 : // remember home altitude as first non-zero altitude
428 2013 : home_alt = _sitl->state.altitude;
429 : }
430 :
431 : #if AP_TERRAIN_AVAILABLE
432 187275482 : if (_sitl->terrain_enable) {
433 : // get height above terrain from AP_Terrain. This assumes
434 : // AP_Terrain is working
435 : float terrain_height_amsl;
436 184821107 : Location location;
437 184821107 : location.lat = _sitl->state.latitude*1.0e7;
438 184821107 : location.lng = _sitl->state.longitude*1.0e7;
439 :
440 184821107 : AP_Terrain *_terrain = AP_Terrain::get_singleton();
441 351408529 : if (_terrain != nullptr &&
442 166587422 : _terrain->height_amsl(location, terrain_height_amsl, false)) {
443 144957246 : _sitl->state.height_agl = _sitl->state.altitude - terrain_height_amsl;
444 144957246 : return;
445 : }
446 : }
447 : #endif
448 :
449 : // fall back to flat earth model
450 42318236 : _sitl->state.height_agl = _sitl->state.altitude - home_alt;
451 : }
452 :
453 : /*
454 : open multicast UDP
455 : */
456 5 : void SITL_State::multicast_state_open(void)
457 : {
458 5 : struct sockaddr_in sockaddr {};
459 : int ret;
460 :
461 : #ifdef HAVE_SOCK_SIN_LEN
462 : sockaddr.sin_len = sizeof(sockaddr);
463 : #endif
464 5 : sockaddr.sin_family = AF_INET;
465 :
466 : /*
467 : open the servo input socket; state is also sent from this socket
468 : so that peripherals can reply to the source address and port they
469 : observe, whatever this instance's servo port is
470 : */
471 5 : servo_in_fd = socket(AF_INET, SOCK_DGRAM, 0);
472 5 : if (servo_in_fd == -1) {
473 0 : fprintf(stderr, "socket failed - %s\n", strerror(errno));
474 0 : exit(1);
475 : }
476 5 : ret = fcntl(servo_in_fd, F_SETFD, FD_CLOEXEC);
477 5 : if (ret == -1) {
478 0 : fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
479 0 : exit(1);
480 : }
481 :
482 : // try to setup for broadcast, this may fail if insufficient privileges
483 5 : int one = 1;
484 5 : setsockopt(servo_in_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
485 :
486 5 : const uint32_t mc_if_addr = sitl_multicast_interface_address();
487 5 : if (mc_if_addr != 0) {
488 : // the state is multicast from this socket, so it needs the same
489 : // interface pinning the other multicast paths have: without it
490 : // the state follows the routing table while the peripheral is
491 : // listening on the interface it was told to use, and never sees
492 : // the vehicle. See sitl_multicast_interface_address()
493 5 : struct in_addr ifaddr {};
494 5 : ifaddr.s_addr = mc_if_addr;
495 5 : if (setsockopt(servo_in_fd, IPPROTO_IP, IP_MULTICAST_IF, &ifaddr, sizeof(ifaddr)) == -1) {
496 0 : fprintf(stderr, "failed to set multicast interface - %s\n", strerror(errno));
497 0 : exit(1);
498 : }
499 : }
500 :
501 5 : sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
502 5 : sockaddr.sin_port = htons(SITL_SERVO_PORT + _instance);
503 :
504 5 : ret = bind(servo_in_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
505 5 : if (ret == -1) {
506 0 : fprintf(stderr, "udp servo bind failed\n");
507 0 : exit(1);
508 : }
509 :
510 : // destination address for the multicast state
511 5 : mc_dest = {};
512 : #ifdef HAVE_SOCK_SIN_LEN
513 : mc_dest.sin_len = sizeof(mc_dest);
514 : #endif
515 5 : mc_dest.sin_family = AF_INET;
516 5 : mc_dest.sin_port = htons(sitl_multicast_state_port(SITL_MCAST_PORT));
517 5 : mc_dest.sin_addr.s_addr = inet_addr(SITL_MCAST_IP);
518 :
519 5 : ::printf("multicast initialised\n");
520 5 : }
521 :
522 : /*
523 : send out SITL state as multicast UDP
524 : */
525 609376 : void SITL_State::multicast_state_send(void)
526 : {
527 609376 : if (_sitl == nullptr) {
528 0 : return;
529 : }
530 609376 : if (servo_in_fd == -1) {
531 5 : multicast_state_open();
532 : }
533 609376 : const auto &sfdm = _sitl->state;
534 609376 : sendto(servo_in_fd, (void*)&sfdm, sizeof(sfdm), 0, (struct sockaddr *)&mc_dest, sizeof(mc_dest));
535 :
536 609376 : check_servo_input();
537 :
538 609376 : if (_periph_lockstep) {
539 60120 : wait_periph_acks(sfdm.timestamp_us);
540 : }
541 : }
542 :
543 : /*
544 : check for ack/servo data from peripherals
545 : */
546 667938 : void SITL_State::check_servo_input(void)
547 : {
548 : // drain any pending packets; we loop to ensure we drain all
549 : // packets from all nodes
550 : struct sitl_mcast_ack ack;
551 : struct sockaddr_in src;
552 667938 : socklen_t src_len = sizeof(src);
553 : ssize_t ret;
554 726322 : while ((ret = recvfrom(servo_in_fd, (void*)&ack, sizeof(ack), MSG_DONTWAIT,
555 726322 : (struct sockaddr *)&src, &src_len)) > 0) {
556 58384 : handle_periph_ack(ack, ret, src);
557 58384 : src_len = sizeof(src);
558 : }
559 667938 : }
560 :
561 : /*
562 : handle one ack/servo packet from a peripheral
563 : */
564 58384 : void SITL_State::handle_periph_ack(const struct sitl_mcast_ack &ack, ssize_t len, const struct sockaddr_in &src)
565 : {
566 58384 : if (len != sizeof(ack)) {
567 : // unknown packet format. The most likely cause is a
568 : // peripheral built from a different source tree, sending the
569 : // old servo-only reply; its servo output will be ignored and
570 : // it can take no part in lockstep, so say so rather than
571 : // discarding its packets in silence
572 0 : if (!_warned_ack_size) {
573 0 : _warned_ack_size = true;
574 0 : ::fprintf(stderr, "SITL: ignoring %d-byte peripheral reply from %s:%u, expected %u bytes; build the peripheral from this source tree\n",
575 0 : int(len), inet_ntoa(src.sin_addr), (unsigned)ntohs(src.sin_port),
576 : (unsigned)sizeof(ack));
577 : }
578 0 : return;
579 : }
580 1926672 : for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
581 : // nan means that node is not outputting this channel
582 1868288 : if (!isnan(ack.servos[i])) {
583 0 : mc_servo[i] = uint16_t(ack.servos[i]);
584 : }
585 : }
586 58384 : if (!_periph_lockstep) {
587 0 : return;
588 : }
589 : // register the peripheral, or update its ack state
590 58384 : struct mcast_periph *periph = nullptr;
591 58384 : for (uint8_t i=0; i<num_mcast_periphs; i++) {
592 58382 : if (mcast_periphs[i].addr.sin_addr.s_addr == src.sin_addr.s_addr &&
593 58382 : mcast_periphs[i].addr.sin_port == src.sin_port) {
594 58382 : periph = &mcast_periphs[i];
595 58382 : break;
596 : }
597 : }
598 58384 : if (periph == nullptr) {
599 : // do not let a recently-evicted peer's stale queued acks
600 : // re-register it; a live peer re-registers with fresh acks
601 : // once the cooldown expires
602 2 : const uint64_t now_ms = wall_millis();
603 2 : for (uint8_t i=0; i<num_evicted_periphs; ) {
604 0 : if (now_ms - evicted_periphs[i].evicted_ms > PERIPH_REJOIN_COOLDOWN_MS) {
605 0 : evicted_periphs[i] = evicted_periphs[--num_evicted_periphs];
606 0 : continue;
607 : }
608 0 : if (evicted_periphs[i].addr.sin_addr.s_addr == src.sin_addr.s_addr &&
609 0 : evicted_periphs[i].addr.sin_port == src.sin_port) {
610 0 : return;
611 : }
612 0 : i++;
613 : }
614 2 : if (num_mcast_periphs >= MAX_MCAST_PERIPHS) {
615 0 : return;
616 : }
617 2 : periph = &mcast_periphs[num_mcast_periphs++];
618 2 : periph->addr = src;
619 2 : ::printf("SITL: peripheral %s:%u joined lockstep\n",
620 2 : inet_ntoa(src.sin_addr), (unsigned)ntohs(src.sin_port));
621 : }
622 58384 : periph->last_ack_us = ack.timestamp_us;
623 58384 : periph->last_heard_ms = wall_millis();
624 : }
625 :
626 : // wall-clock milliseconds; the simulation clock must not be used for
627 : // the lockstep eviction timeout as it is frozen while we wait
628 177068 : uint64_t SITL_State::wall_millis(void)
629 : {
630 : struct timespec ts;
631 177068 : clock_gettime(CLOCK_MONOTONIC, &ts);
632 177068 : return uint64_t(ts.tv_sec) * 1000ULL + ts.tv_nsec / 1000000ULL;
633 : }
634 :
635 : /*
636 : strict simulated-peripheral lockstep: do not return (and so do not
637 : advance the simulation) until every registered peripheral has
638 : acknowledged consuming the state packet with this timestamp. A
639 : peripheral which stops responding for a wall-clock second is
640 : presumed dead (the harness SIGTERMs them with no notice) and is
641 : evicted; it re-registers on its next ack
642 : */
643 118682 : void SITL_State::wait_periph_acks(const uint64_t timestamp_us)
644 : {
645 : while (true) {
646 118682 : bool all_acked = true;
647 118682 : const uint64_t now_ms = wall_millis();
648 235627 : for (uint8_t i=0; i<num_mcast_periphs; ) {
649 116945 : if (mcast_periphs[i].last_ack_us >= timestamp_us) {
650 58382 : i++;
651 58382 : continue;
652 : }
653 58563 : if (now_ms - mcast_periphs[i].last_heard_ms > PERIPH_EVICT_TIMEOUT_MS) {
654 1 : ::fprintf(stderr, "SITL: evicting unresponsive peripheral %s:%u from lockstep\n",
655 1 : inet_ntoa(mcast_periphs[i].addr.sin_addr),
656 1 : (unsigned)ntohs(mcast_periphs[i].addr.sin_port));
657 1 : if (num_evicted_periphs < MAX_MCAST_PERIPHS) {
658 1 : evicted_periphs[num_evicted_periphs].addr = mcast_periphs[i].addr;
659 1 : evicted_periphs[num_evicted_periphs].evicted_ms = now_ms;
660 1 : num_evicted_periphs++;
661 : }
662 1 : mcast_periphs[i] = mcast_periphs[--num_mcast_periphs];
663 1 : continue;
664 : }
665 58562 : all_acked = false;
666 58562 : i++;
667 : }
668 118682 : if (all_acked) {
669 60120 : return;
670 : }
671 58562 : struct pollfd pfd { servo_in_fd, POLLIN, 0 };
672 58562 : poll(&pfd, 1, PERIPH_ACK_POLL_MS);
673 58562 : check_servo_input();
674 58562 : }
675 : }
676 :
677 : /*
678 : overwrite input structure with multicast values
679 : */
680 187275482 : void SITL_State::multicast_servo_update(struct sitl_input &input)
681 : {
682 6180090906 : for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
683 5992815424 : const uint32_t mask = (1U<<i);
684 5992815424 : const uint32_t can_mask = uint32_t(_sitl->can_servo_mask.get());
685 5992815424 : if (can_mask & mask) {
686 0 : input.servos[i] = mc_servo[i];
687 : }
688 : }
689 187275482 : }
690 : #endif
|
