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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
| #import des librairies
import serial
import numpy as np
from matplotlib import pyplot as plt
import time
ser = serial.Serial('com7', 115200) #ouverture du port série. Il faut appareiller au pc le boitier et vérifier le numéro du port
duree=10
plt.ion() #mode interactif pour affichage en temps réel
AGyY=0 #Angle Gyroscope axe Y
AFcY=0 #Angle Filtre Complémentaire axe Y
delta_t=0 #pas de temps entre deux mesures (lecture sur le port série environ 0,04s)
TabFiltreComp = [0] * 1000 #déclaration des tableaux de données
TabGyY=[0] * 1000 #tableau angle gyroscope axe Y
TabAcY=[0] * 1000 #tableau angle accéléromètre axe Y
TabAk=[0] * 1000 #tableau angle calculé Kalman
inc= [0] * 1000 #tableau incréments
i=0 #incrément temps de mesure
fig, ax = plt.subplots() #création du graphe
fig.canvas.draw()
#fonction lecture : renvoi l'accélération en mg (axes X et Z) et la vitesse en °/s (axe Y)
def lecture():
ser.write(b'e') #demande reception message 'a,b,c,d,e' selon le protocole
data = ser.read(10) #affectation à data du message renvoyé
if len(data)==10: #on vérifie que le nombre d'octet correspond à la demande (voir protocole)
#concaténation des octets
Ax=(float(list(bytearray(data))[1]<<8)+float(list(bytearray(data))[0])) #accélération sur x
Az=(float(list(bytearray(data))[3]<<8)+float(list(bytearray(data))[2])) #accélération sur z
Gy=(float(list(bytearray(data))[5]<<8)+float(list(bytearray(data))[4])) #vitesse autour de y
Ak_signe_exp=float(list(bytearray(data))[6])
Ak_mantisse=(float(list(bytearray(data))[9])+float(list(bytearray(data))[8]<<8)+float(list(bytearray(data))[7]<<16)) #mantisse angle calculé Kalman
print(float(list(bytearray(data))[6]))
if Ax>32768: #test pour trouver le signe de l'accélération sur X
Ax=(65520-Ax)/16
else :
Ax=(-Ax/16)
if Az>32768: #test pour trouver le signe de l'accélération sur Z
Az=(65520-Az)/16
else :
Az=(-Az/16)
if Gy>32768: #test pour trouver le signe de la vitesse autour de Y
Gy=(65535-Gy)*0.0175
else :
Gy=(-Gy*0.0175)
if Ak_signe_exp>127:
Ak=-Ak_mantisse#*(np.exp2(Ak_signe_exp-127))
else :
Ak=-Ak_mantisse#*(np.exp2(Ak_signe_exp))
print(Ak)
return (Ax,Az,Gy,Ak)
#fonction gyro: calcul de l'angle (axeY) issu de la mesure du gyroscope
def gyro(GyY,AGyY,delta_t):
AGyY=((AGyY+float(GyY)*delta_t)) #intégration de la vitesse
return (AGyY)
#fonction accel: calcul l'angle issu de le mesure de l'accéléromètre
def accel(AcX,AcZ):
AAcY=np.arctan(float(AcX)/float(AcZ))*180/np.pi #calcul de projection
return (AAcY)
#fonction filtre_comp: calcul de l'angle filtré à partir des mesures de l'accéléromètre et du gyroscope
def filtre_comp(AcX,AcZ,GyY,AFcY,delta_t):
AFcY=0.8*(AFcY+float(GyY)*delta_t) + 0.2*np.arctan(float(AcX)/float(AcZ))*180/np.pi
return (AFcY)
while (i<duree):
try:
t=time.time()
AcX,AcZ,GyY,Ak=lecture()
delta_t=(time.time()-t)
#print(delta_t) #affichage possible des données dans la console...
#print(AcX,AcZ,GyY)
AGyY=gyro(GyY,AGyY,delta_t)
AAcY=accel(AcX,AcZ)
AFcY=filtre_comp(AcX,AcZ,GyY,AFcY,delta_t)
TabGyY.append(AGyY) #on entre au fur et à mesure les données dans les tableaux
TabAcY.append(AAcY)
TabFiltreComp.append(AFcY)
TabAk.append(Ak)
inc.append(i)
del TabFiltreComp[0] #on supprime au fur et à mesure la valeur la plus ancienne dans les tableaux (décalage)
del TabGyY[0]
del TabAcY[0]
del TabAk[0]
del inc[0]
i=i+delta_t #incrément
except:
print("Oups!! problème...")
line1,=plt.plot(inc,TabFiltreComp) #tracés des données comprises dans les tableaux
plt.plot(inc,TabGyY)
plt.plot(inc,TabAcY)
plt.plot(inc,TabAk)
plt.draw() #affichage des courbes
plt.pause(1e-10) #petite pause pour laisser le temps nécessaire à l'affichage
plt.clf() #efface le tracé precedent
plt.ioff() #fin du mode interactif
trace_filtre,=plt.plot(inc,TabFiltreComp, label="Filtre complémentaire") #affichage des tracés avec légendes
trace_gyr,=plt.plot(inc,TabGyY, label="Gyroscope")
trace_accel,=plt.plot(inc,TabAcY, label="Accéléromètre")
trace_kalman,=plt.plot(inc,TabAcY, label="Filtre de Kalman")
plt.legend(handles=[trace_filtre,trace_gyr,trace_accel,trace_kalman], loc=4)
plt.show()
ser.close() #fermeture du port série
print("port série fermé :",ser.port) |
