EL4121 Embedded System / mbed-os

mbed-os

Dependents:   cobaLCDJoyMotor_Thread odometry_omni_3roda_v3 odometry_omni_3roda_v1 odometry_omni_3roda_v2 ... more

tools/dev/dsp_fir.py@0:b74591d5ab33, 2017-12-11 (annotated)

Committer:
be_bryan
Date:
Mon Dec 11 17:54:04 2017 +0000
Revision:
0:b74591d5ab33
motor ++

Who changed what in which revision?

UserRevisionLine numberNew contents of line
be_bryan 0:b74591d5ab33 1 """
be_bryan 0:b74591d5ab33 2 mbed SDK
be_bryan 0:b74591d5ab33 3 Copyright (c) 2011-2013 ARM Limited
be_bryan 0:b74591d5ab33 4
be_bryan 0:b74591d5ab33 5 Licensed under the Apache License, Version 2.0 (the "License");
be_bryan 0:b74591d5ab33 6 you may not use this file except in compliance with the License.
be_bryan 0:b74591d5ab33 7 You may obtain a copy of the License at
be_bryan 0:b74591d5ab33 8
be_bryan 0:b74591d5ab33 9 http://www.apache.org/licenses/LICENSE-2.0
be_bryan 0:b74591d5ab33 10
be_bryan 0:b74591d5ab33 11 Unless required by applicable law or agreed to in writing, software
be_bryan 0:b74591d5ab33 12 distributed under the License is distributed on an "AS IS" BASIS,
be_bryan 0:b74591d5ab33 13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
be_bryan 0:b74591d5ab33 14 See the License for the specific language governing permissions and
be_bryan 0:b74591d5ab33 15 limitations under the License.
be_bryan 0:b74591d5ab33 16 """
be_bryan 0:b74591d5ab33 17 from numpy import sin, arange, pi
be_bryan 0:b74591d5ab33 18 from scipy.signal import lfilter, firwin
be_bryan 0:b74591d5ab33 19 from pylab import figure, plot, grid, show
be_bryan 0:b74591d5ab33 20
be_bryan 0:b74591d5ab33 21 #------------------------------------------------
be_bryan 0:b74591d5ab33 22 # Create a signal for demonstration.
be_bryan 0:b74591d5ab33 23 #------------------------------------------------
be_bryan 0:b74591d5ab33 24 # 320 samples of (1000Hz + 15000 Hz) at 48 kHz
be_bryan 0:b74591d5ab33 25 sample_rate = 48000.
be_bryan 0:b74591d5ab33 26 nsamples = 320
be_bryan 0:b74591d5ab33 27
be_bryan 0:b74591d5ab33 28 F_1KHz = 1000.
be_bryan 0:b74591d5ab33 29 A_1KHz = 1.0
be_bryan 0:b74591d5ab33 30
be_bryan 0:b74591d5ab33 31 F_15KHz = 15000.
be_bryan 0:b74591d5ab33 32 A_15KHz = 0.5
be_bryan 0:b74591d5ab33 33
be_bryan 0:b74591d5ab33 34 t = arange(nsamples) / sample_rate
be_bryan 0:b74591d5ab33 35 signal = A_1KHz * sin(2*pi*F_1KHz*t) + A_15KHz*sin(2*pi*F_15KHz*t)
be_bryan 0:b74591d5ab33 36
be_bryan 0:b74591d5ab33 37 #------------------------------------------------
be_bryan 0:b74591d5ab33 38 # Create a FIR filter and apply it to signal.
be_bryan 0:b74591d5ab33 39 #------------------------------------------------
be_bryan 0:b74591d5ab33 40 # The Nyquist rate of the signal.
be_bryan 0:b74591d5ab33 41 nyq_rate = sample_rate / 2.
be_bryan 0:b74591d5ab33 42
be_bryan 0:b74591d5ab33 43 # The cutoff frequency of the filter: 6KHz
be_bryan 0:b74591d5ab33 44 cutoff_hz = 6000.0
be_bryan 0:b74591d5ab33 45
be_bryan 0:b74591d5ab33 46 # Length of the filter (number of coefficients, i.e. the filter order + 1)
be_bryan 0:b74591d5ab33 47 numtaps = 29
be_bryan 0:b74591d5ab33 48
be_bryan 0:b74591d5ab33 49 # Use firwin to create a lowpass FIR filter
be_bryan 0:b74591d5ab33 50 fir_coeff = firwin(numtaps, cutoff_hz/nyq_rate)
be_bryan 0:b74591d5ab33 51
be_bryan 0:b74591d5ab33 52 # Use lfilter to filter the signal with the FIR filter
be_bryan 0:b74591d5ab33 53 filtered_signal = lfilter(fir_coeff, 1.0, signal)
be_bryan 0:b74591d5ab33 54
be_bryan 0:b74591d5ab33 55 #------------------------------------------------
be_bryan 0:b74591d5ab33 56 # Plot the original and filtered signals.
be_bryan 0:b74591d5ab33 57 #------------------------------------------------
be_bryan 0:b74591d5ab33 58
be_bryan 0:b74591d5ab33 59 # The first N-1 samples are "corrupted" by the initial conditions
be_bryan 0:b74591d5ab33 60 warmup = numtaps - 1
be_bryan 0:b74591d5ab33 61
be_bryan 0:b74591d5ab33 62 # The phase delay of the filtered signal
be_bryan 0:b74591d5ab33 63 delay = (warmup / 2) / sample_rate
be_bryan 0:b74591d5ab33 64
be_bryan 0:b74591d5ab33 65 figure(1)
be_bryan 0:b74591d5ab33 66 # Plot the original signal
be_bryan 0:b74591d5ab33 67 plot(t, signal)
be_bryan 0:b74591d5ab33 68
be_bryan 0:b74591d5ab33 69 # Plot the filtered signal, shifted to compensate for the phase delay
be_bryan 0:b74591d5ab33 70 plot(t-delay, filtered_signal, 'r-')
be_bryan 0:b74591d5ab33 71
be_bryan 0:b74591d5ab33 72 # Plot just the "good" part of the filtered signal. The first N-1
be_bryan 0:b74591d5ab33 73 # samples are "corrupted" by the initial conditions.
be_bryan 0:b74591d5ab33 74 plot(t[warmup:]-delay, filtered_signal[warmup:], 'g', linewidth=4)
be_bryan 0:b74591d5ab33 75
be_bryan 0:b74591d5ab33 76 grid(True)
be_bryan 0:b74591d5ab33 77
be_bryan 0:b74591d5ab33 78 show()
be_bryan 0:b74591d5ab33 79
be_bryan 0:b74591d5ab33 80 #------------------------------------------------
be_bryan 0:b74591d5ab33 81 # Print values
be_bryan 0:b74591d5ab33 82 #------------------------------------------------
be_bryan 0:b74591d5ab33 83 def print_values(label, values):
be_bryan 0:b74591d5ab33 84 var = "float32_t %s[%d]" % (label, len(values))
be_bryan 0:b74591d5ab33 85 print "%-30s = {%s}" % (var, ', '.join(["%+.10f" % x for x in values]))
be_bryan 0:b74591d5ab33 86
be_bryan 0:b74591d5ab33 87 print_values('signal', signal)
be_bryan 0:b74591d5ab33 88 print_values('fir_coeff', fir_coeff)
be_bryan 0:b74591d5ab33 89 print_values('filtered_signal', filtered_signal)