Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: use motor_mask from SITL for which outputs are motors 

generate noise based on motor_mask
This commit is contained in:
Andrew Tridgell 2022-08-18 17:21:43 +10:00 committed by Randy Mackay
parent 4dba724fc7
commit d062688b30
3 changed files with 20 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor_NONE.h
View File

@ -47,8 +47,8 @@ private:
uint64_t next_accel_sample;
float gyro_time;
float accel_time;
float gyro_motor_phase[12];
float accel_motor_phase[12];
float gyro_motor_phase[32];
float accel_motor_phase[32];
static uint8_t bus_id;
};

24
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -136,19 +136,23 @@ 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++) {
uint32_t mask = sitl->state.motor_mask;
uint8_t mbit;
while ((mbit = __builtin_ffs(mask)) != 0) {
const uint8_t motor = mbit-1;
mask &= ~(1U<<motor);
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);
const float base_freq = calculate_noise(sitl->state.rpm[motor] / 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);
const float phase = accel_motor_phase[motor] * 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);
accel_motor_phase[motor] = wrap_PI(accel_motor_phase[motor] + phase_incr);
}
}
@ -241,19 +245,23 @@ 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++) {
uint32_t mask = sitl->state.motor_mask;
uint8_t mbit;
while ((mbit = __builtin_ffs(mask)) != 0) {
const uint8_t motor = mbit-1;
mask &= ~(1U<<motor);
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);
const float base_freq = calculate_noise(sitl->state.rpm[motor] / 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);
const float phase = gyro_motor_phase[motor] * 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);
gyro_motor_phase[motor] = wrap_PI(gyro_motor_phase[motor] + phase_incr);
}
}

4
libraries/AP_InertialSensor/AP_InertialSensor_SITL.h
View File

@ -43,8 +43,8 @@ private:
uint64_t next_accel_sample;
float gyro_time;
float accel_time;
float gyro_motor_phase[12];
float accel_motor_phase[12];
float gyro_motor_phase[32];
float accel_motor_phase[32];
uint32_t temp_start_ms;
static uint8_t bus_id;