Line data Source code
1 : #pragma once
2 :
3 : #include <AP_HAL/AP_HAL_Boards.h>
4 :
5 : #if AP_SIM_ENABLED
6 :
7 : #include <AP_Math/AP_Math.h>
8 : #include <GCS_MAVLink/GCS_MAVLink.h>
9 : #include <AP_Baro/AP_Baro.h>
10 : #include <AP_Airspeed/AP_Airspeed.h>
11 : #include <AP_Common/Location.h>
12 : #include <AP_Compass/AP_Compass.h>
13 : #include <AP_DDS/AP_DDS_config.h>
14 : #include <AP_InertialSensor/AP_InertialSensor.h>
15 :
16 : #include "SIM_Buzzer.h"
17 : #include "SIM_Gripper_EPM.h"
18 : #include "SIM_Gripper_Servo.h"
19 : #include "SIM_I2C.h"
20 : #include "SIM_SPI.h"
21 : #include "SIM_Parachute.h"
22 : #include "SIM_Precland.h"
23 : #include "SIM_Sprayer.h"
24 : #include "SIM_ToneAlarm.h"
25 : #include "SIM_EFI_MegaSquirt.h"
26 : #include "SIM_RichenPower.h"
27 : #include "SIM_Loweheiser.h"
28 : #include "SIM_FETtecOneWireESC.h"
29 : #include "SIM_IntelligentEnergy24.h"
30 : #include "SIM_Ship.h"
31 : #include "SIM_SlungPayload.h"
32 : #include "SIM_Tether.h"
33 : #include "SIM_GPS.h"
34 : #include "SIM_DroneCANDevice.h"
35 : #include "SIM_ADSB_Sagetech_MXS.h"
36 : #include "SIM_Volz.h"
37 : #include "SIM_AIS.h"
38 :
39 : namespace SITL {
40 :
41 : enum class LedLayout {
42 : ROWS=0,
43 : LUMINOUSBEE=1,
44 : };
45 :
46 : struct vector3f_array {
47 : uint16_t length;
48 : Vector3f *data;
49 : };
50 :
51 : struct float_array {
52 : uint16_t length;
53 : float *data;
54 : };
55 :
56 : class StratoBlimp;
57 : class Glider;
58 : class FlightAxis;
59 :
60 : struct sitl_fdm {
61 : // this is the structure passed between FDM models and the main SITL code
62 : uint64_t timestamp_us;
63 : uint64_t flightaxis_imu_frame_num; // the sitl frame number that should have a corresponding imu sample
64 : Location home;
65 : double latitude, longitude; // degrees
66 : double altitude; // MSL
67 : double heading; // degrees
68 : double speedN, speedE, speedD; // m/s
69 : double xAccel, yAccel, zAccel; // m/s/s in body frame
70 : double rollRate, pitchRate, yawRate; // degrees/s in body frame
71 : double rollDeg, pitchDeg, yawDeg; // euler angles, degrees
72 : Quaternion quaternion;
73 : double airspeed; // m/s, EAS
74 : Vector3f velocity_air_bf; // velocity relative to airmass, body frame, TAS
75 : double battery_voltage; // Volts
76 : double battery_current; // Amps
77 : uint8_t num_motors;
78 : uint32_t motor_mask;
79 : float rpm[32]; // RPM of all motors
80 : uint8_t rcin_chan_count;
81 : float rcin[12]; // RC input 0..1
82 : double range; // rangefinder value
83 : Vector3f bodyMagField; // Truth XYZ magnetic field vector in body-frame. Includes motor interference. Units are milli-Gauss.
84 : Vector3f angAccel; // Angular acceleration in degrees/s/s about the XYZ body axes
85 :
86 : struct {
87 : // data from simulated laser scanner, if available
88 : struct vector3f_array points;
89 : struct float_array ranges;
90 : } scanner;
91 :
92 : #define SITL_NUM_RANGEFINDERS 10
93 : float rangefinder_m[SITL_NUM_RANGEFINDERS];
94 : float airspeed_raw_pressure[AIRSPEED_MAX_SENSORS];
95 :
96 : struct {
97 : float speed;
98 : float direction;
99 : } wind_vane_apparent;
100 :
101 : bool is_lock_step_scheduled;
102 :
103 : // earthframe wind, from backends that know it
104 : Vector3f wind_ef;
105 :
106 : // AGL altitude, usually derived from the terrain database in simulation:
107 : float height_agl;
108 :
109 : };
110 :
111 : // number of rc output channels
112 : #define SITL_NUM_CHANNELS 32
113 :
114 : class SIM {
115 : public:
116 :
117 140432 : SIM() {
118 2096 : if (_singleton != nullptr) {
119 0 : AP_HAL::panic("Too many SITL instances");
120 : }
121 2096 : _singleton = this;
122 2096 : }
123 :
124 : /* Do not allow copies */
125 : CLASS_NO_COPY(SIM);
126 :
127 : static SIM *_singleton;
128 4454508175 : static SIM *get_singleton() { return _singleton; }
129 :
130 2094 : void init() {
131 2094 : if (init_done) {
132 0 : return;
133 : }
134 2094 : init_done = true;
135 :
136 2094 : AP_Param::setup_object_defaults(this, var_info);
137 2094 : AP_Param::setup_object_defaults(this, var_info2);
138 2094 : AP_Param::setup_object_defaults(this, var_info3);
139 : #if AP_SIM_GPS_ENABLED
140 2094 : AP_Param::setup_object_defaults(this, var_gps);
141 : #endif
142 2094 : AP_Param::setup_object_defaults(this, var_mag);
143 2094 : AP_Param::setup_object_defaults(this, var_ins);
144 2094 : AP_Param::setup_object_defaults(this, var_sonar);
145 : #ifdef SFML_JOYSTICK
146 : AP_Param::setup_object_defaults(this, var_sfml_joystick);
147 : #endif // SFML_JOYSTICK
148 8376 : for (uint8_t i=0; i<BARO_MAX_INSTANCES; i++) {
149 6282 : AP_Param::setup_object_defaults(&baro[i], baro[i].var_info);
150 : }
151 6282 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
152 4188 : AP_Param::setup_object_defaults(&airspeed[i], airspeed[i].var_info);
153 : }
154 : // set compass offset
155 8376 : for (uint8_t i = 0; i < HAL_COMPASS_MAX_SENSORS; i++) {
156 6282 : mag_ofs[i].set(Vector3f(5, 13, -18));
157 : }
158 : }
159 : bool init_done;
160 :
161 : enum SITL_RCFail {
162 : SITL_RCFail_None = 0,
163 : SITL_RCFail_NoPulses = 1,
164 : SITL_RCFail_Throttle950 = 2,
165 : SITL_RCFail_Protocol_Fail_Bit_Set = 3,
166 : };
167 :
168 : enum GPSHeading {
169 : GPS_HEADING_NONE = 0,
170 : GPS_HEADING_HDT = 1,
171 : GPS_HEADING_THS = 2,
172 : GPS_HEADING_KSXT = 3,
173 : GPS_HEADING_BASE = 4, // act as an RTK base
174 : };
175 :
176 : enum class GPSOptions : uint32_t {
177 : UBX_IS_F9P = 1U << 0,
178 : };
179 :
180 : struct sitl_fdm state;
181 :
182 : // throttle when motors are active. 0 = 'no throttle', 1 = 'full throttle'
183 : float throttle;
184 :
185 : static const struct AP_Param::GroupInfo var_info[];
186 : static const struct AP_Param::GroupInfo var_info2[];
187 : static const struct AP_Param::GroupInfo var_info3[];
188 : #if AP_SIM_GPS_ENABLED
189 : static const struct AP_Param::GroupInfo var_gps[];
190 : #endif
191 : static const struct AP_Param::GroupInfo var_mag[];
192 : static const struct AP_Param::GroupInfo var_ins[];
193 : static const struct AP_Param::GroupInfo var_sonar[];
194 : #ifdef SFML_JOYSTICK
195 : static const struct AP_Param::GroupInfo var_sfml_joystick[];
196 : #endif //SFML_JOYSTICK
197 :
198 : // Board Orientation (and inverse)
199 : Matrix3f ahrs_rotation;
200 : Matrix3f ahrs_rotation_inv;
201 :
202 : AP_Float mag_noise; // in mag units (earth field is 818)
203 : AP_Vector3f mag_mot; // in mag units per amp
204 : AP_Vector3f mag_ofs[HAL_COMPASS_MAX_SENSORS]; // in mag units
205 : AP_Vector3f mag_diag[HAL_COMPASS_MAX_SENSORS]; // diagonal corrections
206 : AP_Vector3f mag_offdiag[HAL_COMPASS_MAX_SENSORS]; // off-diagonal corrections
207 : AP_Int8 mag_orient[HAL_COMPASS_MAX_SENSORS]; // external compass orientation
208 : AP_Int8 mag_fail[HAL_COMPASS_MAX_SENSORS]; // fail magnetometer, 1 for no data, 2 for freeze
209 : AP_Int8 mag_save_ids;
210 :
211 : // apply the transformation a simulated compass applies to a vector
212 : // after SIM_MAGn_OFS has been subtracted from it: the orientations,
213 : // the board trim and the scale factor. Shared with the simulated
214 : // sensors so that they and get_mag_offsets_for_devid() cannot drift apart.
215 : void mag_sensor_transform(uint8_t instance, Vector3f &v) const;
216 :
217 : // return the offsets a perfectly-calibrated compass instance would
218 : // end up with, in milligauss. SIM_MAGn_OFS is subtracted from the
219 : // field before the sensor transformation above is applied, so the
220 : // offset the compass wants back is that same transformation applied
221 : // to SIM_MAGn_OFS. This depends only on parameters, so it needs no
222 : // field data and no updating.
223 : // the caller has a compass priority index, which is not a
224 : // simulated-sensor index once COMPASS_PRIO*_ID reorders things;
225 : // take the device id so the two cannot be confused
226 : bool get_mag_offsets_for_devid(uint32_t devid, Vector3f &offsets) const;
227 :
228 : AP_Float sonar_glitch;// probability between 0-1 that any given sonar sample will read as max distance
229 : AP_Float sonar_noise; // in metres
230 : AP_Float sonar_scale; // meters per volt
231 : AP_Int8 sonar_rot; // from rotations enumeration
232 : AP_Float sonar_offset; // offset measurement, in meters. Can be used for error injection.
233 :
234 : AP_Float drift_speed; // degrees/second/minute
235 : AP_Float drift_time; // period in minutes
236 : AP_Float engine_mul; // engine multiplier
237 : AP_Int32 engine_fail; // mask of engine/motor servo outputs to fail
238 :
239 : // initial offset on GPS lat/lon, used to shift origin
240 : AP_Float gps_init_lat_ofs;
241 : AP_Float gps_init_lon_ofs;
242 : AP_Float gps_init_alt_ofs;
243 :
244 : // log number for GPS::update_file()
245 : AP_Int16 gps_log_num;
246 :
247 : AP_Float batt_voltage; // battery voltage base
248 : AP_Float batt_capacity_ah; // battery capacity in Ah
249 : AP_Float batt_resistance; // battery internal resistance override in ohms
250 : AP_Int8 rc_fail; // fail RC input
251 : AP_Int8 rc_chancount; // channel count
252 : AP_Int8 float_exception; // enable floating point exception checks
253 : AP_Int32 can_servo_mask; // mask of servos/escs coming from CAN
254 :
255 : #if HAL_NUM_CAN_IFACES
256 : enum class CANTransport : uint8_t {
257 : None = 0,
258 : MulticastUDP = 1,
259 : SocketCAN = 2,
260 : };
261 : AP_Enum<CANTransport> can_transport[HAL_NUM_CAN_IFACES];
262 : #endif
263 :
264 : AP_Int8 flow_enable; // enable simulated optflow
265 : AP_Int16 flow_rate; // optflow data rate (Hz)
266 : AP_Int8 flow_delay; // optflow data delay
267 : AP_Int8 terrain_enable; // enable using terrain for height
268 : AP_Int16 pin_mask; // for GPIO emulation
269 : AP_Float speedup; // simulation speedup
270 : AP_Int8 odom_enable; // enable visual odometry data
271 : AP_Int8 telem_baudlimit_enable; // enable baudrate limiting on links
272 : AP_Float flow_noise; // optical flow measurement noise (rad/sec)
273 : AP_Int8 baro_count; // number of simulated baros to create
274 : AP_Int8 imu_count; // number of simulated IMUs to create
275 : AP_Int32 loop_delay; // extra delay to add to every loop
276 : AP_Float mag_scaling[MAX_CONNECTED_MAGS]; // scaling factor
277 : AP_Int32 mag_devid[MAX_CONNECTED_MAGS]; // Mag devid
278 : AP_Float buoyancy; // submarine buoyancy in Newtons
279 : AP_Int16 loop_rate_hz;
280 : AP_Int16 loop_time_jitter_us;
281 : AP_Int32 on_hardware_output_enable_mask; // mask of output channels passed through to actual hardware
282 : AP_Int16 on_hardware_relay_enable_mask; // mask of relays passed through to actual hardware
283 :
284 : AP_Float uart_byte_loss_pct;
285 :
286 : #ifdef AP_DDS_ENABLED
287 : bool use_dds_sim_time = false; // use ROS2 simulation time for DDS topics
288 : #endif
289 :
290 : #ifdef SFML_JOYSTICK
291 : AP_Int8 sfml_joystick_id;
292 : AP_Int8 sfml_joystick_axis[8];
293 : #endif
294 :
295 : // baro parameters
296 : class BaroParm {
297 : public:
298 : static const struct AP_Param::GroupInfo var_info[];
299 : AP_Float noise; // in metres
300 : AP_Float drift; // in metres per second
301 : AP_Float glitch; // glitch in meters
302 : AP_Int8 freeze; // freeze baro to last recorded altitude
303 : AP_Int8 disable; // disable simulated barometers
304 : AP_Int16 delay; // barometer data delay in ms
305 :
306 : // wind coefficients
307 : AP_Float wcof_xp;
308 : AP_Float wcof_xn;
309 : AP_Float wcof_yp;
310 : AP_Float wcof_yn;
311 : AP_Float wcof_zp;
312 : AP_Float wcof_zn;
313 :
314 : AP_Float ground_effect_alt_err; // rotor downwash baro altitude error in metres (0 disables)
315 : };
316 : BaroParm baro[BARO_MAX_INSTANCES];
317 :
318 : // airspeed parameters
319 : class AirspeedParm {
320 : public:
321 : static const struct AP_Param::GroupInfo var_info[];
322 : AP_Float noise; // pressure noise
323 : AP_Float fail; // airspeed value in m/s to fail to
324 : AP_Float fail_pressure; // pitot tube failure pressure in Pa
325 : AP_Float fail_pitot_pressure; // pitot tube failure pressure in Pa
326 : AP_Float offset; // airspeed sensor offset in m/s
327 : AP_Float ratio; // airspeed ratios
328 : AP_Int8 signflip;
329 : };
330 : AirspeedParm airspeed[AIRSPEED_MAX_SENSORS];
331 :
332 : class ServoParams {
333 : public:
334 2096 : ServoParams(void) {
335 2096 : AP_Param::setup_object_defaults(this, var_info);
336 2096 : }
337 : static const struct AP_Param::GroupInfo var_info[];
338 : AP_Float servo_speed; // servo speed in seconds per 60 degrees
339 : AP_Float servo_delay; // servo delay in seconds
340 : AP_Float servo_filter; // servo 2p filter in Hz
341 : };
342 : ServoParams servo;
343 :
344 : class GPSParms {
345 : public:
346 8384 : GPSParms(void) {
347 8384 : AP_Param::setup_object_defaults(this, var_info);
348 8384 : }
349 : static const struct AP_Param::GroupInfo var_info[];
350 :
351 : AP_Float noise_vertical; // amplitude of the gps altitude error
352 : AP_Int16 lock_time; // delay in seconds before GPS gets lock
353 : AP_Int16 alt_offset; // gps alt error
354 : AP_Int8 enabled; // enable simulated GPS
355 : AP_Int16 delay_ms; // delay in milliseconds
356 : AP_Int8 type; // see enum SITL::GPS::Type
357 : AP_Float byteloss;// byte loss as a percent
358 : AP_Int8 numsats; // number of visible satellites
359 : AP_Vector3f glitch; // glitch offsets in lat, lon and altitude
360 : AP_Int8 hertz; // GPS update rate in Hz
361 : AP_Int8 hdg_enabled; // enable the output of a NMEA heading HDT sentence or UBLOX RELPOSNED
362 : AP_Float drift_alt; // altitude drift error
363 : AP_Vector3f pos_offset; // XYZ position of the GPS antenna phase centre relative to the body frame origin (m)
364 : AP_Float accuracy;
365 : AP_Vector3f vel_err; // Velocity error offsets in NED (x = N, y = E, z = D)
366 : AP_Int8 jam; // jamming simulation enable
367 : AP_Float heading_offset; // heading offset in degrees
368 : AP_Int32 options; // GPS options bitmask
369 : AP_Int8 fix_type; // GPS fix type
370 : AP_Float noise_horizontal; // horizontal noise radius in meters
371 : };
372 : GPSParms gps[AP_SIM_MAX_GPS_SENSORS];
373 :
374 : #if AP_SIM_VICON_ENABLED
375 : class ViconParms {
376 : public:
377 2096 : ViconParms(void) {
378 2096 : AP_Param::setup_object_defaults(this, var_info);
379 2096 : }
380 : static const struct AP_Param::GroupInfo var_info[];
381 :
382 : // vicon parameters
383 : AP_Vector3f pos_offset; // XYZ position of the vicon sensor relative to the body frame origin (m)
384 : AP_Vector3f glitch; // glitch in meters in vicon's local NED frame
385 : AP_Float pos_stddev; // noise in meters in vicon's local NED frame
386 : AP_Float vel_stddev; // noise in m/s in vicon's local NED frame
387 : AP_Int8 fail; // trigger vicon failure
388 : AP_Int16 yaw; // vicon local yaw in degrees
389 : AP_Int16 yaw_error; // vicon yaw error in degrees (added to reported yaw sent to vehicle)
390 : AP_Int8 type_mask; // vicon message type mask (bit0:vision position estimate, bit1:vision speed estimate, bit2:vicon position estimate)
391 : AP_Vector3f vel_glitch; // velocity glitch in m/s in vicon's local frame
392 : AP_Int16 rate_hz; // vicon data rate in Hz
393 : AP_Int8 quality; // odometry quality [-1,100]
394 : };
395 : ViconParms vicon;
396 : #endif // AP_SIM_VICON_ENABLED
397 :
398 : // physics model parameters
399 : class ModelParm {
400 : public:
401 : static const struct AP_Param::GroupInfo var_info[];
402 : #if AP_SIM_STRATOBLIMP_ENABLED
403 : StratoBlimp *stratoblimp_ptr;
404 : #endif
405 : #if AP_SIM_SHIP_ENABLED
406 : ShipSim shipsim;
407 : #endif
408 : #if AP_SIM_GLIDER_ENABLED
409 : Glider *glider_ptr;
410 : #endif
411 : // multicopter/quadplane VTOL frame model
412 : class Frame *simframe_ptr;
413 : #if AP_SIM_SLUNGPAYLOAD_ENABLED
414 : SlungPayloadSim slung_payload_sim;
415 : #endif
416 : #if AP_SIM_TETHER_ENABLED
417 : TetherSim tether_sim;
418 : #endif
419 : #if AP_SIM_FLIGHTAXIS_ENABLED
420 : FlightAxis *flightaxis_ptr;
421 : #endif
422 : #if AP_SIM_AIS_ENABLED
423 : class AIS *ais_ptr;
424 : #endif // AP_SIM_AIS_ENABLED
425 : };
426 : ModelParm models;
427 :
428 : // EFI type
429 : enum EFIType {
430 : EFI_TYPE_NONE = 0,
431 : EFI_TYPE_MS = 1,
432 : EFI_TYPE_LOWEHEISER = 2,
433 : EFI_TYPE_HIRTH = 8,
434 : };
435 :
436 : AP_Int8 efi_type;
437 :
438 : // wind control
439 : enum WindType {
440 : WIND_TYPE_SQRT = 0,
441 : WIND_TYPE_NO_LIMIT = 1,
442 : WIND_TYPE_COEF = 2,
443 : };
444 :
445 : float wind_speed_active;
446 : float wind_direction_active;
447 : float wind_dir_z_active;
448 : AP_Float wind_speed;
449 : AP_Float wind_direction;
450 : AP_Float wind_turbulance;
451 : AP_Float wind_dir_z;
452 : AP_Float wind_change_tc;
453 : AP_Int8 wind_type; // enum WindLimitType
454 : AP_Float wind_type_alt;
455 : AP_Float wind_type_coef;
456 :
457 : AP_Int16 mag_delay; // magnetometer data delay in ms
458 :
459 : // ADSB related run-time options
460 : enum class ADSBType {
461 : Shortcut = 0,
462 : SageTechMXS = 3,
463 : };
464 : AP_Enum<ADSBType> adsb_types; // bitmask of active ADSB types
465 : AP_Int16 adsb_plane_count;
466 : AP_Float adsb_radius_m;
467 : AP_Float adsb_altitude_m;
468 : AP_Int8 adsb_tx;
469 :
470 : // Earth magnetic field anomaly
471 : AP_Vector3f mag_anomaly_ned; // NED anomaly vector at ground level (mGauss)
472 : AP_Float mag_anomaly_hgt; // height above ground where anomally strength has decayed to 1/8 of the ground level value (m)
473 :
474 : // Body frame sensor position offsets
475 : AP_Vector3f imu_pos_offset; // XYZ position of the IMU accelerometer relative to the body frame origin (m)
476 : AP_Vector3f rngfnd_pos_offset; // XYZ position of the range finder zero range datum relative to the body frame origin (m)
477 : AP_Vector3f optflow_pos_offset; // XYZ position of the optical flow sensor focal point relative to the body frame origin (m)
478 :
479 : // barometer temperature control
480 : AP_Float temp_start; // [deg C] Barometer start temperature
481 : AP_Float temp_board_offset; // [deg C] Barometer board temperature offset from atmospheric temperature
482 : AP_Float temp_tconst; // [deg C] Barometer warmup temperature time constant
483 : AP_Float temp_baro_factor;
484 :
485 : AP_Int8 thermal_scenario;
486 :
487 : // weight on wheels pin
488 : AP_Int8 wow_pin;
489 :
490 : // vibration frequencies in Hz on each axis
491 : AP_Vector3f vibe_freq;
492 :
493 : // max frequency to use as baseline for adding motor noise for the gyros and accels
494 : AP_Float vibe_motor;
495 : // amplitude scaling of motor noise relative to gyro/accel noise
496 : AP_Float vibe_motor_scale;
497 :
498 : // what harmonics to generate
499 : AP_Int16 vibe_motor_harmonics;
500 :
501 : // what servos are motors
502 : AP_Int32 vibe_motor_mask;
503 :
504 : // minimum throttle for addition of ins noise
505 : AP_Float ins_noise_throttle_min;
506 :
507 : struct {
508 : AP_Float x;
509 : AP_Float y;
510 : AP_Float z;
511 : AP_Int32 t;
512 :
513 : uint32_t start_ms;
514 : } shove;
515 :
516 : struct {
517 : AP_Float x;
518 : AP_Float y;
519 : AP_Float z;
520 : AP_Int32 t;
521 :
522 : uint32_t start_ms;
523 : } twist;
524 :
525 : AP_Int8 gnd_behav;
526 :
527 : AP_Enum<Rotation> imu_orientation;
528 :
529 : struct {
530 : AP_Int8 enable; // 0: disabled, 1: roll and pitch, 2: roll, pitch and heave
531 : AP_Float length; // m
532 : AP_Float amp; // m
533 : AP_Float direction; // deg (direction wave is coming from)
534 : AP_Float speed; // m/s
535 : } wave;
536 :
537 : struct {
538 : AP_Float direction; // deg (direction tide is coming from)
539 : AP_Float speed; // m/s
540 : } tide;
541 :
542 : // original simulated position
543 : struct {
544 : AP_Float lat;
545 : AP_Float lng;
546 : AP_Float alt; // metres
547 : AP_Float hdg; // 0 to 360
548 : } opos;
549 :
550 : uint16_t irlock_port;
551 : uint16_t rcin_port;
552 : const char *rcin_path = nullptr;
553 :
554 : time_t start_time_UTC;
555 :
556 : void simstate_send(mavlink_channel_t chan) const;
557 : void sim_state_send(mavlink_channel_t chan) const;
558 :
559 : void Log_Write_SIMSTATE();
560 :
561 : // convert a set of roll rates from earth frame to body frame
562 : static void convert_body_frame(double rollDeg, double pitchDeg,
563 : double rollRate, double pitchRate, double yawRate,
564 : double *p, double *q, double *r);
565 :
566 : // convert a set of roll rates from body frame to earth frame
567 : static Vector3f convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro);
568 :
569 3308705 : int i2c_ioctl(uint8_t i2c_operation, void *data) {
570 3308705 : return i2c_sim.ioctl(i2c_operation, data);
571 : }
572 :
573 263095 : int spi_ioctl(uint8_t bus, uint8_t cs_pin, uint8_t spi_operation, void *data) {
574 263095 : return spi_sim.ioctl(bus, cs_pin, spi_operation, data);
575 : }
576 :
577 : #if AP_SIM_SPRAYER_ENABLED
578 : Sprayer sprayer_sim;
579 : #endif // AP_SIM_SPRAYER_ENABLED
580 :
581 : #if AP_SIM_GRIPPER_ENABLED
582 : Gripper_Servo gripper_sim;
583 : #endif // AP_SIM_GRIPPER_ENABLED
584 : #if AP_SIM_GRIPPER_EPM_ENABLED
585 : Gripper_EPM gripper_epm_sim;
586 : #endif // AP_SIM_GRIPPER_EPM_ENABLED
587 :
588 : #if AP_SIM_PARACHUTE_ENABLED
589 : Parachute parachute_sim;
590 : #endif // AP_SIM_PARACHUTE_ENABLED
591 : #if AP_SIM_BUZZER_ENABLED
592 : Buzzer buzzer_sim;
593 : #endif // AP_SIM_BUZZER_ENABLED
594 : I2C i2c_sim;
595 : SPI spi_sim;
596 : ToneAlarm tonealarm_sim;
597 : #if AP_SIM_PRECLAND_ENABLED
598 : SIM_Precland precland_sim;
599 : #endif // AP_SIM_PRECLAND_ENABLED
600 : RichenPower richenpower_sim;
601 : #if AP_SIM_LOWEHEISER_ENABLED
602 : Loweheiser loweheiser_sim;
603 : #endif
604 : IntelligentEnergy24 ie24_sim;
605 : FETtecOneWireESC fetteconewireesc_sim;
606 : #if AP_SIM_VOLZ_ENABLED
607 : Volz volz_sim;
608 : #endif // AP_SIM_VOLZ_ENABLED
609 : #if AP_TEST_DRONECAN_DRIVERS
610 : DroneCANDevice dronecan_sim;
611 : #endif
612 :
613 : // ESC telemetry
614 : AP_Int8 esc_telem;
615 : // RPM when motors are armed
616 : AP_Float esc_rpm_armed;
617 :
618 : struct {
619 : // LED state, for serial LED emulation
620 : struct {
621 : uint8_t rgb[3];
622 : } rgb[16][32];
623 : uint8_t num_leds[16];
624 : uint32_t send_counter;
625 : } led;
626 :
627 : AP_Int8 led_layout;
628 :
629 : // get the rangefinder reading for the desired instance, returns -1 for no data
630 : float get_rangefinder(uint8_t instance);
631 :
632 : float measure_distance_at_angle_bf(const Location &location, float angle) const;
633 :
634 : // get the apparent wind speed and direction as set by external physics backend
635 0 : float get_apparent_wind_dir() const{return state.wind_vane_apparent.direction;}
636 0 : float get_apparent_wind_spd() const{return state.wind_vane_apparent.speed;}
637 :
638 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
639 : // IMU temperature calibration params
640 : AP_Float imu_temp_start;
641 : AP_Float imu_temp_end;
642 : AP_Float imu_temp_tconst;
643 : AP_Float imu_temp_fixed;
644 : AP_InertialSensor_TCal imu_tcal[INS_MAX_INSTANCES];
645 : #endif
646 :
647 : // IMU control parameters
648 : AP_Float gyro_noise[INS_MAX_INSTANCES]; // in degrees/second
649 : AP_Vector3f gyro_scale[INS_MAX_INSTANCES]; // percentage
650 : AP_Vector3f gyro_bias[INS_MAX_INSTANCES]; // in rad/s
651 : AP_Float accel_noise[INS_MAX_INSTANCES]; // in m/s/s
652 : AP_Vector3f accel_bias[INS_MAX_INSTANCES]; // in m/s/s
653 : AP_Vector3f accel_scale[INS_MAX_INSTANCES]; // in m/s/s
654 : AP_Vector3f board_trim; // rigid board mounting offset (rad), rotates accel+gyro+compass
655 : AP_Float accel_fail[INS_MAX_INSTANCES]; // accelerometer failure value
656 : // gyro and accel fail masks
657 : AP_Int8 gyro_fail_mask;
658 : AP_Int8 accel_fail_mask;
659 :
660 : // Sailboat sim only
661 : AP_Int8 sail_type;
662 :
663 : // Master instance to use servos from with slave instances
664 : AP_Int8 ride_along_master;
665 :
666 : // clamp simulation - servo channel starting at offset 1 (usually ailerons)
667 : AP_Int8 clamp_ch;
668 :
669 : // Offsets applied to SIM AHRS type
670 : // For testing stepless handover between AHRS estimators
671 : struct {
672 : AP_Float roll;
673 : AP_Float pitch;
674 : AP_Float yaw;
675 : } sim_ahrs_offset;
676 :
677 : #if AP_SIM_INS_FILE_ENABLED
678 : enum INSFileMode {
679 : INS_FILE_NONE = 0,
680 : INS_FILE_READ = 1,
681 : INS_FILE_WRITE = 2,
682 : INS_FILE_READ_STOP_ON_EOF = 3,
683 : };
684 : AP_Int8 gyro_file_rw;
685 : AP_Int8 accel_file_rw;
686 : #endif
687 :
688 : #ifdef WITH_SITL_OSD
689 : AP_Int16 osd_rows;
690 : AP_Int16 osd_columns;
691 : #endif
692 :
693 : // Allow inhibiting of SITL only sim state messages over MAVLink
694 : // This gives more realistic data rates for testing links
695 0 : void set_stop_MAVLink_sim_state() { stop_MAVLink_sim_state = true; }
696 : bool stop_MAVLink_sim_state;
697 :
698 : /*
699 : used by scripting to control simulated aircraft position
700 : */
701 : bool set_pose(uint8_t instance, const Location &loc, const Quaternion &quat,
702 : const Vector3f &velocity_ef, const Vector3f &gyro_rads);
703 : };
704 :
705 : } // namespace SITL
706 :
707 :
708 : namespace AP {
709 : SITL::SIM *sitl();
710 : };
711 :
712 : #endif // AP_SIM_ENABLED
|
