Discussion :

[tournette]

Bonjour,

J'ai écrit un programme permettant de lire et afficher graphiquement les données d'une centrale inertielle.
La donnée d'un capteur est envoyé avec 4 octets avec le protocole IEEE (voir ci-dessous)


voici mon programme sous python qui réceptionne les données via bluetooth sur le com7.

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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)

Ma question!!
Comment procéder pour convertir mes données arrivant sur les 4 octets en un nombre décimal?
signe/exposant/mantisse... Existe-t-il une fonction faisant rapidement ce calcul

J'ai commencé à écrire quelques lignes mais ce n'est pas correct (voir fonction lecture lignes 33/34 et 48à50)

Merci d'avance pour votre réponse.