AP_InertialSensor: use motor_mask from SITL for which outputs are motors

generate noise based on motor_mask

libraries/AP_InertialSensor/AP_InertialSensor_NONE.h

@@ -50,8 +50,8 @@ class AP_InertialSensor_NONE : public AP_InertialSensor_Backend
     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;
 };

libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp

@@ -136,20 +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++) {
-                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;
+            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[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[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);
                 }
-                else if (phase_incr < -M_PI) {
-                    phase += 2 * M_PI;
-                }
-                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[motor] = wrap_PI(accel_motor_phase[motor] + phase_incr);
             }
         }
 
@@ -242,20 +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++) {
-                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 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[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[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);
                 }
-                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[motor] = wrap_PI(gyro_motor_phase[motor] + phase_incr);
             }
         }
 

libraries/AP_InertialSensor/AP_InertialSensor_SITL.h

@@ -43,8 +43,8 @@ class AP_InertialSensor_SITL : public AP_InertialSensor_Backend
     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;