zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: implemented harmonics in SITL vibration

apm_2208
Andrew Tridgell 3 years ago
parent
6b55073508
commit
c29b390e7b
1 changed files with 22 additions and 24 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 38
      libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp

38
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
Unescape View File

@ -137,20 +137,19 @@ void AP_InertialSensor_SITL::generate_accel() @@ -137,20 +137,19 @@ void AP_InertialSensor_SITL::generate_accel()
// 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[i];
float motor_freq = calculate_noise(sitl->state.rpm[sitl->state.vtol_motor_start+i] / 60.0f, freq_variation);
float phase_incr = motor_freq * 2 * M_PI / (accel_sample_hz * nsamples);
phase += phase_incr;
if (phase_incr > M_PI) {
phase -= 2 * M_PI;
}
else if (phase_incr < -M_PI) {
phase += 2 * M_PI;
}
uint32_t harmonics = uint32_t(sitl->vibe_motor_harmonics);
const float base_freq = calculate_noise(sitl->state.rpm[sitl->state.vtol_motor_start+i] / 60.0f, freq_variation);
while (harmonics != 0) {
const uint8_t bit = __builtin_ffs(harmonics);
harmonics &= ~(1U<<(bit-1U));
const float phase = accel_motor_phase[i] * float(bit);
accel.x += sinf(phase) * calculate_noise(accel_noise * sitl->vibe_motor_scale, noise_variation);
accel.y += sinf(phase) * calculate_noise(accel_noise * sitl->vibe_motor_scale, noise_variation);
accel.z += sinf(phase) * calculate_noise(accel_noise * sitl->vibe_motor_scale, noise_variation);
}
const float phase_incr = base_freq * 2 * M_PI / (accel_sample_hz * nsamples);
accel_motor_phase[i] = wrap_PI(accel_motor_phase[i] + phase_incr);
}
}
// correct for the acceleration due to the IMU position offset and angular acceleration
@ -243,20 +242,19 @@ void AP_InertialSensor_SITL::generate_gyro() @@ -243,20 +242,19 @@ void AP_InertialSensor_SITL::generate_gyro()
// 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 = calculate_noise(sitl->state.rpm[sitl->state.vtol_motor_start+i] / 60.0f, freq_variation);
float phase_incr = motor_freq * 2 * M_PI / (gyro_sample_hz * nsamples);
float &phase = gyro_motor_phase[i];
phase += phase_incr;
if (phase_incr > M_PI) {
phase -= 2 * M_PI;
}
else if (phase_incr < -M_PI) {
phase += 2 * M_PI;
}
uint32_t harmonics = uint32_t(sitl->vibe_motor_harmonics);
const float base_freq = calculate_noise(sitl->state.rpm[sitl->state.vtol_motor_start+i] / 60.0f, freq_variation);
while (harmonics != 0) {
const uint8_t bit = __builtin_ffs(harmonics);
harmonics &= ~(1U<<(bit-1U));
const float phase = gyro_motor_phase[i] * float(bit);
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);
}
const float phase_incr = base_freq * 2 * M_PI / (gyro_sample_hz * nsamples);
gyro_motor_phase[i] = wrap_PI(gyro_motor_phase[i] + phase_incr);
}
}
Vector3f gyro = Vector3f(p, q, r);

Write Preview
Loading…
Cancel
Save