APM:Libraries
SIM_Aircraft.cpp
Go to the documentation of this file.
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 
26 #if defined(__CYGWIN__) || defined(__CYGWIN64__)
27 #include <windows.h>
28 #include <time.h>
29 #include <mmsystem.h>
30 #endif
31 
32 #include <DataFlash/DataFlash.h>
33 #include <AP_Param/AP_Param.h>
34 
35 using namespace SITL;
36 
37 /*
38  parent class for all simulator types
39  */
40 
41 Aircraft::Aircraft(const char *home_str, const char *frame_str) :
42  ground_level(0.0f),
43  frame_height(0.0f),
44  dcm(),
45  gyro(),
46  gyro_prev(),
47  ang_accel(),
48  velocity_ef(),
49  mass(0.0f),
50  accel_body(0.0f, 0.0f, -GRAVITY_MSS),
51  time_now_us(0),
52  gyro_noise(radians(0.1f)),
53  accel_noise(0.3f),
54  rate_hz(1200.0f),
55  autotest_dir(nullptr),
56  frame(frame_str),
57 #if defined(__CYGWIN__) || defined(__CYGWIN64__)
58  min_sleep_time(20000)
59 #else
60  min_sleep_time(5000)
61 #endif
62 {
63  // make the SIM_* variables available to simulator backends
64  sitl = (SITL *)AP_Param::find_object("SIM_");
65 
66  if (!parse_home(home_str, home, home_yaw)) {
67  ::printf("Failed to parse home string (%s). Should be LAT,LON,ALT,HDG e.g. 37.4003371,-122.0800351,0,353\n", home_str);
68  }
69  ::printf("Home: %f %f alt=%fm hdg=%f\n",
70  home.lat*1e-7,
71  home.lng*1e-7,
72  home.alt*0.01,
73  home_yaw);
74  location = home;
75  ground_level = home.alt * 0.01f;
76 
77  dcm.from_euler(0.0f, 0.0f, radians(home_yaw));
78 
79  set_speedup(1.0f);
80 
82  frame_counter = 0;
83 
84  // allow for orientation settings, such as with tailsitters
85  enum ap_var_type ptype;
86  ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype);
87 
88  terrain = (AP_Terrain *)AP_Param::find_object("TERRAIN_");
89 }
90 
91 
92 /*
93  parse a home string into a location and yaw
94  */
95 bool Aircraft::parse_home(const char *home_str, Location &loc, float &yaw_degrees)
96 {
97  char *saveptr = nullptr;
98  char *s = strdup(home_str);
99  if (!s) {
100  free(s);
101  ::printf("No home string supplied\n");
102  return false;
103  }
104  char *lat_s = strtok_r(s, ",", &saveptr);
105  if (!lat_s) {
106  free(s);
107  ::printf("Failed to parse latitude\n");
108  return false;
109  }
110  char *lon_s = strtok_r(nullptr, ",", &saveptr);
111  if (!lon_s) {
112  free(s);
113  ::printf("Failed to parse longitude\n");
114  return false;
115  }
116  char *alt_s = strtok_r(nullptr, ",", &saveptr);
117  if (!alt_s) {
118  free(s);
119  ::printf("Failed to parse altitude\n");
120  return false;
121  }
122  char *yaw_s = strtok_r(nullptr, ",", &saveptr);
123  if (!yaw_s) {
124  free(s);
125  ::printf("Failed to parse yaw\n");
126  return false;
127  }
128 
129  memset(&loc, 0, sizeof(loc));
130  loc.lat = static_cast<int32_t>(strtof(lat_s, nullptr) * 1.0e7f);
131  loc.lng = static_cast<int32_t>(strtof(lon_s, nullptr) * 1.0e7f);
132  loc.alt = static_cast<int32_t>(strtof(alt_s, nullptr) * 1.0e2f);
133 
134  if (loc.lat == 0 && loc.lng == 0) {
135  // default to CMAC instead of middle of the ocean. This makes
136  // SITL in MissionPlanner a bit more useful
137  loc.lat = -35.363261*1e7;
138  loc.lng = 149.165230*1e7;
139  loc.alt = 584*100;
140  }
141 
142  yaw_degrees = strtof(yaw_s, nullptr);
143  free(s);
144 
145  return true;
146 }
147 
148 /*
149  return difference in altitude between home position and current loc
150 */
152 {
153  float h1, h2;
154  if (sitl &&
156  terrain->height_amsl(home, h1, false) &&
157  terrain->height_amsl(location, h2, false)) {
158  return h2 - h1;
159  }
160  return 0.0f;
161 }
162 
163 /*
164  return current height above ground level (metres)
165 */
166 float Aircraft::hagl() const
167 {
169 }
170 /*
171  return true if we are on the ground
172 */
174 {
175  return hagl() <= 0;
176 }
177 
178 /*
179  update location from position
180 */
182 {
183  location = home;
185 
186  location.alt = static_cast<int32_t>(home.alt - position.z * 100.0f);
187 
188 #if 0
189  // logging of raw sitl data
190  Vector3f accel_ef = dcm * accel_body;
191  DataFlash_Class::instance()->Log_Write("SITL", "TimeUS,VN,VE,VD,AN,AE,AD,PN,PE,PD", "Qfffffffff",
194  accel_ef.x, accel_ef.y, accel_ef.z,
196 #endif
197 }
198 
199 /*
200  update body magnetic field from position and rotation
201 */
203 {
204  // get the magnetic field intensity and orientation
205  float intensity;
206  float declination;
207  float inclination;
208  AP_Declination::get_mag_field_ef(location.lat * 1e-7f, location.lng * 1e-7f, intensity, declination, inclination);
209 
210  // create a field vector and rotate to the required orientation
211  Vector3f mag_ef(1e3f * intensity, 0.0f, 0.0f);
212  Matrix3f R;
213  R.from_euler(0.0f, -ToRad(inclination), ToRad(declination));
214  mag_ef = R * mag_ef;
215 
216  // calculate frame height above ground
217  const float frame_height_agl = fmaxf((-position.z) + home.alt * 0.01f - ground_level, 0.0f);
218 
219  // calculate scaling factor that varies from 1 at ground level to 1/8 at sitl->mag_anomaly_hgt
220  // Assume magnetic anomaly strength scales with 1/R**3
221  float anomaly_scaler = (sitl->mag_anomaly_hgt / (frame_height_agl + sitl->mag_anomaly_hgt));
222  anomaly_scaler = anomaly_scaler * anomaly_scaler * anomaly_scaler;
223 
224  // add scaled anomaly to earth field
225  mag_ef += sitl->mag_anomaly_ned.get() * anomaly_scaler;
226 
227  // Rotate into body frame
228  mag_bf = dcm.transposed() * mag_ef;
229 
230  // add motor interference
231  mag_bf += sitl->mag_mot.get() * battery_current;
232 }
233 
234 /* advance time by deltat in seconds */
236 {
237  // we only advance time if it hasn't been advanced already by the
238  // backend
239  if (last_time_us == time_now_us) {
241  }
243  if (use_time_sync) {
244  sync_frame_time();
245  }
246 }
247 
248 /* setup the frame step time */
249 void Aircraft::setup_frame_time(float new_rate, float new_speedup)
250 {
251  rate_hz = new_rate;
252  target_speedup = new_speedup;
253  frame_time_us = static_cast<uint64_t>(1.0e6f/rate_hz);
254 
258 }
259 
260 /* adjust frame_time calculation */
261 void Aircraft::adjust_frame_time(float new_rate)
262 {
263  if (rate_hz != new_rate) {
264  rate_hz = new_rate;
265  frame_time_us = static_cast<uint64_t>(1.0e6f/rate_hz);
267  }
268 }
269 
270 /*
271  try to synchronise simulation time with wall clock time, taking
272  into account desired speedup
273  This tries to take account of possible granularity of
274  get_wall_time_us() so it works reasonably well on windows
275 */
277 {
278  frame_counter++;
279  uint64_t now = get_wall_time_us();
280  if (frame_counter >= 40 &&
281  now > last_wall_time_us) {
282  const float rate = frame_counter * 1.0e6f/(now - last_wall_time_us);
283  achieved_rate_hz = (0.99f*achieved_rate_hz) + (0.01f * rate);
285  scaled_frame_time_us *= 0.999f;
286  } else {
287  scaled_frame_time_us /= 0.999f;
288  }
289 #if 0
290  ::printf("achieved_rate_hz=%.3f rate=%.2f rate_hz=%.3f sft=%.1f\n",
291  static_cast<double>(achieved_rate_hz),
292  static_cast<double>(rate),
293  static_cast<double>(rate_hz),
294  static_cast<double>(scaled_frame_time_us));
295 #endif
296  const uint32_t sleep_time = static_cast<uint32_t>(scaled_frame_time_us * frame_counter);
297  if (sleep_time > min_sleep_time) {
298  usleep(sleep_time);
299  }
300  last_wall_time_us = now;
301  frame_counter = 0;
302  }
303 }
304 
305 /* add noise based on throttle level (from 0..1) */
306 void Aircraft::add_noise(float throttle)
307 {
308  gyro += Vector3f(rand_normal(0, 1),
309  rand_normal(0, 1),
310  rand_normal(0, 1)) * gyro_noise * fabsf(throttle);
312  rand_normal(0, 1),
313  rand_normal(0, 1)) * accel_noise * fabsf(throttle);
314 }
315 
316 /*
317  normal distribution random numbers
318  See
319  http://en.literateprograms.org/index.php?title=Special:DownloadCode/Box-Muller_transform_%28C%29&oldid=7011
320 */
321 double Aircraft::rand_normal(double mean, double stddev)
322 {
323  static double n2 = 0.0;
324  static int n2_cached = 0;
325  if (!n2_cached) {
326  double x, y, r;
327  do
328  {
329  x = 2.0 * rand()/RAND_MAX - 1;
330  y = 2.0 * rand()/RAND_MAX - 1;
331  r = x*x + y*y;
332  } while (is_zero(r) || r > 1.0);
333  const double d = sqrt(-2.0 * log(r)/r);
334  const double n1 = x * d;
335  n2 = y * d;
336  const double result = n1 * stddev + mean;
337  n2_cached = 1;
338  return result;
339  } else {
340  n2_cached = 0;
341  return n2 * stddev + mean;
342  }
343 }
344 
345 
346 
347 
348 /*
349  fill a sitl_fdm structure from the simulator state
350 */
351 void Aircraft::fill_fdm(struct sitl_fdm &fdm)
352 {
353  if (use_smoothing) {
354  smooth_sensors();
355  }
357  if (fdm.home.lat == 0 && fdm.home.lng == 0) {
358  // initialise home
359  fdm.home = home;
360  }
361  fdm.latitude = location.lat * 1.0e-7;
362  fdm.longitude = location.lng * 1.0e-7;
363  fdm.altitude = location.alt * 1.0e-2;
364  fdm.heading = degrees(atan2f(velocity_ef.y, velocity_ef.x));
365  fdm.speedN = velocity_ef.x;
366  fdm.speedE = velocity_ef.y;
367  fdm.speedD = velocity_ef.z;
368  fdm.xAccel = accel_body.x;
369  fdm.yAccel = accel_body.y;
370  fdm.zAccel = accel_body.z;
371  fdm.rollRate = degrees(gyro.x);
372  fdm.pitchRate = degrees(gyro.y);
373  fdm.yawRate = degrees(gyro.z);
374  fdm.angAccel.x = degrees(ang_accel.x);
375  fdm.angAccel.y = degrees(ang_accel.y);
376  fdm.angAccel.z = degrees(ang_accel.z);
377  float r, p, y;
378  dcm.to_euler(&r, &p, &y);
379  fdm.rollDeg = degrees(r);
380  fdm.pitchDeg = degrees(p);
381  fdm.yawDeg = degrees(y);
383  fdm.airspeed = airspeed_pitot;
386  fdm.rpm1 = rpm1;
387  fdm.rpm2 = rpm2;
389  fdm.range = range;
390  memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float));
391  fdm.bodyMagField = mag_bf;
392 
393  if (smoothing.enabled) {
394  fdm.xAccel = smoothing.accel_body.x;
395  fdm.yAccel = smoothing.accel_body.y;
396  fdm.zAccel = smoothing.accel_body.z;
397  fdm.rollRate = degrees(smoothing.gyro.x);
398  fdm.pitchRate = degrees(smoothing.gyro.y);
399  fdm.yawRate = degrees(smoothing.gyro.z);
400  fdm.speedN = smoothing.velocity_ef.x;
401  fdm.speedE = smoothing.velocity_ef.y;
402  fdm.speedD = smoothing.velocity_ef.z;
403  fdm.latitude = smoothing.location.lat * 1.0e-7;
404  fdm.longitude = smoothing.location.lng * 1.0e-7;
405  fdm.altitude = smoothing.location.alt * 1.0e-2;
406  }
407 
408  if (ahrs_orientation != nullptr) {
409  enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get();
410 
411  if (imu_rotation != ROTATION_NONE) {
412  Matrix3f m = dcm;
413  Matrix3f rot;
414  rot.from_rotation(imu_rotation);
415  m = m * rot.transposed();
416 
417  m.to_euler(&r, &p, &y);
418  fdm.rollDeg = degrees(r);
419  fdm.pitchDeg = degrees(p);
420  fdm.yawDeg = degrees(y);
422  }
423  }
424 
425  if (last_speedup != sitl->speedup && sitl->speedup > 0) {
428  }
429 }
430 
432 {
433 #if defined(__CYGWIN__) || defined(__CYGWIN64__)
434  static DWORD tPrev;
435  static uint64_t last_ret_us;
436  if (tPrev == 0) {
437  tPrev = timeGetTime();
438  return 0;
439  }
440  DWORD now = timeGetTime();
441  last_ret_us += (uint64_t)((now - tPrev)*1000UL);
442  tPrev = now;
443  return last_ret_us;
444 #else
445  struct timeval tp;
446  gettimeofday(&tp, nullptr);
447  return static_cast<uint64_t>(tp.tv_sec * 1.0e6 + tp.tv_usec);
448 #endif
449 }
450 
451 /*
452  set simulation speedup
453  */
454 void Aircraft::set_speedup(float speedup)
455 {
456  setup_frame_time(rate_hz, speedup);
457 }
458 
459 /*
460  update the simulation attitude and relative position
461  */
462 void Aircraft::update_dynamics(const Vector3f &rot_accel)
463 {
464  const float delta_time = frame_time_us * 1.0e-6f;
465 
466  // update rotational rates in body frame
467  gyro += rot_accel * delta_time;
468 
469  gyro.x = constrain_float(gyro.x, -radians(2000.0f), radians(2000.0f));
470  gyro.y = constrain_float(gyro.y, -radians(2000.0f), radians(2000.0f));
471  gyro.z = constrain_float(gyro.z, -radians(2000.0f), radians(2000.0f));
472 
473  // estimate angular acceleration using a first order difference calculation
474  // TODO the simulator interface should provide the angular acceleration
475  ang_accel = (gyro - gyro_prev) / delta_time;
476  gyro_prev = gyro;
477 
478  // update attitude
479  dcm.rotate(gyro * delta_time);
480  dcm.normalize();
481 
482  Vector3f accel_earth = dcm * accel_body;
483  accel_earth += Vector3f(0.0f, 0.0f, GRAVITY_MSS);
484 
485  // if we're on the ground, then our vertical acceleration is limited
486  // to zero. This effectively adds the force of the ground on the aircraft
487  if (on_ground() && accel_earth.z > 0) {
488  accel_earth.z = 0;
489  }
490 
491  // work out acceleration as seen by the accelerometers. It sees the kinematic
492  // acceleration (ie. real movement), plus gravity
493  accel_body = dcm.transposed() * (accel_earth + Vector3f(0.0f, 0.0f, -GRAVITY_MSS));
494 
495  // new velocity vector
496  velocity_ef += accel_earth * delta_time;
497 
498  const bool was_on_ground = on_ground();
499  // new position vector
500  position += velocity_ef * delta_time;
501 
502  // velocity relative to air mass, in earth frame
504 
505  // velocity relative to airmass in body frame
507 
508  // airspeed
510 
511  // airspeed as seen by a fwd pitot tube (limited to 120m/s)
512  airspeed_pitot = constrain_float(velocity_air_bf * Vector3f(1.0f, 0.0f, 0.0f), 0.0f, 120.0f);
513 
514  // constrain height to the ground
515  if (on_ground()) {
516  if (!was_on_ground && AP_HAL::millis() - last_ground_contact_ms > 1000) {
517  printf("Hit ground at %f m/s\n", velocity_ef.z);
519  }
521 
522  switch (ground_behavior) {
524  break;
526  // zero roll/pitch, but keep yaw
527  float r, p, y;
528  dcm.to_euler(&r, &p, &y);
529  dcm.from_euler(0.0f, 0.0f, y);
530  // no X or Y movement
531  velocity_ef.x = 0.0f;
532  velocity_ef.y = 0.0f;
533  if (velocity_ef.z > 0.0f) {
534  velocity_ef.z = 0.0f;
535  }
536  gyro.zero();
537  use_smoothing = true;
538  break;
539  }
541  // zero roll/pitch, but keep yaw
542  float r, p, y;
543  dcm.to_euler(&r, &p, &y);
544  dcm.from_euler(0.0f, 0.0f, y);
545  // only fwd movement
546  Vector3f v_bf = dcm.transposed() * velocity_ef;
547  v_bf.y = 0.0f;
548  if (v_bf.x < 0.0f) {
549  v_bf.x = 0.0f;
550  }
551  velocity_ef = dcm * v_bf;
552  if (velocity_ef.z > 0.0f) {
553  velocity_ef.z = 0.0f;
554  }
555  gyro.zero();
556  use_smoothing = true;
557  break;
558  }
560  // point straight up
561  float r, p, y;
562  dcm.to_euler(&r, &p, &y);
563  dcm.from_euler(0.0f, radians(90), y);
564  // no movement
565  if (accel_earth.z > -1.1*GRAVITY_MSS) {
566  velocity_ef.zero();
567  }
568  gyro.zero();
569  use_smoothing = true;
570  break;
571  }
572  }
573  }
574 }
575 
576 /*
577  update wind vector
578 */
579 void Aircraft::update_wind(const struct sitl_input &input)
580 {
581  // wind vector in earth frame
582  wind_ef = Vector3f(cosf(radians(input.wind.direction))*cosf(radians(input.wind.dir_z)),
583  sinf(radians(input.wind.direction))*cosf(radians(input.wind.dir_z)),
584  sinf(radians(input.wind.dir_z))) * input.wind.speed;
585 
586  const float wind_turb = input.wind.turbulence * 10.0f; // scale input.wind.turbulence to match standard deviation when using iir_coef=0.98
587  const float iir_coef = 0.98f; // filtering high frequencies from turbulence
588 
589  if (wind_turb > 0 && !on_ground()) {
590 
591  turbulence_azimuth = turbulence_azimuth + (2 * rand());
592 
594  static_cast<float>(turbulence_horizontal_speed * iir_coef+wind_turb * rand_normal(0, 1) * (1 - iir_coef));
595 
596  turbulence_vertical_speed = static_cast<float>((turbulence_vertical_speed * iir_coef) + (wind_turb * rand_normal(0, 1) * (1 - iir_coef)));
597 
598  wind_ef += Vector3f(
599  cosf(radians(turbulence_azimuth)) * turbulence_horizontal_speed,
600  sinf(radians(turbulence_azimuth)) * turbulence_horizontal_speed,
602  }
603 }
604 
605 /*
606  smooth sensors for kinematic consistancy when we interact with the ground
607  */
609 {
610  uint64_t now = time_now_us;
611  Vector3f delta_pos = position - smoothing.position;
612  if (smoothing.last_update_us == 0 || delta_pos.length() > 10) {
613  smoothing.position = position;
614  smoothing.rotation_b2e = dcm;
615  smoothing.accel_body = accel_body;
616  smoothing.velocity_ef = velocity_ef;
617  smoothing.gyro = gyro;
618  smoothing.last_update_us = now;
619  smoothing.location = location;
620  printf("Smoothing reset at %.3f\n", now * 1.0e-6f);
621  return;
622  }
623  const float delta_time = (now - smoothing.last_update_us) * 1.0e-6f;
624  if (delta_time < 0 || delta_time > 0.1) {
625  return;
626  }
627 
628  // calculate required accel to get us to desired position and velocity in the time_constant
629  const float time_constant = 0.1f;
630  Vector3f dvel = (velocity_ef - smoothing.velocity_ef) + (delta_pos / time_constant);
631  Vector3f accel_e = dvel / time_constant + (dcm * accel_body + Vector3f(0.0f, 0.0f, GRAVITY_MSS));
632  const float accel_limit = 14 * GRAVITY_MSS;
633  accel_e.x = constrain_float(accel_e.x, -accel_limit, accel_limit);
634  accel_e.y = constrain_float(accel_e.y, -accel_limit, accel_limit);
635  accel_e.z = constrain_float(accel_e.z, -accel_limit, accel_limit);
636  smoothing.accel_body = smoothing.rotation_b2e.transposed() * (accel_e + Vector3f(0.0f, 0.0f, -GRAVITY_MSS));
637 
638  // calculate rotational rate to get us to desired attitude in time constant
639  Quaternion desired_q, current_q, error_q;
640  desired_q.from_rotation_matrix(dcm);
641  desired_q.normalize();
642  current_q.from_rotation_matrix(smoothing.rotation_b2e);
643  current_q.normalize();
644  error_q = desired_q / current_q;
645  error_q.normalize();
646 
647  Vector3f angle_differential;
648  error_q.to_axis_angle(angle_differential);
649  smoothing.gyro = gyro + angle_differential / time_constant;
650 
651  float R, P, Y;
652  smoothing.rotation_b2e.to_euler(&R, &P, &Y);
653  float R2, P2, Y2;
654  dcm.to_euler(&R2, &P2, &Y2);
655 
656 #if 0
657  DataFlash_Class::instance()->Log_Write("SMOO", "TimeUS,AEx,AEy,AEz,DPx,DPy,DPz,R,P,Y,R2,P2,Y2",
658  "Qffffffffffff",
660  degrees(angle_differential.x),
661  degrees(angle_differential.y),
662  degrees(angle_differential.z),
663  delta_pos.x, delta_pos.y, delta_pos.z,
664  degrees(R), degrees(P), degrees(Y),
665  degrees(R2), degrees(P2), degrees(Y2));
666 #endif
667 
668 
669  // integrate to get new attitude
670  smoothing.rotation_b2e.rotate(smoothing.gyro * delta_time);
671  smoothing.rotation_b2e.normalize();
672 
673  // integrate to get new position
674  smoothing.velocity_ef += accel_e * delta_time;
675  smoothing.position += smoothing.velocity_ef * delta_time;
676 
677  smoothing.location = home;
678  location_offset(smoothing.location, smoothing.position.x, smoothing.position.y);
679  smoothing.location.alt = static_cast<int32_t>(home.alt - smoothing.position.z * 100.0f);
680 
681  smoothing.last_update_us = now;
682  smoothing.enabled = true;
683 }
684 
685 /*
686  return a filtered servo input as a value from -1 to 1
687  servo is assumed to be 1000 to 2000, trim at 1500
688  */
689 float Aircraft::filtered_idx(float v, uint8_t idx)
690 {
691  if (sitl->servo_speed <= 0) {
692  return v;
693  }
694  const float cutoff = 1.0f / (2 * M_PI * sitl->servo_speed);
695  servo_filter[idx].set_cutoff_frequency(cutoff);
696  return servo_filter[idx].apply(v, frame_time_us * 1.0e-6f);
697 }
698 
699 
700 /*
701  return a filtered servo input as a value from -1 to 1
702  servo is assumed to be 1000 to 2000, trim at 1500
703  */
704 float Aircraft::filtered_servo_angle(const struct sitl_input &input, uint8_t idx)
705 {
706  const float v = (input.servos[idx] - 1500)/500.0f;
707  return filtered_idx(v, idx);
708 }
709 
710 /*
711  return a filtered servo input as a value from 0 to 1
712  servo is assumed to be 1000 to 2000
713  */
714 float Aircraft::filtered_servo_range(const struct sitl_input &input, uint8_t idx)
715 {
716  const float v = (input.servos[idx] - 1000)/1000.0f;
717  return filtered_idx(v, idx);
718 }
719 
720 // extrapolate sensors by a given delta time in seconds
721 void Aircraft::extrapolate_sensors(float delta_time)
722 {
723  Vector3f accel_earth = dcm * accel_body;
724  accel_earth.z += GRAVITY_MSS;
725 
726  dcm.rotate(gyro * delta_time);
727  dcm.normalize();
728 
729  // work out acceleration as seen by the accelerometers. It sees the kinematic
730  // acceleration (ie. real movement), plus gravity
731  accel_body = dcm.transposed() * (accel_earth + Vector3f(0,0,-GRAVITY_MSS));
732 
733  // new velocity and position vectors
734  velocity_ef += accel_earth * delta_time;
735  position += velocity_ef * delta_time;
738 }
739 
740 
Vector3f accel_body
Definition: SIM_Aircraft.h:142
double range
Definition: SITL.h:29
void to_euler(float *roll, float *pitch, float *yaw) const
Definition: matrix3.cpp:49
AP_Vector3f mag_anomaly_ned
Definition: SITL.h:174
int printf(const char *fmt,...)
Definition: stdio.c:113
Vector3f position
Definition: SIM_Aircraft.h:140
float rcin[8]
Definition: SITL.h:28
static AP_Param * find_object(const char *name)
Definition: AP_Param.cpp:939
uint64_t timestamp_us
Definition: SITL.h:12
Aircraft(const char *home_str, const char *frame_str)
void sync_frame_time(void)
struct SITL::Aircraft::@200 smoothing
void update_wind(const struct sitl_input &input)
LowPassFilterFloat servo_filter[4]
Definition: SIM_Aircraft.h:255
Vector3< float > Vector3f
Definition: vector3.h:246
float filtered_idx(float v, uint8_t idx)
const float gyro_noise
Definition: SIM_Aircraft.h:162
void to_axis_angle(Vector3f &v)
Definition: quaternion.cpp:189
ap_var_type
Definition: AP_Param.h:124
double pitchDeg
Definition: SITL.h:20
double xAccel
Definition: SITL.h:18
Vector3f velocity_air_ef
Definition: SIM_Aircraft.h:138
uint64_t last_time_us
Definition: SIM_Aircraft.h:239
struct SITL::Aircraft::sitl_input::@201 wind
float turbulence_horizontal_speed
Definition: SIM_Aircraft.h:155
#define ToRad(x)
Definition: AP_Common.h:53
uint8_t rcin_chan_count
Definition: SITL.h:27
Vector3f angAccel
Definition: SITL.h:31
void add_noise(float throttle)
float battery_voltage
Definition: SIM_Aircraft.h:145
void set_speedup(float speedup)
void setup_frame_time(float rate, float speedup)
Quaternion quaternion
Definition: SITL.h:21
float filtered_servo_range(const struct sitl_input &input, uint8_t idx)
Vector3f bodyMagField
Definition: SITL.h:30
double longitude
Definition: SITL.h:14
Vector3f ang_accel
Definition: SIM_Aircraft.h:135
const uint32_t min_sleep_time
Definition: SIM_Aircraft.h:242
double speedE
Definition: SITL.h:17
static AP_Param * find(const char *name, enum ap_var_type *ptype)
Definition: AP_Param.cpp:821
AP_Vector3f mag_mot
Definition: SITL.h:109
void from_euler(float roll, float pitch, float yaw)
Definition: matrix3.cpp:26
float turbulence_vertical_speed
Definition: SIM_Aircraft.h:156
AP_Float mag_anomaly_hgt
Definition: SITL.h:175
void location_offset(struct Location &loc, float ofs_north, float ofs_east)
Definition: location.cpp:125
float turbulence_azimuth
Definition: SIM_Aircraft.h:154
const char * result
Definition: Printf.cpp:16
Location location
Definition: SIM_Aircraft.h:127
double airspeed
Definition: SITL.h:22
Rotation
Definition: rotations.h:27
void from_rotation_matrix(const Matrix3f &m)
Definition: quaternion.cpp:76
double rpm1
Definition: SITL.h:25
float battery_current
Definition: SIM_Aircraft.h:146
double rollRate
Definition: SITL.h:19
int32_t lat
param 3 - Latitude * 10**7
Definition: AP_Common.h:143
uint64_t get_wall_time_us(void) const
void set_cutoff_frequency(float cutoff_freq)
float hagl() const
double battery_current
Definition: SITL.h:24
float ground_height_difference() const
Vector3f mag_bf
Definition: SIM_Aircraft.h:158
double speedD
Definition: SITL.h:17
Location home
Definition: SITL.h:13
unsigned long DWORD
Definition: integer.h:22
virtual bool on_ground() const
A system for managing and storing variables that are of general interest to the system.
static double rand_normal(double mean, double stddev)
Matrix3< T > transposed(void) const
Definition: matrix3.cpp:189
#define x(i)
double yawRate
Definition: SITL.h:19
float airspeed_pitot
Definition: SIM_Aircraft.h:144
double rollDeg
Definition: SITL.h:20
AP_Int8 * ahrs_orientation
Definition: SIM_Aircraft.h:177
double heading
Definition: SITL.h:16
uint32_t last_ground_contact_ms
Definition: SIM_Aircraft.h:241
int32_t alt
param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M
Definition: AP_Common.h:142
void Log_Write(const char *name, const char *labels, const char *fmt,...)
Definition: DataFlash.cpp:632
const float accel_noise
Definition: SIM_Aircraft.h:163
#define GRAVITY_MSS
Definition: definitions.h:47
bool is_zero(const T fVal1)
Definition: AP_Math.h:40
#define f(i)
Vector3f velocity_ef
Definition: SIM_Aircraft.h:136
T y
Definition: vector3.h:67
double yawDeg
Definition: SITL.h:20
uint32_t millis()
Definition: system.cpp:157
double yAccel
Definition: SITL.h:18
uint64_t time_now_us
Definition: SIM_Aircraft.h:160
double speedN
Definition: SITL.h:17
static DataFlash_Class * instance(void)
Definition: DataFlash.h:62
T z
Definition: vector3.h:67
void from_rotation(enum Rotation rotation)
Definition: matrix3.cpp:63
Vector3f velocity_air_bf
Definition: SIM_Aircraft.h:139
static bool parse_home(const char *home_str, Location &loc, float &yaw_degrees)
uint64_t micros64()
Definition: system.cpp:162
double zAccel
Definition: SITL.h:18
float achieved_rate_hz
Definition: SIM_Aircraft.h:165
float v
Definition: Printf.cpp:15
AP_Float servo_speed
Definition: SITL.h:111
uint8_t rcin_chan_count
Definition: SIM_Aircraft.h:149
double rpm2
Definition: SITL.h:26
AP_Terrain * terrain
Definition: SIM_Aircraft.h:188
AP_Int8 terrain_enable
Definition: SITL.h:139
void adjust_frame_time(float rate)
float length(void) const
Definition: vector3.cpp:288
void free(void *ptr)
Definition: malloc.c:81
void update_mag_field_bf(void)
uint64_t frame_time_us
Definition: SIM_Aircraft.h:167
double pitchRate
Definition: SITL.h:19
float constrain_float(const float amt, const float low, const float high)
Definition: AP_Math.h:142
void smooth_sensors(void)
void normalize()
Definition: quaternion.cpp:297
void update_position(void)
int32_t lng
param 4 - Longitude * 10**7
Definition: AP_Common.h:144
uint32_t frame_counter
Definition: SIM_Aircraft.h:240
static bool get_mag_field_ef(float latitude_deg, float longitude_deg, float &intensity_gauss, float &declination_deg, float &inclination_deg)
static constexpr float radians(float deg)
Definition: AP_Math.h:158
void extrapolate_sensors(float delta_time)
void update_dynamics(const Vector3f &rot_accel)
Vector3f gyro_prev
Definition: SIM_Aircraft.h:134
float scaled_frame_time_us
Definition: SIM_Aircraft.h:168
uint64_t last_wall_time_us
Definition: SIM_Aircraft.h:169
AP_Float speedup
Definition: SITL.h:141
double latitude
Definition: SITL.h:14
void rotate(const Vector3< T > &g)
Definition: matrix3.cpp:115
float target_speedup
Definition: SIM_Aircraft.h:166
#define degrees(x)
Definition: moduletest.c:23
#define M_PI
Definition: definitions.h:10
void normalize(void)
Definition: matrix3.cpp:135
void fill_fdm(struct sitl_fdm &fdm)
enum SITL::Aircraft::@199 ground_behavior
double altitude
Definition: SITL.h:15
float filtered_servo_angle(const struct sitl_input &input, uint8_t idx)
Vector3f wind_ef
Definition: SIM_Aircraft.h:137
double battery_voltage
Definition: SITL.h:23
void zero()
Definition: vector3.h:182
T x
Definition: vector3.h:67
T apply(T sample, float dt)