StairPersonDetector/mpuPlot.py at main · danjperron/StairPersonDetector · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
#!/usr/bin/python3
import secrets
import numpy as np
import matplotlib.pyplot as plt
import socket
from matplotlib.animation import FuncAnimation
import sys
import re
import time
import struct
import threading

# need to be the same than pico mpu6050
#FSAMP   SAMPLE FREQUENCY in HZ
FSAMP = 250

#SAMP  number of sample taken by the FFT
SAMP = 256


UDP_PORT=6001
ServerSocket = socket.socket(family=socket.AF_INET, type = socket.SOCK_DGRAM)

ServerSocket.bind(('',UDP_PORT))

lock = threading.Lock()
numbers = (0)*(SAMP//2)
numbers_t = (0)*(SAMP//2)
maxPeak = 0

#define  RECEIVER_IP  NULL
#define  RECEIVER_PORT 6001


CycleFreq =  (FSAMP/SAMP)
Tcount=0
ExitFlag=False


def GetFFT():
   global Tcount
   global numbers_t
   global ServerSocket
   global ExitFlag
   global maxPeak
   print("thread start")
   while not ExitFlag:
       info = ServerSocket.recvfrom(1024)
       ln = len(info[0])
       if ln < SAMP:
           continue
       numbers_th = struct.unpack('H'*(SAMP//2),info[0][0:SAMP])
       if len(numbers_th) != (SAMP//2):
           continue
       freq = CycleFreq*numbers_th[0]
       nidx = numbers_th[0]
       if nidx >= (SAMP//2):
           continue
       print("Tcount",Tcount," Freq:",freq," value:",numbers_th[0])
       lock.acquire()
       maxPeak= numbers_th[0]
       numbers_t = 0.001 * np.array(numbers_th) # from milli g to g
       #numbers_t = np.array(numbers_th) # milli g
       #if numbers_t[maxPeak] > 0.8:
       Tcount+=1
       lock.release()
   print("Thread close")


xdata= np.arange(0,(SAMP//2)*CycleFreq,CycleFreq)

plt.xticks(xdata)

CurrentCount=0;

thread1= threading.Thread(target=GetFFT)
thread1.start()
try:
    while True:

        lock.acquire()
        if Tcount==CurrentCount:
            lock.release()
            continue
        numbers=numbers_t
        CurrentCount=Tcount
        lock.release()
        print("show ",CurrentCount)
        plt.clf()
        plt.plot(xdata,numbers,color=(0.2, 0.4, 0.6, 0.6))
        plt.title("Freq:{:.1f}Hz max:{:.3f}g".format(xdata[maxPeak],numbers[maxPeak]))
        plt.axis([0,(SAMP//2),0,2])
        plt.pause(0.0001)
except KeyboardInterrupt:
       pass
ExitFlag=True
print("done")