From d73709c4e3ea64e7b2980644811fb7f33b585a0a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 18 Aug 2023 17:43:52 +0100 Subject: [PATCH] mavfft_pid: allow PID target and error frequencies to be visualized --- tools/mavfft_pid.py | 34 +++++++++++++++++++++++++++------- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/tools/mavfft_pid.py b/tools/mavfft_pid.py index e2cc1f744..cd8822136 100755 --- a/tools/mavfft_pid.py +++ b/tools/mavfft_pid.py @@ -21,16 +21,29 @@ def fft(logfile): '''display fft for PID data in logfile''' + sample_rate = args.sample_rate + print("Processing log %s" % filename) mlog = mavutil.mavlink_connection(filename) - sample_rate = args.sample_rate - data = {'PIDR.rate' : 400, - 'PIDP.rate' : 400, - 'PIDY.rate' : 400, } + while True: + m = mlog.recv_match() + if m is None: + break + type = m.get_type() + if type == "PARM" and m.Name == 'SCHED_LOOP_RATE': + sample_rate = int(m.Value) + break + + + mlog = mavutil.mavlink_connection(filename) + + data = {'PIDR.rate' : sample_rate, + 'PIDP.rate' : sample_rate, + 'PIDY.rate' : sample_rate, } for gyr in ['PIDR','PIDP', 'PIDY']: - for ax in ['P', 'I', 'D']: + for ax in ['P', 'I', 'D', 'Tar', 'Err']: data[gyr+'.'+ax] = [] # now gather all the data @@ -43,14 +56,20 @@ def fft(logfile): data[type+'.P'].append(m.P) data[type+'.I'].append(m.I) data[type+'.D'].append(m.D) + data[type+'.Tar'].append(m.Tar) + data[type+'.Err'].append(m.Err) elif type == "PIDP": data[type+'.P'].append(m.P) data[type+'.I'].append(m.I) data[type+'.D'].append(m.D) + data[type+'.Tar'].append(m.Tar) + data[type+'.Err'].append(m.Err) elif type == "PIDY": data[type+'.P'].append(m.P) data[type+'.I'].append(m.I) data[type+'.D'].append(m.D) + data[type+'.Tar'].append(m.Tar) + data[type+'.Err'].append(m.Err) print("Extracted %u data points, sample rate %uHz" % (len(data['PIDR.P']), sample_rate)) @@ -61,7 +80,7 @@ def fft(logfile): for msg in ['PIDR', 'PIDP', 'PIDY']: pylab.figure() - for axis in ['P', 'I', 'D']: + for axis in ['P', 'I', 'D', 'Tar', 'Err']: field = msg + '.' + axis d = data[field] counts = len(d) // fs @@ -81,7 +100,8 @@ def fft(logfile): sum_fft += d_fft freq = numpy.fft.rfftfreq(fs, 1.0 / fs) # compute power spectral density - psd = numpy.sqrt((2 * sum_fft / counts) / (fs * S2)) + psd = numpy.sqrt((2 * sum_fft / counts) / (fs * S2)) + 0.00001 + psd = 10 * numpy.log10 (psd) pylab.plot(freq, psd, label=field) pylab.legend(loc='upper right')