Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: use motor_mask from SITL for which outputs are motors

Browse Source 
generate noise based on motor_mask
This commit is contained in:
Andrew Tridgell 2022-08-18 17:21:43 +10:00
parent ac2701b1bf
commit c42acd553e
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; uint64_t next_accel_sample;
float gyro_time; float gyro_time;
float accel_time; float accel_time;
float gyro_motor_phase[12]; float gyro_motor_phase[32];
float accel_motor_phase[12]; float accel_motor_phase[32];
static uint8_t bus_id; 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 // VIB_MOT_MAX is a rpm-scaled vibration applied to each axis
if (!is_zero(sitl->vibe_motor) && motors_on) { 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); 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) { while (harmonics != 0) {
const uint8_t bit = __builtin_ffs(harmonics); const uint8_t bit = __builtin_ffs(harmonics);
harmonics &= ~(1U<<(bit-1U)); 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.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.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); 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); 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 // VIB_MOT_MAX is a rpm-scaled vibration applied to each axis
if (!is_zero(sitl->vibe_motor) && motors_on) { 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); 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) { while (harmonics != 0) {
const uint8_t bit = __builtin_ffs(harmonics); const uint8_t bit = __builtin_ffs(harmonics);
harmonics &= ~(1U<<(bit-1U)); 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); 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); 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); 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); 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; uint64_t next_accel_sample;
float gyro_time; float gyro_time;
float accel_time; float accel_time;
float gyro_motor_phase[12]; float gyro_motor_phase[32];
float accel_motor_phase[12]; float accel_motor_phase[32];
uint32_t temp_start_ms; uint32_t temp_start_ms;
static uint8_t bus_id; static uint8_t bus_id;