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 : /*
17 : SITL.cpp - software in the loop state
18 : */
19 :
20 : #include "SITL.h"
21 :
22 : #if AP_SIM_ENABLED
23 :
24 : #include <AP_Common/AP_Common.h>
25 : #include <AP_HAL/AP_HAL.h>
26 : #include <AP_Vehicle/AP_Vehicle_Type.h>
27 :
28 : #include <GCS_MAVLink/GCS_MAVLink.h>
29 : #include <AP_Logger/AP_Logger.h>
30 : #include <AP_InertialSensor/AP_InertialSensor.h>
31 :
32 : #ifdef SFML_JOYSTICK
33 : #ifdef HAVE_SFML_GRAPHICS_HPP
34 : #include <SFML/Window/Joystick.hpp>
35 : #elif HAVE_SFML_GRAPHIC_H
36 : #include <SFML/Window/Joystick.h>
37 : #endif
38 : #endif // SFML_JOYSTICK
39 :
40 : #include "SIM_StratoBlimp.h"
41 : #include "SIM_Glider.h"
42 : #include "SIM_FlightAxis.h"
43 : #include "SIM_Frame.h"
44 :
45 : extern const AP_HAL::HAL& hal;
46 :
47 : #ifndef SIM_RATE_HZ_DEFAULT
48 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
49 : #define SIM_RATE_HZ_DEFAULT 1200
50 : #else
51 : #define SIM_RATE_HZ_DEFAULT 400
52 : #endif
53 : #endif
54 :
55 : #if (CONFIG_HAL_BOARD != HAL_BOARD_SITL)
56 : // For on-hardware, set allowed relay channels to zero.
57 : // Requires user to change the param to allow hadware access.
58 : #define SIM_DEFAULT_ENABLED_RELAY_CHANNELS 0
59 : #else
60 : // For SITL, set allowed relay channels to the full mask.
61 : #define SIM_DEFAULT_ENABLED_RELAY_CHANNELS UINT16_MAX
62 : #endif
63 :
64 : #if APM_BUILD_TYPE(APM_BUILD_Heli)
65 : #define SIM_DEFAULT_BATT_VOLTAGE 50.4f
66 : #else
67 : #define SIM_DEFAULT_BATT_VOLTAGE 12.6f
68 : #endif
69 :
70 : namespace SITL {
71 :
72 : SIM *SIM::_singleton = nullptr;
73 :
74 : // table of user settable parameters
75 : const AP_Param::GroupInfo SIM::var_info[] = {
76 :
77 : // @Param: DRIFT_SPEED
78 : // @DisplayName: Gyro drift speed
79 : // @Description: Gyro drift rate of change in degrees/second/minute
80 : AP_GROUPINFO("DRIFT_SPEED", 5, SIM, drift_speed, 0.05f),
81 : // @Param: DRIFT_TIME
82 : // @DisplayName: Gyro drift time
83 : // @Description: Gyro drift duration of one full drift cycle (period in minutes)
84 : AP_GROUPINFO("DRIFT_TIME", 6, SIM, drift_time, 5),
85 : // @Param: ENGINE_MUL
86 : // @DisplayName: Engine failure thrust scaler
87 : // @Description: Thrust from Motors in SIM_ENGINE_FAIL will be multiplied by this factor
88 : // @Range: 0 1
89 : AP_GROUPINFO("ENGINE_MUL", 8, SIM, engine_mul, 0),
90 : // @Param: WIND_SPD
91 : // @DisplayName: Simulated Wind speed
92 : // @Description: Allows you to emulate wind in sim
93 : // @Units: m/s
94 : // @User: Advanced
95 : AP_GROUPINFO("WIND_SPD", 9, SIM, wind_speed, 0),
96 : // @Param: WIND_DIR
97 : // @DisplayName: Direction simulated wind is coming from
98 : // @Description: Allows you to set wind direction (true deg) in sim
99 : // @Units: deg
100 : // @User: Advanced
101 : AP_GROUPINFO("WIND_DIR", 10, SIM, wind_direction, 180),
102 :
103 : // @Param: WIND_TURB
104 : // @DisplayName: Simulated Wind variation
105 : // @Description: Allows you to emulate random wind variations in sim
106 : // @Units: m/s
107 : // @User: Advanced
108 : AP_GROUPINFO("WIND_TURB", 11, SIM, wind_turbulance, 0),
109 :
110 : // @Param: WIND_TC
111 : // @DisplayName: Wind variation time constant
112 : // @Description: this controls the time over which wind changes take effect
113 : // @Units: s
114 : // @User: Advanced
115 : AP_GROUPINFO("WIND_TC", 12, SIM, wind_change_tc, 5),
116 :
117 : // @Group: SERVO_
118 : // @Path: ./ServoModel.cpp
119 : AP_SUBGROUPINFO(servo, "SERVO_", 16, SIM, ServoParams),
120 :
121 : AP_SUBGROUPEXTENSION("", 17, SIM, var_sonar),
122 : // @Param: BATT_VOLTAGE
123 : // @DisplayName: Simulated battery resting voltage
124 : // @Description: Simulated battery resting voltage (no load sag). Defaults to and clipped to the battery model's maximum voltage. Changes re-initialize the state of charge, and values below the maximum indicate a partially-charged battery. For batteries with unlimited capacity, see `SIM_BATT_CAP_AH`. Value ignored when receiving battery state updates from an external source.
125 : // @Units: V
126 : // @User: Advanced
127 : AP_GROUPINFO("BATT_VOLTAGE", 19, SIM, batt_voltage, SIM_DEFAULT_BATT_VOLTAGE),
128 : // @Param: BATT_CAP_AH
129 : // @DisplayName: Simulated battery capacity
130 : // @Description: Simulated battery capacity. Changes re-initialize the state of charge of the battery. Set to 0 for unlimited capacity. Value ignored when receiving battery state updates from an external source.
131 : // @Units: Ah
132 : // @User: Advanced
133 : AP_GROUPINFO("BATT_CAP_AH", 20, SIM, batt_capacity_ah, 0),
134 : // @Param: BATT_RES_OHM
135 : // @DisplayName: Simulated battery internal resistance
136 : // @Description: Simulated battery internal resistance, used to model voltage sag under load (sag = current * resistance) and temperature growth. A negative value implies "use previous resistance", which is the default in order that a model-provided resistance is the default behavior. Set to 0 to disable voltage sag and temperature growth entirely.
137 : // @Units: Ohm
138 : // @User: Advanced
139 : // @RebootRequired: True
140 : AP_GROUPINFO("BATT_RES_OHM", 21, SIM, batt_resistance, -1),
141 : // 23 was SONAR_GLITCH
142 : // 24 was SONAR_RND
143 : // @Param: RC_FAIL
144 : // @DisplayName: Simulated RC signal failure
145 : // @Description: Allows you to emulate rc failures in sim
146 : // @Values: 0:Disabled,1:No RC pusles,2:All Channels neutral except Throttle is 950us
147 : // @User: Advanced
148 : AP_GROUPINFO("RC_FAIL", 25, SIM, rc_fail, 0),
149 : // @Param: FLOAT_EXCEPT
150 : // @DisplayName: Generate floating point exceptions
151 : // @Description: If set, if a numerical error occurs SITL will die with a floating point exception.
152 : // @User: Advanced
153 : AP_GROUPINFO("FLOAT_EXCEPT", 28, SIM, float_exception, 1),
154 :
155 : // @Param: CAN_SRV_MSK
156 : // @DisplayName: Mask of CAN servos/ESCs
157 : // @Description: The set of actuators controlled externally by CAN SITL AP_Periph
158 : // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15, 15: Servo 16, 16: Servo 17, 17: Servo 18, 18: Servo 19, 19: Servo 20, 20: Servo 21, 21: Servo 22, 22: Servo 23, 23: Servo 24, 24: Servo 25, 25: Servo 26, 26: Servo 27, 27: Servo 28, 28: Servo 29, 29: Servo 30, 30: Servo 31, 31: Servo 32
159 : // @User: Advanced
160 : AP_GROUPINFO("CAN_SRV_MSK", 29, SIM, can_servo_mask, 0),
161 :
162 : #if HAL_NUM_CAN_IFACES > 0
163 : // @Param: CAN_TYPE1
164 : // @DisplayName: transport type for first CAN interface
165 : // @Description: transport type for first CAN interface
166 : // @Values: 0:None,1:MulticastUDP,2:SocketCAN
167 : // @User: Advanced
168 : AP_GROUPINFO("CAN_TYPE1", 30, SIM, can_transport[0], uint8_t(CANTransport::MulticastUDP)),
169 : #endif
170 :
171 : #if HAL_NUM_CAN_IFACES > 1
172 : // @Param: CAN_TYPE2
173 : // @DisplayName: transport type for second CAN interface
174 : // @Description: transport type for second CAN interface
175 : // @Values: 0:None,1:MulticastUDP,2:SocketCAN
176 : // @User: Advanced
177 : AP_GROUPINFO("CAN_TYPE2", 31, SIM, can_transport[1], uint8_t(CANTransport::MulticastUDP)),
178 : #endif
179 :
180 : // 32 was SONAR_SCALE
181 : // @Param: FLOW_ENABLE
182 : // @DisplayName: Opflow Enable
183 : // @Description: Enable simulated Optical Flow sensor
184 : // @Values: 0:Disable,1:Enabled
185 : AP_GROUPINFO("FLOW_ENABLE", 33, SIM, flow_enable, 0),
186 : // @Param: TERRAIN
187 : // @DisplayName: Terrain Enable
188 : // @Description: Enable using terrain for height
189 : // @Values: 0:Disable,1:Enabled
190 : AP_GROUPINFO("TERRAIN", 34, SIM, terrain_enable, 1),
191 : // @Param: FLOW_RATE
192 : // @DisplayName: Opflow Rate
193 : // @Description: Opflow Data Rate
194 : // @Units: Hz
195 : AP_GROUPINFO("FLOW_RATE", 35, SIM, flow_rate, 10),
196 : // @Param: FLOW_DELAY
197 : // @DisplayName: Opflow Delay
198 : // @Description: Opflow data delay
199 : // @Units: ms
200 : AP_GROUPINFO("FLOW_DELAY", 36, SIM, flow_delay, 0),
201 : // @Param: ADSB_COUNT
202 : // @DisplayName: Number of ADSB aircrafts
203 : // @Description: Total number of ADSB simulated aircraft
204 : AP_GROUPINFO("ADSB_COUNT", 45, SIM, adsb_plane_count, -1),
205 : // @Param: ADSB_RADIUS
206 : // @DisplayName: ADSB radius stddev of another aircraft
207 : // @Description: Simulated standard deviation of radius in ADSB of another aircraft
208 : // @Units: m
209 : AP_GROUPINFO("ADSB_RADIUS", 46, SIM, adsb_radius_m, 10000),
210 : // @Param: ADSB_ALT
211 : // @DisplayName: ADSB altitude of another aircraft
212 : // @Description: Simulated ADSB altitude of another aircraft
213 : // @Units: m
214 : AP_GROUPINFO("ADSB_ALT", 47, SIM, adsb_altitude_m, 1000),
215 : // @Param: PIN_MASK
216 : // @DisplayName: GPIO emulation
217 : // @Description: SITL GPIO emulation
218 : AP_GROUPINFO("PIN_MASK", 50, SIM, pin_mask, 0),
219 : // @Param: ADSB_TX
220 : // @DisplayName: ADSB transmit enable
221 : // @Description: ADSB transceiever enable and disable
222 : // @Values: 0:Transceiever disable, 1:Transceiever enable
223 : AP_GROUPINFO("ADSB_TX", 51, SIM, adsb_tx, 0),
224 : // @Param: SPEEDUP
225 : // @DisplayName: Sim Speedup
226 : // @Description: Runs the simulation at multiples of normal speed. Do not use if realtime physics, like RealFlight, is being used
227 : // @Range: 1 10
228 : // @User: Advanced
229 : AP_GROUPINFO("SPEEDUP", 52, SIM, speedup, 1),
230 : // @Param: IMU_POS
231 : // @DisplayName: IMU Offsets
232 : // @Description: XYZ position of the IMU accelerometer relative to the body frame origin
233 : // @Units: m
234 : // @Vector3Parameter: 1
235 : AP_GROUPINFO("IMU_POS", 53, SIM, imu_pos_offset, 0),
236 : AP_SUBGROUPEXTENSION("", 54, SIM, var_ins),
237 : // 55 was SONAR_POS
238 : // @Param: FLOW_POS
239 : // @DisplayName: Opflow Pos
240 : // @Description: XYZ position of the optical flow sensor focal point relative to the body frame origin
241 : // @Units: m
242 : // @Vector3Parameter: 1
243 : AP_GROUPINFO("FLOW_POS", 56, SIM, optflow_pos_offset, 0),
244 : // @Param: ENGINE_FAIL
245 : // @DisplayName: Engine Fail Mask
246 : // @Description: mask of motors which SIM_ENGINE_MUL will be applied to
247 : // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15, 15: Servo 16, 16: Servo 17, 17: Servo 18, 18: Servo 19, 19: Servo 20, 20: Servo 21, 21: Servo 22, 22: Servo 23, 23: Servo 24, 24: Servo 25, 25: Servo 26, 26: Servo 27, 27: Servo 28, 28: Servo 29, 29: Servo 30, 30: Servo 31, 31: Servo 32
248 : AP_GROUPINFO("ENGINE_FAIL", 58, SIM, engine_fail, 0),
249 : AP_SUBGROUPINFO(models, "", 59, SIM, SIM::ModelParm),
250 : AP_SUBGROUPEXTENSION("", 60, SIM, var_mag),
251 : #if AP_SIM_GPS_ENABLED
252 : AP_SUBGROUPEXTENSION("", 61, SIM, var_gps),
253 : #endif
254 : AP_SUBGROUPEXTENSION("", 62, SIM, var_info3),
255 : AP_SUBGROUPEXTENSION("", 63, SIM, var_info2),
256 : AP_GROUPEND
257 : };
258 :
259 : // second table of user settable parameters for SITL.
260 : const AP_Param::GroupInfo SIM::var_info2[] = {
261 : // @Param: TEMP_START
262 : // @DisplayName: Start temperature
263 : // @Description: Baro start temperature
264 : // @Units: degC
265 : // @User: Advanced
266 : AP_GROUPINFO("TEMP_START", 1, SIM, temp_start, 25),
267 : // @Param: TEMP_BRD_OFF
268 : // @DisplayName: Baro temperature offset
269 : // @Description: Barometer board temperature offset from atmospheric temperature
270 : // @Units: degC
271 : // @User: Advanced
272 : AP_GROUPINFO("TEMP_BRD_OFF", 2, SIM, temp_board_offset, 20),
273 : // @Param: TEMP_TCONST
274 : // @DisplayName: Warmup time constant
275 : // @Description: Barometer warmup temperature time constant
276 : // @Units: degC
277 : // @User: Advanced
278 : AP_GROUPINFO("TEMP_TCONST", 3, SIM, temp_tconst, 30),
279 :
280 : // @Param: TEMP_BFACTOR
281 : // @DisplayName: Baro temperature factor
282 : // @Description: A pressure change with temperature that closely matches what has been observed with a ICM-20789
283 : // @User: Advanced
284 : AP_GROUPINFO("TEMP_BFACTOR", 4, SIM, temp_baro_factor, 0),
285 : // @Param: WIND_DIR_Z
286 : // @DisplayName: Simulated wind vertical direction
287 : // @Description: Allows you to set vertical wind direction (true deg) in sim. 0 means pure horizontal wind. 90 means pure updraft.
288 : // @Units: deg
289 : // @User: Advanced
290 : AP_GROUPINFO("WIND_DIR_Z", 10, SIM, wind_dir_z, 0),
291 : // @Param: WIND_T
292 : // @DisplayName: Wind Profile Type
293 : // @Description: Selects how wind varies from surface to WIND_T_ALT
294 : // @Values: 0:square law,1: none, 2:linear-see WIND_T_COEF
295 : // @User: Advanced
296 : AP_GROUPINFO("WIND_T" ,15, SIM, wind_type, SIM::WIND_TYPE_SQRT),
297 : // @Param: WIND_T_ALT
298 : // @DisplayName: Full Wind Altitude
299 : // @Description: Altitude at which wind reaches full strength, decaying from full strength as altitude lowers to ground level
300 : // @Units: m
301 : // @User: Advanced
302 : AP_GROUPINFO("WIND_T_ALT" ,16, SIM, wind_type_alt, 60),
303 : // @Param: WIND_T_COEF
304 : // @DisplayName: Linear Wind Curve Coeff
305 : // @Description: For linear wind profile,wind is reduced by (Altitude-WIND_T_ALT) x this value
306 : // @User: Advanced
307 : AP_GROUPINFO("WIND_T_COEF", 17, SIM, wind_type_coef, 0.01f),
308 : // @Param: RC_CHANCOUNT
309 : // @DisplayName: RC channel count
310 : // @Description: SITL RC channel count
311 : AP_GROUPINFO("RC_CHANCOUNT",21, SIM, rc_chancount, 16),
312 : #if AP_SIM_SPRAYER_ENABLED
313 : // @Group: SPR_
314 : // @Path: ./SIM_Sprayer.cpp
315 : AP_SUBGROUPINFO(sprayer_sim, "SPR_", 22, SIM, Sprayer),
316 : #endif // AP_SIM_SPRAYER_ENABLED
317 : #if AP_SIM_GRIPPER_ENABLED
318 : // @Group: GRPS_
319 : // @Path: ./SIM_Gripper_Servo.cpp
320 : AP_SUBGROUPINFO(gripper_sim, "GRPS_", 23, SIM, Gripper_Servo),
321 : #endif // AP_SIM_GRIPPER_ENABLED
322 : #if AP_SIM_GRIPPER_EPM_ENABLED
323 : // @Group: GRPE_
324 : // @Path: ./SIM_Gripper_EPM.cpp
325 : AP_SUBGROUPINFO(gripper_epm_sim, "GRPE_", 24, SIM, Gripper_EPM),
326 : #endif // AP_SIM_GRIPPER_EPM_ENABLED
327 :
328 : // @Param: WOW_PIN
329 : // @DisplayName: Weight on Wheels Pin
330 : // @Description: SITL set this simulated pin to true if vehicle is on ground
331 : // @User: Advanced
332 : AP_GROUPINFO("WOW_PIN", 25, SIM, wow_pin, -1),
333 :
334 : // @Param: VIB_FREQ
335 : // @DisplayName: Vibration frequency
336 : // @Description: Frequency of vibration applied to IMU readings in SITL
337 : // @Units: Hz
338 : // @Vector3Parameter: 1
339 : AP_GROUPINFO("VIB_FREQ", 26, SIM, vibe_freq, 0),
340 :
341 : #if AP_SIM_PARACHUTE_ENABLED
342 : // @Group: PARA_
343 : // @Path: ./SIM_Parachute.cpp
344 : AP_SUBGROUPINFO(parachute_sim, "PARA_", 27, SIM, Parachute),
345 : #endif // AP_SIM_PARACHUTE_ENABLED
346 :
347 : // @Param: BAUDLIMIT_EN
348 : // @DisplayName: Telemetry bandwidth limitting
349 : // @Description: SITL enable bandwidth limitting on telemetry ports with non-zero values
350 : AP_GROUPINFO("BAUDLIMIT_EN", 28, SIM, telem_baudlimit_enable, 0),
351 :
352 : #if AP_SIM_PRECLAND_ENABLED
353 : // @Group: PLD_
354 : // @Path: ./SIM_Precland.cpp
355 : AP_SUBGROUPINFO(precland_sim, "PLD_", 29, SIM, SIM_Precland),
356 : #endif // AP_SIM_PRECLAND_ENABLED
357 :
358 : // @Param: SHOVE_X
359 : // @DisplayName: Acceleration of shove x
360 : // @Description: Acceleration of shove to vehicle in x axis
361 : // @Units: m/s/s
362 : AP_GROUPINFO("SHOVE_X", 30, SIM, shove.x, 0),
363 : // @Param: SHOVE_Y
364 : // @DisplayName: Acceleration of shove y
365 : // @Description: Acceleration of shove to vehicle in y axis
366 : // @Units: m/s/s
367 : AP_GROUPINFO("SHOVE_Y", 31, SIM, shove.y, 0),
368 : // @Param: SHOVE_Z
369 : // @DisplayName: Acceleration of shove z
370 : // @Description: Acceleration of shove to vehicle in z axis
371 : // @Units: m/s/s
372 : AP_GROUPINFO("SHOVE_Z", 32, SIM, shove.z, 0),
373 : // @Param: SHOVE_TIME
374 : // @DisplayName: Time length for shove
375 : // @Description: Force to the vehicle over a period of time
376 : // @Units: ms
377 : AP_GROUPINFO("SHOVE_TIME", 33, SIM, shove.t, 0),
378 :
379 : // @Param: FLOW_RND
380 : // @DisplayName: Opflow noise
381 : // @Description: Optical Flow sensor measurement noise
382 : // @Units: rad/s
383 : AP_GROUPINFO("FLOW_RND", 34, SIM, flow_noise, 0.05f),
384 :
385 : // @Param: TWIST_X
386 : // @DisplayName: Twist x
387 : // @Description: Rotational acceleration of twist x axis
388 : // @Units: rad/s/s
389 : AP_GROUPINFO("TWIST_X", 37, SIM, twist.x, 0),
390 : // @Param: TWIST_Y
391 : // @DisplayName: Twist y
392 : // @Description: Rotational acceleration of twist y axis
393 : // @Units: rad/s/s
394 : AP_GROUPINFO("TWIST_Y", 38, SIM, twist.y, 0),
395 : // @Param: TWIST_Z
396 : // @DisplayName: Twist z
397 : // @Description: Rotational acceleration of twist z axis
398 : // @Units: rad/s/s
399 : AP_GROUPINFO("TWIST_Z", 39, SIM, twist.z, 0),
400 : // @Param: TWIST_TIME
401 : // @DisplayName: Twist time
402 : // @Description: Time that twist is applied on the vehicle
403 : // @Units: ms
404 : AP_GROUPINFO("TWIST_TIME", 40, SIM, twist.t, 0),
405 :
406 : // @Param: GND_BEHAV
407 : // @DisplayName: Ground behavior
408 : // @Description: Ground behavior of aircraft (tailsitter, no movement, forward only)
409 : AP_GROUPINFO("GND_BEHAV", 41, SIM, gnd_behav, -1),
410 :
411 : // @Param: IMU_ORIENT
412 : // @CopyFieldsFrom: AHRS_ORIENTATION
413 : // @DisplayName: IMU orientation
414 : // @Description: Simulated orientation of the IMUs
415 : AP_GROUPINFO("IMU_ORIENT", 42, SIM, imu_orientation, 0),
416 :
417 : // sailboat wave and tide simulation parameters
418 :
419 : // @Param: WAVE_ENABLE
420 : // @DisplayName: Wave enable
421 : // @Description: Wave enable and modes
422 : // @Values: 0:disabled, 1: roll and pitch, 2: roll and pitch and heave
423 : AP_GROUPINFO("WAVE_ENABLE", 44, SIM, wave.enable, 0.0f),
424 : // @Param: WAVE_LENGTH
425 : // @DisplayName: Wave length
426 : // @Description: Wave length in SITL
427 : // @Units: m
428 : AP_GROUPINFO("WAVE_LENGTH", 45, SIM, wave.length, 10.0f),
429 : // @Param: WAVE_AMP
430 : // @DisplayName: Wave amplitude
431 : // @Description: Wave amplitude in SITL
432 : // @Units: m
433 : AP_GROUPINFO("WAVE_AMP", 46, SIM, wave.amp, 0.5f),
434 : // @Param: WAVE_DIR
435 : // @DisplayName: Wave direction
436 : // @Description: Direction wave is coming from
437 : // @Units: deg
438 : AP_GROUPINFO("WAVE_DIR", 47, SIM, wave.direction, 0.0f),
439 : // @Param: WAVE_SPEED
440 : // @DisplayName: Wave speed
441 : // @Description: Wave speed in SITL
442 : // @Units: m/s
443 : AP_GROUPINFO("WAVE_SPEED", 48, SIM, wave.speed, 0.5f),
444 : // @Param: TIDE_DIR
445 : // @DisplayName: Tide direction
446 : // @Description: Tide direction wave is coming from
447 : // @Units: deg
448 : AP_GROUPINFO("TIDE_DIR", 49, SIM, tide.direction, 0.0f),
449 : // @Param: TIDE_SPEED
450 : // @DisplayName: Tide speed
451 : // @Description: Tide speed in simulation
452 : // @Units: m/s
453 : AP_GROUPINFO("TIDE_SPEED", 50, SIM, tide.speed, 0.0f),
454 :
455 : // the following coordinates are for CMAC, in Canberra
456 : // @Param: OPOS_LAT
457 : // @DisplayName: Original Position (Latitude)
458 : // @Description: Specifies vehicle's startup latitude
459 : // @User: Advanced
460 : AP_GROUPINFO("OPOS_LAT", 51, SIM, opos.lat, -35.363261f),
461 : // @Param: OPOS_LNG
462 : // @DisplayName: Original Position (Longitude)
463 : // @Description: Specifies vehicle's startup longitude
464 : // @User: Advanced
465 : AP_GROUPINFO("OPOS_LNG", 52, SIM, opos.lng, 149.165230f),
466 : // @Param: OPOS_ALT
467 : // @DisplayName: Original Position (Altitude)
468 : // @Description: Specifies vehicle's startup altitude (AMSL)
469 : // @User: Advanced
470 : AP_GROUPINFO("OPOS_ALT", 53, SIM, opos.alt, 584.0f),
471 : // @Param: OPOS_HDG
472 : // @DisplayName: Original Position (Heading)
473 : // @Description: Specifies vehicle's startup heading (0-360)
474 : // @User: Advanced
475 : AP_GROUPINFO("OPOS_HDG", 54, SIM, opos.hdg, 353.0f),
476 : // @Param: LOOP_DELAY
477 : // @DisplayName: Extra delay per main loop
478 : // @Description: Extra time delay per main loop
479 : // @Units: us
480 : AP_GROUPINFO("LOOP_DELAY", 55, SIM, loop_delay, 0),
481 :
482 : #if AP_SIM_BUZZER_ENABLED
483 : // @Group: BZ_
484 : // @Path: ./SIM_Buzzer.cpp
485 : AP_SUBGROUPINFO(buzzer_sim, "BZ_", 56, SIM, Buzzer),
486 : #endif // AP_SIM_BUZZER_ENABLED
487 :
488 : // @Group: TA_
489 : // @Path: ./SIM_ToneAlarm.cpp
490 : AP_SUBGROUPINFO(tonealarm_sim, "TA_", 57, SIM, ToneAlarm),
491 :
492 : // @Param: EFI_TYPE
493 : // @DisplayName: Type of Electronic Fuel Injection
494 : // @Description: Different types of Electronic Fuel Injection (EFI) systems
495 : // @Values: 0:None,1:MegaSquirt EFI system, 2:Löweheiser EFI system, 8:Hirth engines
496 : AP_GROUPINFO("EFI_TYPE", 58, SIM, efi_type, SIM::EFI_TYPE_NONE),
497 :
498 : // 59 was SAFETY_STATE
499 :
500 : // @Param: VIB_MOT_HMNC
501 : // @DisplayName: Motor harmonics
502 : // @Description: Motor harmonics generated in SITL
503 : AP_GROUPINFO("VIB_MOT_HMNC", 60, SIM, vibe_motor_harmonics, 1),
504 : // @Param: VIB_MOT_MASK
505 : // @DisplayName: Motor mask
506 : // @Description: Motor mask, allowing external simulators to mark motors
507 : AP_GROUPINFO("VIB_MOT_MASK", 5, SIM, vibe_motor_mask, 0),
508 : // @Param: VIB_MOT_MAX
509 : // @DisplayName: Max motor vibration frequency
510 : // @Description: Max frequency to use as baseline for adding motor noise for the gyros and accels
511 : // @Units: Hz
512 : AP_GROUPINFO("VIB_MOT_MAX", 61, SIM, vibe_motor, 0.0f),
513 : // @Param: INS_THR_MIN
514 : // @DisplayName: Minimum throttle INS noise
515 : // @Description: Minimum throttle for simulated ins noise
516 : AP_GROUPINFO("INS_THR_MIN", 62, SIM, ins_noise_throttle_min, 0.1f),
517 : // @Param: VIB_MOT_MULT
518 : // @DisplayName: Vibration motor scale
519 : // @Description: Amplitude scaling of motor noise relative to gyro/accel noise
520 : AP_GROUPINFO("VIB_MOT_MULT", 63, SIM, vibe_motor_scale, 1.0f),
521 :
522 :
523 : AP_GROUPEND
524 :
525 : };
526 :
527 : // third table of user settable parameters for SITL.
528 : const AP_Param::GroupInfo SIM::var_info3[] = {
529 : // @Param: ODOM_ENABLE
530 : // @DisplayName: Odometry enable
531 : // @Description: SITL odometry enabl
532 : // @Values: 0:Disable, 1:Enable
533 : AP_GROUPINFO("ODOM_ENABLE", 1, SIM, odom_enable, 0),
534 :
535 : // @Param: LED_LAYOUT
536 : // @DisplayName: LED layout
537 : // @Description: LED layout config value
538 : AP_GROUPINFO("LED_LAYOUT", 11, SIM, led_layout, 0),
539 :
540 : // @Param: THML_SCENARI
541 : // @DisplayName: Thermal scenarios
542 : // @Description: Scenario for thermalling simulation, for soaring
543 : AP_GROUPINFO("THML_SCENARI", 12, SIM, thermal_scenario, 0),
544 :
545 : // @Param{Sub}: BUOYANCY
546 : // @DisplayName: Buoyancy
547 : // @Description: Net buoyancy force (in Newtons). 0 is neutrally buoyant, negative means the vehicle sinks.
548 : AP_GROUPINFO_FRAME("BUOYANCY", 15, SIM, buoyancy, 1, AP_PARAM_FRAME_SUB),
549 :
550 : // @Param: RATE_HZ
551 : // @DisplayName: Loop rate
552 : // @Description: SITL Loop rate
553 : // @Units: Hz
554 : AP_GROUPINFO("RATE_HZ", 22, SIM, loop_rate_hz, SIM_RATE_HZ_DEFAULT),
555 :
556 : // @Param: IMU_COUNT
557 : // @DisplayName: IMU count
558 : // @Description: Number of simulated IMUs to create
559 : AP_GROUPINFO("IMU_COUNT", 23, SIM, imu_count, 2),
560 :
561 : // @Group: FTOWESC_
562 : // @Path: ./SIM_FETtecOneWireESC.cpp
563 : AP_SUBGROUPINFO(fetteconewireesc_sim, "FTOWESC_", 30, SIM, FETtecOneWireESC),
564 :
565 : // @Group: RICH_
566 : // @Path: ./SIM_RichenPower.cpp
567 : AP_SUBGROUPINFO(richenpower_sim, "RICH_", 31, SIM, RichenPower),
568 :
569 : // @Group: IE24_
570 : // @Path: ./SIM_IntelligentEnergy24.cpp
571 : AP_SUBGROUPINFO(ie24_sim, "IE24_", 32, SIM, IntelligentEnergy24),
572 :
573 : // user settable barometer parameters
574 :
575 : // @Param: BARO_COUNT
576 : // @DisplayName: Baro count
577 : // @Description: Number of simulated baros to create in SITL
578 : // @Range: 0 3
579 : AP_GROUPINFO("BARO_COUNT", 33, SIM, baro_count, 2),
580 :
581 : // @Group: BARO_
582 : // @Path: ./SITL_Baro.cpp
583 : AP_SUBGROUPINFO(baro[0], "BARO_", 34, SIM, BaroParm),
584 : #if BARO_MAX_INSTANCES > 1
585 : // @Group: BAR2_
586 : // @Path: ./SITL_Baro.cpp
587 : AP_SUBGROUPINFO(baro[1], "BAR2_", 35, SIM, BaroParm),
588 : #endif
589 : #if BARO_MAX_INSTANCES > 2
590 : // @Group: BAR3_
591 : // @Path: ./SITL_Baro.cpp
592 : AP_SUBGROUPINFO(baro[2], "BAR3_", 36, SIM, BaroParm),
593 : #endif
594 :
595 : // @Param: TIME_JITTER
596 : // @DisplayName: Loop time jitter
597 : // @Description: Upper limit of random jitter in loop time
598 : // @Units: us
599 : // @User: Advanced
600 : AP_GROUPINFO("TIME_JITTER", 37, SIM, loop_time_jitter_us, 0),
601 :
602 : // @Param: ESC_TELEM
603 : // @DisplayName: Simulated ESC Telemetry
604 : // @Description: enable perfect simulated ESC telemetry
605 : // @User: Advanced
606 : AP_GROUPINFO("ESC_TELEM", 40, SIM, esc_telem, 1),
607 :
608 : // @Param: ESC_ARM_RPM
609 : // @DisplayName: ESC RPM when armed
610 : // @Description: Simulated RPM when motors are armed
611 : // @User: Advanced
612 : AP_GROUPINFO("ESC_ARM_RPM", 41, SIM, esc_rpm_armed, 0.0f),
613 :
614 : // @Param: UART_LOSS
615 : // @DisplayName: UART byte loss percentage
616 : // @Description: Sets percentage of outgoing byte loss on UARTs
617 : // @Units: %
618 : // @User: Advanced
619 : AP_GROUPINFO("UART_LOSS", 42, SIM, uart_byte_loss_pct, 0),
620 :
621 : // @Group: ARSPD_
622 : // @Path: ./SITL_Airspeed.cpp
623 : AP_SUBGROUPINFO(airspeed[0], "ARSPD_", 50, SIM, AirspeedParm),
624 : #if AIRSPEED_MAX_SENSORS > 1
625 : // @Group: ARSPD2_
626 : // @Path: ./SITL_Airspeed.cpp
627 : AP_SUBGROUPINFO(airspeed[1], "ARSPD2_", 51, SIM, AirspeedParm),
628 : #endif
629 :
630 : // @Param: ADSB_TYPES
631 : // @DisplayName: Simulated ADSB Type mask
632 : // @Description: specifies which simulated ADSB types are active
633 : // @User: Advanced
634 : // @Bitmask: 0:MAVLink,3:SageTechMXS
635 : AP_GROUPINFO("ADSB_TYPES", 52, SIM, adsb_types, 1),
636 :
637 : #ifdef WITH_SITL_OSD
638 : // @Param: OSD_COLUMNS
639 : // @DisplayName: Simulated OSD number of text columns
640 : // @Description: Simulated OSD number of text columns
641 : // @Range: 10 100
642 : AP_GROUPINFO("OSD_COLUMNS", 53, SIM, osd_columns, 30),
643 :
644 : // @Param: OSD_ROWS
645 : // @DisplayName: Simulated OSD number of text rows
646 : // @Description: Simulated OSD number of text rows
647 : // @Range: 10 100
648 : AP_GROUPINFO("OSD_ROWS", 54, SIM, osd_rows, 16),
649 : #endif
650 :
651 : #if AP_SIM_VOLZ_ENABLED
652 : // @Group: VOLZ_
653 : // @Path: ./SIM_Volz.cpp
654 : AP_SUBGROUPINFO(volz_sim, "VOLZ_", 55, SIM, Volz),
655 : #endif // AP_SIM_VOLZ_ENABLED
656 :
657 : #if AP_SIM_VICON_ENABLED
658 : // @Group: VICON_
659 : // @Path: ./SIM_Vicon.cpp
660 : AP_SUBGROUPINFO(vicon, "VICON_", 56, SIM, ViconParms),
661 : #endif // AP_SIM_VICON_ENABLED
662 :
663 : #ifdef SFML_JOYSTICK
664 : AP_SUBGROUPEXTENSION("", 63, SIM, var_sfml_joystick),
665 : #endif // SFML_JOYSTICK
666 : //
667 : // 57 was SONAR_OFFSET
668 :
669 : AP_GROUPEND
670 : };
671 :
672 :
673 : #if AP_SIM_GPS_ENABLED
674 : // GPS SITL parameters
675 : const AP_Param::GroupInfo SIM::var_gps[] = {
676 : // 1 was GPS_DISABLE
677 : // 2 was GPS_LAG_MS
678 : // 3 was GPS_TYPE
679 : // 4 was GPS_BYTELOSS
680 : // 5 was GPS_NUMSATS
681 : // 6 was GPS_GLITCH
682 : // 7 was GPS_HZ
683 : // 8 was GPS_DRIFTALT
684 : // 9 was GPS_POS
685 : // 10 was GPS_NOISE
686 : // 11 was GPS_LOCKTIME
687 : // 12 was GPS_ALT_OFS
688 : // 13 was GPS_HDG
689 : // 14 was GPS_ACC
690 : // 15 was GPS_VERR
691 : // 16 was GPS_JAM
692 :
693 : // 30 was GPS2_DISABLE
694 : // 31 was GPS2_LAG_MS
695 : // 32 was GPS2_TYPE
696 : // 33 was GPS2_BYTELOSS
697 : // 34 was GPS2_NUMSATS
698 : // 35 was GPS2_GLITCH
699 : // 36 was GPS2_HZ
700 : // 37 was GPS2_DRIFTALT
701 : // 38 was GPS2_POS
702 : // 39 was GPS2_NOISE
703 : // 40 was GPS2_LOCKTIME
704 : // 41 was GPS2_ALT_OFS
705 : // 42 was GPS2_HDG
706 : // 43 was GPS2_ACC
707 : // 44 was GPS2_VERR
708 :
709 : // @Param: INIT_LAT_OFS
710 : // @DisplayName: Initial Latitude Offset
711 : // @Description: GPS initial lat offset from origin
712 : AP_GROUPINFO("INIT_LAT_OFS", 45, SIM, gps_init_lat_ofs, 0),
713 : // @Param: INIT_LON_OFS
714 : // @DisplayName: Initial Longitude Offset
715 : // @Description: GPS initial lon offset from origin
716 : AP_GROUPINFO("INIT_LON_OFS", 46, SIM, gps_init_lon_ofs, 0),
717 : // @Param: INIT_ALT_OFS
718 : // @DisplayName: Initial Altitude Offset
719 : // @Description: GPS initial alt offset from origin
720 : AP_GROUPINFO("INIT_ALT_OFS", 47, SIM, gps_init_alt_ofs, 0),
721 :
722 : // @Param: GPS_LOG_NUM
723 : // @DisplayName: GPS Log Number
724 : // @Description: Log number for GPS:update_file()
725 : AP_GROUPINFO("GPS_LOG_NUM", 48, SIM, gps_log_num, 0),
726 :
727 : // 49 was GPS2_JAM
728 :
729 : #if AP_SIM_MAX_GPS_SENSORS > 0
730 : // @Group: GPS1_
731 : // @Path: ./SIM_GPS.cpp
732 : AP_SUBGROUPINFO(gps[0], "GPS1_", 50, SIM, GPSParms),
733 : #endif
734 : #if AP_SIM_MAX_GPS_SENSORS > 1
735 : // @Group: GPS2_
736 : // @Path: ./SIM_GPS.cpp
737 : AP_SUBGROUPINFO(gps[1], "GPS2_", 51, SIM, GPSParms),
738 : #endif
739 : #if AP_SIM_MAX_GPS_SENSORS > 2
740 : // @Group: GPS3_
741 : // @Path: ./SIM_GPS.cpp
742 : AP_SUBGROUPINFO(gps[2], "GPS3_", 52, SIM, GPSParms),
743 : #endif
744 : #if AP_SIM_MAX_GPS_SENSORS > 3
745 : // @Group: GPS4_
746 : // @Path: ./SIM_GPS.cpp
747 : AP_SUBGROUPINFO(gps[3], "GPS4_", 53, SIM, GPSParms),
748 : #endif
749 :
750 : AP_GROUPEND
751 : };
752 : #endif // AP_SIM_GPS_ENABLED
753 :
754 : // Mag SITL parameters
755 : const AP_Param::GroupInfo SIM::var_mag[] = {
756 : // @Param: MAG_RND
757 : // @DisplayName: Mag motor noise factor
758 : // @Description: Scaling factor for simulated vibration from motors
759 : // @User: Advanced
760 : AP_GROUPINFO("MAG_RND", 1, SIM, mag_noise, 0),
761 : // @Param: MAG_MOT
762 : // @DisplayName: Motor magnetic interference
763 : // @Description: Simulates distortion of magnetometer readings caused by motor current
764 : // @Units: mGauss/A
765 : // @User: Advanced
766 : // @Vector3Parameter: 1
767 : AP_GROUPINFO("MAG_MOT", 2, SIM, mag_mot, 0),
768 : // @Param: MAG_DELAY
769 : // @DisplayName: Mag measurement delay
770 : // @Description: Magnetometer measurement delay
771 : // @Units: ms
772 : // @User: Advanced
773 : AP_GROUPINFO("MAG_DELAY", 3, SIM, mag_delay, 0),
774 : // @Param: MAG1_OFS
775 : // @DisplayName: Magnetometer offset applied to SITL
776 : // @Description: Magnetometer offset injected into the simulation
777 : // @User: Advanced
778 : // @Vector3Parameter: 1
779 : AP_GROUPINFO("MAG1_OFS", 4, SIM, mag_ofs[0], 0),
780 : // @Param: MAG_ALY
781 : // @DisplayName: NED anomaly vector at ground level
782 : // @Description: Simulates localized magnetic field distortions at ground level that decays with altitude.
783 : // @Units: mGauss
784 : // @User: Advanced
785 : // @Vector3Parameter: 1
786 : AP_GROUPINFO("MAG_ALY", 5, SIM, mag_anomaly_ned, 0),
787 : // @Param: MAG_ALY_HGT
788 : // @DisplayName: Magnetic anomaly height
789 : // @Description: Height above ground where anomally strength has decayed to 1/8 of the ground level value
790 : // @Units: m
791 : // @User: Advanced
792 : AP_GROUPINFO("MAG_ALY_HGT", 6, SIM, mag_anomaly_hgt, 1.0f),
793 : // @Param: MAG1_DIA_X
794 : // @DisplayName: Magnetometer soft-iron diagonal X component
795 : // @Description: DIA_X in the magnetometer soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
796 : // @User: Advanced
797 :
798 : // @Param: MAG1_DIA_Y
799 : // @DisplayName: Magnetometer soft-iron diagonal Y component
800 : // @Description: DIA_Y in the magnetometer soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
801 : // @User: Advanced
802 :
803 : // @Param: MAG1_DIA_Z
804 : // @DisplayName: Magnetometer soft-iron diagonal Z component
805 : // @Description: DIA_Z in the magnetometer soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
806 : // @User: Advanced
807 : AP_GROUPINFO("MAG1_DIA", 7, SIM, mag_diag[0], 0),
808 : // @Param: MAG1_ODI_X
809 : // @DisplayName: Magnetometer soft-iron off-diagonal X component
810 : // @Description: ODI_X in the magnetometer soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
811 : // @User: Advanced
812 :
813 : // @Param: MAG1_ODI_Y
814 : // @DisplayName: Magnetometer soft-iron off-diagonal Y component
815 : // @Description: ODI_Y in the magnetometer soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
816 : // @User: Advanced
817 :
818 : // @Param: MAG1_ODI_Z
819 : // @DisplayName: Magnetometer soft-iron off-diagonal Z component
820 : // @Description: ODI_Z in the magnetometer soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
821 : // @User: Advanced
822 : AP_GROUPINFO("MAG1_ODI", 8, SIM, mag_offdiag[0], 0),
823 : // @Param: MAG1_ORIENT
824 : // @DisplayName: MAG1 Orientation
825 : // @Description: MAG1 external compass orientation
826 : // @User: Advanced
827 : AP_GROUPINFO("MAG1_ORIENT", 9, SIM, mag_orient[0], 0),
828 : // @Param: MAG1_SCALING
829 : // @DisplayName: MAG1 Scaling factor
830 : // @Description: Scale the compass 1 to simulate sensor scale factor errors
831 : // @User: Advanced
832 : AP_GROUPINFO("MAG1_SCALING", 10, SIM, mag_scaling[0], 1),
833 : // @Param: MAG1_DEVID
834 : // @DisplayName: MAG1 Device ID
835 : // @Description: Device ID of simulated compass 1
836 : // @User: Advanced
837 : AP_GROUPINFO("MAG1_DEVID", 11, SIM, mag_devid[0], 97539),
838 : // @Param: MAG2_DEVID
839 : // @DisplayName: MAG2 Device ID
840 : // @Description: Device ID of simulated compass 2
841 : // @User: Advanced
842 : AP_GROUPINFO("MAG2_DEVID", 12, SIM, mag_devid[1], 131874),
843 : #if MAX_CONNECTED_MAGS > 2
844 : // @Param: MAG3_DEVID
845 : // @DisplayName: MAG3 Device ID
846 : // @Description: Device ID of simulated compass 3
847 : // @User: Advanced
848 : AP_GROUPINFO("MAG3_DEVID", 13, SIM, mag_devid[2], 263178),
849 : #endif
850 : #if MAX_CONNECTED_MAGS > 3
851 : // @Param: MAG4_DEVID
852 : // @DisplayName: MAG2 Device ID
853 : // @Description: Device ID of simulated compass 4
854 : // @User: Advanced
855 : AP_GROUPINFO("MAG4_DEVID", 14, SIM, mag_devid[3], 97283),
856 : #endif
857 : #if MAX_CONNECTED_MAGS > 4
858 : // @Param: MAG5_DEVID
859 : // @DisplayName: MAG5 Device ID
860 : // @Description: Device ID of simulated compass 5
861 : // @User: Advanced
862 : AP_GROUPINFO("MAG5_DEVID", 15, SIM, mag_devid[4], 97795),
863 : #endif
864 : #if MAX_CONNECTED_MAGS > 5
865 : // @Param: MAG6_DEVID
866 : // @DisplayName: MAG6 Device ID
867 : // @Description: Device ID of simulated compass 6
868 : // @User: Advanced
869 : AP_GROUPINFO("MAG6_DEVID", 16, SIM, mag_devid[5], 98051),
870 : #endif
871 : #if MAX_CONNECTED_MAGS > 6
872 : // @Param: MAG7_DEVID
873 : // @DisplayName: MAG7 Device ID
874 : // @Description: Device ID of simulated compass 7
875 : // @User: Advanced
876 : AP_GROUPINFO("MAG7_DEVID", 17, SIM, mag_devid[6], 0),
877 : #endif
878 : #if MAX_CONNECTED_MAGS > 7
879 : // @Param: MAG8_DEVID
880 : // @DisplayName: MAG8 Device ID
881 : // @Description: Device ID of simulated compass 8
882 : // @User: Advanced
883 : AP_GROUPINFO("MAG8_DEVID", 18, SIM, mag_devid[7], 0),
884 : #endif
885 : // @Param: MAG1_FAIL
886 : // @DisplayName: MAG1 Failure
887 : // @Description: Simulated failure of MAG1
888 : // @Values: 0:Disabled, 1:MAG1 Failure
889 : // @User: Advanced
890 : AP_GROUPINFO("MAG1_FAIL", 26, SIM, mag_fail[0], 0),
891 : #if HAL_COMPASS_MAX_SENSORS > 1
892 :
893 : // @Param: MAG2_OFS
894 : // @CopyFieldsFrom: SIM_MAG1_OFS
895 : // @Vector3Parameter: 1
896 : AP_GROUPINFO("MAG2_OFS", 19, SIM, mag_ofs[1], 0),
897 : // @Param: MAG2_DIA_X
898 : // @CopyFieldsFrom: SIM_MAG1_DIA_X
899 :
900 : // @Param: MAG2_DIA_Y
901 : // @CopyFieldsFrom: SIM_MAG1_DIA_Y
902 :
903 : // @Param: MAG2_DIA_Z
904 : // @CopyFieldsFrom: SIM_MAG1_DIA_Z
905 : AP_GROUPINFO("MAG2_DIA", 20, SIM, mag_diag[1], 0),
906 : // @Param: MAG2_ODI_X
907 : // @CopyFieldsFrom: SIM_MAG1_ODI_X
908 :
909 : // @Param: MAG2_ODI_Y
910 : // @CopyFieldsFrom: SIM_MAG1_ODI_Y
911 :
912 : // @Param: MAG2_ODI_Z
913 : // @CopyFieldsFrom: SIM_MAG1_ODI_Z
914 : AP_GROUPINFO("MAG2_ODI", 21, SIM, mag_offdiag[1], 0),
915 : // @Param: MAG2_ORIENT
916 : // @DisplayName: MAG2 Orientation
917 : // @Description: MAG2 external compass orientation
918 : // @User: Advanced
919 : AP_GROUPINFO("MAG2_ORIENT", 22, SIM, mag_orient[1], 0),
920 : // @Param: MAG2_FAIL
921 : // @DisplayName: MAG2 Failure
922 : // @Description: Simulated failure of MAG2
923 : // @Values: 0:Disabled, 1:MAG2 Failure
924 : // @User: Advanced
925 : AP_GROUPINFO("MAG2_FAIL", 27, SIM, mag_fail[1], 0),
926 : // @Param: MAG2_SCALING
927 : // @DisplayName: MAG2 Scaling factor
928 : // @Description: Scale the compass 2 to simulate sensor scale factor errors
929 : // @User: Advanced
930 : AP_GROUPINFO("MAG2_SCALING", 28, SIM, mag_scaling[1], 1),
931 : #endif
932 : #if HAL_COMPASS_MAX_SENSORS > 2
933 :
934 : // @Param: MAG3_OFS
935 : // @CopyFieldsFrom: SIM_MAG1_OFS
936 : // @Vector3Parameter: 1
937 : AP_GROUPINFO("MAG3_OFS", 23, SIM, mag_ofs[2], 0),
938 : // @Param: MAG3_DIA_X
939 : // @CopyFieldsFrom: SIM_MAG1_DIA_X
940 :
941 : // @Param: MAG3_DIA_Y
942 : // @CopyFieldsFrom: SIM_MAG1_DIA_Y
943 :
944 : // @Param: MAG3_DIA_Z
945 : // @CopyFieldsFrom: SIM_MAG1_DIA_Z
946 : AP_GROUPINFO("MAG3_DIA", 24, SIM, mag_diag[2], 0),
947 : // @Param: MAG3_ODI_X
948 : // @CopyFieldsFrom: SIM_MAG1_ODI_X
949 :
950 : // @Param: MAG3_ODI_Y
951 : // @CopyFieldsFrom: SIM_MAG1_ODI_Y
952 :
953 : // @Param: MAG3_ODI_Z
954 : // @CopyFieldsFrom: SIM_MAG1_ODI_Z
955 : AP_GROUPINFO("MAG3_ODI", 25, SIM, mag_offdiag[2], 0),
956 : // @Param: MAG3_FAIL
957 : // @DisplayName: MAG3 Failure
958 : // @Description: Simulated failure of MAG3
959 : // @Values: 0:Disabled, 1:MAG3 Failure
960 : // @User: Advanced
961 : AP_GROUPINFO("MAG3_FAIL", 29, SIM, mag_fail[2], 0),
962 : // @Param: MAG3_SCALING
963 : // @DisplayName: MAG3 Scaling factor
964 : // @Description: Scale the compass 3 to simulate sensor scale factor errors
965 : // @User: Advanced
966 : AP_GROUPINFO("MAG3_SCALING", 30, SIM, mag_scaling[2], 1),
967 : // @Param: MAG3_ORIENT
968 : // @DisplayName: MAG3 Orientation
969 : // @Description: MAG3 external compass orientation
970 : // @User: Advanced
971 : AP_GROUPINFO("MAG3_ORIENT", 36, SIM, mag_orient[2], 0),
972 : #endif
973 :
974 : // @Param: MAG_SAVE_IDS
975 : // @DisplayName: Save MAG devids on startup
976 : // @Description: This forces saving of compass devids on startup so that simulated compasses start as calibrated
977 : // @Values: 0:Disabled, 1:Enabled
978 : // @User: Advanced
979 : AP_GROUPINFO("MAG_SAVE_IDS", 37, SIM, mag_save_ids, 1),
980 :
981 : AP_GROUPEND
982 : };
983 :
984 : #ifdef SFML_JOYSTICK
985 : const AP_Param::GroupInfo SIM::var_sfml_joystick[] = {
986 : AP_GROUPINFO("SF_JS_STICK", 1, SIM, sfml_joystick_id, 0),
987 : AP_GROUPINFO("SF_JS_AXIS1", 2, SIM, sfml_joystick_axis[0], sf::Joystick::Axis::X),
988 : AP_GROUPINFO("SF_JS_AXIS2", 3, SIM, sfml_joystick_axis[1], sf::Joystick::Axis::Y),
989 : AP_GROUPINFO("SF_JS_AXIS3", 4, SIM, sfml_joystick_axis[2], sf::Joystick::Axis::Z),
990 : AP_GROUPINFO("SF_JS_AXIS4", 5, SIM, sfml_joystick_axis[3], sf::Joystick::Axis::U),
991 : AP_GROUPINFO("SF_JS_AXIS5", 6, SIM, sfml_joystick_axis[4], sf::Joystick::Axis::V),
992 : AP_GROUPINFO("SF_JS_AXIS6", 7, SIM, sfml_joystick_axis[5], sf::Joystick::Axis::R),
993 : AP_GROUPINFO("SF_JS_AXIS7", 8, SIM, sfml_joystick_axis[6], sf::Joystick::Axis::PovX),
994 : AP_GROUPINFO("SF_JS_AXIS8", 9, SIM, sfml_joystick_axis[7], sf::Joystick::Axis::PovY),
995 : AP_GROUPEND
996 : };
997 : #endif //SFML_JOYSTICK
998 :
999 : // INS SITL parameters
1000 : const AP_Param::GroupInfo SIM::var_ins[] = {
1001 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
1002 : // @Param: IMUT_START
1003 : // @DisplayName: IMU temperature start
1004 : // @Description: Starting IMU temperature of a curve
1005 : AP_GROUPINFO("IMUT_START", 1, SIM, imu_temp_start, 25),
1006 : // @Param: IMUT_END
1007 : // @DisplayName: IMU temperature end
1008 : // @Description: Ending IMU temperature of a curve
1009 : AP_GROUPINFO("IMUT_END", 2, SIM, imu_temp_end, 45),
1010 : // @Param: IMUT_TCONST
1011 : // @DisplayName: IMU temperature time constant
1012 : // @Description: IMU temperature time constant of the curve
1013 : AP_GROUPINFO("IMUT_TCONST", 3, SIM, imu_temp_tconst, 300),
1014 : // @Param: IMUT_FIXED
1015 : // @DisplayName: IMU fixed temperature
1016 : // @Description: IMU fixed temperature by user
1017 : AP_GROUPINFO("IMUT_FIXED", 4, SIM, imu_temp_fixed, 0),
1018 : #endif
1019 : // @Param: ACC1_BIAS
1020 : // @DisplayName: Accel 1 bias
1021 : // @Description: bias of simulated accelerometer sensor
1022 : // @User: Advanced
1023 : // @Vector3Parameter: 1
1024 : AP_GROUPINFO("ACC1_BIAS", 5, SIM, accel_bias[0], 0),
1025 : #if INS_MAX_INSTANCES > 1
1026 : // @Param: ACC2_BIAS
1027 : // @DisplayName: Accel 2 bias
1028 : // @CopyFieldsFrom: SIM_ACC1_BIAS
1029 : // @Vector3Parameter: 1
1030 : AP_GROUPINFO("ACC2_BIAS", 6, SIM, accel_bias[1], 0),
1031 : #endif
1032 : #if INS_MAX_INSTANCES > 2
1033 : // @Param: ACC3_BIAS
1034 : // @DisplayName: Accel 3 bias
1035 : // @CopyFieldsFrom: SIM_ACC1_BIAS
1036 : // @Vector3Parameter: 1
1037 : AP_GROUPINFO("ACC3_BIAS", 7, SIM, accel_bias[2], 0),
1038 : #endif
1039 : // @Param: GYR1_RND
1040 : // @DisplayName: Gyro 1 motor noise factor
1041 : // @Description: scaling factor for simulated vibration from motors
1042 : // @User: Advanced
1043 : AP_GROUPINFO("GYR1_RND", 8, SIM, gyro_noise[0], 0),
1044 : #if INS_MAX_INSTANCES > 1
1045 : // @Param: GYR2_RND
1046 : // @DisplayName: Gyro 2 motor noise factor
1047 : // @CopyFieldsFrom: SIM_GYR1_RND
1048 : AP_GROUPINFO("GYR2_RND", 9, SIM, gyro_noise[1], 0),
1049 : #endif
1050 : #if INS_MAX_INSTANCES > 2
1051 : // @Param: GYR3_RND
1052 : // @DisplayName: Gyro 3 motor noise factor
1053 : // @CopyFieldsFrom: SIM_GYR1_RND
1054 : AP_GROUPINFO("GYR3_RND", 10, SIM, gyro_noise[2], 0),
1055 : #endif
1056 : // @Param: ACC1_RND
1057 : // @DisplayName: Accel 1 motor noise factor
1058 : // @Description: scaling factor for simulated vibration from motors
1059 : // @User: Advanced
1060 : AP_GROUPINFO("ACC1_RND", 11, SIM, accel_noise[0], 0),
1061 : #if INS_MAX_INSTANCES > 1
1062 : // @Param: ACC2_RND
1063 : // @DisplayName: Accel 2 motor noise factor
1064 : // @CopyFieldsFrom: SIM_ACC1_RND
1065 : AP_GROUPINFO("ACC2_RND", 12, SIM, accel_noise[1], 0),
1066 : #endif
1067 : #if INS_MAX_INSTANCES > 2
1068 : // @Param: ACC3_RND
1069 : // @DisplayName: Accel 3 motor noise factor
1070 : // @CopyFieldsFrom: SIM_ACC1_RND
1071 : AP_GROUPINFO("ACC3_RND", 13, SIM, accel_noise[2], 0),
1072 : #endif
1073 : // @Param: GYR1_SCALE
1074 : // @DisplayName: Gyro 1 scaling factor
1075 : // @Description: scaling factors applied to simulated gyroscope
1076 : // @User: Advanced
1077 : // @Vector3Parameter: 1
1078 : AP_GROUPINFO("GYR1_SCALE", 14, SIM, gyro_scale[0], 0),
1079 : #if INS_MAX_INSTANCES > 1
1080 : // @Param: GYR2_SCALE
1081 : // @DisplayName: Gyro 2 scaling factor
1082 : // @CopyFieldsFrom: SIM_GYR1_SCALE
1083 : // @Vector3Parameter: 1
1084 : AP_GROUPINFO("GYR2_SCALE", 15, SIM, gyro_scale[1], 0),
1085 : #endif
1086 : #if INS_MAX_INSTANCES > 2
1087 : // @Param: GYR3_SCALE
1088 : // @DisplayName: Gyro 3 scaling factor
1089 : // @CopyFieldsFrom: SIM_GYR1_SCALE
1090 : // @Vector3Parameter: 1
1091 : AP_GROUPINFO("GYR3_SCALE", 16, SIM, gyro_scale[2], 0),
1092 : #endif
1093 : // @Param: ACCEL1_FAIL
1094 : // @DisplayName: ACCEL1 Failure
1095 : // @Description: Simulated failure of ACCEL1
1096 : // @Values: 0:Disabled, 1:ACCEL1 Failure
1097 : // @User: Advanced
1098 : AP_GROUPINFO("ACCEL1_FAIL", 17, SIM, accel_fail[0], 0),
1099 : #if INS_MAX_INSTANCES > 1
1100 : // @Param: ACCEL2_FAIL
1101 : // @DisplayName: ACCEL2 Failure
1102 : // @Description: Simulated failure of ACCEL2
1103 : // @Values: 0:Disabled, 1:ACCEL2 Failure
1104 : // @User: Advanced
1105 : AP_GROUPINFO("ACCEL2_FAIL", 18, SIM, accel_fail[1], 0),
1106 : #endif
1107 : #if INS_MAX_INSTANCES > 2
1108 : // @Param: ACCEL3_FAIL
1109 : // @DisplayName: ACCEL3 Failure
1110 : // @Description: Simulated failure of ACCEL3
1111 : // @Values: 0:Disabled, 1:ACCEL3 Failure
1112 : // @User: Advanced
1113 : AP_GROUPINFO("ACCEL3_FAIL", 19, SIM, accel_fail[2], 0),
1114 : #endif
1115 : // @Param: GYR_FAIL_MSK
1116 : // @DisplayName: Gyro Failure Mask
1117 : // @Description: Determines if the gyro reading updates are stopped when for an IMU simulated failure by ACCELx_FAIL params
1118 : // @Values: 0:Disabled, 1:Readings stopped
1119 : // @User: Advanced
1120 : AP_GROUPINFO("GYR_FAIL_MSK", 20, SIM, gyro_fail_mask, 0),
1121 : // @Param: ACC_FAIL_MSK
1122 : // @DisplayName: Accelerometer Failure Mask
1123 : // @Description: Determines if the acclerometer reading updates are stopped when for an IMU simulated failure by ACCELx_FAIL params
1124 : // @Values: 0:Disabled, 1:Readings stopped
1125 : // @User: Advanced
1126 : AP_GROUPINFO("ACC_FAIL_MSK", 21, SIM, accel_fail_mask, 0),
1127 :
1128 : // @Param: ACC1_SCAL
1129 : // @DisplayName: Accel 1 scaling factor
1130 : // @Description: scaling factors applied to simulated accelerometer
1131 : // @User: Advanced
1132 : // @Vector3Parameter: 1
1133 : AP_GROUPINFO("ACC1_SCAL", 22, SIM, accel_scale[0], 0),
1134 : #if INS_MAX_INSTANCES > 1
1135 : // @Param: ACC2_SCAL
1136 : // @DisplayName: Accel 2 scaling factor
1137 : // @CopyFieldsFrom: SIM_ACC1_SCAL
1138 : // @Vector3Parameter: 1
1139 : AP_GROUPINFO("ACC2_SCAL", 23, SIM, accel_scale[1], 0),
1140 : #endif
1141 : #if INS_MAX_INSTANCES > 2
1142 : // @Param: ACC3_SCAL
1143 : // @DisplayName: Accel 3 scaling factor
1144 : // @CopyFieldsFrom: SIM_ACC1_SCAL
1145 : // @Vector3Parameter: 1
1146 : AP_GROUPINFO("ACC3_SCAL", 24, SIM, accel_scale[2], 0),
1147 : #endif
1148 : // @Param: BRD_TRIM
1149 : // @DisplayName: Board mounting trim
1150 : // @Description: Rigid board mounting offset (roll, pitch, yaw in radians) applied as a rotation to the simulated accelerometers, gyros and compasses, as a tilted autopilot mounting would be. Intended to be cancelled by AHRS_TRIM.
1151 : // @User: Advanced
1152 : // @Vector3Parameter: 1
1153 : AP_GROUPINFO("BRD_TRIM", 25, SIM, board_trim, 0),
1154 :
1155 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
1156 : // @Param{Rover}: SAIL_TYPE
1157 : // @DisplayName: Sailboat simulation sail type
1158 : // @Description: 0: mainsail with sheet, 1: directly actuated wing
1159 : AP_GROUPINFO("SAIL_TYPE", 26, SIM, sail_type, 0),
1160 : #endif
1161 :
1162 : // @Param: JSON_MASTER
1163 : // @DisplayName: JSON master instance
1164 : // @Description: the instance number to take servos from
1165 : AP_GROUPINFO("JSON_MASTER", 27, SIM, ride_along_master, 0),
1166 :
1167 : // @Param: OH_MASK
1168 : // @DisplayName: SIM-on_hardware Output Enable Mask
1169 : // @Description: channels which are passed through to actual hardware when running sim on actual hardware
1170 : AP_GROUPINFO("OH_MASK", 28, SIM, on_hardware_output_enable_mask, 0),
1171 : #if AP_SIM_INS_FILE_ENABLED
1172 :
1173 : // @Param: GYR_FILE_RW
1174 : // @DisplayName: Gyro data to/from files
1175 : // @Description: Read and write gyro data to/from files
1176 : // @Values: 0:Stop writing data, 1:Read data from file, 2:Write data to a file, 3: Read data from file and stop on EOF
1177 : AP_GROUPINFO("GYR_FILE_RW", 29, SIM, gyro_file_rw, INSFileMode::INS_FILE_NONE),
1178 : // @Param: ACC_FILE_RW
1179 : // @DisplayName: Accelerometer data to/from files
1180 : // @Description: Read and write accelerometer data to/from files
1181 : // @Values: 0:Stop writing data, 1:Read data from file, 2:Write data to a file, 3: Read data from file and stop on EOF
1182 : AP_GROUPINFO("ACC_FILE_RW", 30, SIM, accel_file_rw, INSFileMode::INS_FILE_NONE),
1183 : #endif
1184 :
1185 : // @Param: GYR1_BIAS
1186 : // @DisplayName: First Gyro bias
1187 : // @Description: First Gyro bias
1188 : // @Units: rad/s
1189 : // @User: Advanced
1190 : // @Vector3Parameter: 1
1191 : AP_GROUPINFO("GYR1_BIAS", 31, SIM, gyro_bias[0], 0),
1192 : #if INS_MAX_INSTANCES > 1
1193 : // @Param: GYR2_BIAS
1194 : // @CopyFieldsFrom: SIM_GYR1_BIAS
1195 : // @DisplayName: Second Gyro bias
1196 : // @Description: Second Gyro bias
1197 : // @Vector3Parameter: 1
1198 : AP_GROUPINFO("GYR2_BIAS", 32, SIM, gyro_bias[1], 0),
1199 : #endif
1200 : #if INS_MAX_INSTANCES > 2
1201 : // @Param: GYR3_BIAS
1202 : // @CopyFieldsFrom: SIM_GYR1_BIAS
1203 : // @DisplayName: Third Gyro bias
1204 : // @Description: Third Gyro bias
1205 : // @Vector3Parameter: 1
1206 : AP_GROUPINFO("GYR3_BIAS", 33, SIM, gyro_bias[2], 0),
1207 : #endif
1208 :
1209 : #if INS_MAX_INSTANCES > 3
1210 : // @Param: ACC4_SCAL
1211 : // @DisplayName: Accel 4 scaling factor
1212 : // @CopyFieldsFrom: SIM_ACC1_SCAL
1213 : // @Vector3Parameter: 1
1214 : AP_GROUPINFO("ACC4_SCAL", 34, SIM, accel_scale[3], 0),
1215 :
1216 : // @Param: ACCEL4_FAIL
1217 : // @DisplayName: ACCEL4 Failure
1218 : // @Description: Simulated failure of ACCEL4
1219 : // @Values: 0:Disabled, 1:ACCEL4 Failure
1220 : // @User: Advanced
1221 : AP_GROUPINFO("ACCEL4_FAIL", 35, SIM, accel_fail[3], 0),
1222 :
1223 : // @Param: GYR4_SCALE
1224 : // @DisplayName: Gyro 4 scaling factor
1225 : // @CopyFieldsFrom: SIM_GYR1_SCALE
1226 : // @Vector3Parameter: 1
1227 : AP_GROUPINFO("GYR4_SCALE", 36, SIM, gyro_scale[3], 0),
1228 :
1229 : // @Param: ACC4_RND
1230 : // @DisplayName: Accel 4 motor noise factor
1231 : // @CopyFieldsFrom: SIM_ACC1_RND
1232 : AP_GROUPINFO("ACC4_RND", 37, SIM, accel_noise[3], 0),
1233 :
1234 : // @Param: GYR4_RND
1235 : // @DisplayName: Gyro 4 motor noise factor
1236 : // @CopyFieldsFrom: SIM_GYR1_RND
1237 : AP_GROUPINFO("GYR4_RND", 38, SIM, gyro_noise[3], 0),
1238 :
1239 : // @Param: ACC4_BIAS
1240 : // @DisplayName: Accel 4 bias
1241 : // @CopyFieldsFrom: SIM_ACC1_BIAS
1242 : // @Vector3Parameter: 1
1243 : AP_GROUPINFO("ACC4_BIAS", 39, SIM, accel_bias[3], 0),
1244 :
1245 : // @Param: GYR4_BIAS
1246 : // @CopyFieldsFrom: SIM_GYR1_BIAS
1247 : // @DisplayName: Fourth Gyro bias
1248 : // @Description: Fourth Gyro bias
1249 : // @Vector3Parameter: 1
1250 : AP_GROUPINFO("GYR4_BIAS", 40, SIM, gyro_bias[3], 0),
1251 :
1252 : #endif
1253 :
1254 : #if INS_MAX_INSTANCES > 4
1255 : // @Param: ACC5_SCAL
1256 : // @DisplayName: Accel 4 scaling factor
1257 : // @CopyFieldsFrom: SIM_ACC1_SCAL
1258 : // @Vector3Parameter: 1
1259 : AP_GROUPINFO("ACC5_SCAL", 41, SIM, accel_scale[4], 0),
1260 :
1261 :
1262 : // @Param: ACCEL5_FAIL
1263 : // @DisplayName: ACCEL5 Failure
1264 : // @Description: Simulated failure of ACCEL5
1265 : // @Values: 0:Disabled, 1:ACCEL5 Failure
1266 : // @User: Advanced
1267 : AP_GROUPINFO("ACCEL5_FAIL", 42, SIM, accel_fail[4], 0),
1268 :
1269 : // @Param: GYR5_SCALE
1270 : // @DisplayName: Gyro 5 scaling factor
1271 : // @CopyFieldsFrom: SIM_GYR1_SCALE
1272 : // @Vector3Parameter: 1
1273 : AP_GROUPINFO("GYR5_SCALE", 43, SIM, gyro_scale[4], 0),
1274 :
1275 : // @Param: ACC5_RND
1276 : // @DisplayName: Accel 5 motor noise factor
1277 : // @CopyFieldsFrom: SIM_ACC1_RND
1278 : AP_GROUPINFO("ACC5_RND", 44, SIM, accel_noise[4], 0),
1279 :
1280 : // @Param: GYR5_RND
1281 : // @DisplayName: Gyro 5 motor noise factor
1282 : // @CopyFieldsFrom: SIM_GYR1_RND
1283 : AP_GROUPINFO("GYR5_RND", 45, SIM, gyro_noise[4], 0),
1284 :
1285 : // @Param: ACC5_BIAS
1286 : // @DisplayName: Accel 5 bias
1287 : // @CopyFieldsFrom: SIM_ACC1_BIAS
1288 : // @Vector3Parameter: 1
1289 : AP_GROUPINFO("ACC5_BIAS", 46, SIM, accel_bias[4], 0),
1290 :
1291 : // @Param: GYR5_BIAS
1292 : // @CopyFieldsFrom: SIM_GYR1_BIAS
1293 : // @DisplayName: Fifth Gyro bias
1294 : // @Description: Fifth Gyro bias
1295 : // @Vector3Parameter: 1
1296 : AP_GROUPINFO("GYR5_BIAS", 47, SIM, gyro_bias[4], 0),
1297 : #endif
1298 :
1299 : // @Param: OH_RELAY_MSK
1300 : // @DisplayName: SIM-on_hardware Relay Enable Mask
1301 : // @Description: Allow relay output operation when running SIM-on-hardware
1302 : AP_GROUPINFO("OH_RELAY_MSK", 48, SIM, on_hardware_relay_enable_mask, SIM_DEFAULT_ENABLED_RELAY_CHANNELS),
1303 :
1304 : // @Param: CLAMP_CH
1305 : // @DisplayName: Simulated Clamp Channel
1306 : // @Description: If non-zero the vehicle will be clamped in position until the value on this servo channel passes 1800PWM
1307 : AP_GROUPINFO("CLAMP_CH", 49, SIM, clamp_ch, 0),
1308 :
1309 : // @Param: AHRS_OFF_RLL
1310 : // @DisplayName: Sim AHRS offset roll
1311 : // @Description: Roll offset applied to SIM AHRS type. For testing stepless handover between AHRS estimators.
1312 : // @Range: -10 10
1313 : // @Units: deg
1314 : AP_GROUPINFO("AHRS_OFF_RLL", 50, SIM, sim_ahrs_offset.roll, 0),
1315 :
1316 : // @Param: AHRS_OFF_PIT
1317 : // @DisplayName: Sim AHRS offset pitch
1318 : // @Description: Pitch offset applied to SIM AHRS type. For testing stepless handover between AHRS estimators.
1319 : // @Range: -10 10
1320 : // @Units: deg
1321 : AP_GROUPINFO("AHRS_OFF_PIT", 51, SIM, sim_ahrs_offset.pitch, 0),
1322 :
1323 : // @Param: AHRS_OFF_YAW
1324 : // @DisplayName: Sim AHRS offset yaw
1325 : // @Description: Yaw offset applied to SIM AHRS type. For testing stepless handover between AHRS estimators.
1326 : // @Range: -10 10
1327 : // @Units: deg
1328 : AP_GROUPINFO("AHRS_OFF_YAW", 52, SIM, sim_ahrs_offset.yaw, 0),
1329 :
1330 : // the IMUT parameters must be last due to the enable parameters
1331 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
1332 :
1333 : // @Param: IMUT1_ENABLE
1334 : // @DisplayName: Enable simulated temperature disturbance for sensor data
1335 : // @Description: Enable the injection of temperature disturbance to the accelerometer and gyroscope data to simulate temperature calibration
1336 : // @Values: 0:Disable,1:Enabled, 2: Learn Calibration
1337 : // @User: Advanced
1338 :
1339 : // @Param: IMUT1_ACC1
1340 : // @DisplayName: Applied simulated acceleration to accelerometer
1341 : // @Description: This is the applied simulated acceleration to the 1st accelerometer
1342 : // @User: Advanced
1343 : // @Vector3Parameter: 1
1344 :
1345 : // @Param: IMUT1_ACC2
1346 : // @DisplayName: Applied simulated acceleration to accelerometer
1347 : // @Description: This is the applied simulated acceleration to the 2nd accelerometer
1348 : // @User: Advanced
1349 : // @Vector3Parameter: 1
1350 :
1351 : // @Param: IMUT1_ACC3
1352 : // @DisplayName: Applied simulated acceleration to accelerometer
1353 : // @Description: This is the applied simulated acceleration to the 3rd accelerometer
1354 : // @User: Advanced
1355 : // @Vector3Parameter: 1
1356 :
1357 : // @Param: IMUT1_GYR1
1358 : // @DisplayName: Applied simulated angular rate to gyroscope
1359 : // @Description: This is the applied simulated angular rate to the 1st gyroscope
1360 : // @User: Advanced
1361 : // @Vector3Parameter: 1
1362 :
1363 : // @Param: IMUT1_GYR2
1364 : // @DisplayName: Applied simulated angular rate to gyroscope
1365 : // @Description: This is the applied simulated angular rate to the 2nd gyroscope
1366 : // @User: Advanced
1367 : // @Vector3Parameter: 1
1368 :
1369 : // @Param: IMUT1_GYR3
1370 : // @DisplayName: Applied simulated angular rate to gyroscope
1371 : // @Description: This is the applied simulated angular rate to the 3rd gyroscope
1372 : // @User: Advanced
1373 : // @Vector3Parameter: 1
1374 :
1375 : // @Param: IMUT1_TMAX
1376 : // @DisplayName: Simulated temperature calibration max
1377 : // @Description: The maximum simulated temperature that the calibration is valid for. This must be at least 10 degrees above TMIN for calibration
1378 : // @Units: degC
1379 : // @Range: -70 80
1380 : // @User: Advanced
1381 :
1382 : // @Param: IMUT1_TMIN
1383 : // @DisplayName: Simulated temperature calibration min
1384 : // @Description: The minimum simulated temperature that the calibration is valid for
1385 : // @Units: degC
1386 : // @Range: -70 80
1387 : // @User: Advanced
1388 : AP_SUBGROUPINFO(imu_tcal[0], "IMUT1_", 61, SIM, AP_InertialSensor_TCal),
1389 : #if INS_MAX_INSTANCES > 1
1390 :
1391 : // @Param: IMUT2_ENABLE
1392 : // @CopyFieldsFrom: SIM_IMUT1_ENABLE
1393 : // @DisplayName: Enable simulated temperature disturbance for sensor data
1394 :
1395 : // @Param: IMUT2_ACC1
1396 : // @CopyFieldsFrom: SIM_IMUT1_ACC1
1397 : // @Vector3Parameter: 1
1398 :
1399 : // @Param: IMUT2_ACC2
1400 : // @CopyFieldsFrom: SIM_IMUT1_ACC2
1401 : // @Vector3Parameter: 1
1402 :
1403 : // @Param: IMUT2_ACC3
1404 : // @CopyFieldsFrom: SIM_IMUT1_ACC3
1405 : // @Vector3Parameter: 1
1406 :
1407 : // @Param: IMUT2_GYR1
1408 : // @CopyFieldsFrom: SIM_IMUT1_GYR1
1409 : // @Vector3Parameter: 1
1410 :
1411 : // @Param: IMUT2_GYR2
1412 : // @CopyFieldsFrom: SIM_IMUT1_GYR2
1413 : // @Vector3Parameter: 1
1414 :
1415 : // @Param: IMUT2_GYR3
1416 : // @CopyFieldsFrom: SIM_IMUT1_GYR3
1417 : // @Vector3Parameter: 1
1418 :
1419 : // @Param: IMUT2_TMAX
1420 : // @CopyFieldsFrom: SIM_IMUT1_TMAX
1421 : // @DisplayName: Simulated temperature calibration max
1422 :
1423 : // @Param: IMUT2_TMIN
1424 : // @CopyFieldsFrom: SIM_IMUT1_TMIN
1425 : // @DisplayName: Simulated temperature calibration min
1426 : AP_SUBGROUPINFO(imu_tcal[1], "IMUT2_", 62, SIM, AP_InertialSensor_TCal),
1427 : #endif
1428 : #if INS_MAX_INSTANCES > 2
1429 :
1430 : // @Param: IMUT3_ENABLE
1431 : // @CopyFieldsFrom: SIM_IMUT1_ENABLE
1432 : // @DisplayName: Enable simulated temperature disturbance for sensor data
1433 :
1434 : // @Param: IMUT3_ACC1
1435 : // @CopyFieldsFrom: SIM_IMUT1_ACC1
1436 : // @Vector3Parameter: 1
1437 :
1438 : // @Param: IMUT3_ACC2
1439 : // @CopyFieldsFrom: SIM_IMUT1_ACC2
1440 : // @Vector3Parameter: 1
1441 :
1442 : // @Param: IMUT3_ACC3
1443 : // @CopyFieldsFrom: SIM_IMUT1_ACC3
1444 : // @Vector3Parameter: 1
1445 :
1446 : // @Param: IMUT3_GYR1
1447 : // @CopyFieldsFrom: SIM_IMUT1_GYR1
1448 : // @Vector3Parameter: 1
1449 :
1450 : // @Param: IMUT3_GYR2
1451 : // @CopyFieldsFrom: SIM_IMUT1_GYR2
1452 : // @Vector3Parameter: 1
1453 :
1454 : // @Param: IMUT3_GYR3
1455 : // @CopyFieldsFrom: SIM_IMUT1_GYR3
1456 : // @Vector3Parameter: 1
1457 :
1458 : // @Param: IMUT3_TMAX
1459 : // @CopyFieldsFrom: SIM_IMUT1_TMAX
1460 : // @DisplayName: Simulated temperature calibration max
1461 :
1462 : // @Param: IMUT3_TMIN
1463 : // @CopyFieldsFrom: SIM_IMUT1_TMIN
1464 : // @DisplayName: Simulated temperature calibration min
1465 : AP_SUBGROUPINFO(imu_tcal[2], "IMUT3_", 63, SIM, AP_InertialSensor_TCal),
1466 : #endif
1467 : #if INS_MAX_INSTANCES > 3
1468 :
1469 : // @Param: IMUT4_ENABLE
1470 : // @CopyFieldsFrom: SIM_IMUT1_ENABLE
1471 : // @DisplayName: Enable simulated temperature disturbance for sensor data
1472 :
1473 : // @Param: IMUT4_ACC1
1474 : // @CopyFieldsFrom: SIM_IMUT1_ACC1
1475 : // @Vector3Parameter: 1
1476 :
1477 : // @Param: IMUT4_ACC2
1478 : // @CopyFieldsFrom: SIM_IMUT1_ACC2
1479 : // @Vector3Parameter: 1
1480 :
1481 : // @Param: IMUT4_ACC3
1482 : // @CopyFieldsFrom: SIM_IMUT1_ACC3
1483 : // @Vector3Parameter: 1
1484 :
1485 : // @Param: IMUT4_GYR1
1486 : // @CopyFieldsFrom: SIM_IMUT1_GYR1
1487 : // @Vector3Parameter: 1
1488 :
1489 : // @Param: IMUT4_GYR2
1490 : // @CopyFieldsFrom: SIM_IMUT1_GYR2
1491 : // @Vector3Parameter: 1
1492 :
1493 : // @Param: IMUT4_GYR3
1494 : // @CopyFieldsFrom: SIM_IMUT1_GYR3
1495 : // @Vector3Parameter: 1
1496 :
1497 : // @Param: IMUT4_TMAX
1498 : // @CopyFieldsFrom: SIM_IMUT1_TMAX
1499 : // @DisplayName: Simulated temperature calibration max
1500 :
1501 : // @Param: IMUT4_TMIN
1502 : // @CopyFieldsFrom: SIM_IMUT1_TMIN
1503 : // @DisplayName: Simulated temperature calibration min
1504 : AP_SUBGROUPINFO(imu_tcal[3], "IMUT4_", 60, SIM, AP_InertialSensor_TCal),
1505 : #endif
1506 : #if INS_MAX_INSTANCES > 4
1507 :
1508 : // @Param: IMUT5_ENABLE
1509 : // @CopyFieldsFrom: SIM_IMUT1_ENABLE
1510 : // @DisplayName: Enable simulated temperature disturbance for sensor data
1511 :
1512 : // @Param: IMUT5_ACC1
1513 : // @CopyFieldsFrom: SIM_IMUT1_ACC1
1514 : // @Vector3Parameter: 1
1515 :
1516 : // @Param: IMUT5_ACC2
1517 : // @CopyFieldsFrom: SIM_IMUT1_ACC2
1518 : // @Vector3Parameter: 1
1519 :
1520 : // @Param: IMUT5_ACC3
1521 : // @CopyFieldsFrom: SIM_IMUT1_ACC3
1522 : // @Vector3Parameter: 1
1523 :
1524 : // @Param: IMUT5_GYR1
1525 : // @CopyFieldsFrom: SIM_IMUT1_GYR1
1526 : // @Vector3Parameter: 1
1527 :
1528 : // @Param: IMUT5_GYR2
1529 : // @CopyFieldsFrom: SIM_IMUT1_GYR2
1530 : // @Vector3Parameter: 1
1531 :
1532 : // @Param: IMUT5_GYR3
1533 : // @CopyFieldsFrom: SIM_IMUT1_GYR3
1534 : // @Vector3Parameter: 1
1535 :
1536 : // @Param: IMUT5_TMAX
1537 : // @CopyFieldsFrom: SIM_IMUT1_TMAX
1538 : // @DisplayName: Simulated temperature calibration max
1539 :
1540 : // @Param: IMUT5_TMIN
1541 : // @CopyFieldsFrom: SIM_IMUT1_TMIN
1542 : // @DisplayName: Simulated temperature calibration min
1543 : AP_SUBGROUPINFO(imu_tcal[4], "IMUT5_", 59, SIM, AP_InertialSensor_TCal),
1544 : #endif
1545 : #endif // HAL_INS_TEMPERATURE_CAL_ENABLE
1546 : AP_GROUPEND
1547 : };
1548 :
1549 : const AP_Param::GroupInfo SIM::var_sonar[] = {
1550 : // @Param: SONAR_ROT
1551 : // @DisplayName: Sonar rotation
1552 : // @Description: Sonar rotation from rotations enumeration
1553 : AP_GROUPINFO("SONAR_ROT", 17, SIM, sonar_rot, Rotation::ROTATION_PITCH_270),
1554 : // @Param: SONAR_GLITCH
1555 : // @DisplayName: Sonar glitch probablility
1556 : // @Description: Probablility a sonar glitch would happen
1557 : // @Range: 0 1
1558 : // @User: Advanced
1559 : AP_GROUPINFO("SONAR_GLITCH", 23, SIM, sonar_glitch, 0),
1560 : // @Param: SONAR_RND
1561 : // @DisplayName: Sonar noise factor
1562 : // @Description: Scaling factor for simulated sonar noise
1563 : // @User: Advanced
1564 : AP_GROUPINFO("SONAR_RND", 24, SIM, sonar_noise, 0),
1565 : // @Param: SONAR_SCALE
1566 : // @DisplayName: Sonar conversion scale
1567 : // @Description: Sonar conversion scale from distance to voltage
1568 : // @Units: m/V
1569 : AP_GROUPINFO("SONAR_SCALE", 32, SIM, sonar_scale, 12.1212f),
1570 : // @Param: SONAR_POS
1571 : // @DisplayName: Sonar Offsets
1572 : // @Description: XYZ position of the sonar relative to the body frame origin
1573 : // @Units: m
1574 : // @Vector3Parameter: 1
1575 : AP_GROUPINFO("SONAR_POS", 55, SIM, rngfnd_pos_offset, 0),
1576 : // @Param: SONAR_OFFSET
1577 : // @DisplayName: Sonar measurement offset.
1578 : // @Description: Sonar measurement offset, in meters. Can be used for error injection.
1579 : // @User: Advanced
1580 : AP_GROUPINFO("SONAR_OFFSET", 57, SIM, sonar_offset, 0),
1581 : AP_GROUPEND
1582 : };
1583 :
1584 : // user settable parameters for the physics models
1585 : const AP_Param::GroupInfo SIM::ModelParm::var_info[] = {
1586 :
1587 : #if AP_SIM_SHIP_ENABLED
1588 : // @Group: SHIP_
1589 : // @Path: ./SIM_Ship.cpp
1590 : AP_SUBGROUPINFO(shipsim, "SHIP_", 1, SIM::ModelParm, ShipSim),
1591 : #endif
1592 : #if AP_SIM_STRATOBLIMP_ENABLED
1593 : // @Group: SB_
1594 : // @Path: ./SIM_StratoBlimp.cpp
1595 : AP_SUBGROUPPTR(stratoblimp_ptr, "SB_", 2, SIM::ModelParm, StratoBlimp),
1596 : #endif
1597 :
1598 : #if AP_SIM_GLIDER_ENABLED
1599 : // @Group: GLD_
1600 : // @Path: ./SIM_Glider.cpp
1601 : AP_SUBGROUPPTR(glider_ptr, "GLD_", 3, SIM::ModelParm, Glider),
1602 : #endif
1603 :
1604 : #if AP_SIM_SLUNGPAYLOAD_ENABLED
1605 : // @Group: SLUP_
1606 : // @Path: ./SIM_SlungPayload.cpp
1607 : AP_SUBGROUPINFO(slung_payload_sim, "SLUP_", 4, SIM::ModelParm, SlungPayloadSim),
1608 : #endif
1609 :
1610 : #if AP_SIM_FLIGHTAXIS_ENABLED
1611 : // @Group: RFL_
1612 : // @Path: ./SIM_FlightAxis.cpp
1613 : AP_SUBGROUPPTR(flightaxis_ptr, "RFL_", 5, SIM::ModelParm, FlightAxis),
1614 : #endif
1615 :
1616 : #if AP_SIM_TETHER_ENABLED
1617 : // @Group: TETH_
1618 : // @Path: ./SIM_Tether.cpp
1619 : AP_SUBGROUPINFO(tether_sim, "TETH_", 6, SIM::ModelParm, TetherSim),
1620 : #endif
1621 :
1622 : #if AP_SIM_AIS_ENABLED
1623 : // @Group: AIS_
1624 : // @Path: ./SIM_AIS.cpp
1625 : AP_SUBGROUPPTR(ais_ptr, "AIS_", 7, SIM::ModelParm, AIS),
1626 : #endif // AP_SIM_AIS_ENABLED
1627 :
1628 : // @Group: FRM_
1629 : // @Path: ./SIM_Frame.cpp
1630 : AP_SUBGROUPPTR(simframe_ptr, "FRM_", 8, SIM::ModelParm, Frame),
1631 :
1632 : AP_GROUPEND
1633 : };
1634 :
1635 : const Location post_origin {
1636 : 518752066,
1637 : 146487830,
1638 : 0,
1639 : Location::AltFrame::ABSOLUTE
1640 : };
1641 :
1642 : /* report SITL state via MAVLink SIMSTATE*/
1643 1127758 : void SIM::simstate_send(mavlink_channel_t chan) const
1644 : {
1645 1127758 : if (stop_MAVLink_sim_state) {
1646 : // Sim only MAVLink messages disabled to give more relaistic data rates
1647 0 : return;
1648 : }
1649 :
1650 : float yaw;
1651 :
1652 : // convert to same conventions as DCM
1653 1127758 : yaw = state.yawDeg;
1654 1127758 : if (yaw > 180) {
1655 0 : yaw -= 360;
1656 : }
1657 :
1658 1127758 : mavlink_msg_simstate_send(chan,
1659 1127758 : radians(state.rollDeg),
1660 1127758 : radians(state.pitchDeg),
1661 : radians(yaw),
1662 1127758 : state.xAccel,
1663 1127758 : state.yAccel,
1664 1127758 : state.zAccel,
1665 1127758 : radians(state.rollRate),
1666 1127758 : radians(state.pitchRate),
1667 1127758 : radians(state.yawRate),
1668 1127758 : state.latitude*1.0e7,
1669 1127758 : state.longitude*1.0e7);
1670 : }
1671 :
1672 : /* report SITL state via MAVLink SIM_STATE */
1673 2791 : void SIM::sim_state_send(mavlink_channel_t chan) const
1674 : {
1675 2791 : if (stop_MAVLink_sim_state) {
1676 : // Sim only MAVLink messages disabled to give more relaistic data rates
1677 0 : return;
1678 : }
1679 :
1680 : // convert to same conventions as DCM
1681 2791 : float yaw = state.yawDeg;
1682 2791 : if (yaw > 180) {
1683 0 : yaw -= 360;
1684 : }
1685 :
1686 16746 : mavlink_msg_sim_state_send(chan,
1687 2791 : state.quaternion.q1,
1688 2791 : state.quaternion.q2,
1689 2791 : state.quaternion.q3,
1690 2791 : state.quaternion.q4,
1691 2791 : radians(state.rollDeg),
1692 2791 : radians(state.pitchDeg),
1693 : radians(yaw),
1694 2791 : state.xAccel,
1695 2791 : state.yAccel,
1696 2791 : state.zAccel,
1697 2791 : radians(state.rollRate),
1698 2791 : radians(state.pitchRate),
1699 2791 : radians(state.yawRate),
1700 2791 : state.latitude*1.0e7,
1701 2791 : state.longitude*1.0e7,
1702 2791 : (float)state.altitude,
1703 : 0.0,
1704 : 0.0,
1705 2791 : state.speedN,
1706 2791 : state.speedE,
1707 2791 : state.speedD,
1708 2791 : (int32_t)(state.latitude*1.0e7),
1709 2791 : (int32_t)(state.longitude*1.0e7));
1710 : }
1711 :
1712 : #if HAL_LOGGING_ENABLED
1713 : /* report SITL state to AP_Logger */
1714 2461314 : void SIM::Log_Write_SIMSTATE()
1715 : {
1716 : float yaw;
1717 :
1718 : // convert to same conventions as DCM
1719 2461314 : yaw = state.yawDeg;
1720 2461314 : if (yaw > 180) {
1721 0 : yaw -= 360;
1722 : }
1723 :
1724 2461314 : struct log_AHRS pkt = {
1725 : LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG),
1726 2461314 : time_us : AP_HAL::micros64(),
1727 2461314 : roll : (int16_t)(state.rollDeg*100),
1728 2461314 : pitch : (int16_t)(state.pitchDeg*100),
1729 4922628 : yaw : (uint16_t)(wrap_360_cd(yaw*100)),
1730 2461314 : alt : (float)state.altitude,
1731 2461314 : lat : (int32_t)(state.latitude*1.0e7),
1732 2461314 : lng : (int32_t)(state.longitude*1.0e7),
1733 2461314 : q1 : state.quaternion.q1,
1734 2461314 : q2 : state.quaternion.q2,
1735 2461314 : q3 : state.quaternion.q3,
1736 2461314 : q4 : state.quaternion.q4,
1737 2461314 : };
1738 2461314 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
1739 2461314 : }
1740 : #endif
1741 :
1742 : /*
1743 : convert a set of roll rates from earth frame to body frame
1744 : output values are in radians/second
1745 : */
1746 0 : void SIM::convert_body_frame(double rollDeg, double pitchDeg,
1747 : double rollRate, double pitchRate, double yawRate,
1748 : double *p, double *q, double *r)
1749 : {
1750 : double phi, theta, phiDot, thetaDot, psiDot;
1751 :
1752 0 : phi = radians(rollDeg);
1753 0 : theta = radians(pitchDeg);
1754 0 : phiDot = radians(rollRate);
1755 0 : thetaDot = radians(pitchRate);
1756 0 : psiDot = radians(yawRate);
1757 :
1758 0 : *p = phiDot - psiDot*sin(theta);
1759 0 : *q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta);
1760 0 : *r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot;
1761 0 : }
1762 :
1763 :
1764 : /*
1765 : convert angular velocities from body frame to
1766 : earth frame.
1767 :
1768 : all inputs and outputs are in radians/s
1769 : */
1770 0 : Vector3f SIM::convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro)
1771 : {
1772 0 : float p = gyro.x;
1773 0 : float q = gyro.y;
1774 0 : float r = gyro.z;
1775 :
1776 : float phi, theta, psi;
1777 0 : dcm.to_euler(&phi, &theta, &psi);
1778 :
1779 0 : float phiDot = p + tanf(theta)*(q*sinf(phi) + r*cosf(phi));
1780 0 : float thetaDot = q*cosf(phi) - r*sinf(phi);
1781 0 : if (fabsf(cosf(theta)) < 1.0e-20f) {
1782 0 : theta += 1.0e-10f;
1783 : }
1784 0 : float psiDot = (q*sinf(phi) + r*cosf(phi))/cosf(theta);
1785 0 : return Vector3f(phiDot, thetaDot, psiDot);
1786 : }
1787 :
1788 : // get the rangefinder reading for the desired rotation, returns -1 for no data
1789 98037 : float SIM::get_rangefinder(uint8_t instance) {
1790 98037 : if (instance < ARRAY_SIZE(state.rangefinder_m)) {
1791 98037 : return state.rangefinder_m[instance];
1792 : }
1793 0 : return nanf("");
1794 : };
1795 :
1796 1120473 : float SIM::measure_distance_at_angle_bf(const Location &location, float angle) const
1797 : {
1798 : // should we populate state.rangefinder_m[...] from this?
1799 1120473 : Vector2f vehicle_pos_cm;
1800 1120473 : if (!location.get_vector_xy_from_origin_NE_cm(vehicle_pos_cm)) {
1801 : // should probably use SITL variables...
1802 447949 : return 0.0f;
1803 : }
1804 :
1805 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1806 : static uint64_t count = 0;
1807 :
1808 672524 : if (count == 0) {
1809 7 : unlink("/tmp/rayfile.scr");
1810 7 : unlink("/tmp/intersectionsfile.scr");
1811 : }
1812 :
1813 672524 : count++;
1814 :
1815 : // the 1000 here is so the files don't grow unbounded
1816 672524 : const bool write_debug_files = count < 1000;
1817 :
1818 672524 : FILE *rayfile = nullptr;
1819 672524 : if (write_debug_files) {
1820 6993 : rayfile = fopen("/tmp/rayfile.scr", "a");
1821 : }
1822 : #endif
1823 :
1824 : // cast a ray from location out 200m...
1825 672524 : Location location2 = location;
1826 672524 : location2.offset_bearing(wrap_180(angle + state.yawDeg), 200);
1827 672524 : Vector2f ray_endpos_cm;
1828 672524 : if (!location2.get_vector_xy_from_origin_NE_cm(ray_endpos_cm)) {
1829 : // should probably use SITL variables...
1830 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1831 0 : if (rayfile != nullptr) {
1832 0 : fclose(rayfile);
1833 : }
1834 : #endif
1835 0 : return 0.0f;
1836 : }
1837 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1838 672524 : if (rayfile != nullptr) {
1839 6993 : ::fprintf(rayfile, "map icon %f %f barrell\n", location2.lat*1e-7, location2.lng*1e-7);
1840 6993 : fclose(rayfile);
1841 : }
1842 :
1843 : // setup a grid of posts
1844 672524 : FILE *postfile = nullptr;
1845 672524 : FILE *intersectionsfile = nullptr;
1846 672524 : if (write_debug_files) {
1847 : static bool postfile_written;
1848 6993 : if (!postfile_written) {
1849 7 : ::fprintf(stderr, "Writing /tmp/post-locations.scr\n");
1850 7 : postfile_written = true;
1851 7 : postfile = fopen("/tmp/post-locations.scr", "w");
1852 : }
1853 6993 : intersectionsfile = fopen("/tmp/intersections.scr", "a");
1854 : }
1855 : #endif
1856 :
1857 672524 : const float radius_cm = 100.0f;
1858 672524 : float min_dist_cm = 1000000.0;
1859 672524 : const uint8_t num_post_offset = 10;
1860 14123004 : for (int8_t x=-num_post_offset; x<num_post_offset; x++) {
1861 282460080 : for (int8_t y=-num_post_offset; y<num_post_offset; y++) {
1862 269009600 : Location post_location = post_origin;
1863 269009600 : post_location.offset(x*10+3, y*10+2);
1864 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1865 269009600 : if (postfile != nullptr) {
1866 2800 : ::fprintf(postfile, "map circle %f %f %f blue\n", post_location.lat*1e-7, post_location.lng*1e-7, radius_cm*0.01);
1867 : }
1868 : #endif
1869 269009600 : Vector2f post_position_cm;
1870 269009600 : if (!post_location.get_vector_xy_from_origin_NE_cm(post_position_cm)) {
1871 : // should probably use SITL variables...
1872 0 : min_dist_cm = 0;
1873 0 : goto OUT;
1874 : }
1875 269009600 : Vector2f intersection_point_cm;
1876 269009600 : if (Vector2f::circle_segment_intersection(ray_endpos_cm, vehicle_pos_cm, post_position_cm, radius_cm, intersection_point_cm)) {
1877 1406380 : float dist_cm = (intersection_point_cm-vehicle_pos_cm).length();
1878 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1879 1406380 : if (intersectionsfile != nullptr) {
1880 15630 : Location intersection_point = location;
1881 15630 : intersection_point.offset(intersection_point_cm.x*0.01,
1882 15630 : intersection_point_cm.y*0.01);
1883 15630 : ::fprintf(intersectionsfile,
1884 : "map icon %f %f barrell\n",
1885 15630 : intersection_point.lat*1e-7,
1886 15630 : intersection_point.lng*1e-7);
1887 : }
1888 : #endif
1889 1406380 : if (dist_cm < min_dist_cm) {
1890 1218140 : min_dist_cm = dist_cm;
1891 : }
1892 : }
1893 : }
1894 : }
1895 :
1896 672524 : OUT:
1897 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1898 672524 : if (postfile != nullptr) {
1899 7 : fclose(postfile);
1900 : }
1901 672524 : if (intersectionsfile != nullptr) {
1902 6993 : fclose(intersectionsfile);
1903 : }
1904 : #endif
1905 :
1906 : // ::fprintf(stderr, "Distance @%f = %fm\n", angle, min_dist_cm*0.01f);
1907 672524 : return min_dist_cm * 0.01f;
1908 : }
1909 :
1910 : } // namespace SITL
1911 :
1912 : namespace AP {
1913 :
1914 4454509271 : SITL::SIM *sitl()
1915 : {
1916 4454509271 : return SITL::SIM::get_singleton();
1917 : }
1918 :
1919 : };
1920 :
1921 : #endif // AP_SIM_ENABLED
|
