Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
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 :
12 : #include <stdio.h>
13 : #include <signal.h>
14 : #include <unistd.h>
15 : #include <stdlib.h>
16 : #include <errno.h>
17 : #include <sys/select.h>
18 :
19 : #include <AP_Param/AP_Param.h>
20 : #include <SITL/SIM_JSBSim.h>
21 : #include <AP_HAL/utility/Socket_native.h>
22 : #include "SITL_State_common.h"
23 :
24 : #include <SITL/SIM_RF_Ainstein_LR_D1.h>
25 : #include <SITL/SIM_RF_Ainstein_LR_D1_v19.h>
26 : #include <SITL/SIM_RF_Benewake_TF02.h>
27 : #include <SITL/SIM_RF_Benewake_TF03.h>
28 : #include <SITL/SIM_RF_Benewake_TFmini.h>
29 : #include <SITL/SIM_RF_BLping.h>
30 : #include <SITL/SIM_RF_DTS6012M.h>
31 : #include <SITL/SIM_RF_GYUS42v2.h>
32 : #include <SITL/SIM_RF_JRE.h>
33 : #include <SITL/SIM_RF_Lanbao.h>
34 : #include <SITL/SIM_RF_LeddarOne.h>
35 : #include <SITL/SIM_RF_LightWareSerialBinary.h>
36 : #include <SITL/SIM_RF_LightWareSerial.h>
37 : #include <SITL/SIM_RF_MAVLink.h>
38 : #include <SITL/SIM_RF_MaxsonarSerialLV.h>
39 : #include <SITL/SIM_RF_NMEA.h>
40 : #include <SITL/SIM_RF_NoopLoop.h>
41 : #include <SITL/SIM_RF_RDS02UF.h>
42 : #include <SITL/SIM_RF_TeraRanger_Serial.h>
43 : #include <SITL/SIM_RF_USD1_v0.h>
44 : #include <SITL/SIM_RF_USD1_v1.h>
45 : #include <SITL/SIM_RF_Wasp.h>
46 : #include <SITL/SIM_RF_LightWare_GRF.h>
47 :
48 : using namespace HALSITL;
49 :
50 : static const struct {
51 : const char *name;
52 : SITL::SerialRangeFinder *(*createfn)();
53 : } serial_rangefinder_definitions[] {
54 : { "ainsteinlrd1", SITL::RF_Ainstein_LR_D1::create },
55 : { "ainsteinlrd1_v19", SITL::RF_Ainstein_LR_D1_v19::create },
56 : { "benewake_tf02", SITL::RF_Benewake_TF02::create },
57 : { "benewake_tf03", SITL::RF_Benewake_TF03::create },
58 : { "benewake_tfmini", SITL::RF_Benewake_TFmini::create },
59 : { "blping", SITL::RF_BLping::create },
60 : { "dts6012m", SITL::RF_DTS6012M::create },
61 : { "gyus42v2", SITL::RF_GYUS42v2::create },
62 : { "jre", SITL::RF_JRE::create },
63 : { "lanbao", SITL::RF_Lanbao::create },
64 : { "leddarone", SITL::RF_LeddarOne::create },
65 : { "leddarone", SITL::RF_LeddarOne::create },
66 : { "lightwareserial-binary", SITL::RF_LightWareSerialBinary::create },
67 : { "lightwareserial", SITL::RF_LightWareSerial::create },
68 : { "lightware_grf", SITL::RF_LightWareGRF::create },
69 : { "maxsonarseriallv", SITL::RF_MaxsonarSerialLV::create },
70 : { "nmea", SITL::RF_NMEA::create },
71 : { "nmea", SITL::RF_NMEA::create },
72 : { "nooploop_tofsense", SITL::RF_Nooploop::create },
73 : { "rds02uf", SITL::RF_RDS02UF::create },
74 : #if !defined(HAL_BUILD_AP_PERIPH)
75 : { "rf_mavlink", SITL::RF_MAVLink::create },
76 : #endif
77 : { "teraranger_serial", SITL::RF_TeraRanger_Serial::create },
78 : { "USD1_v0", SITL::RF_USD1_v0::create },
79 : { "USD1_v1", SITL::RF_USD1_v1::create },
80 : { "wasp", SITL::RF_Wasp::create },
81 : };
82 :
83 : #define streq(a, b) (!strcmp(a, b))
84 14769 : SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const char *arg, const uint8_t portNumber)
85 : {
86 383608 : for (const auto &definition : serial_rangefinder_definitions) {
87 368868 : if (!streq(definition.name, name)) {
88 368839 : continue;
89 : }
90 29 : if (num_serial_rangefinders >= ARRAY_SIZE(serial_rangefinders)) {
91 0 : AP_HAL::panic("Too many simulated serial rangefinders");
92 : }
93 29 : serial_rangefinders[num_serial_rangefinders] = definition.createfn();
94 29 : return serial_rangefinders[num_serial_rangefinders++];
95 : }
96 :
97 : if (false) {
98 : // this is an empty clause to ease else-if syntax
99 : #if !defined(HAL_BUILD_AP_PERIPH)
100 14740 : } else if (streq(name, "vicon")) {
101 8 : if (vicon != nullptr) {
102 0 : AP_HAL::panic("Only one vicon system at a time");
103 : }
104 8 : vicon = NEW_NOTHROW SITL::Vicon();
105 8 : return vicon;
106 : #endif
107 : #if AP_SIM_ADSB_ENABLED
108 14732 : } else if (streq(name, "adsb")) {
109 : // ADSB is a stand-out as it is the only serial device which
110 : // will cope with begin() being called multiple times on a
111 : // serial port
112 6 : if (adsb == nullptr) {
113 2 : adsb = NEW_NOTHROW SITL::ADSB();
114 : }
115 6 : sitl_model->set_adsb(adsb);
116 6 : return adsb;
117 : #endif // AP_SIM_ADSB_ENABLED
118 14726 : } else if (streq(name, "frsky-d")) {
119 0 : if (frsky_d != nullptr) {
120 0 : AP_HAL::panic("Only one frsky_d at a time");
121 : }
122 0 : frsky_d = NEW_NOTHROW SITL::Frsky_D();
123 0 : return frsky_d;
124 : #if AP_SIM_NOOPLOOP_ENABLED
125 14726 : } else if (streq(name, "nooploop")) {
126 1 : if (nooploop != nullptr) {
127 0 : AP_HAL::panic("Only one nooploop at a time");
128 : }
129 1 : nooploop = NEW_NOTHROW SITL::Beacon_NoopLoop();
130 1 : return nooploop;
131 : #endif // AP_SIM_NOOPLOOP_ENABLED
132 : // } else if (streq(name, "frsky-SPort")) {
133 : // if (frsky_sport != nullptr) {
134 : // AP_HAL::panic("Only one frsky_sport at a time");
135 : // }
136 : // frsky_sport = NEW_NOTHROW SITL::Frsky_SPort();
137 : // return frsky_sport;
138 :
139 : // } else if (streq(name, "frsky-SPortPassthrough")) {
140 : // if (frsky_sport_passthrough != nullptr) {
141 : // AP_HAL::panic("Only one frsky_sport passthrough at a time");
142 : // }
143 : // frsky_sport = NEW_NOTHROW SITL::Frsky_SPortPassthrough();
144 : // return frsky_sportpassthrough;
145 : #if AP_SIM_CRSF_ENABLED
146 14725 : } else if (streq(name, "crsf")) {
147 0 : if (crsf != nullptr) {
148 0 : AP_HAL::panic("Only one crsf at a time");
149 : }
150 0 : crsf = NEW_NOTHROW SITL::CRSF();
151 0 : return crsf;
152 : #endif
153 : #if AP_SIM_PS_LD06_ENABLED
154 14725 : } else if (streq(name, "ld06")) {
155 1 : if (ld06 != nullptr) {
156 0 : AP_HAL::panic("Only one ld06 at a time");
157 : }
158 1 : ld06 = NEW_NOTHROW SITL::PS_LD06();
159 1 : return ld06;
160 : #endif // AP_SIM_PS_LD06_ENABLED
161 : #if AP_SIM_PS_RPLIDARA2_ENABLED
162 14724 : } else if (streq(name, "rplidara2")) {
163 2 : if (rplidara2 != nullptr) {
164 0 : AP_HAL::panic("Only one rplidara2 at a time");
165 : }
166 2 : rplidara2 = NEW_NOTHROW SITL::PS_RPLidarA2();
167 2 : return rplidara2;
168 : #endif
169 : #if AP_SIM_PS_RPLIDARA1_ENABLED
170 14722 : } else if (streq(name, "rplidara1")) {
171 0 : if (rplidara1 != nullptr) {
172 0 : AP_HAL::panic("Only one rplidara1 at a time");
173 : }
174 0 : rplidara1 = NEW_NOTHROW SITL::PS_RPLidarA1();
175 0 : return rplidara1;
176 : #endif
177 : #if AP_SIM_PS_RPLIDARS2_ENABLED
178 14722 : } else if (streq(name, "rplidars2")) {
179 1 : if (rplidars2 != nullptr) {
180 0 : AP_HAL::panic("Only one rplidars2 at a time");
181 : }
182 1 : rplidars2 = NEW_NOTHROW SITL::PS_RPLidarS2();
183 1 : return rplidars2;
184 : #endif
185 : #if AP_SIM_PS_TERARANGERTOWER_ENABLED
186 14721 : } else if (streq(name, "terarangertower")) {
187 1 : if (terarangertower != nullptr) {
188 0 : AP_HAL::panic("Only one terarangertower at a time");
189 : }
190 1 : terarangertower = NEW_NOTHROW SITL::PS_TeraRangerTower();
191 1 : return terarangertower;
192 : #endif
193 : #if AP_SIM_PS_LIGHTWARE_SF45B_ENABLED
194 14720 : } else if (streq(name, "sf45b")) {
195 1 : if (sf45b != nullptr) {
196 0 : AP_HAL::panic("Only one sf45b at a time");
197 : }
198 1 : sf45b = NEW_NOTHROW SITL::PS_LightWare_SF45B();
199 1 : return sf45b;
200 : #endif
201 : #if AP_SIM_ADSB_SAGETECH_MXS_ENABLED
202 14719 : } else if (streq(name, "sagetech_mxs")) {
203 1 : if (sagetech_mxs != nullptr) {
204 0 : AP_HAL::panic("Only one sagetech_mxs at a time");
205 : }
206 1 : sagetech_mxs = NEW_NOTHROW SITL::ADSB_Sagetech_MXS();
207 1 : if (adsb == nullptr) {
208 0 : adsb = NEW_NOTHROW SITL::ADSB();
209 : }
210 1 : sitl_model->set_adsb(adsb);
211 1 : return sagetech_mxs;
212 : #endif
213 : #if AP_SIM_LOWEHEISER_ENABLED
214 14718 : } else if (streq(name, "loweheiser")) {
215 10 : sitl_model->set_loweheiser(&_sitl->loweheiser_sim);
216 10 : return &_sitl->loweheiser_sim;
217 : #endif
218 : #if !defined(HAL_BUILD_AP_PERIPH)
219 14708 : } else if (streq(name, "richenpower")) {
220 1 : sitl_model->set_richenpower(&_sitl->richenpower_sim);
221 1 : return &_sitl->richenpower_sim;
222 14707 : } else if (streq(name, "fetteconewireesc")) {
223 16 : sitl_model->set_fetteconewireesc(&_sitl->fetteconewireesc_sim);
224 16 : return &_sitl->fetteconewireesc_sim;
225 14691 : } else if (streq(name, "ie24")) {
226 4 : sitl_model->set_ie24(&_sitl->ie24_sim);
227 4 : return &_sitl->ie24_sim;
228 : #endif // HAL_BUILD_AP_PERIPH
229 : #if AP_SIM_VOLZ_ENABLED
230 14687 : } else if (streq(name, "volz")) {
231 3 : sitl_model->set_volz(&_sitl->volz_sim);
232 3 : return &_sitl->volz_sim;
233 : #endif // AP_SIM_VOLZ_ENABLED
234 : #if AP_SIM_SIYI_ZT30_ENABLED
235 14684 : } else if (streq(name, "siyi_zt30")) {
236 1 : const auto siyi = NEW_NOTHROW SITL::Siyi_ZT30();
237 1 : sitl_model->add_gimbal_sim(*siyi);
238 1 : return siyi;
239 : #endif // AP_SIM_SIYI_ZT30_ENABLED
240 : #if AP_SIM_TOPOTEK_ENABLED
241 14683 : } else if (streq(name, "topotek")) {
242 2 : const auto topotek = NEW_NOTHROW SITL::Topotek();
243 2 : sitl_model->add_gimbal_sim(*topotek);
244 2 : return topotek;
245 : #endif // AP_SIM_TOPOTEK_ENABLED
246 : #if AP_SIM_VIEWPRO_ENABLED
247 14681 : } else if (streq(name, "viewpro")) {
248 1 : const auto viewpro = NEW_NOTHROW SITL::Viewpro();
249 1 : sitl_model->add_gimbal_sim(*viewpro);
250 1 : return viewpro;
251 : #endif // AP_SIM_VIEWPRO_ENABLED
252 : #if AP_SIM_AVT_CM62_ENABLED
253 14680 : } else if (streq(name, "avt_cm62_gimbal")) {
254 : static uint8_t mavlink_gimbal_count;
255 5 : const auto avt_cm62 = NEW_NOTHROW SITL::AVT_CM62();
256 5 : avt_cm62->set_instance(mavlink_gimbal_count++);
257 5 : sitl_model->add_gimbal_sim(*avt_cm62);
258 5 : return avt_cm62;
259 : #endif // AP_SIM_AVT_CM62_ENABLED
260 14675 : } else if (streq(name, "megasquirt")) {
261 1 : if (efi_ms != nullptr) {
262 0 : AP_HAL::panic("Only one megasquirt at a time");
263 : }
264 1 : efi_ms = NEW_NOTHROW SITL::EFI_MegaSquirt();
265 1 : return efi_ms;
266 14674 : } else if (streq(name, "hirth")) {
267 1 : if (efi_hirth != nullptr) {
268 0 : AP_HAL::panic("Only one hirth at a time");
269 : }
270 1 : efi_hirth = NEW_NOTHROW SITL::EFI_Hirth();
271 1 : return efi_hirth;
272 14673 : } else if (streq(name, "VectorNav")) {
273 20 : if (vectornav != nullptr) {
274 0 : AP_HAL::panic("Only one VectorNav at a time");
275 : }
276 20 : vectornav = NEW_NOTHROW SITL::VectorNav(SITL::VectorNav::VNModel::VN300);
277 20 : return vectornav;
278 14653 : } else if (streq(name, "VectorNav_VN100")) {
279 0 : if (vectornav != nullptr) {
280 0 : AP_HAL::panic("Only one VectorNav at a time");
281 : }
282 0 : vectornav = NEW_NOTHROW SITL::VectorNav(SITL::VectorNav::VNModel::VN100);
283 0 : return vectornav;
284 14653 : } else if (streq(name, "MicroStrain5")) {
285 17 : if (microstrain5 != nullptr) {
286 0 : AP_HAL::panic("Only one MicroStrain5 at a time");
287 : }
288 17 : microstrain5 = NEW_NOTHROW SITL::MicroStrain5();
289 17 : return microstrain5;
290 :
291 14636 : } else if (streq(name, "MicroStrain7")) {
292 24 : if (microstrain7 != nullptr) {
293 0 : AP_HAL::panic("Only one MicroStrain7 at a time");
294 : }
295 24 : microstrain7 = NEW_NOTHROW SITL::MicroStrain7();
296 24 : return microstrain7;
297 :
298 14612 : } else if (streq(name, "ILabs")) {
299 17 : if (inertiallabs != nullptr) {
300 0 : AP_HAL::panic("Only one InertialLabs INS at a time");
301 : }
302 17 : inertiallabs = NEW_NOTHROW SITL::InertialLabs();
303 17 : return inertiallabs;
304 :
305 14595 : } else if (streq(name, "SensAItion")) {
306 6 : if (sensaition != nullptr) {
307 0 : AP_HAL::panic("Only one SensAItion at a time");
308 : }
309 6 : sensaition = NEW_NOTHROW SITL::SensAItion(false);
310 6 : return sensaition;
311 14589 : } else if (streq(name, "SensAItionINS")) {
312 3 : if (sensaition != nullptr) {
313 0 : AP_HAL::panic("Only one SensAItion at a time");
314 : }
315 3 : sensaition = NEW_NOTHROW SITL::SensAItion(true);
316 3 : return sensaition;
317 :
318 : #if AP_SIM_AERON_ENABLED
319 14586 : } else if (streq(name, "Aeron-PLX3")) {
320 53 : if (aeron != nullptr) {
321 0 : AP_HAL::panic("Only one Aeron-PLX3 INS at a time");
322 : }
323 53 : aeron = NEW_NOTHROW SITL::Aeron();
324 53 : return aeron;
325 : #endif // AP_SIM_AERON_ENABLED
326 : #if AP_SIM_AIS_ENABLED
327 14533 : } else if (streq(name, "AIS")) {
328 3 : if ((ais != nullptr) || (ais_replay != nullptr)) {
329 0 : AP_HAL::panic("Only one AIS at a time");
330 : }
331 3 : ais = NEW_NOTHROW SITL::AIS();
332 3 : return ais;
333 14530 : } else if (streq(name, "AISReplay")) {
334 0 : if ((ais != nullptr) || (ais_replay != nullptr)) {
335 0 : AP_HAL::panic("Only one AIS at a time");
336 : }
337 0 : ais_replay = NEW_NOTHROW SITL::AIS_Replay();
338 0 : return ais_replay;
339 : #endif
340 14530 : } else if (strncmp(name, "gps", 3) == 0) {
341 14530 : uint8_t x = atoi(arg);
342 14530 : if (x <= 0 || x > ARRAY_SIZE(gps)) {
343 0 : AP_HAL::panic("Bad GPS number %u (%s)", x, arg);
344 : }
345 14530 : gps[x-1] = NEW_NOTHROW SITL::GPS(x-1);
346 14530 : return gps[x-1];
347 0 : } else if (streq(name, "ELRS")) {
348 : // Only allocate if not done already
349 : // MAVLink serial ports have begin called several times
350 0 : if (elrs == nullptr) {
351 0 : elrs = NEW_NOTHROW SITL::ELRS(portNumber, this);
352 0 : _sitl->set_stop_MAVLink_sim_state();
353 : }
354 0 : return elrs;
355 : }
356 :
357 0 : AP_HAL::panic("unknown simulated device: %s", name);
358 : }
359 :
360 : #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
361 : /*
362 : create a simulated device which the autopilot connects to over TCP
363 : rather than over one of its simulated serial ports. This is used to
364 : simulate devices attached to the autopilot's network ports (NET_Pn).
365 : spec is of the form NAME:TCPPORT e.g. "topotek:15005"
366 : */
367 1 : void SITL_State_Common::create_net_serial_sim(const char *spec)
368 : {
369 1 : if (num_net_serial_sims >= ARRAY_SIZE(net_serial_sims)) {
370 0 : AP_HAL::panic("Too many network-attached simulated devices");
371 : }
372 :
373 1 : char *s = strdup(spec);
374 1 : if (s == nullptr) {
375 0 : AP_HAL::panic("out of memory");
376 : }
377 1 : char *saveptr = nullptr;
378 1 : const char *name = strtok_r(s, ":", &saveptr);
379 1 : const char *port_str = strtok_r(nullptr, ":", &saveptr);
380 1 : if (name == nullptr || port_str == nullptr) {
381 0 : AP_HAL::panic("Bad network device (%s); expected NAME:TCPPORT", spec);
382 : }
383 :
384 1 : SITL::SerialDevice *device = create_serial_sim(name, nullptr, 0);
385 1 : if (!device->listen_on_tcp_port(atoi(port_str))) {
386 0 : AP_HAL::panic("Failed to attach %s to TCP port %s", name, port_str);
387 : }
388 1 : net_serial_sims[num_net_serial_sims++] = device;
389 :
390 1 : free(s);
391 1 : }
392 : #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED
393 :
394 : /*
395 : update simulators
396 : */
397 187275482 : void SITL_State_Common::sim_update(void)
398 : {
399 : #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
400 : // move data between the autopilot and any device attached via TCP;
401 : // for serially-attached devices the SITL UART driver does this:
402 187282727 : for (uint8_t i=0; i<num_net_serial_sims; i++) {
403 7245 : net_serial_sims[i]->network_update();
404 : }
405 : #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED
406 :
407 : #if AP_SIM_SOLOGIMBAL_ENABLED
408 187275482 : if (gimbal != nullptr) {
409 248542 : gimbal->update(*sitl_model);
410 : }
411 : #endif
412 : #if AP_SIM_ADSB_ENABLED
413 187275482 : if (adsb != nullptr) {
414 29376 : adsb->update(*sitl_model);
415 : }
416 : #endif // AP_SIM_ADSB_ENABLED
417 : #if AP_SIM_VICON_ENABLED
418 187275482 : if (vicon != nullptr) {
419 886133 : Quaternion attitude;
420 886133 : sitl_model->get_attitude(attitude);
421 886133 : vicon->update(sitl_model->get_location(),
422 1772266 : sitl_model->get_position_relhome(),
423 886133 : sitl_model->get_velocity_ef(),
424 : attitude);
425 : }
426 : #endif // AP_SIM_VICON_ENABLED
427 190098968 : for (uint8_t i=0; i<num_serial_rangefinders; i++) {
428 2823486 : serial_rangefinders[i]->update(sitl_model->rangefinder_range());
429 : }
430 : #if AP_SIM_NOOPLOOP_ENABLED
431 187275482 : if (nooploop != nullptr) {
432 137493 : nooploop->update();
433 : }
434 : #endif // AP_SIM_NOOPLOOP_ENABLED
435 187275482 : if (efi_ms != nullptr) {
436 31243 : efi_ms->update(*sitl_model);
437 : }
438 187275482 : if (efi_hirth != nullptr) {
439 31243 : efi_hirth->update(*sitl_model);
440 : }
441 :
442 187275482 : if (frsky_d != nullptr) {
443 0 : frsky_d->update();
444 : }
445 : // if (frsky_sport != nullptr) {
446 : // frsky_sport->update();
447 : // }
448 : // if (frsky_sportpassthrough != nullptr) {
449 : // frsky_sportpassthrough->update();
450 : // }
451 :
452 : #if AP_SIM_CRSF_ENABLED
453 187275482 : if (crsf != nullptr) {
454 0 : crsf->update();
455 : }
456 : #endif
457 :
458 : #if AP_SIM_PS_LD06_ENABLED
459 187275482 : if (ld06 != nullptr) {
460 52407 : ld06->update(sitl_model->get_location());
461 : }
462 : #endif // AP_SIM_PS_LD06_ENABLED
463 :
464 : #if AP_SIM_PS_RPLIDARA2_ENABLED
465 187275482 : if (rplidara2 != nullptr) {
466 106255 : rplidara2->update(sitl_model->get_location());
467 : }
468 : #endif
469 :
470 : #if AP_SIM_PS_RPLIDARA1_ENABLED
471 187275482 : if (rplidara1 != nullptr) {
472 0 : rplidara1->update(sitl_model->get_location());
473 : }
474 : #endif
475 :
476 : #if AP_SIM_PS_RPLIDARS2_ENABLED
477 187275482 : if (rplidars2 != nullptr) {
478 52422 : rplidars2->update(sitl_model->get_location());
479 : }
480 : #endif
481 :
482 : #if AP_SIM_PS_TERARANGERTOWER_ENABLED
483 187275482 : if (terarangertower != nullptr) {
484 54064 : terarangertower->update(sitl_model->get_location());
485 : }
486 : #endif
487 :
488 : #if AP_SIM_PS_LIGHTWARE_SF45B_ENABLED
489 187275482 : if (sf45b != nullptr) {
490 52440 : sf45b->update(sitl_model->get_location());
491 : }
492 : #endif
493 :
494 : #if AP_SIM_ADSB_SAGETECH_MXS_ENABLED
495 187275482 : if (sagetech_mxs != nullptr) {
496 14460 : sagetech_mxs->update(sitl_model);
497 : }
498 : #endif
499 :
500 187275482 : if (vectornav != nullptr) {
501 498127 : vectornav->update();
502 : }
503 :
504 187275482 : if (sensaition != nullptr) {
505 385829 : sensaition->update();
506 : }
507 :
508 187275482 : if (microstrain5 != nullptr) {
509 394667 : microstrain5->update();
510 : }
511 :
512 187275482 : if (microstrain7 != nullptr) {
513 435169 : microstrain7->update();
514 : }
515 187275482 : if (inertiallabs != nullptr) {
516 277079 : inertiallabs->update();
517 : }
518 :
519 : #if AP_SIM_AERON_ENABLED
520 187275482 : if (aeron != nullptr) {
521 835635 : aeron->update();
522 : }
523 : #endif // AP_SIM_AERON_ENABLED
524 :
525 : #if AP_SIM_AIS_ENABLED
526 187275482 : if (ais != nullptr) {
527 59718 : ais->update(*sitl_model);
528 : }
529 187275482 : if (ais_replay != nullptr) {
530 0 : ais_replay->update();
531 : }
532 : #endif
533 936377410 : for (uint8_t i=0; i<ARRAY_SIZE(gps); i++) {
534 749101928 : if (gps[i] != nullptr) {
535 370347508 : gps[i]->update();
536 : }
537 : }
538 :
539 187275482 : if (elrs != nullptr) {
540 0 : elrs->update();
541 : }
542 187275482 : }
543 :
544 : /*
545 : set voltage and current pin values
546 : */
547 187275482 : void SITL_State_Common::set_voltage_current_pins(float voltage, float current_amp)
548 : {
549 : // assume 3DR power brick
550 187275482 : voltage_pin_voltage = (voltage / 10.1f);
551 187275482 : current_pin_voltage = current_amp / 17.0f;
552 : // fake battery2 as just a 25% gain on the first one
553 187275482 : voltage2_pin_voltage = voltage_pin_voltage * 0.25f;
554 187275482 : current2_pin_voltage = current_pin_voltage * 0.25f;
555 187275482 : }
556 :
557 : #endif // HAL_BOARD_SITL
558 :
|
