AP_InertialSensor: make SITL sensor rate match a Pixhawk1

use 760Hz 2nd gyro and 800 Hz 2nd accel

libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp

@@ -1,6 +1,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include "AP_InertialSensor_SITL.h"
 #include <SITL/SITL.h>
+#include <stdio.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
@@ -36,8 +37,8 @@ bool AP_InertialSensor_SITL::init_sensor(void)
 
     // grab the used instances
     for (uint8_t i=0; i<INS_SITL_INSTANCES; i++) {
-        gyro_instance[i] = _imu.register_gyro(sitl->update_rate_hz, i);
-        accel_instance[i] = _imu.register_accel(sitl->update_rate_hz, i);
+        gyro_instance[i] = _imu.register_gyro(gyro_sample_hz[i], i);
+        accel_instance[i] = _imu.register_accel(accel_sample_hz[i], i);
     }
 
     hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void));
@@ -45,32 +46,25 @@ bool AP_InertialSensor_SITL::init_sensor(void)
     return true;
 }
 
-void AP_InertialSensor_SITL::timer_update(void)
+/*
+  generate an accelerometer sample
+ */
+void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
 {
     // minimum noise levels are 2 bits, but averaged over many
     // samples, giving around 0.01 m/s/s
     float accel_noise = 0.01f;
-    float accel2_noise = 0.01f;
 
-    // minimum gyro noise is also less than 1 bit
-    float gyro_noise = ToRad(0.04f);
     if (sitl->motors_on) {
         // add extra noise when the motors are on
         accel_noise += sitl->accel_noise;
-        accel2_noise += sitl->accel2_noise;
-        gyro_noise += ToRad(sitl->gyro_noise);
     }
 
     // add accel bias and noise
-    Vector3f accel_bias = sitl->accel_bias.get();
-    float xAccel1 = sitl->state.xAccel + accel_noise * rand_float() + accel_bias.x;
-    float yAccel1 = sitl->state.yAccel + accel_noise * rand_float() + accel_bias.y;
-    float zAccel1 = sitl->state.zAccel + accel_noise * rand_float() + accel_bias.z;
-
-    accel_bias = sitl->accel2_bias.get();
-    float xAccel2 = sitl->state.xAccel + accel2_noise * rand_float() + accel_bias.x;
-    float yAccel2 = sitl->state.yAccel + accel2_noise * rand_float() + accel_bias.y;
-    float zAccel2 = sitl->state.zAccel + accel2_noise * rand_float() + accel_bias.z;
+    Vector3f accel_bias = instance==0?sitl->accel_bias.get():sitl->accel2_bias.get();
+    float xAccel = sitl->state.xAccel + accel_noise * rand_float() + accel_bias.x;
+    float yAccel = sitl->state.yAccel + accel_noise * rand_float() + accel_bias.y;
+    float zAccel = sitl->state.zAccel + accel_noise * rand_float() + accel_bias.z;
 
     // correct for the acceleration due to the IMU position offset and angular acceleration
     // correct for the centripetal acceleration
@@ -87,49 +81,71 @@ void AP_InertialSensor_SITL::timer_update(void)
         Vector3f centripetal_accel = angular_rate % (angular_rate % pos_offset);
 
         // apply corrections
-        xAccel1 += lever_arm_accel.x + centripetal_accel.x;
-        yAccel1 += lever_arm_accel.y + centripetal_accel.y;
-        zAccel1 += lever_arm_accel.z + centripetal_accel.z;
+        xAccel += lever_arm_accel.x + centripetal_accel.x;
+        yAccel += lever_arm_accel.y + centripetal_accel.y;
+        zAccel += lever_arm_accel.z + centripetal_accel.z;
     }
 
     if (fabsf(sitl->accel_fail) > 1.0e-6f) {
-        xAccel1 = sitl->accel_fail;
-        yAccel1 = sitl->accel_fail;
-        zAccel1 = sitl->accel_fail;
+        xAccel = sitl->accel_fail;
+        yAccel = sitl->accel_fail;
+        zAccel = sitl->accel_fail;
     }
 
-    Vector3f accel0 = Vector3f(xAccel1, yAccel1, zAccel1) + _imu.get_accel_offsets(0);
-    Vector3f accel1 = Vector3f(xAccel2, yAccel2, zAccel2) + _imu.get_accel_offsets(1);
-    _notify_new_accel_raw_sample(accel_instance[0], accel0);
-    _notify_new_accel_raw_sample(accel_instance[1], accel1);
+    Vector3f accel = Vector3f(xAccel, yAccel, zAccel) + _imu.get_accel_offsets(instance);
+
+    _notify_new_accel_raw_sample(accel_instance[instance], accel, AP_HAL::micros64());
+}
+
+/*
+  generate a gyro sample
+ */
+void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
+{
+    // minimum gyro noise is less than 1 bit
+    float gyro_noise = ToRad(0.04f);
+    
+    if (sitl->motors_on) {
+        // add extra noise when the motors are on
+        gyro_noise += ToRad(sitl->gyro_noise);
+    }
 
     float p = radians(sitl->state.rollRate) + gyro_drift();
     float q = radians(sitl->state.pitchRate) + gyro_drift();
     float r = radians(sitl->state.yawRate) + gyro_drift();
 
-    float p1 = p + gyro_noise * rand_float();
-    float q1 = q + gyro_noise * rand_float();
-    float r1 = r + gyro_noise * rand_float();
-
-    float p2 = p + gyro_noise * rand_float();
-    float q2 = q + gyro_noise * rand_float();
-    float r2 = r + gyro_noise * rand_float();
+    p += gyro_noise * rand_float();
+    q += gyro_noise * rand_float();
+    r += gyro_noise * rand_float();
 
-    Vector3f gyro0 = Vector3f(p1, q1, r1) + _imu.get_gyro_offsets(0);
-    Vector3f gyro1 = Vector3f(p2, q2, r2) + _imu.get_gyro_offsets(1);
+    Vector3f gyro = Vector3f(p, q, r) + _imu.get_gyro_offsets(instance);
 
     // add in gyro scaling
     Vector3f scale = sitl->gyro_scale;
-    gyro0.x *= (1 + scale.x*0.01);
-    gyro0.y *= (1 + scale.y*0.01);
-    gyro0.z *= (1 + scale.z*0.01);
+    gyro.x *= (1 + scale.x*0.01);
+    gyro.y *= (1 + scale.y*0.01);
+    gyro.z *= (1 + scale.z*0.01);
 
-    gyro1.x *= (1 + scale.x*0.01);
-    gyro1.y *= (1 + scale.y*0.01);
-    gyro1.z *= (1 + scale.z*0.01);
-    
-    _notify_new_gyro_raw_sample(gyro_instance[0], gyro0);
-    _notify_new_gyro_raw_sample(gyro_instance[1], gyro1);
+    _notify_new_gyro_raw_sample(gyro_instance[instance], gyro, AP_HAL::micros64());
+}
+
+void AP_InertialSensor_SITL::timer_update(void)
+{
+    uint64_t now = AP_HAL::micros64();
+    for (uint8_t i=0; i<INS_SITL_INSTANCES; i++) {
+        if (now >= next_accel_sample[i]) {
+            generate_accel(i);
+            while (now >= next_accel_sample[i]) {
+                next_accel_sample[i] += 1000000UL / accel_sample_hz[i];
+            }
+        }
+        if (now >= next_gyro_sample[i]) {
+            generate_gyro(i);
+            while (now >= next_gyro_sample[i]) {
+                next_gyro_sample[i] += 1000000UL / gyro_sample_hz[i];
+            }
+        }
+    }
 }
 
 // generate a random float between -1 and 1

libraries/AP_InertialSensor/AP_InertialSensor_SITL.h

@@ -23,9 +23,17 @@ private:
     void timer_update();
     float rand_float(void);
     float gyro_drift(void);
+    void generate_accel(uint8_t instance);
+    void generate_gyro(uint8_t instance);
 
     SITL::SITL *sitl;
 
+    // simulated sensor rates in Hz. This matches a pixhawk1
+    const uint16_t gyro_sample_hz[INS_SITL_INSTANCES]  { 1000, 760 };
+    const uint16_t accel_sample_hz[INS_SITL_INSTANCES] { 1000, 800 };
+
     uint8_t gyro_instance[INS_SITL_INSTANCES];
     uint8_t accel_instance[INS_SITL_INSTANCES];
+    uint64_t next_gyro_sample[INS_SITL_INSTANCES];
+    uint64_t next_accel_sample[INS_SITL_INSTANCES];
 };