Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Simulate motor noise in the gyros and accels #12431

Open
wants to merge 7 commits into
base: master
from
@@ -7,6 +7,7 @@
import os
import shutil
import time
import numpy

from pymavlink import mavutil
from pymavlink import mavextra
@@ -1847,6 +1848,77 @@ 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()
vibe_log = self.buildlogs_path("ArduCopter-vibration-log.bin")
self.log_download(vibe_log)
psd = self.mavfft_fttd(vibe_log, 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()
vibe_log = self.buildlogs_path("ArduCopter-vibration-log-notch.bin")
self.log_download(vibe_log)
psd = self.mavfft_fttd(vibe_log, 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 +4475,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

@@ -11,6 +11,7 @@
import pexpect
import fnmatch
import operator
import numpy

from MAVProxy.modules.lib import mp_util

@@ -3662,3 +3663,99 @@ def autotest(self):
ret = self.run_tests(tests)
self.post_tests_announcements()
return ret

def mavfft_fttd(self, logfile, sensor_type, sensor_instance, since, until):
'''display fft for raw ACC data in 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)

print("Processing log %s" % logfile)
mlog = mavutil.mavlink_connection(logfile)

# 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
@@ -181,4 +181,5 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin
define BOARD_PWM_COUNT_DEFAULT 9

define STM32_PWM_USE_ADVANCED TRUE
define HAL_MINIMIZE_FEATURES 1

@@ -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
@@ -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);

}


ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.