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 : parent class for aircraft simulators
17 : */
18 :
19 : #pragma once
20 :
21 : #if AP_SIM_ENABLED
22 :
23 : #include <AP_Math/AP_Math.h>
24 :
25 : #include "SITL.h"
26 : #include "SITL_Input.h"
27 : #include "SIM_RichenPower.h"
28 : #include "SIM_Loweheiser.h"
29 : #include "SIM_FETtecOneWireESC.h"
30 : #include "SIM_Volz.h"
31 : #include "SIM_I2C.h"
32 : #include "SIM_Battery.h"
33 : #include <Filter/Filter.h>
34 : #include "SIM_JSON_Master.h"
35 : #include "ServoModel.h"
36 : #include "SIM_GPIO_LED_1.h"
37 : #include "SIM_GPIO_LED_2.h"
38 : #include "SIM_GPIO_LED_3.h"
39 : #include "SIM_GPIO_LED_RGB.h"
40 : #include "SIM_Siyi.h"
41 : #include "SIM_Topotek.h"
42 : #include "SIM_Viewpro.h"
43 : #include "SIM_Mount.h"
44 :
45 : #define MAX_SIM_INSTANCES 16
46 :
47 : namespace SITL {
48 :
49 : /*
50 : parent class for all simulator types
51 : */
52 : class Aircraft {
53 : public:
54 : Aircraft(const char *frame_str);
55 :
56 : // called directly after constructor:
57 : virtual void set_start_location(const Location &start_loc, const float start_yaw);
58 :
59 : /*
60 : set simulation speedup
61 : */
62 : void set_speedup(float speedup);
63 148005893 : float get_speedup() const { return target_speedup; }
64 :
65 : /*
66 : set instance number
67 : */
68 2094 : void set_instance(uint8_t _instance) {
69 2094 : instance = _instance;
70 : // register at 0 whatever our instance number is: there is only
71 : // ever one Aircraft in a SITL process, and _instance is the -I
72 : // number, which separates the ports and directories of separate
73 : // processes rather than indexing aircraft within one. Indexing
74 : // by it left instances[0] empty for every -I but zero, so
75 : // scripts calling sim:set_pose(0, ...) - as the shipped
76 : // sim_arming_pos.lua example does - silently did nothing.
77 2094 : instances[0] = this;
78 2094 : }
79 :
80 : /*
81 : set directory for additional files such as aircraft models
82 : */
83 2094 : void set_autotest_dir(const char *_autotest_dir) {
84 2094 : autotest_dir = _autotest_dir;
85 2094 : }
86 :
87 : /* Create and set in/out socket for extenal simulator */
88 2094 : virtual void set_interface_ports(const char* address, const int port_in, const int port_out) {};
89 :
90 : /*
91 : Start the external simulator process, for backends that manage one. Called
92 : once the model is fully configured, so the child's command line can depend
93 : on anything the setters above supply.
94 : */
95 2094 : virtual void launch_external_sim(void) {};
96 :
97 : /*
98 : step the FDM by one time step
99 : */
100 : virtual void update(const struct sitl_input &input) = 0;
101 :
102 : void update_model(const struct sitl_input &input);
103 :
104 : void update_home();
105 :
106 : /* fill a sitl_fdm structure from the simulator state */
107 : void fill_fdm(struct sitl_fdm &fdm);
108 :
109 : /* smooth sensors to provide kinematic consistancy */
110 : void smooth_sensors(void);
111 :
112 : /* return normal distribution random numbers */
113 : static double rand_normal(double mean, double stddev);
114 :
115 : // get frame rate of model in Hz
116 : float get_rate_hz(void) const { return rate_hz; }
117 :
118 108505077 : const Vector3f &get_gyro(void) const {
119 108505077 : return gyro;
120 : }
121 :
122 42572386 : const Vector3f &get_velocity_ef(void) const {
123 42572386 : return velocity_ef;
124 : }
125 :
126 : // return TAS airspeed in earth frame
127 107945231 : const Vector3f &get_velocity_air_ef(void) const {
128 107945231 : return velocity_air_ef;
129 : }
130 :
131 109876522 : const Matrix3f &get_dcm(void) const {
132 109876522 : return dcm;
133 : }
134 :
135 : const Vector3f &get_mag_field_bf(void) const {
136 : return mag_bf;
137 : }
138 :
139 107945231 : float gross_mass() const { return mass + external_payload_mass; }
140 :
141 2094 : virtual void set_config(const char* config) {
142 2094 : config_ = config;
143 2094 : }
144 :
145 : // return simulation origin:
146 80556 : const Location &get_origin() const { return origin; }
147 :
148 110163645 : const Location &get_location() const { return location; }
149 :
150 : // get position relative to home
151 : Vector3d get_position_relhome() const;
152 :
153 : // get air density in kg/m^3
154 : float get_air_density(float alt_amsl) const;
155 :
156 : // distance the rangefinder is perceiving
157 : float rangefinder_range() const;
158 :
159 179646075 : void get_attitude(Quaternion &attitude) const {
160 179646075 : attitude.from_rotation_matrix(dcm);
161 179646075 : }
162 :
163 2094 : const Location &get_home() const { return home; }
164 2094 : float get_home_yaw() const { return home_yaw; }
165 :
166 1 : void set_richenpower(RichenPower *_richenpower) { richenpower = _richenpower; }
167 7 : void set_adsb(class ADSB *_adsb) { adsb = _adsb; }
168 : #if AP_SIM_LOWEHEISER_ENABLED
169 10 : void set_loweheiser(Loweheiser *_loweheiser) { loweheiser = _loweheiser; }
170 : #endif
171 16 : void set_fetteconewireesc(FETtecOneWireESC *_fetteconewireesc) { fetteconewireesc = _fetteconewireesc; }
172 : #if AP_SIM_VOLZ_ENABLED
173 3 : void set_volz(Volz *_volz) { volz = _volz; }
174 : #endif
175 4 : void set_ie24(IntelligentEnergy24 *_ie24) { ie24 = _ie24; }
176 1176889686 : float get_battery_voltage() const { return battery_voltage; }
177 0 : float get_battery_temperature_degC() const { return battery_temperature_degC; }
178 187275482 : float get_battery_current() const { return battery_current; }
179 :
180 : float ambient_outside_temperature_degC() const;
181 : float ambient_outside_pressure_Pascal() const;
182 : float baro_temperature_degC() const;
183 :
184 : #if AP_SIM_MOUNT_ENABLED
185 9 : void add_gimbal_sim(Mount &sim) {
186 11 : for (uint8_t i = 0; i < GIMBAL_SIM_MAX; i++) {
187 11 : if (gimbal_sims[i] == nullptr) {
188 9 : gimbal_sims[i] = ∼
189 9 : return;
190 : }
191 : }
192 0 : AP_HAL::panic("Too many gimbal simulators");
193 : }
194 : #endif // AP_SIM_MOUNT_ENABLED
195 :
196 : ADSB *adsb;
197 :
198 : // takes a PWM range between 1000 and 2000 and returns a floating
199 : // point value between -1 and 1
200 30540181 : float normalise_servo_input(uint16_t input) const {
201 30540181 : return 2*((input-1000)/1000.0f - 0.5f);
202 : }
203 :
204 : ServoModel servo_filter[16];
205 :
206 1728432 : float get_airspeed_pitot() const { return airspeed_pitot; }
207 :
208 : /*
209 : used by scripting to control simulated aircraft position
210 : */
211 : static bool set_pose(uint8_t instance, const Location &loc, const Quaternion &quat,
212 : const Vector3f &velocity_ef, const Vector3f &gyro_rads);
213 :
214 : protected:
215 : SIM *sitl;
216 : // origin of position vector
217 : Location origin;
218 : // home location
219 : Location home;
220 : bool home_is_set;
221 : Location location;
222 :
223 : float ground_level;
224 : float home_yaw;
225 : float frame_height;
226 : Matrix3f dcm; // rotation matrix, APM conventions, from body to earth
227 : Vector3f gyro; // rad/s
228 : Vector3f velocity_ef; // m/s, earth frame
229 : Vector3f wind_ef; // m/s, earth frame
230 : Vector3f velocity_air_ef; // velocity relative to airmass, earth frame (true airspeed)
231 : Vector3f velocity_air_bf; // velocity relative to airmass, body frame
232 : Vector3d position; // meters, NED from origin
233 : float mass; // kg
234 : float external_payload_mass; // kg
235 : Vector3f accel_body{0.0f, 0.0f, -GRAVITY_MSS}; // m/s/s NED, body frame
236 : float airspeed; // m/s, EAS airspeed
237 : float airspeed_pitot; // m/s, EAS airspeed, as seen by fwd pitot tube
238 : float battery_voltage;
239 : float battery_current;
240 : float battery_temperature_degC;
241 : float local_ground_level; // ground level at local position
242 : bool lock_step_scheduled;
243 : bool flightaxis_sync_imus_to_frames; // causes the frame counter to be incremented on each timestep, IMUs will then update at the same rate
244 : uint32_t last_one_hz_ms;
245 :
246 : // OPTIONAL internal battery model.
247 : // ("OPTIONAL" because a child can ignore it and directly set battery_* protected members, exposed publicly via getters.)
248 : // (Note that some aircraft get battery-info from an external source, ignoring this "internal" one.)
249 : Battery battery;
250 :
251 : uint32_t motor_mask;
252 : float rpm[32];
253 : uint8_t rcin_chan_count;
254 : float rcin[12];
255 :
256 2049944438 : virtual float rangefinder_beam_width() const { return 0; }
257 : virtual float perpendicular_distance_to_rangefinder_surface() const;
258 :
259 : struct {
260 : // data from simulated laser scanner, if available
261 : struct vector3f_array points;
262 : struct float_array ranges;
263 : } scanner;
264 :
265 : // Rangefinder
266 : float rangefinder_m[SITL_NUM_RANGEFINDERS];
267 :
268 : // Windvane apparent wind
269 : struct {
270 : float speed;
271 : float direction;
272 : } wind_vane_apparent;
273 :
274 : // Wind Turbulence simulated Data
275 : float turbulence_azimuth;
276 : float turbulence_horizontal_speed; // m/s
277 : float turbulence_vertical_speed; // m/s
278 :
279 : Vector3f mag_bf; // local earth magnetic field vector in Gauss, earth frame
280 :
281 : uint64_t time_now_us;
282 :
283 : const float gyro_noise = radians(0.1f);
284 : const float accel_noise = 0.3f;
285 : float rate_hz = 1200.0f;
286 : float target_speedup;
287 : uint64_t frame_time_us;
288 : uint64_t last_wall_time_us;
289 : uint32_t last_fps_report_ms;
290 : float achieved_rate_hz; // achieved speedup rate
291 : int64_t sleep_debt_us;
292 : uint32_t last_frame_count;
293 : uint8_t instance;
294 : const char *autotest_dir;
295 : bool use_time_sync = true;
296 : float last_speedup = -1.0f;
297 : const char *config_ = "";
298 : float eas2tas = 1.0;
299 : float air_density = SSL_AIR_DENSITY;
300 :
301 : // allow for AHRS_ORIENTATION
302 : AP_Int8 *ahrs_orientation;
303 : enum Rotation last_imu_rotation;
304 : AP_Float* custom_roll;
305 : AP_Float* custom_pitch;
306 : AP_Float* custom_yaw;
307 :
308 : enum GroundBehaviour {
309 : GROUND_BEHAVIOR_NONE = 0,
310 : GROUND_BEHAVIOR_NO_MOVEMENT,
311 : GROUND_BEHAVIOR_FWD_ONLY,
312 : GROUND_BEHAVIOR_TAILSITTER,
313 : } ground_behavior;
314 :
315 : bool use_smoothing;
316 : bool disable_origin_movement;
317 :
318 : float ground_height_difference() const;
319 :
320 : virtual bool on_ground() const;
321 :
322 : // returns height above ground level in metres
323 : float hagl() const; // metres
324 :
325 : /* update location from position */
326 : void update_position(void);
327 :
328 : /* update body frame magnetic field */
329 : void update_mag_field_bf(void);
330 :
331 : /* advance time by deltat in seconds */
332 : void time_advance();
333 :
334 : /* setup the frame step time */
335 : void setup_frame_time(float rate, float speedup);
336 :
337 : /* adjust frame_time calculation */
338 : void adjust_frame_time(float rate);
339 :
340 : /* try to synchronise simulation time with wall clock time, taking
341 : into account desired speedup */
342 : void sync_frame_time(void);
343 :
344 : /* add noise based on throttle level (from 0..1) */
345 : void add_noise(float throttle);
346 :
347 : /* return a monotonic wall clock time in microseconds */
348 : uint64_t get_wall_time_us(void) const;
349 :
350 : // update attitude and relative position
351 : void update_dynamics(const Vector3f &rot_accel);
352 :
353 : // update wind vector
354 : void update_wind(const struct sitl_input &input);
355 :
356 : // return filtered servo input as -1 to 1 range
357 : float filtered_servo_angle(const struct sitl_input &input, uint8_t idx);
358 : float filtered_servo_range(const struct sitl_input &input, uint8_t idx);
359 : void filtered_servo_setup(uint8_t idx, uint16_t pwm_min, uint16_t pwm_max, float deflection_deg);
360 :
361 : // extrapolate sensors by a given delta time in seconds
362 : void extrapolate_sensors(float delta_time);
363 :
364 : // update battery
365 : virtual void update_battery();
366 : virtual void update_battery(const struct sitl_input &input);
367 :
368 : // update external payload/sensor dynamic
369 : void update_external_payload(const struct sitl_input &input);
370 :
371 : void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel);
372 : void add_twist_forces(Vector3f &rot_accel);
373 :
374 : // add body-frame force due to payload
375 : void add_external_forces(Vector3f &body_accel);
376 :
377 : // get local thermal updraft
378 : float get_local_updraft(const Vector3d ¤tPos);
379 :
380 : // update EAS speeds
381 : void update_eas_airspeed();
382 :
383 : // clamp support
384 : class Clamp {
385 : public:
386 : bool clamped(class Aircraft&, const struct sitl_input &input); // true if the vehicle is currently clamped down
387 : private:
388 : bool currently_clamped;
389 : bool grab_attempted; // avoid warning multiple times about missed grab
390 : } clamp;
391 :
392 : private:
393 : uint64_t last_time_us;
394 : uint32_t frame_counter;
395 : uint32_t last_ground_contact_ms;
396 : #if defined(__CYGWIN__) || defined(__CYGWIN64__)
397 : const uint32_t min_sleep_time{20000};
398 : #else
399 : const uint32_t min_sleep_time{5000};
400 : #endif
401 :
402 : struct {
403 : Vector3f accel_body;
404 : Vector3f gyro;
405 : Matrix3f rotation_b2e;
406 : Vector3d position;
407 : Vector3f velocity_ef;
408 : uint64_t last_update_us;
409 : Location location;
410 : } smoothing;
411 :
412 : RichenPower *richenpower;
413 : #if AP_SIM_LOWEHEISER_ENABLED
414 : Loweheiser *loweheiser;
415 : #endif
416 : FETtecOneWireESC *fetteconewireesc;
417 : #if AP_SIM_VOLZ_ENABLED
418 : Volz *volz;
419 : #endif // AP_SIM_VOLZ_ENABLED
420 :
421 : IntelligentEnergy24 *ie24;
422 :
423 :
424 : #if AP_SIM_GPIO_LED_1_ENABLED
425 : GPIO_LED_1 sim_led1{8}; // pin to match sitl.h
426 : #endif
427 : #if AP_SIM_GPIO_LED_2_ENABLED
428 : GPIO_LED_2 sim_led2{13, 14}; // pins to match sitl.h
429 : #endif
430 : #if AP_SIM_GPIO_LED_3_ENABLED
431 : GPIO_LED_3 sim_led3{13, 14, 15}; // pins to match sitl.h
432 : #endif
433 : #if AP_SIM_GPIO_LED_RGB_ENABLED
434 : GPIO_LED_RGB sim_ledrgb{8, 9, 10}; // pins to match sitl.h
435 : #endif
436 :
437 : static Aircraft *instances[MAX_SIM_INSTANCES];
438 : HAL_Semaphore pose_sem;
439 :
440 : #if AP_SIM_MOUNT_ENABLED
441 : static constexpr uint8_t GIMBAL_SIM_MAX = 8;
442 : Mount *gimbal_sims[GIMBAL_SIM_MAX];
443 : #endif
444 : };
445 :
446 : } // namespace SITL
447 :
448 : #endif // AP_SIM_ENABLED
|
