From 41b41c05f3aebd799a364bd0ef05ada17bdcf799 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 28 Jan 2020 22:27:03 +0000 Subject: [PATCH] AP_InertialSensor: scale SITL motor noise by SIM_VIB_MOT_MULT --- .../AP_InertialSensor/AP_InertialSensor_SITL.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index 782dec087c..01e435291c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -100,9 +100,9 @@ void AP_InertialSensor_SITL::generate_accel() else if (phase_incr < -M_PI) { phase += 2 * M_PI; } - 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); + xAccel += sinf(phase) * calculate_noise(accel_noise * sitl->vibe_motor_scale, noise_variation); + yAccel += sinf(phase) * calculate_noise(accel_noise * sitl->vibe_motor_scale, noise_variation); + zAccel += sinf(phase) * calculate_noise(accel_noise * sitl->vibe_motor_scale, noise_variation); } } @@ -199,9 +199,9 @@ void AP_InertialSensor_SITL::generate_gyro() else if (phase_incr < -M_PI) { phase += 2 * M_PI; } - 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); + p += sinf(phase) * calculate_noise(gyro_noise * sitl->vibe_motor_scale, noise_variation); + q += sinf(phase) * calculate_noise(gyro_noise * sitl->vibe_motor_scale, noise_variation); + r += sinf(phase) * calculate_noise(gyro_noise * sitl->vibe_motor_scale, noise_variation); } }