## python code to find compass bearing for phidget spatial

Supporting 2.7 and 3.2+
chezulkhairi
Fresh meat
Posts: 1
Joined: Wed Sep 14, 2011 5:51 am
Contact:

### python code to find compass bearing for phidget spatial

Hi, i've been trying to convert the cpp Spatial code for bearing into python. it is not working properly. have anyone managed to get the compass bearing working. Thanks, Zul.

here is my code:

import math
from math import *

def create_2n_matrix(n):
i=0
j=1
return [[(j,i) for i in range(n)] for j in range(n)]

def mult(matrix1,matrix2):
# Matrix multiplication
if len(matrix1[0]) != len(matrix2):
# Check matrix dimensions
print 'Matrices must be m*n and n*p to multiply!'
else:
# Multiply if correct dimensions
new_matrix = zero(len(matrix1),len(matrix2[0]))
for i in range(len(matrix1)):
for j in range(len(matrix2[0])):
for k in range(len(matrix2)):
new_matrix[i][j] += matrix1[i][k]*matrix2[k][j]
return new_matrix

def calculateCompassBearing(a2,b2,c2,l2,m2,n2):
timeConstant = 0.7
##########################################
global lastBearing
lastBearing=0.
global compassBearingFilter
compassBearingFilter=[]
global compassBearingFilterSize
compassBearingFilterSize = 10.
global pitchAngle
global rollAngle
pitchAngle=0
rollAngle=0
Xh = 0
Yh = 0
gravity=[(a2,b2,c2)]

#find the tilt of the board wrt gravity
#Vector3 gravity = Vector3.Normalize-->#spatial.accelerometerAxes[0].Acceleration /a2 ....
#spatial.accelerometerAxes[2].Acceleration /b2 #spatial.accelerometerAxes[1].Acceleration) /c2

if a2<1:
pitchAngle = asin(a2)
if c2<1:
rollAngle = asin(c2)
#The board is up-side down
if (a2 < 0):
pitchAngle = -pitchAngle
rollAngle = -rollAngle
#print pitchAngle,rollAngle
#Construct a rotation matrix for rotating vectors measured in the body frame, into the earth frame
#this is done by using the angles between the board and the gravity vector.
#xRotMatrix = create_2n_matrix(3)
xRotMatrix = zero(3,3)
#xRotMatrix = show(a)
xRotMatrix[0][0] = math.cos(pitchAngle)
xRotMatrix[0][1] = math.sin(pitchAngle)
xRotMatrix[0][2] = 0
xRotMatrix[1][0] = -math.sin(pitchAngle)
xRotMatrix[1][1] = math.cos(pitchAngle)
xRotMatrix[1][2] = 0
xRotMatrix[2][0] = 0
xRotMatrix[2][1] = 0
xRotMatrix[2][2] = 1
#print 'x',xRotMatrix
#zRotMatrix = create_2n_matrix(3)
zRotMatrix = zero(3,3)
#zRotMatrix = show(b)
zRotMatrix[0][0] = 1
zRotMatrix[0][1] = 0
zRotMatrix[0][2] = 0
zRotMatrix[1][0] = 0
zRotMatrix[1][1] = math.cos(rollAngle)
zRotMatrix[1][2] = math.sin(rollAngle)
zRotMatrix[2][0] = 0
zRotMatrix[2][1] = -math.sin(rollAngle)
zRotMatrix[2][2] = math.cos(rollAngle)
#print 'z',zRotMatrix
#rotMatrix = MultiplyMM(xRotMatrix,zRotMatrix) # matrix 3x3 multiply matrix 3x3 MultiplyVM
rotMatrix = mult(xRotMatrix,zRotMatrix)
#print rotMatrix
data = [(l2, m2, -n2)]
#correctedData = MultiplyMM(data,rotMatrix)
correctedData = mult(data,rotMatrix)
#//These represent the x and y components of the magnetic field vector in the earth frame
Xh = -(-n2)
Yh = -l2
#//we use the computed X-Y to find a magnetic North bearing in the earth frame
bearing = 0
_360inRads = (360 * math.pi / 180.0)
if Xh < 0:
bearing = math.pi - math.atan(Yh / Xh)
elif Xh > 0 and Yh < 0:
bearing = -math.atan(Yh / Xh)
elif Xh > 0 and Yh > 0:
bearing = math.pi * 2 - math.atan(Yh / Xh)
elif Xh == 0 and Yh < 0:
bearing = math.pi / 2.0
elif Xh == 0 and Yh > 0:
bearing = math.pi * 1.5
print 'bearing=',math.degrees(4-bearing)
#//The board is up-side down
if b2 < 0:
#print 'br',bearing,lastBearing
#//passing the 0 <-> 360 point, need to make sure the filter never contains both values near 0 and values near 360 at the same time.
if abs(bearing - lastBearing) > 2:# //2 radians == ~115 degrees
if(bearing > lastBearing):
for stuff in compassBearingFilter:
else:
for stuff in compassBearingFilter:
compassBearingFilter.append((bearing, pitchAngle, rollAngle))
if len(compassBearingFilter) > compassBearingFilterSize:
compassBearingFilter.pop(0)
bearing = pitchAngle = rollAngle = 0
for stuff in compassBearingFilter:
bearing =bearing + stuff[0]
pitchAngle =pitchAngle+ stuff[1]
rollAngle =rollAngle+ stuff[2]
bearing2 = bearing /len(compassBearingFilter)
pitchAngle = pitchAngle /len(compassBearingFilter)
rollAngle = rollAngle /len(compassBearingFilter)
compassBearing = bearing2 * (180.0 / math.pi)
lastBearing = bearing2
m=a2,b2,c2
print 'bearing2 =',math.degrees(4-bearing2)

lforet3252
Fresh meat
Posts: 1
Joined: Thu Nov 08, 2012 10:57 am
Contact:

### Re: python code to find compass bearing for phidget spatial

reward: \$20 via paypal for python code (function) which returns heading from phidgets 3/3/3/ compass degrees.. ie: 0,180,360: etc....
must be documented..

so pass in phidgets data: returns compass heading in degrees..

thanks!

Cloverseer
Fresh meat
Posts: 1
Joined: Thu Mar 24, 2016 8:25 am
Contact: