Simulate motor noise in the gyros and accels

Tools/autotest/arducopter.py

@@ -7,6 +7,7 @@
 import os
 import shutil
 import time
+import numpy
 
 from pymavlink import mavutil
 from pymavlink import mavextra
@@ -1847,6 +1848,73 @@ def fly_motor_fail(self, fail_servo=0, fail_mul=0.0, holdtime=30):
 
         self.do_RTL()
 
+    def fly_motor_vibration(self):
+        """Test flight with motor vibration"""
+        self.context_push()
+
+        ex = None
+        try:
+            self.set_rc_default()
+            self.set_parameter("INS_LOG_BAT_MASK", 3)
+            self.set_parameter("INS_LOG_BAT_OPT", 2)
+            self.set_parameter("LOG_BITMASK", 958)
+            self.set_parameter("LOG_DISARMED", 0)
+            self.set_parameter("SIM_VIB_MOT_MAX", 350)
+            self.set_parameter("SIM_GYR_RND", 100)
+            self.set_parameter("SIM_DRIFT_SPEED", 0)
+            self.set_parameter("SIM_DRIFT_TIME", 0)
+            self.reboot_sitl()
+
+            self.takeoff(10, mode="LOITER")
+            self.change_alt(alt_min=15)
+
+            hover_time = 5
+            tstart = self.get_sim_time()
+            self.progress("Hovering for %u seconds" % hover_time)
+            while self.get_sim_time_cached() < tstart + hover_time:
+                attitude = self.mav.recv_match(type='ATTITUDE', blocking=True)
+            tend = self.get_sim_time()
+
+            self.do_RTL()
+            psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
+            # ignore the first 20Hz
+            ignore_bins = 20
+            freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]
+            if numpy.amax(psd["X"][ignore_bins:]) < -22 or freq < 200 or freq > 300:
+                raise NotAchievedException("Did not detect a motor peak, found %f at %f dB" % (freq, numpy.amax(psd["X"][ignore_bins:])))
+            else:
+                self.progress("Detected motor peak at %fHz" % freq)
+
+            # now add a notch and check that the peak is squashed
+            self.set_parameter("INS_NOTCH_ENABLE", 1)
+            self.set_parameter("INS_NOTCH_FREQ", freq)
+            self.set_parameter("INS_NOTCH_ATT", 50)
+            self.set_parameter("INS_NOTCH_BW", freq/2)
+            self.reboot_sitl()
+
+            self.takeoff(10, mode="LOITER")
+            self.change_alt(alt_min=15)
+
+            tstart = self.get_sim_time()
+            self.progress("Hovering for %u seconds" % hover_time)
+            while self.get_sim_time_cached() < tstart + hover_time:
+                attitude = self.mav.recv_match(type='ATTITUDE', blocking=True)
+            tend = self.get_sim_time()
+            self.do_RTL()
+            psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
+            freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]
+            if numpy.amax(psd["X"][ignore_bins:]) < -22:
+                self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, numpy.amax(psd["X"][ignore_bins:])))
+            else:
+                raise NotAchievedException("Detected motor peak at %f Hz" % (freq))
+        except Exception as e:
+            ex = e
+
+        self.context_pop()
+
+        if ex is not None:
+            raise ex
+
     def fly_vision_position(self):
         """Disable GPS navigation, enable Vicon input."""
         # scribble down a location we can set origin to:
@@ -4403,12 +4471,15 @@ def tests(self):
              "Test rangefinder drivers",
              self.fly_rangefinder_drivers),
 
+            ("MotorVibration",
+             "Fly motor vibration test",
+             self.fly_motor_vibration),
+
             ("LogDownLoad",
              "Log download",
              lambda: self.log_download(
                  self.buildlogs_path("ArduCopter-log.bin"),
                  upload_logs=len(self.fail_list) > 0))
-
         ])
         return ret
 

Tools/autotest/common.py

@@ -11,6 +11,7 @@
 import pexpect
 import fnmatch
 import operator
+import numpy
 
 from MAVProxy.modules.lib import mp_util
 
@@ -3669,3 +3670,98 @@ def autotest(self):
         ret = self.run_tests(tests)
         self.post_tests_announcements()
         return ret
+
+    def mavfft_fttd(self, sensor_type, sensor_instance, since, until):
+        '''display fft for raw ACC data in current logfile'''
+
+        '''object to store data about a single FFT plot'''
+        class MessageData(object):
+            def __init__(self, ffth):
+                self.seqno = -1
+                self.fftnum = ffth.N
+                self.sensor_type = ffth.type
+                self.instance = ffth.instance
+                self.sample_rate_hz = ffth.smp_rate
+                self.multiplier = ffth.mul
+                self.sample_us = ffth.SampleUS
+                self.data = {}
+                self.data["X"] = []
+                self.data["Y"] = []
+                self.data["Z"] = []
+                self.holes = False
+                self.freq = None
+
+            def add_fftd(self, fftd):
+                self.seqno += 1
+                self.data["X"].extend(fftd.x)
+                self.data["Y"].extend(fftd.y)
+                self.data["Z"].extend(fftd.z)
+
+        mlog = self.dfreader_for_current_onboard_log()
+
+        # see https://holometer.fnal.gov/GH_FFT.pdf for a description of the techniques used here
+        messages = []
+        messagedata = None
+        while True:
+            m = mlog.recv_match()
+            if m is None:
+                break
+            msg_type = m.get_type()
+            if msg_type == "ISBH":
+                if messagedata is not None:
+                    if messagedata.sensor_type == sensor_type and messagedata.instance == sensor_instance and messagedata.sample_us > since and messagedata.sample_us < until:
+                        messages.append(messagedata)
+                messagedata = MessageData(m)
+                continue
+
+            if msg_type == "ISBD":
+                if messagedata is not None and messagedata.sensor_type == sensor_type and messagedata.instance == sensor_instance:
+                    messagedata.add_fftd(m)
+
+        fft_len = len(messages[0].data["X"])
+        sum_fft = {
+                "X": numpy.zeros(fft_len/2+1),
+                "Y": numpy.zeros(fft_len/2+1),
+                "Z": numpy.zeros(fft_len/2+1),
+            }
+        sample_rate = 0
+        counts = 0
+        window = numpy.hanning(fft_len)
+        freqmap = numpy.fft.rfftfreq(fft_len, 1.0 / messages[0].sample_rate_hz)
+
+        # calculate NEBW constant
+        S2 = numpy.inner(window, window)
+
+        for message in messages:
+            for axis in [ "X","Y","Z" ]:
+                # normalize data and convert to dps in order to produce more meaningful magnitudes
+                if message.sensor_type == 1:
+                    d = numpy.array(numpy.degrees(message.data[axis])) / float(message.multiplier)
+                else:
+                    d = numpy.array(message.data[axis]) / float(message.multiplier)
+
+                # apply window to the input
+                d *= window
+                # perform RFFT
+                d_fft = numpy.fft.rfft(d)
+                # convert to squared complex magnitude
+                d_fft = numpy.square(abs(d_fft))
+                # remove DC component
+                d_fft[0] = 0
+                d_fft[-1] = 0
+                # accumulate the sums
+                sum_fft[axis] += d_fft
+
+            sample_rate = message.sample_rate_hz
+            counts += 1
+
+        numpy.seterr(divide = 'ignore')
+        psd = {}
+        for axis in [ "X","Y","Z" ]:
+            # normalize output to averaged PSD
+            psd[axis] = 2 * (sum_fft[axis] / counts) / (sample_rate * S2)
+            psd[axis] = 10 * numpy.log10 (psd[axis])
+
+        psd["F"] = freqmap
+
+        return psd

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -177,6 +177,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 
         dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
         _imu._gyro_last_sample_us[instance] = AP_HAL::micros64();
+        sample_us = _imu._gyro_last_sample_us[instance];
     }
 
 #if AP_MODULE_SUPPORTED
@@ -353,6 +354,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
 
         dt = 1.0f / _imu._accel_raw_sample_rates[instance];
         _imu._accel_last_sample_us[instance] = AP_HAL::micros64();
+        sample_us = _imu._accel_last_sample_us[instance];
     }
 
 #if AP_MODULE_SUPPORTED

libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp

@@ -66,24 +66,44 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
 
     if (sitl->motors_on) {
         // add extra noise when the motors are on
-        accel_noise += instance==0?sitl->accel_noise:sitl->accel2_noise;
+        accel_noise += instance == 0 ? sitl->accel_noise : sitl->accel2_noise;
     }
 
     // add accel bias and noise
-    Vector3f accel_bias = instance==0?sitl->accel_bias.get():sitl->accel2_bias.get();
+    Vector3f accel_bias = instance == 0 ? sitl->accel_bias.get() : sitl->accel2_bias.get();
     float xAccel = sitl->state.xAccel + accel_bias.x;
     float yAccel = sitl->state.yAccel + accel_bias.y;
     float zAccel = sitl->state.zAccel + accel_bias.z;
     const Vector3f &vibe_freq = sitl->vibe_freq;
-    if (vibe_freq.is_zero()) {
+    bool vibe_motor = !is_zero(sitl->vibe_motor);
+    if (vibe_freq.is_zero() && !vibe_motor) {
         xAccel += accel_noise * rand_float();
         yAccel += accel_noise * rand_float();
         zAccel += accel_noise * rand_float();
     } else {
-        float t = AP_HAL::micros() * 1.0e-6f;
-        xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * accel_noise;
-        yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * accel_noise;
-        zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * accel_noise;
+        if (vibe_motor) {
+            for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
+                float& phase = accel_motor_phase[instance][i];
+                float motor_freq = sitl->state.rpm[i] / 60.0f;
+                float phase_incr = motor_freq * 2 * M_PI / accel_sample_hz[instance];
+                phase += phase_incr;
+                if (phase_incr > M_PI) {
+                    phase -= 2 * M_PI;
+                }
+                else if (phase_incr < -M_PI) {
+                    phase += 2 * M_PI;
+                }
+                xAccel += sinf(phase) * accel_noise;
+                yAccel += sinf(phase) * accel_noise;
+                zAccel += sinf(phase) * accel_noise;
+            }
+        }
+        if (!vibe_freq.is_zero()) {
+            float t = AP_HAL::micros() * 1.0e-6f;
+            xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * accel_noise;
+            yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * accel_noise;
+            zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * accel_noise;
+        }
     }
 
     // correct for the acceleration due to the IMU position offset and angular acceleration
@@ -116,7 +136,7 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
 
     _rotate_and_correct_accel(accel_instance[instance], accel);
 
-    uint8_t nsamples = enable_fast_sampling(accel_instance[instance])?4:1;
+    uint8_t nsamples = enable_fast_sampling(accel_instance[instance]) ? 4 : 1;
     for (uint8_t i=0; i<nsamples; i++) {
         _notify_new_accel_raw_sample(accel_instance[instance], accel);
     }
@@ -142,29 +162,50 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
     float r = radians(sitl->state.yawRate) + gyro_drift();
 
     const Vector3f &vibe_freq = sitl->vibe_freq;
-    if (vibe_freq.is_zero()) {
+    bool vibe_motor = !is_zero(sitl->vibe_motor);
+    if (vibe_freq.is_zero() && !vibe_motor) {
         p += gyro_noise * rand_float();
         q += gyro_noise * rand_float();
         r += gyro_noise * rand_float();
     } else {
-        float t = AP_HAL::micros() * 1.0e-6f;
-        p += sinf(t * 2 * M_PI * vibe_freq.x) * gyro_noise;
-        q += sinf(t * 2 * M_PI * vibe_freq.y) * gyro_noise;
-        r += sinf(t * 2 * M_PI * vibe_freq.z) * gyro_noise;
+        if (vibe_motor) {
+            for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
+                float motor_freq = sitl->state.rpm[i] / 60.0f;
+                float phase_incr = motor_freq * 2 * M_PI / gyro_sample_hz[instance];
+                float& phase = gyro_motor_phase[instance][i];
+                phase += phase_incr;
+                if (phase_incr > M_PI) {
+                    phase -= 2 * M_PI;
+                }
+                else if (phase_incr < -M_PI) {
+                    phase += 2 * M_PI;
+                }
+
+                p += sinf(phase) * gyro_noise;
+                q += sinf(phase) * gyro_noise;
+                r += sinf(phase) * gyro_noise;
+            }
+        }
+        if (!vibe_freq.is_zero()) {
+            float t = AP_HAL::micros() * 1.0e-6f;
+            p += sinf(t * 2 * M_PI * vibe_freq.x) * gyro_noise;
+            q += sinf(t * 2 * M_PI * vibe_freq.y) * gyro_noise;
+            r += sinf(t * 2 * M_PI * vibe_freq.z) * gyro_noise;
+        }
     }
 
     Vector3f gyro = Vector3f(p, q, r);
 
     // add in gyro scaling
     Vector3f scale = sitl->gyro_scale;
-    gyro.x *= (1 + scale.x*0.01f);
-    gyro.y *= (1 + scale.y*0.01f);
-    gyro.z *= (1 + scale.z*0.01f);
+    gyro.x *= (1 + scale.x * 0.01f);
+    gyro.y *= (1 + scale.y * 0.01f);
+    gyro.z *= (1 + scale.z * 0.01f);
 
     _rotate_and_correct_gyro(gyro_instance[instance], gyro);
 
-    uint8_t nsamples = enable_fast_sampling(gyro_instance[instance])?8:1;
-    for (uint8_t i=0; i<nsamples; i++) {
+    uint8_t nsamples = enable_fast_sampling(gyro_instance[instance]) ? 8 : 1;
+    for (uint8_t i = 0; i < nsamples; i++) {
         _notify_new_gyro_raw_sample(gyro_instance[instance], gyro);
     }
 }
@@ -183,16 +224,26 @@ void AP_InertialSensor_SITL::timer_update(void)
         if (now >= next_accel_sample[i]) {
             if (((1U<<i) & sitl->accel_fail_mask) == 0) {
                 generate_accel(i);
-                while (now >= next_accel_sample[i]) {
-                    next_accel_sample[i] += 1000000UL / accel_sample_hz[i];
+                if (next_accel_sample[i] == 0) {
+                    next_accel_sample[i] = now + 1000000UL / accel_sample_hz[i];
+                }
+                else {
+                    while (now >= next_accel_sample[i]) {
+                        next_accel_sample[i] += 1000000UL / accel_sample_hz[i];
+                    }
                 }
             }
         }
         if (now >= next_gyro_sample[i]) {
             if (((1U<<i) & sitl->gyro_fail_mask) == 0) {
                 generate_gyro(i);
-                while (now >= next_gyro_sample[i]) {
-                    next_gyro_sample[i] += 1000000UL / gyro_sample_hz[i];
+                if (next_gyro_sample[i] == 0) {
+                    next_gyro_sample[i] = now + 1000000UL / gyro_sample_hz[i];
+                }
+                else {
+                    while (now >= next_gyro_sample[i]) {
+                        next_gyro_sample[i] += 1000000UL / gyro_sample_hz[i];
+                    }
                 }
             }
         }
@@ -211,7 +262,6 @@ float AP_InertialSensor_SITL::gyro_drift(void)
         return minutes * ToRad(sitl->drift_speed);
     }
     return (period - minutes) * ToRad(sitl->drift_speed);
-
 }
 
 

libraries/AP_InertialSensor/AP_InertialSensor_SITL.h

@@ -36,5 +36,7 @@ class AP_InertialSensor_SITL : public AP_InertialSensor_Backend
     uint8_t accel_instance[INS_SITL_INSTANCES];
     uint64_t next_gyro_sample[INS_SITL_INSTANCES];
     uint64_t next_accel_sample[INS_SITL_INSTANCES];
+    float gyro_motor_phase[INS_SITL_INSTANCES][12];
+    float accel_motor_phase[INS_SITL_INSTANCES][12];
 };
 #endif // CONFIG_HAL_BOARD