diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index c36ca6f5e0..4ca1a31a17 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -55,37 +55,60 @@ bool AP_InertialSensor_SITL::init_sensor(void) return true; } +// calculate a noisy noise component +static float calculate_noise(float noise, float noise_variation) { + return noise * (1.0f + noise_variation * rand_float()); +} + /* 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; + Vector3f accel_accum; + uint8_t nsamples = enable_fast_sampling(accel_instance[instance]) ? 4 : 1; + for (uint8_t j = 0; j < nsamples; j++) { - if (sitl->motors_on) { - // add extra noise when the motors are on - accel_noise += instance == 0 ? sitl->accel_noise : sitl->accel2_noise; - } + // add accel bias and noise + Vector3f accel_bias = instance == 0 ? sitl->accel_bias.get() : sitl->accel2_bias.get(); + float xAccel = sitl->state.xAccel + accel_bias.x; + float yAccel = sitl->state.yAccel + accel_bias.y; + float zAccel = sitl->state.zAccel + accel_bias.z; + + // minimum noise levels are 2 bits, but averaged over many + // samples, giving around 0.01 m/s/s + float accel_noise = 0.01f; + float noise_variation = 0.05; + // this smears the individual motor peaks somewhat emulating physical motors + float freq_variation = 0.12; - // add accel bias and noise - Vector3f accel_bias = instance == 0 ? sitl->accel_bias.get() : sitl->accel2_bias.get(); - float xAccel = sitl->state.xAccel + accel_bias.x; - float yAccel = sitl->state.yAccel + accel_bias.y; - float zAccel = sitl->state.zAccel + accel_bias.z; - const Vector3f &vibe_freq = sitl->vibe_freq; - bool vibe_motor = !is_zero(sitl->vibe_motor); - if (vibe_freq.is_zero() && !vibe_motor) { xAccel += accel_noise * rand_float(); yAccel += accel_noise * rand_float(); zAccel += accel_noise * rand_float(); - } else { - if (vibe_motor) { + + bool motors_on = sitl->throttle > sitl->ins_noise_throttle_min; + // on a real 180mm copter gyro noise varies between 0.8-4 m/s/s for throttle 0.2-0.8 + // giving a accel noise variation of 5.33 m/s/s over the full throttle range + if (motors_on) { + // add extra noise when the motors are on + accel_noise = (instance == 0 ? sitl->accel_noise : sitl->accel2_noise) * sitl->throttle; + } + + // VIB_FREQ is a static vibration applied to each axis + const Vector3f &vibe_freq = sitl->vibe_freq; + if (!vibe_freq.is_zero() && motors_on) { + float t = AP_HAL::micros() * 1.0e-6f; + xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * calculate_noise(accel_noise, noise_variation); + yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * calculate_noise(accel_noise, noise_variation); + zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * calculate_noise(accel_noise, noise_variation); + } + + // VIB_MOT_MAX is a rpm-scaled vibration applied to each axis + if (!is_zero(sitl->vibe_motor) && motors_on) { for (uint8_t i = 0; i < sitl->state.num_motors; i++) { - float& phase = accel_motor_phase[instance][i]; - float motor_freq = sitl->state.rpm[i] / 60.0f; - float phase_incr = motor_freq * 2 * M_PI / accel_sample_hz[instance]; + float &phase = accel_motor_phase[instance][i]; + float motor_freq = calculate_noise(sitl->state.rpm[i] / 60.0f, freq_variation); + float phase_incr = motor_freq * 2 * M_PI / (accel_sample_hz[instance] * nsamples); phase += phase_incr; if (phase_incr > M_PI) { phase -= 2 * M_PI; @@ -93,54 +116,49 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance) else if (phase_incr < -M_PI) { phase += 2 * M_PI; } - xAccel += sinf(phase) * accel_noise; - yAccel += sinf(phase) * accel_noise; - zAccel += sinf(phase) * accel_noise; + xAccel += sinf(phase) * calculate_noise(accel_noise, noise_variation); + yAccel += sinf(phase) * calculate_noise(accel_noise, noise_variation); + zAccel += sinf(phase) * calculate_noise(accel_noise, noise_variation); } } - if (!vibe_freq.is_zero()) { - float t = AP_HAL::micros() * 1.0e-6f; - xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * accel_noise; - yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * accel_noise; - zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * accel_noise; + + // correct for the acceleration due to the IMU position offset and angular acceleration + // correct for the centripetal acceleration + // only apply corrections to first accelerometer + Vector3f pos_offset = sitl->imu_pos_offset; + if (!pos_offset.is_zero()) { + // calculate sensed acceleration due to lever arm effect + // Note: the % operator has been overloaded to provide a cross product + Vector3f angular_accel = Vector3f(radians(sitl->state.angAccel.x), radians(sitl->state.angAccel.y), radians(sitl->state.angAccel.z)); + Vector3f lever_arm_accel = angular_accel % pos_offset; + + // calculate sensed acceleration due to centripetal acceleration + Vector3f angular_rate = Vector3f(radians(sitl->state.rollRate), radians(sitl->state.pitchRate), radians(sitl->state.yawRate)); + Vector3f centripetal_accel = angular_rate % (angular_rate % pos_offset); + + // apply corrections + xAccel += lever_arm_accel.x + centripetal_accel.x; + yAccel += lever_arm_accel.y + centripetal_accel.y; + zAccel += lever_arm_accel.z + centripetal_accel.z; } - } - - // correct for the acceleration due to the IMU position offset and angular acceleration - // correct for the centripetal acceleration - // only apply corrections to first accelerometer - Vector3f pos_offset = sitl->imu_pos_offset; - if (!pos_offset.is_zero()) { - // calculate sensed acceleration due to lever arm effect - // Note: the % operator has been overloaded to provide a cross product - Vector3f angular_accel = Vector3f(radians(sitl->state.angAccel.x) , radians(sitl->state.angAccel.y) , radians(sitl->state.angAccel.z)); - Vector3f lever_arm_accel = angular_accel % pos_offset; - - // calculate sensed acceleration due to centripetal acceleration - Vector3f angular_rate = Vector3f(radians(sitl->state.rollRate), radians(sitl->state.pitchRate), radians(sitl->state.yawRate)); - Vector3f centripetal_accel = angular_rate % (angular_rate % pos_offset); - - // apply corrections - 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) { - xAccel = sitl->accel_fail; - yAccel = sitl->accel_fail; - zAccel = sitl->accel_fail; - } + if (fabsf(sitl->accel_fail) > 1.0e-6f) { + xAccel = sitl->accel_fail; + yAccel = sitl->accel_fail; + zAccel = sitl->accel_fail; + } - Vector3f accel = Vector3f(xAccel, yAccel, zAccel); + Vector3f accel = Vector3f(xAccel, yAccel, zAccel); - _rotate_and_correct_accel(accel_instance[instance], accel); + _rotate_and_correct_accel(accel_instance[instance], accel); + _notify_new_accel_sensor_rate_sample(instance, accel); - uint8_t nsamples = enable_fast_sampling(accel_instance[instance]) ? 4 : 1; - for (uint8_t i=0; imotors_on) { - // add extra noise when the motors are on - gyro_noise += ToRad(sitl->gyro_noise); - } + Vector3f gyro_accum; + uint8_t nsamples = enable_fast_sampling(gyro_instance[instance]) ? 8 : 1; + for (uint8_t j = 0; j < nsamples; j++) { + 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 p = radians(sitl->state.rollRate) + gyro_drift(); - float q = radians(sitl->state.pitchRate) + gyro_drift(); - float r = radians(sitl->state.yawRate) + gyro_drift(); + // minimum gyro noise is less than 1 bit + float gyro_noise = ToRad(0.04f); + float noise_variation = 0.05f; + // this smears the individual motor peaks somewhat emulating physical motors + float freq_variation = 0.12f; - const Vector3f &vibe_freq = sitl->vibe_freq; - bool vibe_motor = !is_zero(sitl->vibe_motor); - if (vibe_freq.is_zero() && !vibe_motor) { p += gyro_noise * rand_float(); q += gyro_noise * rand_float(); r += gyro_noise * rand_float(); - } else { - if (vibe_motor) { + + bool motors_on = sitl->throttle > sitl->ins_noise_throttle_min; + // on a real 180mm copter gyro noise varies between 0.2-0.4 rad/s for throttle 0.2-0.8 + // giving a gyro noise variation of 0.33 rad/s or 20deg/s over the full throttle range + if (motors_on) { + // add extra noise when the motors are on + gyro_noise = ToRad(sitl->gyro_noise) * sitl->throttle; + } + + // VIB_FREQ is a static vibration applied to each axis + const Vector3f &vibe_freq = sitl->vibe_freq; + if (!vibe_freq.is_zero() && motors_on) { + float t = AP_HAL::micros() * 1.0e-6f; + p += sinf(t * 2 * M_PI * vibe_freq.x) * calculate_noise(gyro_noise, noise_variation); + q += sinf(t * 2 * M_PI * vibe_freq.y) * calculate_noise(gyro_noise, noise_variation); + r += sinf(t * 2 * M_PI * vibe_freq.z) * calculate_noise(gyro_noise, noise_variation); + } + + // VIB_MOT_MAX is a rpm-scaled vibration applied to each axis + if (!is_zero(sitl->vibe_motor) && motors_on) { for (uint8_t i = 0; i < sitl->state.num_motors; i++) { - float motor_freq = sitl->state.rpm[i] / 60.0f; - float phase_incr = motor_freq * 2 * M_PI / gyro_sample_hz[instance]; - float& phase = gyro_motor_phase[instance][i]; + float motor_freq = calculate_noise(sitl->state.rpm[i] / 60.0f, freq_variation); + float phase_incr = motor_freq * 2 * M_PI / (gyro_sample_hz[instance] * nsamples); + float &phase = gyro_motor_phase[instance][i]; phase += phase_incr; if (phase_incr > M_PI) { phase -= 2 * M_PI; @@ -180,34 +214,26 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance) else if (phase_incr < -M_PI) { phase += 2 * M_PI; } - - p += sinf(phase) * gyro_noise; - q += sinf(phase) * gyro_noise; - r += sinf(phase) * gyro_noise; + p += sinf(phase) * calculate_noise(gyro_noise, noise_variation); + q += sinf(phase) * calculate_noise(gyro_noise, noise_variation); + r += sinf(phase) * calculate_noise(gyro_noise, noise_variation); } } - if (!vibe_freq.is_zero()) { - float t = AP_HAL::micros() * 1.0e-6f; - p += sinf(t * 2 * M_PI * vibe_freq.x) * gyro_noise; - q += sinf(t * 2 * M_PI * vibe_freq.y) * gyro_noise; - r += sinf(t * 2 * M_PI * vibe_freq.z) * gyro_noise; - } - } - Vector3f gyro = Vector3f(p, q, r); + Vector3f gyro = Vector3f(p, q, r); - // add in gyro scaling - Vector3f scale = sitl->gyro_scale; - gyro.x *= (1 + scale.x * 0.01f); - gyro.y *= (1 + scale.y * 0.01f); - gyro.z *= (1 + scale.z * 0.01f); + // add in gyro scaling + Vector3f scale = sitl->gyro_scale; + gyro.x *= (1 + scale.x * 0.01f); + gyro.y *= (1 + scale.y * 0.01f); + gyro.z *= (1 + scale.z * 0.01f); - _rotate_and_correct_gyro(gyro_instance[instance], gyro); - - uint8_t nsamples = enable_fast_sampling(gyro_instance[instance]) ? 8 : 1; - for (uint8_t i = 0; i < nsamples; i++) { - _notify_new_gyro_raw_sample(gyro_instance[instance], gyro); + _rotate_and_correct_gyro(gyro_instance[instance], gyro); + gyro_accum += gyro; + _notify_new_gyro_sensor_rate_sample(instance, gyro); } + gyro_accum /= nsamples; + _notify_new_gyro_raw_sample(gyro_instance[instance], gyro_accum); } void AP_InertialSensor_SITL::timer_update(void)