52 lines
1.3 KiB
Python
52 lines
1.3 KiB
Python
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
|
|
|
|
class fundamental_freq_meas(object):
|
|
"""docstring for fft_meas"""
|
|
def __init__(self, fs=50e3, tn=10):
|
|
self.fs = fs
|
|
self.ts = 1/self.fs
|
|
self.tn = tn
|
|
self.time = 0
|
|
self.y_old = 0
|
|
self.tn_start_time = 0
|
|
self.tn_end_time = 0
|
|
self.zerocross_count = 0
|
|
# Calculated Frequency
|
|
self.freq = 50
|
|
|
|
self.t_zc = 0
|
|
|
|
def step(self, y, zerocross_flag):
|
|
|
|
# Integrate time
|
|
self.time += self.ts
|
|
|
|
if zerocross_flag:
|
|
denom = (y - self.y_old)
|
|
if denom != 0.0: # avoid divide-by-zero
|
|
# linear interpolation to find zero-crossing time
|
|
frac = -self.y_old / denom
|
|
self.t_zc = (self.time - self.ts) + frac * self.ts
|
|
|
|
# store first crossing time
|
|
if self.zerocross_count == 0:
|
|
self.tn_start_time = self.t_zc
|
|
|
|
self.zerocross_count += 1
|
|
self.tn_end_time = self.t_zc # always keep last crossing time in this window
|
|
|
|
# window finished (independent of whether a crossing happened exactly here)
|
|
if self.time >= self.tn:
|
|
if self.zerocross_count >= 2 and self.tn_end_time > self.tn_start_time:
|
|
self.freq = (self.zerocross_count - 1) / (self.tn_end_time - self.tn_start_time)
|
|
|
|
# reset window state
|
|
self.zerocross_count = 0
|
|
self.tn_start_time = 0.0
|
|
self.tn_end_time = 0.0
|
|
self.time -= self.tn # avoids drift vs. self.time = 0
|
|
|
|
self.y_old = y
|