[PYTHON] Erfassen / Anzeigen der Werte des Beschleunigungssensors und des geomagnetischen Sensors des Alps IoT Smart Module mit anyPi
Mechatrax Co., Ltd. stellte das Raspeye IoT Starter Kit "anyPi" zur Verfügung. Ich habe versucht, [Alps IoT Smart Module] anzuschließen (http://www.alps.com/j/iotsmart-network/index.html). Berechnet Nick, Roll und Kurs aus den Beschleunigungs- und Erdmagnetsensoren des Alps IoT Smart Module und zeigt die Ergebnisse auf einer Textanzeige an. Wenn Tonhöhe, Roll und Kurs innerhalb eines bestimmten Bereichs liegen, ertönt der elektronische Summer des PiConsole I / F. Bei Pitch and Roll werden Informationen von zwei LEDs angezeigt, wenn sie in einen bestimmten Bereich fallen.
anyPi wird mit Raspberry Pi 3, 3GPI, PiConsole I / F in der Reihenfolge von unten gestapelt. Dieses Mal werden Tonhöhe, Roll und Überschrift auf der Textanzeige von PiConsole I / F und innerhalb eines vorgegebenen Bereichs angezeigt. Wenn es eintritt, ertönt der elektronische Summer, wenn die Tonhöhe eintritt, leuchtet die LED (gelb) und wenn die Rolle eintritt, leuchtet die LED (rot) auf. Das Alps IoT Smart Module wird über das Bluetooth Low Energy (BLE) -Protokoll mit dem Raspberry Pi 3 verbunden, um Bewegungsdaten vom Beschleunigungssensor und vom geomagnetischen Sensor zu erfassen. Das Alps IoT Smart Module wird auf ein Skalenpapier mit einem durch 30 ° geteilten Umfang gelegt, damit die Überschrift leicht bestätigt werden kann.
Erstellen Sie das Programm
Das Programm verwendet die Python-Sprache "actionmain.py", die das Ganze steuert, "alpsaction.py", die das Alps IoT Smart Module steuert, um Sensorinformationen und Daten auf der Textanzeige der piConsole I / F von anyPi zu erfassen. Es besteht aus "lcd.py", das angezeigt werden soll. Legen Sie außerdem die Definition des Schwellenwerts fest, der verwendet wird, um zu überprüfen, ob die berechnete Neigung, Rolle und Richtung innerhalb des vorgegebenen Bereichs in "constant.conf" liegen. Darüber hinaus verwendet "lcd.py" dasselbe Programm wie das Beispielprogramm für LCD von "Eingabe / Ausgabe von PiConsole I / F von anyPi". ..
_Installation der Python-Schnittstelle "bluepy" _
Um BLE in der Python-Sprache verarbeiten zu können, muss die Python-Schnittstelle "bluepy" mit dem folgenden Befehl installiert werden.
$ 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
_Gesamtsteuermodul "actionmain.py" _
"Actionmain.py" startet zuerst und führt Folgendes aus:
--BLE-Kommunikation verwendet "AlpsSensor", der die Klasse "Peripheral" erbt.
- Mit der Summerfunktion werden die berechnete Neigung, der Roll und der Kursbereich überprüft und die LED ein- und ausgeschaltet (rot / gelb).
- Die Funktion displayAngle zeigt die Tonhöhe, den Roll und den Kurs auf der PiConsole I / F-Textanzeige an. Die Funktion displayAngle wird als Rückruffunktion von der handleNotification-Methode der NtfyDelegate-Klasse aufgerufen.
- Beim Programmstart werden die Schwellendaten der Bereichsprüfung aus der Schwellwertdefinitionsdatei "constant.conf" abgerufen.
# -*- 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 #Summer
LED_red = 20 #rot
LED_yellow = 21 #Gelb
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)
#Konfigurationsdatei lesen
conf = ConfigParser.SafeConfigParser()
conf.read('constant.conf')
#Bereichsüberprüfungsdaten speichern
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 Sensorinformationserfassung "alpsaction.py" _
Um Bewegungsdaten vom Beschleunigungssensor und vom geomagnetischen Sensor des Alps IoT Smart Module zu erfassen, lesen Sie die Befehlsanleitung des Alps IoT Smart Module "[http://www.alps.com/j]. Ich habe die folgenden Einstellungen unter Bezugnahme auf "2.2 Beispiel für die Einstellung per Befehl" von "/iotsmart-network/pdf/msm_command_manual.pdf)" vorgenommen.
| Aufbau | Funktion |
|---|---|
| Messmodus | Schneller Modus |
| Messintervall | 100ms |
| Acceleration | 〇 |
| Geo-Magnetic | 〇 |
| Pressure | － |
| Humidity | － |
| Temperature | － |
| UV | － |
| Ambient Light | － |
Die BLE-Kommunikation verwendet die Methode "Write Characteristic" und die Befehlsanleitung für das Alps IoT Smart Module " Sensornetzwerkmodul" Wird mit der setSensor-Methode der AlpsSensor-Klasse gemäß dem Evaluierungskit Application Note Command Guide festgelegt. Benachrichtigungsnachrichten werden von der handleNotification-Methode der NtfyDelegate-Klasse empfangen, die von der setDelegate-Methode festgelegt wurde. Bei der handleNotification-Methode verarbeitet der Ereigniscode, der die Bewegungsdaten enthält, nur das Datenpaket 1 "0xF2". Die geomagnetischen Daten und Beschleunigungsdaten im Datenpaket sind vorzeichenbehaftete 2-Byte-Ganzzahlen. Die geomagnetischen Daten [uT] werden mit "Magnetic x 0,15" berechnet, und die Beschleunigungsdaten [G] werden mit "Acceleration / 4096 (im Fall von ± 2G Range)" berechnet.
Über die zu verwendende Formel
Beschleunigungsdaten (A
Ermitteln Sie zunächst die Neigung und den Wurf anhand der folgenden Berechnung.
Pitch = ρ = arcsin(Ax) Roll = γ = arcsin(Ay/cosρ )
Suchen Sie als Nächstes die Überschrift mit der folgenden Formel.
Mx1 = Mxcosρ + Mzsinρ
My1 = Mxsinγsinρ + Mycosγ - Mzsinγcosρ
Überschrift = ψ = arctan (M y1 / M x1 ) M x1 > 0 und M y1 > = 0
= 180° + arctan(My1/Mx1) Mx1<0
= 360 ° + Arctan (M y1 / M x1 ) M x1 > 0 und M y1 <= 0
= 90 ° M x1 = 0 und M y1 <0
= 270 ° M x1 = 0 und M y1 > 0
Wie in der folgenden Abbildung gezeigt, ist die Überschrift der Winkel zwischen der Xb-Achse und dem magnetischen Norden in der horizontalen Ebene, der Abstand ist der Winkel zwischen der Xb-Achse und der horizontalen Ebene und die Rolle ist Y Sie wird durch den Winkel zwischen der Achse sub> b </ sub> und der horizontalen Ebene definiert.
Die Sensorempfindlichkeitsachse des Alps IoT Smart Module ist wie folgt.
Details finden Sie unter Abrufen von Pitch, Roll und Heading vom Bewegungssensor des Alps IoT Smart Module.
# -*- 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):
#Bewegungserkennung 100 ms Intervall (nur Bewegungssensor)
# 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)
_ Schwellenwertdefinitionsdatei "constant.conf" _
Der Schwellenwert "Schwellwert", mit dem überprüft wird, ob die berechnete Neigung, der Roll und der Steuerkurs innerhalb eines vorgegebenen Bereichs liegen, wird wie folgt eingestellt. Die Einheit ist "°", Nick und Roll werden durch absolute Winkel angezeigt, die auf 0 ° zentriert sind, und die Richtung wird durch Winkel im Uhrzeigersinn und gegen den Uhrzeigersinn angezeigt, die auf 0 ° zentriert sind. Dieser Wert wird beim Programmstart gelesen.
#Schwellendaten
[THRESHOLD]
thresholdAngl= 10
Programm ausführen
Führen Sie das Programm aus, platzieren Sie das Alps IoT Smart Module horizontal und stellen Sie den Kurs im Bereich von 350 ° bis 10 ° mit 0 ° dazwischen ein. Der elektronische Summer der PiConsole I / F ertönt. Wenn Sie das Alps IoT Smart Module auf der Skala platzieren, die den Umfang durch 30 ° teilt, und ihn drehen, ändert sich die Überschrift der Textanzeige der PiConsole I / F entsprechend. Die LED (gelb) leuchtet auf, wenn sie in Nickrichtung auf 0 ± 10 ° oder weniger geneigt ist, und die LED (rot) leuchtet auf, wenn sie in Rollrichtung auf 0 ± 10 ° oder weniger geneigt ist.
Die Anzeige der Textanzeige von PiConsole I / F und die Anzeige der LED sind unten dargestellt. "R" zeigt Roll an, "P" zeigt Pitch an und "Az" zeigt Kurs an.
Darüber hinaus werden die auf dem Ausführungsbildschirm angezeigten Berechnungsergebnisse für Roll, Pitch und Heading unten angezeigt.
$ 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
Recommended Posts