mavfft_pid: allow PID target and error frequencies to be visualized

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')