Mechatrax Co., Ltd. a fourni le kit de démarrage Raspeye IoT "anyPi". , J'ai essayé de connecter Alps IoT Smart Module. Calcule le tangage, le roulis et le cap à partir des capteurs d'accélération et géomagnétiques du module intelligent Alps IoT et affiche les résultats sur un écran de texte. Lorsque le tangage, le roulis et le cap sont dans une certaine plage, le buzzer électronique du PiConsole I / F retentit. Pour le tangage et le roulis, les informations sont affichées par deux LED si elles se situent dans une certaine plage.

anyPi est empilé avec Raspberry Pi 3, 3GPI, PiConsole I / F dans l'ordre à partir du bas, et cette fois, le tangage, le roulis et le cap sont affichés sur l'affichage texte de PiConsole I / F, et il se trouve dans la plage prédéterminée. Quand il entre, le buzzer électronique retentit, quand le pitch entre, la LED (jaune) s'allume, et quand le roulement entre, la LED (rouge) s'allume. Le module intelligent Alps IoT se connecte au Raspberry Pi 3 via le protocole Bluetooth Low Energy (BLE) pour acquérir les données de mouvement du capteur d'accélération et du capteur géomagnétique. Le module intelligent Alps IoT est placé sur une feuille d'échelle avec une circonférence divisée par 30 ° afin que le cap puisse être facilement confirmé.

Créer un programme

Le programme utilise le langage Python, "actionmain.py" qui contrôle l'ensemble, "alpsaction.py" qui contrôle le module intelligent Alps IoT pour acquérir des informations sur les capteurs et des données sur l'affichage de texte de PiConsole I / F. Il se compose de "lcd.py" à afficher. Définissez également la définition du seuil utilisé pour vérifier si le tangage, le roulis et le cap calculés sont dans la plage prédéterminée dans "constant.conf". De plus, "lcd.py" utilise le même programme que l'exemple de programme pour LCD de "entrée / sortie de PiConsole I / F of anyPi". ..

_Installation de l'interface Python "bluepy" _

Afin de gérer BLE dans le langage Python, il est nécessaire d'installer l'interface Python "bluepy" avec la commande suivante.

$ sudo apt-get install python-pip libglib2.0-dev
$ sudo pip install bluepy
$ sudo apt-get install git build-essential libglib2.0-dev
$ git clone https://github.com/IanHarvey/bluepy.git
$ cd bluepy
$ sudo python setup.py build
$ sudo python setup.py install

_Module de contrôle global "actionmain.py" _

"Actionmain.py" démarre en premier et effectue les opérations suivantes:

La communicationBLE utilise "AlpsSensor" qui hérite de la classe "Peripheral".
La fonction buzzer est utilisée pour vérifier le tangage, le roulis et la plage de cap calculés, et pour allumer / éteindre la LED (rouge / jaune).
La fonction displayAngle affiche la hauteur, le roulis et le cap sur l'écran de texte PiConsole I / F. La fonction displayAngle est appelée comme fonction de rappel à partir de la méthode handleNotification de la classe NtfyDelegate. --Lorsque le programme démarre, les données de seuil du contrôle de plage sont acquises à partir du fichier de définition de seuil "constant.conf".

# -*- coding: utf-8 -*- 
import sys
import signal
sys.path.append('/home/pi/bluepy/bluepy')
from btle import Peripheral
import btle 
import ConfigParser

from lcd import St7032iLCD
from alpsaction import AlpsSensor
import pigpio

lcd = St7032iLCD()
Buzzer = 25        #avertisseur sonore
LED_red = 20	#rouge
LED_yellow = 21	#Jaune
pi = pigpio.pi()
pi.set_mode(Buzzer, pigpio.OUTPUT)
pi.set_mode(LED_red, pigpio.OUTPUT)
pi.set_mode(LED_yellow, pigpio.OUTPUT)

global thresholdAngl
exitflg = False

def exithandler(signal, frame):
    print('exithandler !!')
    exitflg = True

def buzzer(Roll,Pitch,Heading):
    print ('Roll:{0:.3f} thresholdAngl:{1:.3f}'.format(abs(Roll ),thresholdAngl))
    if Heading > 180 :
        Headingdata = Heading - 360
    else :
        Headingdata = Heading
    if thresholdAngl > abs(Roll) and thresholdAngl > abs(Pitch) and thresholdAngl > abs(Headingdata):
        pi.write(Buzzer, 1)
    else :
        pi.write(Buzzer, 0)
    if thresholdAngl > abs(Roll):
        pi.write(LED_red,1)
    else :
        pi.write(LED_red,0)
    if thresholdAngl > abs(Pitch):
        pi.write(LED_yellow,1)
    else :
        pi.write(LED_yellow,0)
    


def displayAngle(Roll,Pitch,Heading):
    print ('Roll:{0:.3f} Pitch:{1:.3f} Heading:{2:.3f}'.format(Roll,Pitch,Heading ))
    lcd.set_cursor(0, 0)
    lcd.put_line ('R:{0:.2f} P:{1:.2f}'.format(Roll, Pitch))
    lcd.set_cursor(0, 1)
    lcd.put_line ('Az:{0:.2f} '.format(Heading))
    buzzer(Roll,Pitch,Heading)
    

def main():
    global thresholdAngl

    signal.signal(signal.SIGINT, exithandler)

#Lire le fichier de configuration
    conf = ConfigParser.SafeConfigParser()
    conf.read('constant.conf')
#Enregistrement des données de contrôle de portée
    thresholdAngl = int(conf.get('THRESHOLD', 'thresholdAngl'))
    
    alps = AlpsSensor(displayAngle)
    alps.setSensor()
    
# Main loop --------
    try:
        while True:
            if exitflg :
                break
            if alps.waitForNotifications(1.0):
            # handleNotification() was called
                continue

            print ("Waiting...")
            # Perhaps do something else here
    except Exception, e:
        print e, 'error occurred'
    sys.exit(0)

   
if __name__ == "__main__":
    main()

_Alps IoT Smart Module Sensor acquisition d'informations "alpsaction.py" _

Pour acquérir des données de mouvement du capteur d'accélération et du capteur géomagnétique du module Alps IoT Smart, le guide de commande du module intelligent Alps IoT "[Sensor Network Module Evaluation Kit Application Note Command Guide](http://www.alps.com/j] J'ai fait les réglages suivants en me référant à "2.2 Exemple de réglage par commande" de "/iotsmart-network/pdf/msm_command_manual.pdf)".

Réglage	une fonction
Mode de mesure	Mode rapide
Intervalle de mesure	100ms
Acceleration	〇
Geo-Magnetic	〇
Pressure	－
Humidity	－
Temperature	－
UV	－
Ambient Light	－

La communication BLE utilise la méthode "write Characteristic" et le guide de commande du module intelligent Alps IoT " Sensor Network Module Défini par la méthode setSensor de la classe AlpsSensor conformément au kit d'évaluation Application Note Command Guide . Les messages de notification sont reçus par la méthode handleNotification de la classe NtfyDelegate définie par la méthode setDelegate. Dans la méthode handleNotification, le code d'événement contenant les données de mouvement ne traite que le paquet de données 1 "0xF2". Les données géomagnétiques et les données d'accélération dans le paquet de données sont des entiers signés de 2 octets. Les données géomagnétiques [uT] sont calculées par "Magnétique x 0,15" et les données d'accélération [G] par "Accélération / 4096 (dans le cas d'une plage de ± 2G)".

A propos de la formule à utiliser

Données d'accélération (A _{x , A _{y , A _{z ) du capteur d'accélération de l'Alps IoT Smart Module, et données géomagnétiques (M) du capteur géomagnétique. _{x , M _{y , M _{z ) seront saisis. A l'origine, il est nécessaire de calibrer ou de normaliser les données acquises, mais les données de mouvement acquises sont utilisées telles quelles. La formule de calcul est indiquée ci-dessous. Le calcul réel est effectué par la méthode computeAngle de la classe NtfyDelegate.}}}}}}

Tout d'abord, trouvez le tangage et le roulis par le calcul suivant.

Pitch = ρ = arcsin(A_x)
Roll = γ = arcsin(A_y/cosρ )

Ensuite, recherchez l'en-tête avec la formule suivante.

M_x1 = M_xcosρ + M_zsinρ
M_y1 = M_xsinγsinρ + M_ycosγ - M_zsinγcosρ
 En-tête = ψ = arctan (M _{y1  / M _{x1 ) M _{x1 > 0 et M _{y1 > = 0
            = 180° + arctan(M_y1/M_x1) M_x1<0 
 = 360 ° + arctan (M _{y1  / M _{x1 ) M _{x1 > 0 et M _{y1  <= 0
 = 90 ° M _{x1  = 0 et M _{y1  <0
 = 270 ° M _{x1  = 0 et M _{y1 > 0}}}}}}}}}}}}

Comme le montre la figure suivante, le cap est l'angle entre l'axe X _{b et le nord magnétique sur le plan horizontal, le pas est l'angle entre l'axe X _{b et le plan horizontal, et le roulis est Y < Il est défini par l'angle entre l'axe sub> b et le plan horizontal.}}

L'axe de sensibilité du capteur du module intelligent Alps IoT est le suivant.

Les détails sont affichés dans Obtenir la hauteur, le roulis et le cap à partir du capteur de mouvement du module intelligent Alps IoT.

# -*- coding: utf-8 -*- 
import sys 
sys.path.append('/home/pi/bluepy/bluepy')
from btle import Peripheral
import struct 
import btle
import binascii 
import math

class NtfyDelegate(btle.DefaultDelegate):
    def __init__(self, param, callback):
        btle.DefaultDelegate.__init__(self)
        # ... initialise here
        self.callbackFunc = callback
        
    def dataConv(self, msb, lsb): 
        #    cal = 'f214fefff100320175000000ceef90010501007b'
        if (int(msb,16) & 0x80) == 0x80:
            data = -(65536-(int(msb+lsb,16)))
        else:
            data = int(msb+lsb,16)
        #print ('data:{0}'.format(data))
        return data
        
    def computeHeading(self, My, Mx): 
        if Mx > 0 and My >= 0 :
            Heading = math.atan(My/Mx) 
        elif Mx < 0 : 
            Heading = math.pi + math.atan(My/Mx) 
        elif Mx > 0 and My <= 0 :
            Heading = 2*math.pi + math.atan(My/Mx) 
        elif Mx == 0 and My < 0 :
            Heading = math.pi/2 
        else :
            Heading = math.pi+math.pi/2 
        return Heading
        
    def computeAngle( self, GeoMagnetic_X,GeoMagnetic_Y,GeoMagnetic_Z,Acceleration_X,Acceleration_Y,Acceleration_Z):
        Pitch = math.asin(-Acceleration_X)
        Roll = math.asin(Acceleration_Y/math.cos(Pitch))
        
        Mx = GeoMagnetic_X*math.cos(Pitch)+GeoMagnetic_Z*math.sin(Pitch)
        #print ('cos:{0:.3f} sin:{1:.3f}'.format(GeoMagnetic_X*math.cos(Pitch),math.sin(Pitch) ))
        
        My=GeoMagnetic_X*math.sin(Roll)*math.sin(Pitch)+GeoMagnetic_Y*math.cos(Roll)-GeoMagnetic_Z*math.sin(Roll)*math.cos(Pitch)
        Heading = self.computeHeading(My,Mx) 
        
        return math.degrees(Roll),math.degrees(Pitch),math.degrees(Heading)
            
    def handleNotification(self, cHandle, data): 
        # ... perhaps check cHandle
        # ... process 'data'
        cal = binascii.b2a_hex(data) 
        #print u'handleNotification : {0}-{1}:'.format(cHandle, cal)
        
        if int((cal[0:2]), 16) == 0xf2:
            #print 'cal:{0}'.format(type(cal)) 
            GeoMagnetic_X = self.dataConv(cal[6:8],cal[4:6]) * 0.15
            GeoMagnetic_Y = self.dataConv(cal[10:12],cal[8:10]) * 0.15
            GeoMagnetic_Z = self.dataConv(cal[14:16],cal[12:14]) * 0.15
            Acceleration_X = self.dataConv(cal[18:20],cal[16:18]) / 4096.0
            Acceleration_Y = self.dataConv(cal[22:24], cal[20:22]) / 4096.0
            Acceleration_Z = self.dataConv(cal[26:28],cal[24:26]) / 4096.0
            #print ('Geo-Magnetic X:{0:.3f} Y:{1:.3f} Z:{2:.3f}'.format(GeoMagnetic_X, GeoMagnetic_Y, GeoMagnetic_Z))
            #print ('Acceleration X:{0:.3f} Y:{1:.3f} Z:{2:.3f}'.format(Acceleration_X, Acceleration_Y, Acceleration_Z))
            Roll,Pitch,Heading = self.computeAngle(GeoMagnetic_X,GeoMagnetic_Y,GeoMagnetic_Z,Acceleration_X,Acceleration_Y,Acceleration_Z)
            self.callbackFunc(Roll,Pitch,Heading)
            
class AlpsSensor(Peripheral):
    def __init__(self,callback):
        Peripheral.__init__(self,"28:A1:83:E1:58:96")
        callbackFunc = callback
        self.setDelegate( NtfyDelegate(btle.DefaultDelegate,callback))
        
    def setSensor(self):
        #Détection de mouvement à intervalle de 100 ms (capteur de mouvement uniquement)
        # alps.writeCharacteristic(0x0018, struct.pack('<bbb', 0x20, 0x03, 0x00), True)
        # alps.writeCharacteristic(0x0018, struct.pack('<bbb', 0x20, 0x03, 0x01), True)

        self.writeCharacteristic(0x0013, struct.pack('<bb', 0x01, 0x00), True)
        self.writeCharacteristic(0x0016, struct.pack('<bb', 0x01, 0x00), True)

        self.writeCharacteristic(0x0018, struct.pack('<bbb', 0x2F, 0x03, 0x03), True)
        self.writeCharacteristic(0x0018, struct.pack('<bbb', 0x01, 0x03, 0x03), True)
        self.writeCharacteristic(0x0018, struct.pack('<bbb', 0x04, 0x03, 0x01), True)
        self.writeCharacteristic(0x0018, struct.pack('<bbbb', 0x06, 0x04, 0x64, 0x00), True)  # 100msec

        self.writeCharacteristic(0x0018, struct.pack('<bbb', 0x2F, 0x03, 0x01), True)
        self.writeCharacteristic(0x0018, struct.pack('<bbb', 0x20, 0x03, 0x01), True)

_ Fichier de définition de seuil "constant.conf" _

Le seuil "thresholdAngl" utilisé pour vérifier si le tangage, le roulis et le cap calculés sont dans une plage prédéterminée est établi comme suit. L'unité est "°", le tangage et le roulis sont indiqués par des angles absolus centrés sur 0 °, et le cap est indiqué par des angles dans le sens horaire et antihoraire centrés sur 0 °. Cette valeur est lue au démarrage du programme.

#Données de seuil
[THRESHOLD]
thresholdAngl= 10

Exécuter de programme

Lorsque vous exécutez le programme, placez le module intelligent Alps IoT horizontalement et définissez le cap dans la plage de 350 ° à 10 ° avec 0 ° entre les deux, le buzzer électronique du PiConsole I / F retentit. De plus, si vous placez le module intelligent Alps IoT sur l'échelle qui divise la circonférence par 30 ° et que vous le faites pivoter, l'en-tête de l'affichage du texte de la PiConsole I / F changera en conséquence. La LED (jaune) s'allume lorsqu'elle est inclinée dans la direction du pas à 0 ± 10 ° ou moins, et la LED (rouge) s'allume lorsqu'elle est inclinée dans la direction du roulis à 0 ± 10 ° ou moins.

L'affichage de l'affichage de texte de PiConsole I / F et l'affichage de la LED sont indiqués ci-dessous. "R" indique le roulis, "P" indique le tangage et "Az" indique le cap.

De plus, les résultats des calculs de roulis, de tangage et de cap affichés sur l'écran d'exécution sont indiqués ci-dessous.

$ python actionmain.py 
Roll:-0.913 Pitch:-2.199 Heading:172.232
Roll:0.913 thresholdAngl:10.000
Roll:-0.913 Pitch:-2.405 Heading:172.427
Roll:0.913 thresholdAngl:10.000
Roll:-0.956 Pitch:-2.244 Heading:172.255
Roll:0.956 thresholdAngl:10.000
Roll:-1.204 Pitch:-2.406 Heading:172.674
Roll:1.204 thresholdAngl:10.000
Roll:-0.952 Pitch:-2.275 Heading:172.906
Roll:0.952 thresholdAngl:10.000
Roll:-1.001 Pitch:-2.127 Heading:173.081
Roll:1.001 thresholdAngl:10.000
Roll:-1.124 Pitch:-2.205 Heading:172.412
Roll:1.124 thresholdAngl:10.000
Roll:-0.878 Pitch:-2.088 Heading:171.820
Roll:0.878 thresholdAngl:10.000
Roll:-1.080 Pitch:-2.284 Heading:172.091
Roll:1.080 thresholdAngl:10.000
Roll:-0.998 Pitch:-2.161 Heading:173.172
Roll:1.041 thresholdAngl:10.000
Roll:-1.041 Pitch:-2.455 Heading:173.014
Roll:1.041 thresholdAngl:10.000
Roll:-0.959 Pitch:-2.167 Heading:173.473
Roll:0.959 thresholdAngl:10.000
Roll:-0.917 Pitch:-2.207 Heading:173.490
Roll:0.917 thresholdAngl:10.000
Roll:-0.956 Pitch:-2.118 Heading:173.107
Roll:0.953 thresholdAngl:10.000
Roll:-0.921 Pitch:-2.218 Heading:173.077
Roll:0.921 thresholdAngl:10.000
Roll:-0.915 Pitch:-2.161 Heading:172.723
Roll:0.915 thresholdAngl:10.000
^Cexithandler !!
(4, 'Interrupted system call') error occurred
Exception btle.BTLEException: BTLEException() in <bound method AlpsSensor.__del__ of <alpsaction.AlpsSensor instance at 0x767684b8>> ignored

Acquérir / afficher les valeurs du capteur d'accélération et du capteur géomagnétique du module intelligent Alps IoT avec anyPi