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 : #include "SIM_Aircraft.h"
20 :
21 : #include <stdio.h>
22 : #include <sys/time.h>
23 : #include <unistd.h>
24 :
25 : #include <GCS_MAVLink/GCS.h>
26 : #include <AP_Logger/AP_Logger.h>
27 : #include <AP_Param/AP_Param.h>
28 : #include <AP_Declination/AP_Declination.h>
29 : #include <AP_Terrain/AP_Terrain.h>
30 : #include <AP_Scheduler/AP_Scheduler.h>
31 : #include <AP_BoardConfig/AP_BoardConfig.h>
32 : #include <AP_JSON/AP_JSON.h>
33 : #include <AP_Filesystem/AP_Filesystem.h>
34 : #include <AP_HAL_SITL/HAL_SITL_Class.h>
35 : #include <AP_Vehicle/AP_Vehicle_Type.h>
36 :
37 : using namespace SITL;
38 :
39 : extern const AP_HAL::HAL& hal;
40 :
41 : // the SITL HAL can add information about pausing the simulation and its effect on the UART. Not present when we're compiling for simulation-on-hardware
42 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
43 : extern const HAL_SITL& hal_sitl;
44 : #endif
45 :
46 : Aircraft *Aircraft::instances[MAX_SIM_INSTANCES];
47 :
48 : /*
49 : parent class for all simulator types
50 : */
51 :
52 35615 : Aircraft::Aircraft(const char *frame_str)
53 : {
54 : // make the SIM_* variables available to simulator backends
55 2095 : sitl = AP::sitl();
56 :
57 2095 : set_speedup(1.0f);
58 :
59 2095 : last_wall_time_us = get_wall_time_us();
60 :
61 : // allow for orientation settings, such as with tailsitters
62 : enum ap_var_type ptype;
63 2095 : ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype);
64 :
65 : // ahrs_orientation->get() returns ROTATION_NONE here, regardless of the actual value
66 2095 : enum Rotation imu_rotation = ahrs_orientation?(enum Rotation)ahrs_orientation->get():ROTATION_NONE;
67 2095 : last_imu_rotation = imu_rotation;
68 : // sitl is null if running example program
69 2095 : if (sitl) {
70 2094 : sitl->ahrs_rotation.from_rotation(imu_rotation);
71 2094 : sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed();
72 : }
73 :
74 : // init rangefinder array to NaN to signify no data
75 23045 : for (uint8_t i = 0; i < ARRAY_SIZE(rangefinder_m); i++){
76 20950 : rangefinder_m[i] = nanf("");
77 : }
78 2095 : }
79 :
80 2094 : void Aircraft::set_start_location(const Location &start_loc, const float start_yaw)
81 : {
82 2094 : home = start_loc;
83 2094 : origin = home;
84 2094 : position.xy().zero();
85 2094 : home_yaw = start_yaw;
86 2094 : home_is_set = true;
87 :
88 2094 : ::printf("Home: %f %f alt=%fm hdg=%f\n",
89 2094 : home.lat*1e-7,
90 2094 : home.lng*1e-7,
91 2094 : home.alt*0.01,
92 2094 : home_yaw);
93 :
94 2094 : location = home;
95 2094 : ground_level = home.alt * 0.01f;
96 :
97 : #if 0
98 : // useful test for home position being very different from origin
99 : home.offset(-3000*1000, 1800*1000);
100 : #endif
101 :
102 2094 : dcm.from_euler(0.0f, 0.0f, radians(home_yaw));
103 2094 : }
104 :
105 : /*
106 : return difference in altitude between home position and current loc
107 : */
108 612398459 : float Aircraft::ground_height_difference() const
109 : {
110 : #if AP_TERRAIN_AVAILABLE
111 612398459 : AP_Terrain *terrain = AP::terrain();
112 : float h1, h2;
113 612398459 : if (sitl &&
114 611882279 : terrain != nullptr &&
115 1215333783 : sitl->terrain_enable &&
116 1828248422 : terrain->height_amsl(home, h1, false) &&
117 548231985 : terrain->height_amsl(location, h2, false)) {
118 547023873 : h2 += local_ground_level;
119 547023873 : return h2 - h1;
120 : }
121 : #endif
122 65374586 : return local_ground_level;
123 : }
124 :
125 283388 : float Aircraft::ambient_outside_temperature_degC() const
126 : {
127 : // FIXME: stop applying autopilot warming to this value
128 283388 : return baro_temperature_degC();
129 : }
130 :
131 : // the ambient temperature for the autopilot baro sensors, *not* the
132 : // ambient temperature for the outside of the vehicle. This
133 : // temperature is adjusted for things like the simulated board heating
134 : // up
135 283388 : float Aircraft::baro_temperature_degC() const
136 : {
137 : // FIXME: AP_Baro_SITL should be getting temperature from the
138 : // simulated aircraft, not the other way around!
139 : #if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) // Periph does not instantiate Baro
140 283388 : return AP::baro().get_temperature();
141 : #endif
142 : return 25.0;
143 : }
144 :
145 : // returns the expected ambient pressure for the vehicle. So pressure
146 : // drops preceived by the sensors due to airflow should not be
147 : // included in this number.
148 3208 : float Aircraft::ambient_outside_pressure_Pascal() const
149 : {
150 : // FIXME: this includes airflow-related things
151 3208 : return AP::baro().get_pressure();
152 : }
153 :
154 : /*
155 : return current height above ground level (metres)
156 : */
157 550973446 : float Aircraft::hagl() const
158 : {
159 550973446 : return (-position.z) + home.alt * 0.01f - ground_level - frame_height - ground_height_difference();
160 : }
161 :
162 : /*
163 : return true if we are on the ground
164 : */
165 547822322 : bool Aircraft::on_ground() const
166 : {
167 547822322 : return hagl() <= 0.001f; // prevent bouncing around ground
168 : }
169 :
170 : /*
171 : update location from position
172 : */
173 187275482 : void Aircraft::update_position(void)
174 : {
175 187275482 : location = origin;
176 187275482 : location.offset(position.x, position.y);
177 :
178 187275482 : location.alt = static_cast<int32_t>(home.alt - position.z * 100.0f);
179 :
180 : #if 0
181 : Vector3d pos_home = position;
182 : pos_home.xy() += home.get_distance_NE_double(origin);
183 :
184 : // logging of raw sitl data
185 : Vector3f accel_ef = dcm * accel_body;
186 : // @LoggerMessage: SITL
187 : // @Description: Simulation data
188 : // @Field: TimeUS: Time since system startup
189 : // @Field: VN: Velocity - North component
190 : // @Field: VE: Velocity - East component
191 : // @Field: VD: Velocity - Down component
192 : // @Field: AN: Acceleration - North component
193 : // @Field: AE: Acceleration - East component
194 : // @Field: AD: Acceleration - Down component
195 : // @Field: PN: Position - North component
196 : // @Field: PE: Position - East component
197 : // @Field: PD: Position - Down component
198 : AP::logger().WriteStreaming("SITL", "TimeUS,VN,VE,VD,AN,AE,AD,PN,PE,PD", "Qfffffffff",
199 : AP_HAL::micros64(),
200 : velocity_ef.x, velocity_ef.y, velocity_ef.z,
201 : accel_ef.x, accel_ef.y, accel_ef.z,
202 : pos_home.x, pos_home.y, pos_home.z);
203 : #endif
204 :
205 187275482 : if (!disable_origin_movement) {
206 186153551 : uint32_t now = AP_HAL::millis();
207 186153551 : if (now - last_one_hz_ms >= 1000) {
208 : // shift origin of position at 1Hz to current location
209 : // this prevents spherical errors building up in the GPS data
210 154065 : last_one_hz_ms = now;
211 154065 : Vector2d diffNE = origin.get_distance_NE_double(location);
212 154065 : position.xy() -= diffNE;
213 154065 : smoothing.position.xy() -= diffNE;
214 154065 : origin.lat = location.lat;
215 154065 : origin.lng = location.lng;
216 : }
217 : }
218 187275482 : }
219 :
220 : /*
221 : update body magnetic field from position and rotation
222 : */
223 187275482 : void Aircraft::update_mag_field_bf()
224 : {
225 : // get the magnetic field intensity and orientation
226 : float intensity;
227 : float declination;
228 : float inclination;
229 187275482 : AP_Declination::get_mag_field_ef(location.lat * 1e-7f, location.lng * 1e-7f, intensity, declination, inclination);
230 :
231 : // create a field vector and rotate to the required orientation
232 187275482 : Vector3f mag_ef(1e3f * intensity, 0.0f, 0.0f);
233 187275482 : Matrix3f R;
234 187275482 : R.from_euler(0.0f, -radians(inclination), radians(declination));
235 187275482 : mag_ef = R * mag_ef;
236 :
237 : // calculate frame height above ground
238 187275482 : const float frame_height_agl = fmaxf((-position.z) + home.alt * 0.01f - ground_level, 0.0f);
239 :
240 187275482 : if (!sitl) {
241 : // running example program
242 0 : return;
243 : }
244 :
245 : // calculate scaling factor that varies from 1 at ground level to 1/8 at sitl->mag_anomaly_hgt
246 : // Assume magnetic anomaly strength scales with 1/R**3
247 187275482 : float anomaly_scaler = (sitl->mag_anomaly_hgt / (frame_height_agl + sitl->mag_anomaly_hgt));
248 187275482 : anomaly_scaler = anomaly_scaler * anomaly_scaler * anomaly_scaler;
249 :
250 : // add scaled anomaly to earth field
251 187275482 : mag_ef += sitl->mag_anomaly_ned.get() * anomaly_scaler;
252 :
253 : // Rotate into body frame
254 187275482 : mag_bf = dcm.transposed() * mag_ef;
255 :
256 : // add motor interference
257 187275482 : mag_bf += sitl->mag_mot.get() * battery_current;
258 : }
259 :
260 : /* advance time by deltat in seconds */
261 187275482 : void Aircraft::time_advance()
262 : {
263 : // we only advance time if it hasn't been advanced already by the
264 : // backend
265 187275482 : if (last_time_us == time_now_us) {
266 187275482 : time_now_us += frame_time_us;
267 : }
268 187275482 : last_time_us = time_now_us;
269 187275482 : if (use_time_sync) {
270 187275482 : sync_frame_time();
271 : }
272 187275482 : }
273 :
274 : /* setup the frame step time */
275 6399 : void Aircraft::setup_frame_time(float new_rate, float new_speedup)
276 : {
277 6399 : rate_hz = new_rate;
278 6399 : target_speedup = new_speedup;
279 6399 : frame_time_us = uint64_t(1.0e6f/rate_hz);
280 :
281 6399 : last_wall_time_us = get_wall_time_us();
282 6399 : }
283 :
284 : /* adjust frame_time calculation */
285 169142737 : void Aircraft::adjust_frame_time(float new_rate)
286 : {
287 169142737 : frame_time_us = uint64_t(1.0e6f/new_rate);
288 169142737 : rate_hz = new_rate;
289 169142737 : }
290 :
291 : /*
292 : try to synchronise simulation time with wall clock time, taking
293 : into account desired speedup
294 : This tries to take account of possible granularity of
295 : get_wall_time_us() so it works reasonably well on windows
296 : */
297 187275482 : void Aircraft::sync_frame_time(void)
298 : {
299 187275482 : frame_counter++;
300 187275482 : uint64_t now = get_wall_time_us();
301 187275482 : uint64_t dt_us = now - last_wall_time_us;
302 :
303 187275482 : const float target_dt_us = 1.0e6/(rate_hz*target_speedup);
304 :
305 : // accumulate sleep debt if we're running too fast
306 187275482 : sleep_debt_us += target_dt_us - dt_us;
307 :
308 187275482 : if (sleep_debt_us < -1.0e5) {
309 : // don't let a large negative debt build up
310 138774552 : sleep_debt_us = -1.0e5;
311 : }
312 187275482 : if (sleep_debt_us > min_sleep_time) {
313 : // sleep if we have built up a debt of min_sleep_tim
314 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
315 209981 : usleep(sleep_debt_us);
316 : #elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
317 : hal.scheduler->delay_microseconds(sleep_debt_us);
318 : #else
319 : // ??
320 : #endif
321 209981 : sleep_debt_us -= (get_wall_time_us() - now);
322 : }
323 187275482 : last_wall_time_us = get_wall_time_us();
324 :
325 187275482 : uint32_t now_ms = last_wall_time_us / 1000ULL;
326 187275482 : float dt_wall = (now_ms - last_fps_report_ms) * 0.001;
327 187275482 : if (dt_wall > 0.01) { // 0.01s average
328 534914 : achieved_rate_hz = (frame_counter - last_frame_count) / dt_wall;
329 : #if 0
330 : ::printf("Rate: target:%.1f achieved:%.1f speedup %.1f/%.1f\n",
331 : rate_hz*target_speedup, achieved_rate_hz,
332 : achieved_rate_hz/rate_hz, target_speedup);
333 : #endif
334 534914 : last_frame_count = frame_counter;
335 534914 : last_fps_report_ms = now_ms;
336 : }
337 187275482 : }
338 :
339 : /* add noise based on throttle level (from 0..1) */
340 64098694 : void Aircraft::add_noise(float throttle)
341 : {
342 64098694 : gyro += Vector3f(rand_normal(0, 1),
343 64098694 : rand_normal(0, 1),
344 128197388 : rand_normal(0, 1)) * gyro_noise * fabsf(throttle);
345 64098694 : accel_body += Vector3f(rand_normal(0, 1),
346 64098694 : rand_normal(0, 1),
347 128197388 : rand_normal(0, 1)) * accel_noise * fabsf(throttle);
348 64098694 : }
349 :
350 : /*
351 : normal distribution random numbers
352 : See
353 : http://en.literateprograms.org/index.php?title=Special:DownloadCode/Box-Muller_transform_%28C%29&oldid=7011
354 : */
355 384686853 : double Aircraft::rand_normal(double mean, double stddev)
356 : {
357 : static double n2 = 0.0;
358 : static int n2_cached = 0;
359 384686853 : if (!n2_cached) {
360 : double x, y, r;
361 : do
362 : {
363 244826974 : x = 2.0 * rand()/RAND_MAX - 1;
364 244826974 : y = 2.0 * rand()/RAND_MAX - 1;
365 244826974 : r = x*x + y*y;
366 244826974 : } while (is_zero(r) || r > 1.0);
367 192343429 : const double d = sqrt(-2.0 * log(r)/r);
368 192343429 : const double n1 = x * d;
369 192343429 : n2 = y * d;
370 192343429 : const double result = n1 * stddev + mean;
371 192343429 : n2_cached = 1;
372 192343429 : return result;
373 : } else {
374 192343424 : n2_cached = 0;
375 192343424 : return n2 * stddev + mean;
376 : }
377 : }
378 :
379 :
380 :
381 :
382 : /*
383 : fill a sitl_fdm structure from the simulator state
384 : */
385 187275482 : void Aircraft::fill_fdm(struct sitl_fdm &fdm)
386 : {
387 187275482 : bool is_smoothed = false;
388 187275482 : if (use_smoothing) {
389 159454654 : smooth_sensors();
390 159454654 : is_smoothed = true;
391 : }
392 187275482 : fdm.timestamp_us = time_now_us;
393 : // keep track of the number of frames processed so that the IMUs can follow
394 187275482 : if (flightaxis_sync_imus_to_frames) {
395 0 : fdm.flightaxis_imu_frame_num++;
396 : }
397 :
398 187275482 : if (fdm.home.lat == 0 && fdm.home.lng == 0) {
399 : // initialise home
400 2094 : fdm.home = home;
401 : }
402 187275482 : fdm.is_lock_step_scheduled = lock_step_scheduled;
403 187275482 : fdm.latitude = location.lat * 1.0e-7;
404 187275482 : fdm.longitude = location.lng * 1.0e-7;
405 187275482 : fdm.altitude = location.alt * 1.0e-2;
406 187275482 : fdm.heading = degrees(atan2f(velocity_ef.y, velocity_ef.x));
407 187275482 : fdm.speedN = velocity_ef.x;
408 187275482 : fdm.speedE = velocity_ef.y;
409 187275482 : fdm.speedD = velocity_ef.z;
410 187275482 : fdm.xAccel = accel_body.x;
411 187275482 : fdm.yAccel = accel_body.y;
412 187275482 : fdm.zAccel = accel_body.z;
413 187275482 : fdm.rollRate = degrees(gyro.x);
414 187275482 : fdm.pitchRate = degrees(gyro.y);
415 187275482 : fdm.yawRate = degrees(gyro.z);
416 : float r, p, y;
417 187275482 : dcm.to_euler(&r, &p, &y);
418 187275482 : fdm.rollDeg = degrees(r);
419 187275482 : fdm.pitchDeg = degrees(p);
420 187275482 : fdm.yawDeg = degrees(y);
421 187275482 : fdm.quaternion.from_rotation_matrix(dcm);
422 187275482 : fdm.airspeed = airspeed_pitot;
423 187275482 : fdm.velocity_air_bf = velocity_air_bf;
424 187275482 : fdm.battery_voltage = battery_voltage;
425 187275482 : fdm.battery_current = battery_current;
426 187275482 : fdm.motor_mask = motor_mask | sitl->vibe_motor_mask;
427 187275482 : memcpy(fdm.rpm, rpm, sizeof(fdm.rpm));
428 187275482 : fdm.rcin_chan_count = rcin_chan_count;
429 187275482 : fdm.range = rangefinder_range();
430 187275482 : memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float));
431 187275482 : fdm.bodyMagField = mag_bf;
432 :
433 187275482 : fdm.bodyMagField.rotate(sitl->imu_orientation);
434 :
435 : // copy laser scanner results
436 187275482 : fdm.scanner.points = scanner.points;
437 187275482 : fdm.scanner.ranges = scanner.ranges;
438 :
439 : // copy rangefinder
440 187275482 : memcpy(fdm.rangefinder_m, rangefinder_m, sizeof(fdm.rangefinder_m));
441 :
442 187275482 : fdm.wind_vane_apparent.direction = wind_vane_apparent.direction;
443 187275482 : fdm.wind_vane_apparent.speed = wind_vane_apparent.speed;
444 :
445 187275482 : fdm.wind_ef = wind_ef;
446 :
447 187275482 : if (is_smoothed) {
448 159454654 : fdm.xAccel = smoothing.accel_body.x;
449 159454654 : fdm.yAccel = smoothing.accel_body.y;
450 159454654 : fdm.zAccel = smoothing.accel_body.z;
451 159454654 : fdm.rollRate = degrees(smoothing.gyro.x);
452 159454654 : fdm.pitchRate = degrees(smoothing.gyro.y);
453 159454654 : fdm.yawRate = degrees(smoothing.gyro.z);
454 159454654 : fdm.speedN = smoothing.velocity_ef.x;
455 159454654 : fdm.speedE = smoothing.velocity_ef.y;
456 159454654 : fdm.speedD = smoothing.velocity_ef.z;
457 159454654 : fdm.latitude = smoothing.location.lat * 1.0e-7;
458 159454654 : fdm.longitude = smoothing.location.lng * 1.0e-7;
459 159454654 : fdm.altitude = smoothing.location.alt * 1.0e-2;
460 : }
461 :
462 :
463 187275482 : if (ahrs_orientation != nullptr) {
464 187275482 : enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get();
465 187275482 : if (imu_rotation != last_imu_rotation) {
466 2 : sitl->ahrs_rotation.from_rotation(imu_rotation);
467 2 : sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed();
468 2 : last_imu_rotation = imu_rotation;
469 : }
470 187275482 : if (imu_rotation != ROTATION_NONE) {
471 2905 : Matrix3f m = dcm;
472 2905 : m = m * sitl->ahrs_rotation_inv;
473 :
474 2905 : m.to_euler(&r, &p, &y);
475 2905 : fdm.rollDeg = degrees(r);
476 2905 : fdm.pitchDeg = degrees(p);
477 2905 : fdm.yawDeg = degrees(y);
478 2905 : fdm.quaternion.from_rotation_matrix(m);
479 : }
480 : }
481 :
482 : // in the first call here, if a speedup option is specified, overwrite it
483 187275482 : if (is_equal(last_speedup, -1.0f) && !is_equal(get_speedup(), 1.0f)) {
484 0 : sitl->speedup.set(get_speedup());
485 : }
486 :
487 187275482 : if (!is_equal(last_speedup, float(sitl->speedup)) && sitl->speedup > 0) {
488 2210 : set_speedup(sitl->speedup);
489 2210 : last_speedup = sitl->speedup;
490 : }
491 :
492 : #if HAL_LOGGING_ENABLED
493 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
494 : // the SITL HAL can add information about pausing the simulation
495 : // and its effect on the UART. Not present when we're compiling
496 : // for simulation-on-hardware
497 187275482 : const uint32_t full_count = hal_sitl.get_uart_output_full_queue_count();
498 : #else
499 : const uint32_t full_count = 0;
500 : #endif
501 : // for EKF comparison log relhome pos and velocity at loop rate
502 : static uint16_t last_ticks;
503 : // FIXME: stop using AP_Scheduler here! It's from "the other side"
504 187275482 : const auto *scheduler = AP_Scheduler::get_singleton();
505 187275482 : if (scheduler == nullptr) {
506 : // not instantiated in examples
507 0 : return;
508 : }
509 187275482 : uint16_t ticks = scheduler->ticks();
510 187275482 : if (last_ticks != ticks) {
511 41686253 : last_ticks = ticks;
512 : // @LoggerMessage: SIM2
513 : // @Description: Additional simulator state
514 : // @Field: TimeUS: Time since system startup
515 : // @Field: PN: North position from home
516 : // @Field: PE: East position from home
517 : // @Field: PD: Down position from home
518 : // @Field: VN: Velocity north
519 : // @Field: VE: Velocity east
520 : // @Field: VD: Velocity down
521 : // @Field: As: Airspeed
522 : // @Field: ASpdU: Achieved simulation speedup value
523 : // @Field: UFC: Number of times simulation paused for serial0 output
524 41686253 : Vector3d pos = get_position_relhome();
525 41686253 : Vector3f vel = get_velocity_ef();
526 83372506 : AP::logger().WriteStreaming(
527 : "SIM2",
528 : "TimeUS,PN,PE,PD,VN,VE,VD,As,ASpdU,UFC",
529 : "QdddfffffI",
530 : AP_HAL::micros64(),
531 : pos.x, pos.y, pos.z,
532 41686253 : vel.x, vel.y, vel.z,
533 41686253 : airspeed_pitot,
534 41686253 : achieved_rate_hz/rate_hz,
535 : full_count
536 : );
537 : }
538 : #endif
539 : }
540 :
541 : /*
542 : rover and copter have special handling for horizontal rangefinders
543 : as distance to obstacles - this takes effect for yaw-only
544 : orientations
545 : */
546 : #define SITL_RANGEFINDER_AS_OBJECT_SENSOR (APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_Rover))
547 : #define SITL_RANGEFINDER_IS_YAW_ONLY(orientation) (orientation <= ROTATION_YAW_315)
548 :
549 : // returns perpendicular height to surface rangefinder is bouncing off
550 1685241307 : float Aircraft::perpendicular_distance_to_rangefinder_surface() const
551 : {
552 : #if SITL_RANGEFINDER_AS_OBJECT_SENSOR
553 1024972219 : const auto orientation = (Rotation)sitl->sonar_rot.get();
554 1024972219 : if (SITL_RANGEFINDER_IS_YAW_ONLY(orientation)) {
555 : // assume these are avoidance sensors
556 632825 : return sitl->measure_distance_at_angle_bf(location, sitl->sonar_rot.get()*45);
557 : }
558 : #endif
559 :
560 : // default is ground sensing rangefinders
561 1684608482 : return sitl->state.height_agl;
562 : }
563 :
564 1750708325 : float Aircraft::rangefinder_range() const
565 : {
566 1750708325 : float altitude = perpendicular_distance_to_rangefinder_surface();
567 :
568 : // sensor position offset in body frame
569 1750708325 : const Vector3f relPosSensorBF = sitl->rngfnd_pos_offset;
570 :
571 : // n.b. the following code is assuming rotation-pitch-270:
572 : // adjust altitude for position of the sensor on the vehicle if position offset is non-zero
573 1750708325 : if (!relPosSensorBF.is_zero()) {
574 : // get a rotation matrix following DCM conventions (body to earth)
575 0 : Matrix3f rotmat;
576 0 : sitl->state.quaternion.rotation_matrix(rotmat);
577 : // rotate the offset into earth frame
578 0 : const Vector3f relPosSensorEF = rotmat * relPosSensorBF;
579 : // correct the altitude at the sensor
580 0 : altitude -= relPosSensorEF.z;
581 : }
582 :
583 1750708325 : altitude += sitl->sonar_offset;
584 :
585 1750708325 : const auto orientation = (Rotation)sitl->sonar_rot.get();
586 : #if SITL_RANGEFINDER_AS_OBJECT_SENSOR
587 :
588 1024972219 : float roll = sitl->state.rollDeg;
589 1024972219 : float pitch = sitl->state.pitchDeg;
590 :
591 1024972219 : if (roll > 0) {
592 278701392 : roll -= rangefinder_beam_width();
593 278701392 : if (roll < 0) {
594 0 : roll = 0;
595 : }
596 : } else {
597 746270827 : roll += rangefinder_beam_width();
598 746270827 : if (roll > 0) {
599 0 : roll = 0;
600 : }
601 : }
602 1024972219 : if (pitch > 0) {
603 237901391 : pitch -= rangefinder_beam_width();
604 237901391 : if (pitch < 0) {
605 0 : pitch = 0;
606 : }
607 : } else {
608 787070828 : pitch += rangefinder_beam_width();
609 787070828 : if (pitch > 0) {
610 0 : pitch = 0;
611 : }
612 : }
613 :
614 : /*
615 : rover and copter using SITL rangefinders as obstacle sensors
616 : */
617 1024972219 : if (SITL_RANGEFINDER_IS_YAW_ONLY(orientation)) {
618 632825 : if (fabs(roll) >= 90.0 || fabs(pitch) >= 90.0) {
619 : // not going to hit the ground....
620 0 : return INFINITY;
621 : }
622 632825 : altitude /= cosf(radians(roll)) * cosf(radians(pitch));
623 : } else
624 : #endif
625 : {
626 : // adjust for rotation based on orientation of the sensor
627 1750075500 : Matrix3f rotmat;
628 1750075500 : sitl->state.quaternion.rotation_matrix(rotmat);
629 1750075500 : Vector3f v{1, 0, 0};
630 1750075500 : v.rotate(orientation);
631 1750075500 : v = rotmat * v;
632 :
633 1750075500 : if (!is_positive(v.z)) {
634 17522462 : return INFINITY;
635 : }
636 1737480126 : altitude /= v.z;
637 :
638 : // this is awful, but there are drawbacks to assuming an
639 : // infinite plane. If we don't do this here then we end up
640 : // with a ridiculous rangefinder range, and that can cause
641 : // floating point exceptions when we return a distance in cm
642 : // from the AP_RangeFinder_SITL.
643 1737480126 : if (altitude > 100000) {
644 4927088 : return INFINITY;
645 : }
646 : }
647 :
648 : // Add some noise on reading
649 1733185863 : altitude += sitl->sonar_noise * rand_float();
650 :
651 : // our starting positions can disagree with the terrain database:
652 1733185863 : if (altitude < 0) {
653 62931252 : altitude = 0;
654 : }
655 :
656 1733185863 : return altitude;
657 : }
658 :
659 : #if defined(__CYGWIN__) || defined(__CYGWIN64__)
660 : extern "C" { uint32_t timeGetTime(); }
661 : #endif
662 :
663 : // potentially replace this with a call to AP_HAL::Util::get_hw_rtc
664 374769439 : uint64_t Aircraft::get_wall_time_us() const
665 : {
666 : #if defined(__CYGWIN__) || defined(__CYGWIN64__)
667 : static uint32_t tPrev;
668 : static uint64_t last_ret_us;
669 : if (tPrev == 0) {
670 : tPrev = timeGetTime();
671 : return 0;
672 : }
673 : uint32_t now = timeGetTime();
674 : last_ret_us += (uint64_t)((now - tPrev)*1000UL);
675 : tPrev = now;
676 : return last_ret_us;
677 : #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
678 : struct timespec ts;
679 374769439 : clock_gettime(CLOCK_MONOTONIC, &ts);
680 374769439 : return uint64_t(ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL);
681 : #else
682 : return AP_HAL::micros64();
683 : #endif
684 : }
685 :
686 : /*
687 : set simulation speedup
688 : */
689 6399 : void Aircraft::set_speedup(float speedup)
690 : {
691 6399 : setup_frame_time(rate_hz, speedup);
692 6399 : }
693 :
694 187275482 : void Aircraft::update_home()
695 : {
696 187275482 : if (!home_is_set) {
697 0 : if (sitl == nullptr) {
698 0 : return;
699 : }
700 : const Location loc{
701 0 : int32_t(sitl->opos.lat.get() * 1.0e7),
702 0 : int32_t(sitl->opos.lng.get() * 1.0e7),
703 0 : int32_t(sitl->opos.alt.get() * 1.0e2),
704 : Location::AltFrame::ABSOLUTE
705 0 : };
706 0 : set_start_location(loc, sitl->opos.hdg.get());
707 : }
708 : }
709 :
710 187275482 : void Aircraft::update_model(const struct sitl_input &input)
711 : {
712 187275482 : local_ground_level = 0.0f;
713 187275482 : if (sitl != nullptr) {
714 187275482 : update(input);
715 : } else {
716 0 : time_advance();
717 : }
718 187275482 : }
719 :
720 : /*
721 : update the simulation attitude and relative position
722 : */
723 169080479 : void Aircraft::update_dynamics(const Vector3f &rot_accel)
724 : {
725 169080479 : WITH_SEMAPHORE(pose_sem);
726 :
727 169080479 : const float delta_time = frame_time_us * 1.0e-6f;
728 :
729 : // update eas2tas and air density
730 169080479 : eas2tas = AP_Baro::get_EAS2TAS_for_alt_amsl(location.alt*0.01);
731 169080479 : air_density = AP_Baro::get_air_density_for_alt_amsl(location.alt*0.01);
732 :
733 : // update rotational rates in body frame
734 169080479 : gyro += rot_accel * delta_time;
735 :
736 169080479 : gyro.x = constrain_float(gyro.x, -radians(2000.0f), radians(2000.0f));
737 169080479 : gyro.y = constrain_float(gyro.y, -radians(2000.0f), radians(2000.0f));
738 169080479 : gyro.z = constrain_float(gyro.z, -radians(2000.0f), radians(2000.0f));
739 :
740 : // limit body accel to 64G
741 169080479 : const float accel_limit = 64*GRAVITY_MSS;
742 169080479 : accel_body.x = constrain_float(accel_body.x, -accel_limit, accel_limit);
743 169080479 : accel_body.y = constrain_float(accel_body.y, -accel_limit, accel_limit);
744 169080479 : accel_body.z = constrain_float(accel_body.z, -accel_limit, accel_limit);
745 :
746 : // update attitude
747 169080479 : dcm.rotate(gyro * delta_time);
748 169080479 : dcm.normalize();
749 :
750 169080479 : Vector3f accel_earth = dcm * accel_body;
751 169080479 : accel_earth += Vector3f(0.0f, 0.0f, GRAVITY_MSS);
752 :
753 : // if we're on the ground, then our vertical acceleration is limited
754 : // to zero. This effectively adds the force of the ground on the aircraft
755 169080479 : if (on_ground() && accel_earth.z > 0) {
756 60950890 : accel_earth.z = 0;
757 : }
758 :
759 : // work out acceleration as seen by the accelerometers. It sees the kinematic
760 : // acceleration (ie. real movement), plus gravity
761 169080479 : accel_body = dcm.transposed() * (accel_earth + Vector3f(0.0f, 0.0f, -GRAVITY_MSS));
762 :
763 : // new velocity vector
764 169080479 : velocity_ef += accel_earth * delta_time;
765 :
766 169080479 : const bool was_on_ground = on_ground();
767 : // new position vector
768 169080479 : position += (velocity_ef * delta_time).todouble();
769 :
770 : // velocity relative to air mass, in earth frame
771 169080479 : velocity_air_ef = velocity_ef - wind_ef;
772 :
773 : // velocity relative to airmass in body frame
774 169080479 : velocity_air_bf = dcm.transposed() * velocity_air_ef;
775 :
776 : // airspeed
777 169080479 : update_eas_airspeed();
778 :
779 : // constrain height to the ground
780 169080479 : if (on_ground()) {
781 61425013 : if (!was_on_ground && AP_HAL::millis() - last_ground_contact_ms > 1000) {
782 704 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SIM Hit ground at %f m/s", velocity_ef.z);
783 704 : last_ground_contact_ms = AP_HAL::millis();
784 : }
785 61425013 : position.z = -(ground_level + frame_height - home.alt * 0.01f + ground_height_difference());
786 :
787 : // get speed of ground movement (for ship takeoff/landing)
788 61425013 : float yaw_rate = 0;
789 : #if AP_SIM_SHIP_ENABLED
790 61425013 : const Vector2f ship_movement = sitl->models.shipsim.get_ground_speed_adjustment(location, yaw_rate);
791 61425013 : const Vector3f gnd_movement(ship_movement.x, ship_movement.y, 0);
792 : #else
793 : const Vector3f gnd_movement;
794 : #endif
795 61425013 : switch (ground_behavior) {
796 3613461 : case GROUND_BEHAVIOR_NONE:
797 3613461 : break;
798 43849048 : case GROUND_BEHAVIOR_NO_MOVEMENT: {
799 : // zero roll/pitch, but keep yaw
800 : float r, p, y;
801 43849048 : dcm.to_euler(&r, &p, &y);
802 43849048 : y = y + yaw_rate * delta_time;
803 43849048 : dcm.from_euler(0.0f, 0.0f, y);
804 : // X, Y movement tracks ground movement
805 43849048 : velocity_ef.x = gnd_movement.x;
806 43849048 : velocity_ef.y = gnd_movement.y;
807 43849048 : if (velocity_ef.z > 0.0f) {
808 597 : velocity_ef.z = 0.0f;
809 : }
810 43849048 : gyro.zero();
811 43849048 : use_smoothing = true;
812 43849048 : break;
813 : }
814 13873237 : case GROUND_BEHAVIOR_FWD_ONLY: {
815 : // zero roll/pitch, but keep yaw
816 : float r, p, y;
817 13873237 : dcm.to_euler(&r, &p, &y);
818 13873237 : if (velocity_ef.length() < 5) {
819 : // at high speeds don't constrain pitch, otherwise we
820 : // can get stuck in takeoff
821 13432325 : p = 0;
822 : } else {
823 440912 : p = MAX(p, 0);
824 : }
825 13873237 : y = y + yaw_rate * delta_time;
826 13873237 : dcm.from_euler(0.0f, p, y);
827 : // only fwd movement
828 13873237 : Vector3f v_bf = dcm.transposed() * velocity_ef;
829 13873237 : v_bf.y = 0.0f;
830 13873237 : if (v_bf.x < 0.0f) {
831 10705099 : v_bf.x = 0.0f;
832 : }
833 :
834 13873237 : Vector3f gnd_movement_bf = dcm.transposed() * gnd_movement;
835 :
836 : // lateral speed equals ground movement
837 13873237 : v_bf.y = gnd_movement_bf.y;
838 :
839 13873237 : if (!gnd_movement_bf.is_zero()) {
840 : // fwd speed slowly approaches ground movement to simulate wheel friction
841 0 : const float tconst = 20; // seconds
842 0 : const float alpha = delta_time/(delta_time+tconst/M_2PI);
843 0 : v_bf.x += (gnd_movement.x - v_bf.x) * alpha;
844 : }
845 :
846 13873237 : velocity_ef = dcm * v_bf;
847 13873237 : if (velocity_ef.z > 0.0f) {
848 7501 : velocity_ef.z = 0.0f;
849 : }
850 13873237 : gyro.zero();
851 13873237 : gyro.z = yaw_rate;
852 13873237 : use_smoothing = true;
853 13873237 : break;
854 : }
855 89267 : case GROUND_BEHAVIOR_TAILSITTER: {
856 : // rotate normal refernce frame to get yaw angle, then rotate back
857 89267 : Matrix3f rot;
858 89267 : rot.from_rotation(ROTATION_PITCH_270);
859 : float r, p, y;
860 89267 : (dcm * rot).to_euler(&r, &p, &y);
861 89267 : y = y + yaw_rate * delta_time;
862 89267 : dcm.from_euler(0.0, 0.0, y);
863 89267 : rot.from_rotation(ROTATION_PITCH_90);
864 89267 : dcm *= rot;
865 : // X, Y movement tracks ground movement
866 89267 : velocity_ef.x = gnd_movement.x;
867 89267 : velocity_ef.y = gnd_movement.y;
868 89267 : if (velocity_ef.z > 0.0f) {
869 2 : velocity_ef.z = 0.0f;
870 : }
871 89267 : gyro.zero();
872 89267 : gyro.x = yaw_rate;
873 89267 : use_smoothing = true;
874 89267 : break;
875 : }
876 : }
877 : }
878 :
879 : // update slung payload
880 : #if AP_SIM_SLUNGPAYLOAD_ENABLED
881 169080479 : sitl->models.slung_payload_sim.update(get_position_relhome(), velocity_ef, accel_earth, wind_ef);
882 : #endif
883 :
884 : // update tether
885 : #if AP_SIM_TETHER_ENABLED
886 169080479 : sitl->models.tether_sim.update(location);
887 : #endif
888 :
889 : // allow for changes in physics step
890 169080479 : adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1));
891 169080479 : }
892 :
893 : /*
894 : update wind vector
895 : */
896 166729106 : void Aircraft::update_wind(const struct sitl_input &input)
897 : {
898 : // wind vector in earth frame
899 166729106 : wind_ef = Vector3f(cosf(radians(input.wind.direction))*cosf(radians(input.wind.dir_z)),
900 166729106 : sinf(radians(input.wind.direction))*cosf(radians(input.wind.dir_z)),
901 333458212 : sinf(radians(input.wind.dir_z))) * input.wind.speed;
902 :
903 166729106 : wind_ef.z += get_local_updraft(position + home.get_distance_NED_double(origin));
904 :
905 166729106 : const float wind_turb = input.wind.turbulence * 10.0f; // scale input.wind.turbulence to match standard deviation when using iir_coef=0.98
906 166729106 : const float iir_coef = 0.98f; // filtering high frequencies from turbulence
907 :
908 166729106 : if (wind_turb > 0 && !on_ground()) {
909 :
910 0 : turbulence_azimuth = turbulence_azimuth + (2 * rand());
911 :
912 0 : turbulence_horizontal_speed =
913 0 : static_cast<float>(turbulence_horizontal_speed * iir_coef+wind_turb * rand_normal(0, 1) * (1 - iir_coef));
914 :
915 0 : turbulence_vertical_speed = static_cast<float>((turbulence_vertical_speed * iir_coef) + (wind_turb * rand_normal(0, 1) * (1 - iir_coef)));
916 :
917 0 : wind_ef += Vector3f(
918 0 : cosf(radians(turbulence_azimuth)) * turbulence_horizontal_speed,
919 0 : sinf(radians(turbulence_azimuth)) * turbulence_horizontal_speed,
920 0 : turbulence_vertical_speed);
921 : }
922 :
923 : // the AHRS wants wind with opposite sense
924 166729106 : wind_ef = -wind_ef;
925 166729106 : }
926 :
927 : /*
928 : smooth sensors for kinematic consistancy when we interact with the ground
929 : */
930 159454654 : void Aircraft::smooth_sensors(void)
931 : {
932 159454654 : uint64_t now = time_now_us;
933 159454654 : Vector3d delta_pos = position - smoothing.position;
934 159454654 : if (smoothing.last_update_us == 0 || delta_pos.length() > 10) {
935 1714 : smoothing.position = position;
936 1714 : smoothing.rotation_b2e = dcm;
937 1714 : smoothing.accel_body = accel_body;
938 1714 : smoothing.velocity_ef = velocity_ef;
939 1714 : smoothing.gyro = gyro;
940 1714 : smoothing.last_update_us = now;
941 1714 : smoothing.location = location;
942 1714 : printf("Smoothing reset at %.3f\n", now * 1.0e-6f);
943 1714 : return;
944 : }
945 159452940 : const float delta_time = (now - smoothing.last_update_us) * 1.0e-6f;
946 159452940 : if (delta_time < 0 || delta_time > 0.1) {
947 0 : return;
948 : }
949 :
950 : // calculate required accel to get us to desired position and velocity in the time_constant
951 159452940 : const float time_constant = 0.1f;
952 159452940 : Vector3f dvel = (velocity_ef - smoothing.velocity_ef) + (delta_pos / time_constant).tofloat();
953 159452940 : Vector3f accel_e = dvel / time_constant + (dcm * accel_body + Vector3f(0.0f, 0.0f, GRAVITY_MSS));
954 159452940 : const float accel_limit = 14 * GRAVITY_MSS;
955 159452940 : accel_e.x = constrain_float(accel_e.x, -accel_limit, accel_limit);
956 159452940 : accel_e.y = constrain_float(accel_e.y, -accel_limit, accel_limit);
957 159452940 : accel_e.z = constrain_float(accel_e.z, -accel_limit, accel_limit);
958 159452940 : smoothing.accel_body = smoothing.rotation_b2e.transposed() * (accel_e + Vector3f(0.0f, 0.0f, -GRAVITY_MSS));
959 :
960 : // calculate rotational rate to get us to desired attitude in time constant
961 159452940 : Quaternion desired_q, current_q, error_q;
962 159452940 : desired_q.from_rotation_matrix(dcm);
963 159452940 : desired_q.normalize();
964 159452940 : current_q.from_rotation_matrix(smoothing.rotation_b2e);
965 159452940 : current_q.normalize();
966 159452940 : error_q = desired_q / current_q;
967 159452940 : error_q.normalize();
968 :
969 159452940 : Vector3f angle_differential;
970 159452940 : error_q.to_axis_angle(angle_differential);
971 159452940 : smoothing.gyro = gyro + angle_differential / time_constant;
972 :
973 : float R, P, Y;
974 159452940 : smoothing.rotation_b2e.to_euler(&R, &P, &Y);
975 : float R2, P2, Y2;
976 159452940 : dcm.to_euler(&R2, &P2, &Y2);
977 :
978 : #if 0
979 : // @LoggerMessage: SMOO
980 : // @Description: Smoothed sensor data fed to EKF to avoid inconsistencies
981 : // @Field: TimeUS: Time since system startup
982 : // @Field: AEx: Angular Velocity (around x-axis)
983 : // @Field: AEy: Angular Velocity (around y-axis)
984 : // @Field: AEz: Angular Velocity (around z-axis)
985 : // @Field: DPx: Velocity (along x-axis)
986 : // @Field: DPy: Velocity (along y-axis)
987 : // @Field: DPz: Velocity (along z-axis)
988 : // @Field: R: Roll
989 : // @Field: P: Pitch
990 : // @Field: Y: Yaw
991 : // @Field: R2: DCM Roll
992 : // @Field: P2: DCM Pitch
993 : // @Field: Y2: DCM Yaw
994 : AP::logger().WriteStreaming("SMOO", "TimeUS,AEx,AEy,AEz,DPx,DPy,DPz,R,P,Y,R2,P2,Y2",
995 : "Qffffffffffff",
996 : AP_HAL::micros64(),
997 : degrees(angle_differential.x),
998 : degrees(angle_differential.y),
999 : degrees(angle_differential.z),
1000 : delta_pos.x, delta_pos.y, delta_pos.z,
1001 : degrees(R), degrees(P), degrees(Y),
1002 : degrees(R2), degrees(P2), degrees(Y2));
1003 : #endif
1004 :
1005 :
1006 : // integrate to get new attitude
1007 159452940 : smoothing.rotation_b2e.rotate(smoothing.gyro * delta_time);
1008 159452940 : smoothing.rotation_b2e.normalize();
1009 :
1010 : // integrate to get new position
1011 159452940 : smoothing.velocity_ef += accel_e * delta_time;
1012 159452940 : smoothing.position += (smoothing.velocity_ef * delta_time).todouble();
1013 :
1014 159452940 : smoothing.location = origin;
1015 159452940 : smoothing.location.offset(smoothing.position.x, smoothing.position.y);
1016 159452940 : smoothing.location.alt = static_cast<int32_t>(home.alt - smoothing.position.z * 100.0f);
1017 :
1018 159452940 : smoothing.last_update_us = now;
1019 : }
1020 :
1021 : /*
1022 : return a filtered servo input as a value from -1 to 1
1023 : servo is assumed to be 1000 to 2000, trim at 1500
1024 : */
1025 209084155 : float Aircraft::filtered_servo_angle(const struct sitl_input &input, uint8_t idx)
1026 : {
1027 209084155 : uint16_t pwm = input.servos[idx];
1028 209084155 : if (pwm == 0) {
1029 : // invalid input, servo does not move, apply current value.
1030 : // glad we have a zero-momentum system.
1031 5295439 : pwm = servo_filter[idx].angle_pwm();
1032 : }
1033 209084155 : return servo_filter[idx].filter_angle(pwm, frame_time_us * 1.0e-6);
1034 : }
1035 :
1036 : /*
1037 : return a filtered servo input as a value from 0 to 1
1038 : servo is assumed to be 1000 to 2000
1039 : */
1040 135454372 : float Aircraft::filtered_servo_range(const struct sitl_input &input, uint8_t idx)
1041 : {
1042 135454372 : return servo_filter[idx].filter_range(input.servos[idx], frame_time_us * 1.0e-6);
1043 : }
1044 :
1045 : // setup filtering for servo
1046 7073484 : void Aircraft::filtered_servo_setup(uint8_t idx, uint16_t pwm_min, uint16_t pwm_max, float deflection_deg)
1047 : {
1048 7073484 : servo_filter[idx].set_pwm_range(pwm_min, pwm_max);
1049 7073484 : servo_filter[idx].set_deflection(deflection_deg);
1050 7073484 : }
1051 :
1052 : // extrapolate sensors by a given delta time in seconds
1053 0 : void Aircraft::extrapolate_sensors(float delta_time)
1054 : {
1055 0 : Vector3f accel_earth = dcm * accel_body;
1056 0 : accel_earth.z += GRAVITY_MSS;
1057 :
1058 0 : dcm.rotate(gyro * delta_time);
1059 0 : dcm.normalize();
1060 :
1061 : // work out acceleration as seen by the accelerometers. It sees the kinematic
1062 : // acceleration (ie. real movement), plus gravity
1063 0 : accel_body = dcm.transposed() * (accel_earth + Vector3f(0,0,-GRAVITY_MSS));
1064 :
1065 : // new velocity and position vectors
1066 0 : velocity_ef += accel_earth * delta_time;
1067 0 : position += (velocity_ef * delta_time).todouble();
1068 0 : velocity_air_ef = velocity_ef - wind_ef;
1069 0 : velocity_air_bf = dcm.transposed() * velocity_air_ef;
1070 0 : }
1071 :
1072 81459271 : bool Aircraft::Clamp::clamped(Aircraft &aircraft, const struct sitl_input &input)
1073 : {
1074 81459271 : const auto clamp_ch = AP::sitl()->clamp_ch;
1075 81459271 : if (clamp_ch < 1) {
1076 81122231 : return false;
1077 : }
1078 337040 : const uint32_t clamp_idx = clamp_ch - 1;
1079 337040 : if (clamp_idx > ARRAY_SIZE(input.servos)) {
1080 0 : return false;
1081 : }
1082 337040 : const uint16_t servo_pos = input.servos[clamp_idx];
1083 337040 : bool new_clamped = currently_clamped;
1084 337040 : if (servo_pos == 0) {
1085 : // invalid value, do nothing
1086 278370 : } else if (servo_pos < 1200) {
1087 203845 : if (currently_clamped) {
1088 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: released vehicle");
1089 2 : new_clamped = false;
1090 : }
1091 203845 : grab_attempted = false;
1092 : } else {
1093 : // re-clamp if < 10cm from home
1094 74525 : if (servo_pos > 1800 && !grab_attempted) {
1095 5 : const Vector3d pos = aircraft.get_position_relhome();
1096 5 : const float distance_from_home = pos.length();
1097 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: dist=%f", distance_from_home);
1098 5 : if (distance_from_home < 0.5) {
1099 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: grabbed vehicle");
1100 4 : new_clamped = true;
1101 1 : } else if (!grab_attempted) {
1102 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: missed vehicle");
1103 : }
1104 5 : grab_attempted = true;
1105 : }
1106 : }
1107 :
1108 337040 : currently_clamped = new_clamped;
1109 :
1110 337040 : return currently_clamped;
1111 : }
1112 :
1113 : // simple battery consumption model
1114 : // does not support the behavior documented by SIM_BATT_VOLTAGE and SIM_BATT_CAP_AH parameters
1115 10283911 : void Aircraft::update_battery()
1116 : {
1117 : // lose 0.7V at full throttle (from the user-specified voltage)
1118 10283911 : battery_voltage = sitl->batt_voltage - 0.7f * fabsf(sitl->throttle);
1119 : // assume 50A at full throttle
1120 10283911 : battery_current = 50.0f * fabsf(sitl->throttle);
1121 10283911 : }
1122 :
1123 0 : void Aircraft::update_battery(const struct sitl_input &input)
1124 : {
1125 0 : update_battery();
1126 0 : }
1127 :
1128 178759942 : void Aircraft::update_external_payload(const struct sitl_input &input)
1129 : {
1130 178759942 : external_payload_mass = 0;
1131 :
1132 : #if AP_SIM_SPRAYER_ENABLED
1133 : // update sprayer
1134 178759942 : if (sitl->sprayer_sim.is_enabled()) {
1135 271619 : sitl->sprayer_sim.update(input);
1136 271619 : external_payload_mass += sitl->sprayer_sim.payload_mass();
1137 : }
1138 : #endif // AP_SIM_SPRAYER_ENABLED
1139 :
1140 : {
1141 178759942 : const float range = rangefinder_range();
1142 178759942 : if (!isinf(range) && range > 100000) {
1143 0 : AP_HAL::panic("Bad rangefinder calculation");
1144 : }
1145 1966359362 : for (uint8_t i=0; i<ARRAY_SIZE(rangefinder_m); i++) {
1146 1787599420 : rangefinder_m[i] = range;
1147 : }
1148 : }
1149 :
1150 : // update i2c
1151 178759942 : sitl->i2c_sim.update(*this);
1152 :
1153 : #if AP_SIM_BUZZER_ENABLED
1154 : // update buzzer
1155 178759942 : if (sitl->buzzer_sim.is_enabled()) {
1156 0 : sitl->buzzer_sim.update(input);
1157 : }
1158 : #endif // AP_SIM_BUZZER_ENABLED
1159 :
1160 : // update grippers
1161 : #if AP_SIM_GRIPPER_ENABLED
1162 178759942 : if (sitl->gripper_sim.is_enabled()) {
1163 810128 : sitl->gripper_sim.set_alt(hagl());
1164 810128 : sitl->gripper_sim.update(input);
1165 810128 : external_payload_mass += sitl->gripper_sim.payload_mass();
1166 : }
1167 : #endif // AP_SIM_GRIPPER_ENABLED
1168 : #if AP_SIM_GRIPPER_EPM_ENABLED
1169 178759942 : if (sitl->gripper_epm_sim.is_enabled()) {
1170 0 : sitl->gripper_epm_sim.update(input);
1171 0 : external_payload_mass += sitl->gripper_epm_sim.payload_mass();
1172 : }
1173 : #endif // AP_SIM_GRIPPER_EPM_ENABLED
1174 :
1175 : #if AP_SIM_PARACHUTE_ENABLED
1176 : // update parachute
1177 178759942 : if (sitl->parachute_sim.is_enabled()) {
1178 1215553 : sitl->parachute_sim.update(input);
1179 : // TODO: add drag to vehicle, presumably proportional to velocity
1180 : }
1181 : #endif // AP_SIM_PARACHUTE_ENABLED
1182 :
1183 : #if AP_SIM_PRECLAND_ENABLED
1184 : // update precland
1185 178759942 : if (sitl->precland_sim.is_enabled()) {
1186 946580 : sitl->precland_sim.update(get_location());
1187 946580 : if (sitl->precland_sim._over_precland_base) {
1188 0 : local_ground_level += sitl->precland_sim._device_height;
1189 : }
1190 : }
1191 : #endif // AP_SIM_PRECLAND_ENABLED
1192 :
1193 : // update RichenPower generator
1194 178759942 : if (richenpower) {
1195 113479 : richenpower->update(input);
1196 : }
1197 :
1198 : #if AP_SIM_LOWEHEISER_ENABLED
1199 : // update Loweheiser generator
1200 178759942 : if (loweheiser) {
1201 922454 : loweheiser->update(*this);
1202 : }
1203 : #endif
1204 :
1205 178759942 : if (fetteconewireesc) {
1206 658525 : fetteconewireesc->update(*this);
1207 : }
1208 :
1209 : #if AP_SIM_VOLZ_ENABLED
1210 178759942 : if (volz) {
1211 218651 : volz->update(*this);
1212 : }
1213 : #endif // AP_SIM_VOLZ_ENABLED
1214 :
1215 : #if AP_SIM_SHIP_ENABLED
1216 178759942 : sitl->models.shipsim.update();
1217 : #endif
1218 :
1219 : // update IntelligentEnergy 2.4kW generator
1220 178759942 : if (ie24) {
1221 420295 : ie24->update(input);
1222 : }
1223 :
1224 : #if AP_TEST_DRONECAN_DRIVERS
1225 : sitl->dronecan_sim.update();
1226 : #endif
1227 :
1228 : #if AP_SIM_GPIO_LED_1_ENABLED
1229 : sim_led1.update(*this);
1230 : #endif
1231 : #if AP_SIM_GPIO_LED_2_ENABLED
1232 : sim_led2.update(*this);
1233 : #endif
1234 : #if AP_SIM_GPIO_LED_3_ENABLED
1235 : sim_led3.update(*this);
1236 : #endif
1237 : #if AP_SIM_GPIO_LED_RGB_ENABLED
1238 : sim_ledrgb.update(*this);
1239 : #endif
1240 :
1241 : #if AP_SIM_MOUNT_ENABLED
1242 1608839478 : for (uint8_t i = 0; i < GIMBAL_SIM_MAX; i++) {
1243 1430079536 : if (gimbal_sims[i] != nullptr) {
1244 844397 : gimbal_sims[i]->update(*this);
1245 : }
1246 : }
1247 : #endif
1248 178759942 : }
1249 :
1250 88212468 : void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel)
1251 : {
1252 88212468 : const uint32_t now = AP_HAL::millis();
1253 88212468 : if (sitl == nullptr) {
1254 0 : return;
1255 : }
1256 88212468 : if (sitl->shove.t == 0) {
1257 88138634 : return;
1258 : }
1259 73834 : if (sitl->shove.start_ms == 0) {
1260 5 : sitl->shove.start_ms = now;
1261 : }
1262 73834 : if (now - sitl->shove.start_ms < uint32_t(sitl->shove.t)) {
1263 : // FIXME: can we get a vector operation here instead?
1264 73829 : body_accel.x += sitl->shove.x;
1265 73829 : body_accel.y += sitl->shove.y;
1266 73829 : body_accel.z += sitl->shove.z;
1267 : } else {
1268 5 : sitl->shove.start_ms = 0;
1269 : // save as well as set: the parameter was written to storage to
1270 : // ask for the shove, so clearing only the live value leaves
1271 : // storage still asking for one. A GCS - or the test suite
1272 : // putting parameters back after a test - then sees the live
1273 : // value already at zero and has no reason to write, and the
1274 : // next reboot loads the old duration and shoves again.
1275 5 : sitl->shove.t.set_and_save(0);
1276 : }
1277 : }
1278 :
1279 166729106 : float Aircraft::get_local_updraft(const Vector3d ¤tPos)
1280 : {
1281 166729106 : int scenario = sitl->thermal_scenario;
1282 :
1283 : #define MAX_THERMALS 10
1284 :
1285 : float thermals_w[MAX_THERMALS];
1286 : float thermals_r[MAX_THERMALS];
1287 : float thermals_x[MAX_THERMALS];
1288 : float thermals_y[MAX_THERMALS];
1289 :
1290 166729106 : int n_thermals = 0;
1291 :
1292 166729106 : switch (scenario) {
1293 165658798 : case 0:
1294 165658798 : return 0;
1295 1070308 : case 1:
1296 1070308 : n_thermals = 1;
1297 1070308 : thermals_w[0] = 2.0;
1298 1070308 : thermals_r[0] = 80.0;
1299 1070308 : thermals_x[0] = -180.0;
1300 1070308 : thermals_y[0] = -260.0;
1301 1070308 : break;
1302 0 : case 2:
1303 0 : n_thermals = 1;
1304 0 : thermals_w[0] = 4.0;
1305 0 : thermals_r[0] = 30.0;
1306 0 : thermals_x[0] = -180.0;
1307 0 : thermals_y[0] = -260.0;
1308 0 : break;
1309 0 : case 3:
1310 0 : n_thermals = 1;
1311 0 : thermals_w[0] = 2.0;
1312 0 : thermals_r[0] = 30.0;
1313 0 : thermals_x[0] = -180.0;
1314 0 : thermals_y[0] = -260.0;
1315 0 : break;
1316 0 : case 4:
1317 0 : n_thermals = 1;
1318 0 : thermals_w[0] = 5.0;
1319 0 : thermals_r[0] = 30.0;
1320 0 : thermals_x[0] = 0;
1321 0 : thermals_y[0] = 0;
1322 0 : break;
1323 0 : default:
1324 0 : AP_BoardConfig::config_error("Bad thermal scenario");
1325 : }
1326 :
1327 : // Wind drift at this altitude
1328 1070308 : float driftX = sitl->wind_speed * (currentPos.z+100) * cosf(sitl->wind_direction * DEG_TO_RAD);
1329 1070308 : float driftY = sitl->wind_speed * (currentPos.z+100) * sinf(sitl->wind_direction * DEG_TO_RAD);
1330 :
1331 : int iThermal;
1332 1070308 : float w = 0.0f;
1333 : float r2;
1334 2140616 : for (iThermal=0;iThermal<n_thermals;iThermal++) {
1335 1070308 : Vector3d thermalPos(thermals_x[iThermal] + driftX/thermals_w[iThermal],
1336 1070308 : thermals_y[iThermal] + driftY/thermals_w[iThermal],
1337 1070308 : 0);
1338 1070308 : Vector3d relVec = currentPos - thermalPos;
1339 1070308 : r2 = relVec.x*relVec.x + relVec.y*relVec.y;
1340 1070308 : w += thermals_w[iThermal]*exp(-r2/(thermals_r[iThermal]*thermals_r[iThermal]));
1341 : }
1342 :
1343 1070308 : return w;
1344 : }
1345 :
1346 81459271 : void Aircraft::add_twist_forces(Vector3f &rot_accel)
1347 : {
1348 81459271 : if (sitl == nullptr) {
1349 0 : return;
1350 : }
1351 81459271 : if (sitl->gnd_behav != -1) {
1352 1048 : ground_behavior = (GroundBehaviour)sitl->gnd_behav.get();
1353 : }
1354 81459271 : const uint32_t now = AP_HAL::millis();
1355 81459271 : if (sitl == nullptr) {
1356 0 : return;
1357 : }
1358 81459271 : if (sitl->twist.t == 0) {
1359 81333134 : return;
1360 : }
1361 126137 : if (sitl->twist.start_ms == 0) {
1362 101 : sitl->twist.start_ms = now;
1363 : }
1364 126137 : if (now - sitl->twist.start_ms < uint32_t(sitl->twist.t)) {
1365 : // FIXME: can we get a vector operation here instead?
1366 126102 : rot_accel.x += sitl->twist.x;
1367 126102 : rot_accel.y += sitl->twist.y;
1368 126102 : rot_accel.z += sitl->twist.z;
1369 : } else {
1370 35 : sitl->twist.start_ms = 0;
1371 35 : sitl->twist.t.set(0);
1372 : }
1373 : }
1374 :
1375 : // add body-frame force due to slung payload and tether
1376 81459271 : void Aircraft::add_external_forces(Vector3f &body_accel)
1377 : {
1378 81459271 : Vector3f total_force;
1379 : #if AP_SIM_SLUNGPAYLOAD_ENABLED
1380 81459271 : Vector3f forces_ef_slung;
1381 81459271 : sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef_slung);
1382 81459271 : total_force += forces_ef_slung;
1383 : #endif
1384 :
1385 : #if AP_SIM_TETHER_ENABLED
1386 81459271 : Vector3f forces_ef_tether;
1387 81459271 : sitl->models.tether_sim.get_forces_on_vehicle(forces_ef_tether);
1388 81459271 : total_force += forces_ef_tether;
1389 : #endif
1390 :
1391 : // convert ef forces to body-frame accelerations (acceleration = force / mass)
1392 81459271 : const Vector3f accel_bf_tether = dcm.transposed() * total_force / mass;
1393 81459271 : body_accel += accel_bf_tether;
1394 81459271 : }
1395 :
1396 : /*
1397 : get position relative to home
1398 : */
1399 398928352 : Vector3d Aircraft::get_position_relhome() const
1400 : {
1401 398928352 : Vector3d pos = position;
1402 398928352 : pos.xy() += home.get_distance_NE_double(origin);
1403 398928352 : return pos;
1404 : }
1405 :
1406 : // get air density in kg/m^3
1407 0 : float Aircraft::get_air_density(float alt_amsl) const
1408 : {
1409 0 : return AP_Baro::get_air_density_for_alt_amsl(alt_amsl);
1410 : }
1411 :
1412 : /*
1413 : update EAS airspeed and pitot speed
1414 : */
1415 169080479 : void Aircraft::update_eas_airspeed()
1416 : {
1417 169080479 : airspeed = velocity_air_ef.length() / eas2tas;
1418 :
1419 : /*
1420 : airspeed as seen by a fwd pitot tube (limited to 120m/s)
1421 : */
1422 169080479 : airspeed_pitot = airspeed;
1423 :
1424 : // calculate angle between the local flow vector and a pitot tube aligned with the X body axis
1425 169080479 : const float pitot_aoa = atan2f(sqrtf(sq(velocity_air_bf.y) + sq(velocity_air_bf.z)), velocity_air_bf.x);
1426 :
1427 : /*
1428 : assume the pitot can correctly capture airspeed up to 20 degrees off the nose
1429 : and follows a cose law outside that range
1430 : */
1431 169080479 : const float max_pitot_aoa = radians(20);
1432 169080479 : if (pitot_aoa > radians(90)) {
1433 45049860 : airspeed_pitot = 0;
1434 124030619 : } else if (pitot_aoa > max_pitot_aoa) {
1435 39717605 : const float gain_factor = M_PI_2 / (radians(90) - max_pitot_aoa);
1436 39717605 : airspeed_pitot *= cosf((pitot_aoa - max_pitot_aoa) * gain_factor);
1437 : }
1438 169080479 : }
1439 :
1440 : /*
1441 : set pose on the aircraft, called from scripting
1442 : */
1443 80 : bool Aircraft::set_pose(uint8_t instance, const Location &loc, const Quaternion &quat, const Vector3f &velocity_ef, const Vector3f &gyro_rads)
1444 : {
1445 80 : if (instance >= MAX_SIM_INSTANCES || instances[instance] == nullptr) {
1446 0 : return false;
1447 : }
1448 80 : auto &aircraft = *instances[instance];
1449 80 : WITH_SEMAPHORE(aircraft.pose_sem);
1450 :
1451 80 : quat.rotation_matrix(aircraft.dcm);
1452 80 : aircraft.velocity_ef = velocity_ef;
1453 80 : aircraft.location = loc;
1454 80 : aircraft.position = aircraft.home.get_distance_NED_double(loc);
1455 80 : aircraft.smoothing.position = aircraft.position;
1456 80 : aircraft.smoothing.rotation_b2e = aircraft.dcm;
1457 80 : aircraft.smoothing.velocity_ef = velocity_ef;
1458 80 : aircraft.smoothing.location = loc;
1459 80 : aircraft.gyro = gyro_rads;
1460 :
1461 80 : return true;
1462 80 : }
1463 :
1464 : /*
1465 : wrapper for scripting access
1466 : */
1467 80 : bool SITL::SIM::set_pose(uint8_t instance, const Location &loc, const Quaternion &quat, const Vector3f &velocity_ef, const Vector3f &gyro_rads)
1468 : {
1469 80 : return Aircraft::set_pose(instance, loc, quat, velocity_ef, gyro_rads);
1470 : }
1471 :
|
