SITL: simulations optimisations

- change to 600Hz physics rate by default
 - don't export rotational acceleration (not needed)
 - report speedup in terminal output at 0.5Hz (wall clock rate)

libraries/SITL/SIM_Aircraft.cpp

@@ -45,8 +45,6 @@ Aircraft::Aircraft(const char *frame_str) :
     frame_height(0.0f),
     dcm(),
     gyro(),
-    gyro_prev(),
-    ang_accel(),
     velocity_ef(),
     mass(0.0f),
     accel_body(0.0f, 0.0f, -GRAVITY_MSS),
@@ -132,6 +130,7 @@ float Aircraft::hagl() const
 {
     return (-position.z) + home.alt * 0.01f - ground_level - frame_height - ground_height_difference();
 }
+
 /*
    return true if we are on the ground
 */
@@ -282,6 +281,13 @@ void Aircraft::sync_frame_time(void)
         last_wall_time_us = now;
         frame_counter = 0;
     }
+    uint32_t now_ms = now / 1000ULL;
+    if (now_ms - last_fps_report_ms > 2000) {
+        last_fps_report_ms = now_ms;
+        ::printf("Rate: target:%.1f achieved:%.1f speedup %.1f/%.1f\n",
+                 rate_hz*target_speedup, achieved_rate_hz,
+                 achieved_rate_hz/rate_hz, target_speedup);
+    }
 }
 
 /* add noise based on throttle level (from 0..1) */
@@ -332,8 +338,10 @@ double Aircraft::rand_normal(double mean, double stddev)
 */
 void Aircraft::fill_fdm(struct sitl_fdm &fdm)
 {
+    bool is_smoothed = false;
     if (use_smoothing) {
         smooth_sensors();
+        is_smoothed = true;
     }
     fdm.timestamp_us = time_now_us;
     if (fdm.home.lat == 0 && fdm.home.lng == 0) {
@@ -353,9 +361,6 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     fdm.rollRate  = degrees(gyro.x);
     fdm.pitchRate = degrees(gyro.y);
     fdm.yawRate   = degrees(gyro.z);
-    fdm.angAccel.x = degrees(ang_accel.x);
-    fdm.angAccel.y = degrees(ang_accel.y);
-    fdm.angAccel.z = degrees(ang_accel.z);
     float r, p, y;
     dcm.to_euler(&r, &p, &y);
     fdm.rollDeg  = degrees(r);
@@ -376,7 +381,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     fdm.scanner.points = scanner.points;
     fdm.scanner.ranges = scanner.ranges;
 
-    if (smoothing.enabled) {
+    if (is_smoothed) {
         fdm.xAccel = smoothing.accel_body.x;
         fdm.yAccel = smoothing.accel_body.y;
         fdm.zAccel = smoothing.accel_body.z;
@@ -490,11 +495,6 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
     gyro.y = constrain_float(gyro.y, -radians(2000.0f), radians(2000.0f));
     gyro.z = constrain_float(gyro.z, -radians(2000.0f), radians(2000.0f));
 
-    // estimate angular acceleration using a first order difference calculation
-    // TODO the simulator interface should provide the angular acceleration
-    ang_accel = (gyro - gyro_prev) / delta_time;
-    gyro_prev = gyro;
-
     // update attitude
     dcm.rotate(gyro * delta_time);
     dcm.normalize();
@@ -726,7 +726,6 @@ void Aircraft::smooth_sensors(void)
     smoothing.location.alt  = static_cast<int32_t>(home.alt - smoothing.position.z * 100.0f);
 
     smoothing.last_update_us = now;
-    smoothing.enabled = true;
 }
 
 /*

libraries/SITL/SIM_Aircraft.h

@@ -152,8 +152,6 @@ protected:
     float frame_height;
     Matrix3f dcm;                        // rotation matrix, APM conventions, from body to earth
     Vector3f gyro;                       // rad/s
-    Vector3f gyro_prev;                  // rad/s
-    Vector3f ang_accel;                  // rad/s/s
     Vector3f velocity_ef;                // m/s, earth frame
     Vector3f wind_ef;                    // m/s, earth frame
     Vector3f velocity_air_ef;            // velocity relative to airmass, earth frame
@@ -195,6 +193,7 @@ protected:
     uint64_t frame_time_us;
     float scaled_frame_time_us;
     uint64_t last_wall_time_us;
+    uint32_t last_fps_report_ms;
     uint8_t instance;
     const char *autotest_dir;
     const char *frame;
@@ -281,7 +280,6 @@ private:
     const uint32_t min_sleep_time;
 
     struct {
-        bool enabled;
         Vector3f accel_body;
         Vector3f gyro;
         Matrix3f rotation_b2e;

libraries/SITL/SIM_Scrimmage.cpp

@@ -96,9 +96,6 @@ void Scrimmage::recv_fdm(const struct sitl_input &input)
     accel_body = Vector3f(pkt.xAccel, pkt.yAccel, pkt.zAccel) - dcm.transposed()*Vector3f(0.0f, 0.0f, GRAVITY_MSS);
     gyro = Vector3f(pkt.rollRate, pkt.pitchRate, pkt.yawRate);
 
-    ang_accel = (gyro - gyro_prev) * std::min((float)1000000, dt_inv);
-    gyro_prev = gyro;
-
     velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);
 
     location.lat = pkt.latitude * 1.0e7;

libraries/SITL/SITL.cpp

@@ -257,7 +257,7 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
     // vicon velocity glitch in NED frame
     AP_GROUPINFO("VICON_VGLI",    21, SITL,  vicon_vel_glitch, 0),
 
-    AP_GROUPINFO("RATE_HZ",  22, SITL,  loop_rate_hz, 1200),
+    AP_GROUPINFO("RATE_HZ",  22, SITL,  loop_rate_hz, 600),
 
 #if HAL_COMPASS_MAX_SENSORS > 1
     AP_GROUPINFO("MAG2_OFS",     23, SITL,  mag_ofs[1], 0),