MechaTracks Co., Ltd. provided the Raspberry IoT Starter Kit "anyPi". , I tried connecting Alps IoT Smart Module. Calculates pitch, roll, and heading from the Alps IoT Smart Module accelerometer and geomagnetic sensor and displays the results on a text display. When the pitch, roll, and heading are within a certain range, the PiConsole I / F's electronic buzzer sounds. For pitch and roll, information is displayed by two LEDs if it falls within a certain range.

anyPi is stacked with Raspberry Pi 3, 3GPI, and PiConsole I / F in order from the bottom, and this time, pitch, roll, and heading are displayed on the text display of PiConsole I / F, and within a predetermined range. When it enters, the electronic buzzer sounds, when the pitch enters, the LED (yellow) lights up, and when the roll enters, the LED (red) lights up. The Alps IoT Smart Module connects to the Raspberry Pi 3 via the Bluetooth Low Energy (BLE) protocol to acquire motion data from the acceleration sensor and geomagnetic sensor. The Alps IoT Smart Module is placed on a scale paper with a circumference divided by 30 ° to make it easier to see the heading.

_ Program creation _

The program uses the Python language, "actionmain.py" that controls the whole, "alpsaction.py" that controls the Alps IoT Smart Module to acquire sensor information, and data on the text display of anyPi's PiConsole I / F. It consists of "lcd.py" to be displayed. Also, set the threshold definition in "constant.conf" to check if the calculated pitch, roll, and heading are within a predetermined range. In addition, "lcd.py" uses the same program as the sample program for LCD of "I / O of PiConsole I / F of anyPi". ..

_Installation of Python interface "bluepy" _

In order to handle BLE in the Python language, you need to install the Python interface "bluepy" with the following command.

$ 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

_Overall control module "actionmain.py" _

"Actionmain.py" starts first and does the following:

--BLE communication uses "AlpsSensor" which inherits "Peripheral" class. --The buzzer function is used to check the calculated pitch, roll, and heading range, and to turn on / off the LED (red / yellow). --The displayAngle function displays the pitch, roll, and heading on the PiConsole I / F text display. The displayAngle function is called as a callback function from the handleNotification method of the NtfyDelegate class. --When the program starts, the threshold data of the range check is acquired from the threshold definition file "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        #buzzer
LED_red = 20	#Red
LED_yellow = 21	#yellow
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)

#Read configuration file
    conf = ConfigParser.SafeConfigParser()
    conf.read('constant.conf')
#Saving range check data
    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 information acquisition "alpsaction.py" _

To acquire motion data from the acceleration sensor and geomagnetic sensor of the Alps IoT Smart Module, the command guide of the Alps IoT Smart Module "[Sensor Network Module Evaluation Kit Application Note Command Guide](http://www.alps.com/j] I made the following settings by referring to "2.2 Example of setting by command" of "/iotsmart-network/pdf/msm_command_manual.pdf)".

Setting	function
Measurement mode	Fast mode
Measurement interval	100ms
Acceleration	〇
Geo-Magnetic	〇
Pressure	－
Humidity	－
Temperature	－
UV	－
Ambient Light	－

BLE communication uses the "write Characteristic" method, and the command guide for the Alps IoT Smart Module " Sensor Network Module According to the evaluation kit Application Note Command Guide , set with the setSensor method of the AlpsSensor class. Notification messages are received by the handleNotification method of the NtfyDelegate class set by the setDelegate method. In the handleNotification method, the Event Code containing the motion data processes only data packet 1 "0xF2". The geomagnetic data and acceleration data in the data packet are signed 2-byte integers. The geomagnetic data [uT] is calculated by "Magnetic x 0.15", and the acceleration data [G] is calculated by "Acceleration / 4096 (in the case of ± 2G Range)".

_About the calculation formula to use _

Acceleration data (A _{x , A _{y , A _{z ) from the acceleration sensor of the Alps IoT Smart Module, and geomagnetic data (M _{x , M _{y , M _{z ) will be entered. Originally, it is necessary to calibrate or normalize the acquired data, but the acquired motion data will be used as it is. The calculation formula is shown below. The actual calculation is performed by the computeAngle method of the NtfyDelegate class.}}}}}}

First, find the pitch and roll by the following calculation.

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

Next, calculate the heading with the following formula.

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

As shown in the following figure, the heading is the angle between the X _{b axis and the magnetic north on the horizontal plane, the pitch is the angle between the X _{b axis and the horizontal plane, and the roll is Y <. It is defined by the angle between the sub> b axis and the horizontal plane.}}

The sensor sensitivity axis of the Alps IoT Smart Module is as follows.

Details are shown in Getting Pitch, Roll, and Heading from the Motion Sensor of the 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):
        #Motion detection 100ms interval (motion sensor only)
        # 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)

_ Threshold definition file "constant.conf" _

The threshold "thresholdAngl" used to check if the calculated pitch, roll, and heading are within a predetermined range is set as follows. The unit is "°", pitch and roll are indicated by absolute angles centered on 0 °, and headings are indicated by clockwise and counterclockwise angles centered on 0 °. This value is read when the program starts.

#Threshold data
[THRESHOLD]
thresholdAngl= 10

_ Program execution _

When you run the program, place the Alps IoT Smart Module horizontally, and set the heading in the range of 350 ° to 10 ° with 0 ° in between, the PiConsole I / F's electronic buzzer sounds. Also, if you place the Alps IoT Smart Module on the scale that divides the circumference by 30 ° and rotate it, the heading of the text display of the PiConsole I / F will change accordingly. The LED (yellow) lights up when tilted in the pitch direction to 0 ± 10 ° or less, and the LED (red) lights up when tilted in the roll direction to 0 ± 10 ° or less.

The display of the text display of PiConsole I / F and the LED display are shown below. "R" indicates roll, "P" indicates pitch, and "Az" indicates heading.

In addition, the calculation results of roll, pitch, and heading displayed on the execution screen are shown below.

$ 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

[PYTHON] Acquire / display the values of the acceleration sensor and geomagnetic sensor of the Alps IoT Smart Module with anyPi