From 74eb7a724381ca4147eae669a4c7e500366ec7b6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 1 May 2017 10:51:15 +1000 Subject: [PATCH] AP_InertialSensor: make SITL sensor rate match a Pixhawk1 use 760Hz 2nd gyro and 800 Hz 2nd accel --- .../AP_InertialSensor_SITL.cpp | 106 ++++++++++-------- .../AP_InertialSensor_SITL.h | 8 ++ 2 files changed, 69 insertions(+), 45 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index 6ceefe8b5e..049e06732f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -1,6 +1,7 @@ #include #include "AP_InertialSensor_SITL.h" #include +#include #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; iupdate_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(); + p += gyro_noise * rand_float(); + q += gyro_noise * rand_float(); + 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(); - - 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= 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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h index 6539a31a79..b366cc261d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h +++ b/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]; };