framelen = 1. # length of each single frame over which to compute response

resplen = 0.5 # length of respose window

cutfreq = 25. # cutfreqency of what is considered as input

tuk_alpha = 1.0 # alpha of tukey window, if used

superpos = 16 # sub windowing (superpos windows in framelen)

threshold = 500. # threshold for 'high input rate'

noise_framelen = 0.3 # window width for noise analysis

noise_superpos = 16 # subsampling for noise analysis windows

def __init__(self, data):

self.data = data

self.input = self.equalize(data['time'], self.pid_in(data['p_err'], data['gyro'], data['P']))[1] # /20.

self.data.update({'input': self.pid_in(data['p_err'], data['gyro'], data['P'])})

self.equalize_data()

self.name = self.data['name']

self.time = self.data['time']

self.dt=self.time[0]-self.time[1]

self.input = self.data['input']

#enable this to generate artifical gyro trace with known system response

#self.data['gyro']=self.toy_out(self.input, delay=0.01, mode='normal')####

self.gyro = self.data['gyro']

self.throttle = self.data['throttle']

self.throt_hist, self.throt_scale = np.histogram(self.throttle, np.linspace(0, 100, 101, dtype=np.float64), normed=True)

self.flen = self.stepcalc(self.time, Trace.framelen) # array len corresponding to framelen in s

self.rlen = self.stepcalc(self.time, Trace.resplen) # array len corresponding to resplen in s

self.time_resp = self.time[0:self.rlen]-self.time[0]

self.stacks = self.winstacker({'time':[],'input':[],'gyro':[], 'throttle':[]}, self.flen, Trace.superpos) # [[time, input, output],]

self.window = np.hanning(self.flen) #self.tukeywin(self.flen, self.tuk_alpha)

self.spec_sm, self.avr_t, self.avr_in, self.max_in, self.max_thr = self.stack_response(self.stacks, self.window)

self.low_mask, self.high_mask = self.low_high_mask(self.max_in, self.threshold) #calcs masks for high and low inputs according to threshold

self.toolow_mask = self.low_high_mask(self.max_in, 20)[1] #mask for ignoring noisy low input

self.resp_sm = self.weighted_mode_avr(self.spec_sm, self.toolow_mask, [-1.5,3.5], 1000)

self.resp_quality = -self.to_mask((np.abs(self.spec_sm -self.resp_sm[0]).mean(axis=1)).clip(0.5-1e-9,0.5))+1.

# masking by setting trottle of unwanted traces to neg

self.thr_response = self.hist2d(self.max_thr * (2. * (self.toolow_mask*self.resp_quality) - 1.), self.time_resp,

(self.spec_sm.transpose() * self.toolow_mask).transpose(), [101, self.rlen])

self.resp_low = self.weighted_mode_avr(self.spec_sm, self.low_mask*self.toolow_mask, [-1.5,3.5], 1000)

if self.high_mask.sum()>0:

self.resp_high = self.weighted_mode_avr(self.spec_sm, self.high_mask*self.toolow_mask, [-1.5,3.5], 1000)

self.noise_winlen = self.stepcalc(self.time, Trace.noise_framelen)

self.noise_stack = self.winstacker({'time':[], 'gyro':[], 'throttle':[], 'd_err':[], 'debug':[]},

self.noise_winlen, Trace.noise_superpos)

self.noise_win = np.hanning(self.noise_winlen)

self.noise_gyro = self.stackspectrum(self.noise_stack['time'],self.noise_stack['throttle'],self.noise_stack['gyro'], self.noise_win)

self.noise_d = self.stackspectrum(self.noise_stack['time'], self.noise_stack['throttle'], self.noise_stack['d_err'], self.noise_win)

self.noise_debug = self.stackspectrum(self.noise_stack['time'], self.noise_stack['throttle'], self.noise_stack['debug'], self.noise_win)

if self.noise_debug['hist2d'].sum()>0:

## mask 0 entries

thr_mask = self.noise_gyro['throt_hist_avr'].clip(0,1)

self.filter_trans = np.average(self.noise_gyro['hist2d'], axis=1, weights=thr_mask)/\

np.average(self.noise_debug['hist2d'], axis=1, weights=thr_mask)

else:

self.filter_trans = self.noise_gyro['hist2d'].mean(axis=1)*0.

@staticmethod

def low_high_mask(signal, threshold):

low = np.copy(signal)

low[low <=threshold] = 1.

low[low > threshold] = 0.

high = -low+1.

if high.sum() < 10: # ignore high pinput that is too short

high *= 0.

return low, high

def to_mask(self, clipped):

clipped-=clipped.min()

clipped/=clipped.max()

return clipped

def pid_in(self, pval, gyro, pidp):

pidin = gyro + pval / (0.032029 * pidp) # 0.032029 is P scaling factor from betaflight

return pidin

def rate_curve(self, rcin, inmax=500., outmax=800., rate=160.):

### an estimated rate curve. not used.

expoin = (np.exp((rcin - inmax) / rate) - np.exp((-rcin - inmax) / rate)) * outmax

return expoin

def calc_delay(self, time, trace1, trace2):

### minimizes trace1-trace2 by shifting trace1

tf1 = interp1d(time[2000:-2000], trace1[2000:-2000], fill_value=0., bounds_error=False)

tf2 = interp1d(time[2000:-2000], trace2[2000:-2000], fill_value=0., bounds_error=False)

fun = lambda x: ((tf1(time - x*0.5) - tf2(time+ x*0.5)) ** 2).mean()

shift = minimize(fun, np.array([0.01])).x[0]

steps = np.round(shift / (time[1] - time[0]))

return {'time':shift, 'steps':int(steps)}

def tukeywin(self, len, alpha=0.5):

### makes tukey widow for envelopig

M = len

n = np.arange(M - 1.) #

if alpha <= 0:

return np.ones(M) # rectangular window

elif alpha >= 1:

return np.hanning(M)

# Normal case

x = np.linspace(0, 1, M, dtype=np.float64)

w = np.ones(x.shape)

# first condition 0 <= x < alpha/2

first_condition = x < alpha / 2

w[first_condition] = 0.5 * (1 + np.cos(2 * np.pi / alpha * (x[first_condition] - alpha / 2)))

# second condition already taken care of

# third condition 1 - alpha / 2 <= x <= 1

third_condition = x >= (1 - alpha / 2)

w[third_condition] = 0.5 * (1 + np.cos(2 * np.pi / alpha * (x[third_condition] - 1 + alpha / 2)))

return w

def toy_out(self, inp, delay=0.01, length=0.01, noise=5., mode='normal', sinfreq=100.):

# generates artificial output for benchmarking

freq= 1./(self.time[1]-self.time[0])

toyresp = np.zeros(int((delay+length)*freq))

toyresp[int((delay)*freq):]=1.

toyresp/=toyresp.sum()

toyout = np.convolve(inp, toyresp, mode='full')[:len(inp)]#*0.9

if mode=='normal':

noise_sig = (np.random.random_sample(len(toyout))-0.5)*noise

elif mode=='sin':

noise_sig = (np.sin(2.*np.pi*self.time*sinfreq)) * noise

else:

noise_sig=0.

return toyout+noise_sig

def equalize(self, time, data):

### equalizes time scale

data_f = interp1d(time, data)

newtime = np.linspace(time[0], time[-1], len(time), dtype=np.float64)

return newtime, data_f(newtime)

def equalize_data(self):

### equalizes full dict of data

time = self.data['time']

newtime = np.linspace(time[0], time[-1], len(time), dtype=np.float64)

for key in self.data:

if isinstance(self.data[key],np.ndarray):

if len(self.data[key])==len(time):

self.data[key]= interp1d(time, self.data[key])(newtime)

self.data['time']=newtime

def stepcalc(self, time, duration):

### calculates frequency and resulting windowlength

tstep = (time[1]-time[0])

freq = 1./tstep

arr_len = duration * freq

return int(arr_len)

def winstacker(self, stackdict, flen, superpos):

### makes stack of windows for deconvolution

tlen = len(self.data['time'])

shift = int(flen/superpos)

wins = int(tlen/shift)-superpos

for i in np.arange(wins):

for key in stackdict.keys():

stackdict[key].append(self.data[key][i * shift:i * shift + flen])

for k in stackdict.keys():

#print 'key',k

#print stackdict[k]

stackdict[k]=np.array(stackdict[k], dtype=np.float64)

return stackdict

def wiener_deconvolution(self, input, output, cutfreq): # input/output are two-dimensional

pad = 1024 - (len(input[0]) % 1024) # padding to power of 2, increases transform speed

input = np.pad(input, [[0,0],[0,pad]], mode='constant')

output = np.pad(output, [[0, 0], [0, pad]], mode='constant')

H = np.fft.fft(input, axis=-1)

G = np.fft.fft(output,axis=-1)

freq = np.abs(np.fft.fftfreq(len(input[0]), self.dt))

sn = self.to_mask(np.clip(np.abs(freq), cutfreq-1e-9, cutfreq))

len_lpf=np.sum(np.ones_like(sn)-sn)

sn=self.to_mask(gaussian_filter1d(sn,len_lpf/6.))

sn= 10.*(-sn+1.+1e-9) # +1e-9 to prohibit 0/0 situations

Hcon = np.conj(H)

deconvolved_sm = np.real(np.fft.ifft(G * Hcon / (H * Hcon + 1./sn),axis=-1))

return deconvolved_sm

def stack_response(self, stacks, window):

inp = stacks['input'] * window

outp = stacks['gyro'] * window

thr = stacks['throttle'] * window

deconvolved_sm = self.wiener_deconvolution(inp, outp, self.cutfreq)[:, :self.rlen]

delta_resp = deconvolved_sm.cumsum(axis=1)

max_thr = np.abs(np.abs(thr)).max(axis=1)

avr_in = np.abs(np.abs(inp)).mean(axis=1)

max_in = np.max(np.abs(inp), axis=1)

avr_t = stacks['time'].mean(axis=1)

return delta_resp, avr_t, avr_in, max_in, max_thr

def spectrum(self, time, traces):

### fouriertransform for noise analysis. returns frequencies and spectrum.

pad = 1024 - (len(traces[0]) % 1024) # padding to power of 2, increases transform speed

traces = np.pad(traces, [[0, 0], [0, pad]], mode='constant')

trspec = np.fft.rfft(traces, axis=-1, norm='ortho')

trfreq = np.fft.rfftfreq(len(traces[0]), time[1] - time[0])

return trfreq, trspec

def stackfilter(self, time, trace_ref, trace_filt, window):

### calculates filter transmission and phaseshift from stack of windows. Not in use, maybe later.

# slicing off last 2s to get rid of landing

#maybe pass throttle for further analysis...

filt = trace_filt[:-int(Trace.noise_superpos * 2. / Trace.noise_framelen), :] * window

ref = trace_ref[:-int(Trace.noise_superpos * 2. / Trace.noise_framelen), :] * window

time = time[:-int(Trace.noise_superpos * 2. / Trace.noise_framelen), :]

full_freq_f, full_spec_f = self.spectrum(self.data['time'], [self.data['gyro']])

full_freq_r, full_spec_r = self.spectrum(self.data['time'], [self.data['debug']])

f_amp_freq, f_amp_hist =np.histogram(full_freq_f, weights=np.abs(full_spec_f.real).flatten(), bins=int(full_freq_f[-1]))

r_amp_freq, r_amp_hist = np.histogram(full_freq_r, weights=np.abs(full_spec_r.real).flatten(), bins=int(full_freq_r[-1]))

def hist2d(self, x, y, weights, bins): #bins[nx,ny]

### generates a 2d hist from input 1d axis for x,y. repeats them to match shape of weights X*Y (data points)

### x will be 0-100%

freqs = np.repeat(np.array([y], dtype=np.float64), len(x), axis=0)

throts = np.repeat(np.array([x], dtype=np.float64), len(y), axis=0).transpose()

throt_hist_avr, throt_scale_avr = np.histogram(x, 101, [0, 100])

hist2d = np.histogram2d(throts.flatten(), freqs.flatten(),

range=[[0, 100], [y[0], y[-1]]],

bins=bins, weights=weights.flatten(), normed=False)[0].transpose()

hist2d = np.array(abs(hist2d), dtype=np.float64)

hist2d_norm = np.copy(hist2d)

hist2d_norm /= (throt_hist_avr + 1e-9)

return {'hist2d_norm':hist2d_norm, 'hist2d':hist2d, 'throt_hist':throt_hist_avr,'throt_scale':throt_scale_avr}

def stackspectrum(self, time, throttle, trace, window):

### calculates spectrogram from stack of windows against throttle.

# slicing off last 2s to get rid of landing

gyro = trace[:-int(Trace.noise_superpos*2./Trace.noise_framelen),:] * window

thr = throttle[:-int(Trace.noise_superpos*2./Trace.noise_framelen),:] * window

time = time[:-int(Trace.noise_superpos*2./Trace.noise_framelen),:]

freq, spec = self.spectrum(time[0], gyro)

weights = abs(spec.real)

avr_thr = np.abs(thr).max(axis=1)

hist2d=self.hist2d(avr_thr, freq,weights,[101,len(freq)/4])

filt_width = 3 # width of gaussian smoothing for hist data

hist2d_sm = gaussian_filter1d(hist2d['hist2d_norm'], filt_width, axis=1, mode='constant')

# get max value in histogram >100hz

thresh = 100.

mask = self.to_mask(freq[:-1:4].clip(thresh-1e-9,thresh))

maxval = np.max(hist2d_sm.transpose()*mask)

return {'throt_hist_avr':hist2d['throt_hist'],'throt_axis':hist2d['throt_scale'],'freq_axis':freq[::4],

'hist2d_norm':hist2d['hist2d_norm'], 'hist2d_sm':hist2d_sm, 'hist2d':hist2d['hist2d'], 'max':maxval}

def weighted_mode_avr(self, values, weights, vertrange, vertbins):

### finds the most common trace and std

threshold = 0.5 # threshold for std calculation

filt_width = 7 # width of gaussian smoothing for hist data

resp_y = np.linspace(vertrange[0], vertrange[-1], vertbins, dtype=np.float64)

times = np.repeat(np.array([self.time_resp],dtype=np.float64), len(values), axis=0)

weights = np.repeat(weights, len(values[0]))

hist2d = np.histogram2d(times.flatten(), values.flatten(),

range=[[self.time_resp[0], self.time_resp[-1]], vertrange],

bins=[len(times[0]), vertbins], weights=weights.flatten())[0].transpose()

### shift outer edges by +-1e-5 (10us) bacause of dtype32. Otherwise different precisions lead to artefacting.

### solution to this --> somethings strage here. In outer most edges some bins are doubled, some are empty.

### Hence sometimes produces "divide by 0 error" in "/=" operation.

if hist2d.sum():

hist2d_sm = gaussian_filter1d(hist2d, filt_width, axis=0, mode='constant')

hist2d_sm /= np.max(hist2d_sm, 0)

pixelpos = np.repeat(resp_y.reshape(len(resp_y), 1), len(times[0]), axis=1)

avr = np.average(pixelpos, 0, weights=hist2d_sm * hist2d_sm)

else:

hist2d_sm = hist2d

avr = np.zeros_like(self.time_resp)

# only used for monochrome error width

hist2d[hist2d <= threshold] = 0.

hist2d[hist2d > threshold] = 0.5 / (vertbins / (vertrange[-1] - vertrange[0]))

std = np.sum(hist2d, 0)

return avr, std, [self.time_resp, resp_y, hist2d_sm]

### calculates weighted avverage and resulting errors

def weighted_avg_and_std(self, values, weights):

average = np.average(values, axis=0, weights=weights)

variance = np.average((values - average) ** 2, axis=0, weights=weights)

def __init__(self, fpath, name, headdict, noise_bounds):

self.file = fpath

self.name = name

self.headdict = headdict

self.data = self.readcsv(self.file)

logging.info('Processing:')

self.traces = self.find_traces(self.data)

self.roll, self.pitch, self.yaw = self.__analyze()

self.fig_resp = self.plot_all_resp([self.roll, self.pitch, self.yaw])

self.fig_noise = self.plot_all_noise([self.roll, self.pitch, self.yaw],noise_bounds)

def check_lims_list(self,lims):

if type(lims) is list:

l=np.array(lims)

if str(np.shape(l))=='(4L, 2L)':

ll=l[:,1]-l[:,0]

if np.sum(np.abs((ll-np.abs(ll))))==0:

return True

else:

logging.info('noise_bounds is no valid list')

return False

def plot_all_noise(self, traces, lims): #style='fancy' gives 2d hist for response

textsize = 7

rcParams.update({'font.size': 9})

logging.info('Making noise plot...')

fig = plt.figure('Noise plot: Log number: ' + self.headdict['logNum']+' '+self.file , figsize=(16, 8))

### gridspec devides window into 25 horizontal, 31 vertical fields

gs1 = GridSpec(25, 3 * 10+2, wspace=0.6, hspace=0.7, left=0.04, right=1., bottom=0.05, top=0.97)

max_noise_gyro = np.max([traces[0].noise_gyro['max'],traces[1].noise_gyro['max'],traces[2].noise_gyro['max']])+1.

max_noise_debug = np.max([traces[0].noise_debug['max'], traces[1].noise_debug['max'], traces[2].noise_debug['max']])+1.

max_noise_d = np.max([traces[0].noise_d['max'], traces[1].noise_d['max'], traces[2].noise_d['max']])+1.

meanspec = np.array([traces[0].noise_gyro['hist2d_sm'].mean(axis=1).flatten(),

traces[1].noise_gyro['hist2d_sm'].mean(axis=1).flatten(),

traces[2].noise_gyro['hist2d_sm'].mean(axis=1).flatten()],dtype=np.float64)

thresh = 100.

mask = traces[0].to_mask(traces[0].noise_gyro['freq_axis'].clip(thresh-1e-9,thresh))

meanspec_max = np.max(meanspec*mask[:-1])

if not self.check_lims_list(lims):

lims=np.array([[1,max_noise_gyro],[1, max_noise_debug], [1, max_noise_d], [0,meanspec_max*1.5]])

if lims[0,1] == 1:

lims[0,1]=100.

if lims[1, 1] == 1:

lims[1, 1]=100.

if lims[2, 1] == 1:

lims[2, 1]=100.

else:

lims=np.array(lims)

cax_gyro = plt.subplot(gs1[0, 0:7])

cax_debug = plt.subplot(gs1[0, 8:15])

cax_d = plt.subplot(gs1[0, 16:23])

cmap='viridis'

axes_gyro = []

axes_debug = []

axes_d = []

axes_trans = []

for i, tr in enumerate(traces):

if tr.noise_gyro['freq_axis'][-1]>1000:

pltlim = [0,1000]

else:

pltlim = [tr.noise_gyro['freq_axis'][-0],tr.noise_gyro['freq_axis'][-1]]

# gyro plots

ax0 = plt.subplot(gs1[1+i*8:1+i*8+8 , 0:7])

if len(axes_gyro):

axes_gyro[0].get_shared_x_axes().join(axes_gyro[0], ax0)

axes_gyro.append(ax0)

ax0.set_title('gyro '+tr.name, y=0.88, color='w')

pc0 = plt.pcolormesh(tr.noise_gyro['throt_axis'], tr.noise_gyro['freq_axis'], tr.noise_gyro['hist2d_sm']+1.,norm=colors.LogNorm(vmin=lims[0,0],vmax=lims[0,1]),cmap=cmap)

ax0.set_ylabel('frequency in Hz')

ax0.grid()

ax0.set_ylim(pltlim)

if i < 2:

plt.setp(ax0.get_xticklabels(), visible=False)

else:

ax0.set_xlabel('throttle in %')

fig.colorbar(pc0, cax_gyro, orientation='horizontal')

cax_gyro.xaxis.set_ticks_position('top')

cax_gyro.xaxis.set_tick_params(pad=-0.5)

if max_noise_gyro == 1.:

ax0.text(0.5, 0.5, 'no gyro[' + str(i) + '] trace found!\n',

horizontalalignment='center', verticalalignment='center',

transform=ax0.transAxes, fontdict={'color': 'white'})

# debug plots

ax1 = plt.subplot(gs1[1+i*8:1+i*8+8 , 8:15])

if len(axes_debug):

axes_debug[0].get_shared_x_axes().join(axes_debug[0], ax1)

axes_debug.append(ax1)

ax1.set_title('debug ' + tr.name, y=0.88, color='w')

pc1 = plt.pcolormesh(tr.noise_debug['throt_axis'],tr.noise_debug['freq_axis'], tr.noise_debug['hist2d_sm']+1., norm=colors.LogNorm(vmin=lims[1,0],vmax=lims[1,1]),cmap=cmap)

ax1.set_ylabel('frequency in Hz')

ax1.grid()

ax1.set_ylim(pltlim)

if i<2:

plt.setp(ax1.get_xticklabels(), visible=False)

else:

ax1.set_xlabel('throttle in %')

fig.colorbar(pc1, cax_debug, orientation='horizontal')

cax_debug.xaxis.set_ticks_position('top')

cax_debug.xaxis.set_tick_params(pad=-0.5)

if max_noise_debug==1.:

ax1.text(0.5, 0.5, 'no debug['+str(i)+'] trace found!\n'

'To get transmission of\n'

'- all filters: set debug_mode = NOTCH\n'

'- LPF only: set debug_mode = GYRO', horizontalalignment='center', verticalalignment = 'center',

transform = ax1.transAxes,fontdict={'color': 'white'})

if i<2:

# dterm plots

ax2 = plt.subplot(gs1[1 + i * 8:1 + i * 8 + 8, 16:23])

if len(axes_d):

axes_d[0].get_shared_x_axes().join(axes_d[0], ax2)

axes_d.append(ax2)

ax2.set_title('D-term ' + tr.name, y=0.88, color='w')

pc2 = plt.pcolormesh(tr.noise_d['throt_axis'], tr.noise_d['freq_axis'], tr.noise_d['hist2d_sm']+1., norm=colors.LogNorm(vmin=lims[2,0],vmax=lims[2,1]),cmap=cmap)

ax2.set_ylabel('frequency in Hz')

ax2.grid()

ax2.set_ylim(pltlim)

plt.setp(ax2.get_xticklabels(), visible=False)

fig.colorbar(pc2, cax_d, orientation='horizontal')

cax_d.xaxis.set_ticks_position('top')

cax_d.xaxis.set_tick_params(pad=-0.5)

if max_noise_d == 1.:

ax2.text(0.5, 0.5, 'no D[' + str(i) + '] trace found!\n',

horizontalalignment='center', verticalalignment='center',

transform=ax2.transAxes, fontdict={'color': 'white'})

else:

# throttle plots

ax21 = plt.subplot(gs1[1 + i * 8:1 + i * 8 + 4, 16:23])

ax22 = plt.subplot(gs1[1 + i * 8 + 5:1 + i * 8 + 8, 16:23])

ax21.bar(tr.throt_scale[:-1], tr.throt_hist*100., width=1.,align='edge', color='black', alpha=0.2, label='throttle distribution')

axes_d[0].get_shared_x_axes().join(axes_d[0], ax21)

ax21.vlines(self.headdict['tpa_percent'], 0., 100., label='tpa', colors='red', alpha=0.5)

ax21.grid()

ax21.set_ylim([0., np.max(tr.throt_hist) * 100. * 1.1])

ax21.set_xlabel('throttle in %')

ax21.set_ylabel('usage %')

ax21.set_xlim([0.,100.])

handles, labels = ax21.get_legend_handles_labels()

ax21.legend(handles[::-1], labels[::-1])

ax22.fill_between(tr.time, 0., tr.throttle, label='throttle input', facecolors='black', alpha=0.2)

ax22.hlines(self.headdict['tpa_percent'],tr.time[0], tr.time[-1], label='tpa', colors='red', alpha=0.5)

ax22.set_ylabel('throttle in %')

ax22.legend()

ax22.grid()

ax22.set_ylim([0.,100.])

ax22.set_xlim([tr.time[0],tr.time[-1]])

ax22.set_xlabel('time in s')

# transmission plots

ax3 = plt.subplot(gs1[1+i*8:1+i*8+8 , 24:30])

if len(axes_trans):

axes_trans[0].get_shared_x_axes().join(axes_trans[0], ax3)

axes_trans.append(ax3)

ax3.fill_between(tr.noise_gyro['freq_axis'][:-1], 0, meanspec[i], label=tr.name + ' gyro noise', alpha=0.2)

ax3.set_ylim(lims[3])

ax3.set_ylabel(tr.name+' gyro noise a.u.')

ax3.grid()

ax3r = plt.twinx(ax3)

ax3r.plot(tr.noise_gyro['freq_axis'][:-1], tr.filter_trans*100., label=tr.name + ' filter transmission')

ax3r.set_ylabel('transmission in %')

ax3r.set_ylim([0., 100.])

ax3r.set_xlim([tr.noise_gyro['freq_axis'][0],tr.noise_gyro['freq_axis'][-2]])

lines, labels = ax3.get_legend_handles_labels()

lines2, labels2 = ax3r.get_legend_handles_labels()

ax3r.legend(lines+lines2, labels+labels2, loc=1)

if i < 2:

plt.setp(ax3.get_xticklabels(), visible=False)

else:

ax3.set_xlabel('frequency in hz')

meanfreq = 1./(traces[0].time[1]-traces[0].time[0])

ax4 = plt.subplot(gs1[12, -1])

t = Version+"| Betaflight: Version "+self.headdict['version']+' | Craftname: '+self.headdict['craftName']+\

' | meanFreq: '+str(int(meanfreq))+' | rcRate/Expo: '+self.headdict['rcRate']+'/'+ self.headdict['rcExpo']+'\nrcYawRate/Expo: '+self.headdict['rcYawRate']+'/' \

+self.headdict['rcYawExpo']+' | deadBand: '+self.headdict['deadBand']+' | yawDeadBand: '+self.headdict['yawDeadBand'] \

+' | Throttle min/tpa/max: ' + self.headdict['minThrottle']+'/'+self.headdict['tpa_breakpoint']+'/'+self.headdict['maxThrottle'] \

+ ' | dynThrPID: ' + self.headdict['dynThrottle']+ '| D-TermSP: ' + self.headdict['dTermSetPoint']+'| vbatComp: ' + self.headdict['vbatComp']+' | debug '+ self.headdict['debug_mode']

ax4.text(0, 0, t, ha='left', va='center', rotation=90, color='grey', alpha=0.5, fontsize=textsize)

ax4.axis('off')

ax5l = plt.subplot(gs1[:1, 24:27])

ax5r = plt.subplot(gs1[:1, 27:30])

ax5l.axis('off')

ax5r.axis('off')

filt_settings_l = 'G lpf type: '+self.headdict['gyro_lpf']+' at '+self.headdict['gyro_lowpass_hz']+'\n'+\

'G notch at: '+self.headdict['gyro_notch_hz']+' cut '+self.headdict['gyro_notch_cutoff']+'\n'\

'gyro lpf 2: '+self.headdict['gyro_lowpass_type']

filt_settings_r = '| D lpf type: ' + self.headdict['dterm_filter_type'] + ' at ' + self.headdict['dterm_lpf_hz'] + '\n' + \

'| D notch at: ' + self.headdict['dterm_notch_hz'] + ' cut ' + self.headdict['dterm_notch_cutoff'] + '\n' + \

'| Yaw lpf at: ' + self.headdict['yaw_lpf_hz']

ax5l.text(0, 0, filt_settings_l, ha='left', fontsize=textsize)

ax5r.text(0, 0, filt_settings_r, ha='left', fontsize=textsize)

logging.info('Saving as image...')

plt.savefig(self.file[:-13] + self.name + '_' + str(self.headdict['logNum'])+'_noise.png')

return fig

def plot_all_resp(self, traces, style='ra'): # style='raw' for response vs. time in color plot

textsize = 7

titelsize = 10

rcParams.update({'font.size': 9})

logging.info('Making PID plot...')

fig = plt.figure('Response plot: Log number: ' + self.headdict['logNum']+' '+self.file , figsize=(16, 8))

### gridspec devides window into 24 horizontal, 3*10 vertical fields

gs1 = GridSpec(24, 3 * 10, wspace=0.6, hspace=0.7, left=0.04, right=1., bottom=0.05, top=0.97)

for i, tr in enumerate(traces):

ax0 = plt.subplot(gs1[0:6, i*10:i*10+9])

plt.title(tr.name)

plt.plot(tr.time, tr.gyro, label=tr.name + ' gyro')

plt.plot(tr.time, tr.input, label=tr.name + ' loop input')

plt.ylabel('degrees/second')

ax0.get_yaxis().set_label_coords(-0.1, 0.5)

plt.grid()

tracelim = np.max([np.abs(tr.gyro),np.abs(tr.input)])

plt.ylim([-tracelim*1.1, tracelim*1.1])

plt.legend(loc=1)

plt.setp(ax0.get_xticklabels(), visible=False)

ax1 = plt.subplot(gs1[6:8, i*10:i*10+9], sharex=ax0)

plt.hlines(self.headdict['tpa_percent'], tr.time[0], tr.time[-1], label='tpa', colors='red', alpha=0.5)

plt.fill_between(tr.time, 0., tr.throttle, label='throttle', color='grey', alpha=0.2)

plt.ylabel('throttle %')

ax1.get_yaxis().set_label_coords(-0.1, 0.5)

plt.grid()

plt.xlim([tr.time[0], tr.time[-1]])

plt.ylim([0, 100])

plt.legend(loc=1)

plt.xlabel('log time in s')

if style =='raw':

###old raw data plot.

plt.setp(ax1.get_xticklabels(), visible=False)

ax2 = plt.subplot(gs1[9:16, i*10:i*10+9], sharex=ax0)

plt.pcolormesh(tr.avr_t, tr.time_resp, np.transpose(tr.spec_sm), vmin=0, vmax=2.)

plt.ylabel('response time in s')

ax2.get_yaxis().set_label_coords(-0.1, 0.5)

plt.xlabel('log time in s')

plt.xlim([tr.avr_t[0], tr.avr_t[-1]])

else:

###response vs throttle plot. more useful.

ax2 = plt.subplot(gs1[9:16, i * 10:i * 10 + 9])

plt.title(tr.name + ' response', y=0.88, color='w')

plt.pcolormesh(tr.thr_response['throt_scale'], tr.time_resp, tr.thr_response['hist2d_norm'], vmin=0., vmax=2.)

plt.ylabel('response time in s')

ax2.get_yaxis().set_label_coords(-0.1, 0.5)

plt.xlabel('throttle in %')

plt.xlim([0.,100.])

theCM = plt.cm.get_cmap('Blues')

theCM._init()

alphas = np.abs(np.linspace(0., 0.5, theCM.N, dtype=np.float64))

theCM._lut[:-3,-1] = alphas

ax3 = plt.subplot(gs1[17:, i*10:i*10+9])

plt.contourf(*tr.resp_low[2], cmap=theCM, linestyles=None, antialiased=True, levels=np.linspace(0,1,20, dtype=np.float64))

plt.plot(tr.time_resp, tr.resp_low[0],

label=tr.name + ' step response ' + '(<' + str(int(Trace.threshold)) + ') '

+ ' PID ' + self.headdict[tr.name + 'PID'])

if tr.high_mask.sum() > 0:

theCM = plt.cm.get_cmap('Oranges')

theCM._init()

alphas = np.abs(np.linspace(0., 0.5, theCM.N, dtype=np.float64))

theCM._lut[:-3,-1] = alphas

plt.contourf(*tr.resp_high[2], cmap=theCM, linestyles=None, antialiased=True, levels=np.linspace(0,1,20, dtype=np.float64))

plt.plot(tr.time_resp, tr.resp_high[0],

label=tr.name + ' step response ' + '(>' + str(int(Trace.threshold)) + ') '

+ ' PID ' + self.headdict[tr.name + 'PID'])

plt.xlim([-0.001,0.501])

plt.legend(loc=1)

plt.ylim([0., 2])

plt.ylabel('strength')

ax3.get_yaxis().set_label_coords(-0.1, 0.5)

plt.xlabel('response time in s')

plt.grid()

meanfreq = 1./(traces[0].time[1]-traces[0].time[0])

ax4 = plt.subplot(gs1[12, -1])

t = Version+" | Betaflight: Version "+self.headdict['version']+' | Craftname: '+self.headdict['craftName']+\

' | meanFreq: '+str(int(meanfreq))+' | rcRate/Expo: '+self.headdict['rcRate']+'/'+ self.headdict['rcExpo']+'\nrcYawRate/Expo: '+self.headdict['rcYawRate']+'/' \

+self.headdict['rcYawExpo']+' | deadBand: '+self.headdict['deadBand']+' | yawDeadBand: '+self.headdict['yawDeadBand'] \

+' | Throttle min/tpa/max: ' + self.headdict['minThrottle']+'/'+self.headdict['tpa_breakpoint']+'/'+self.headdict['maxThrottle'] \

+ ' | dynThrPID: ' + self.headdict['dynThrottle']+ '| D-TermSP: ' + self.headdict['dTermSetPoint']+'| vbatComp: ' + self.headdict['vbatComp']

plt.text(0, 0, t, ha='left', va='center', rotation=90, color='grey', alpha=0.5, fontsize=textsize)

ax4.axis('off')

logging.info('Saving as image...')

plt.savefig(self.file[:-13] + self.name + '_' + str(self.headdict['logNum'])+'_response.png')

return fig

def __analyze(self):

analyzed = []

for t in self.traces:

logging.info(t['name'] + '... ')

analyzed.append(Trace(t))

return analyzed

def readcsv(self, fpath):

logging.info('Reading: Log '+str(self.headdict['logNum']))

datdic = {}

### keycheck for 'usecols' only reads usefull traces, uncommend if needed

wanted = ['time (us)',

'rcCommand[0]', 'rcCommand[1]', 'rcCommand[2]', 'rcCommand[3]',

'axisP[0]','axisP[1]','axisP[2]',

'axisI[0]', 'axisI[1]', 'axisI[2]',

'axisD[0]', 'axisD[1]','axisD[2]',

'gyroADC[0]', 'gyroADC[1]', 'gyroADC[2]',

'gyroData[0]', 'gyroData[1]', 'gyroData[2]',

'ugyroADC[0]', 'ugyroADC[1]', 'ugyroADC[2]',

#'accSmooth[0]','accSmooth[1]', 'accSmooth[2]',

'debug[0]', 'debug[1]', 'debug[2]','debug[3]',

#'motor[0]', 'motor[1]', 'motor[2]', 'motor[3]',

#'energyCumulative (mAh)','vbatLatest (V)', 'amperageLatest (A)'

]

data = read_csv(fpath, header=0, skipinitialspace=1, usecols=lambda k: k in wanted, dtype=np.float64)

datdic.update({'time_us': data['time (us)'].values * 1e-6})

datdic.update({'throttle': data['rcCommand[3]'].values})

for i in ['0', '1', '2']:

datdic.update({'rcCommand' + i: data['rcCommand['+i+']'].values})

#datdic.update({'PID loop in' + i: data['axisP[' + i + ']'].values})

try:

datdic.update({'debug' + i: data['debug[' + i + ']'].values})

except:

logging.warning('No debug['+str(i)+'] trace found!')

datdic.update({'debug' + i: np.zeros_like(data['rcCommand[' + i + ']'].values)})

# get P trace (including case of missing trace)

try:

datdic.update({'PID loop in' + i: data['axisP[' + i + ']'].values})

except:

logging.warning('No P['+str(i)+'] trace found!')

datdic.update({'PID loop in' + i: np.zeros_like(data['rcCommand[' + i + ']'].values)})

try:

datdic.update({'d_err'+i: data['axisD[' + i+']'].values})

except:

logging.warning('No D['+str(i)+'] trace found!')