Page 1 of 1

python code to find compass bearing for phidget spatial

Posted: Fri Oct 12, 2012 11:53 am
by chezulkhairi
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:
bearing = abs(bearing - _360inRads)
#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:
stuffs[0] = stuff+ _360inRads
else:
for stuff in compassBearingFilter:
stuffs[0] = stuff- _360inRads
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)

Re: python code to find compass bearing for phidget spatial

Posted: Thu Nov 08, 2012 11:02 am
by lforet3252
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!

Re: python code to find compass bearing for phidget spatial

Posted: Thu Mar 24, 2016 8:29 am
by Cloverseer
Very late reply to a (probably) long dead post but still...

In case anyone else was struggling. A (very) simple compass in python
https://github.com/Cloverseer/Python_Phidget_Compass.git