class Strapdown(object):
""" class Strapdown generates bearing, velocity and position from 9 degree IMU
"""
def __init__(self):
""" creates Strapdown object which includes the current bearing, velocity and position
bearing (phi, theta, psi) in degrees, velocity (ax, ay, az) in m/s, position (x,y,z) in m
"""
self.quaternion = Quaternion()
self.velocity = Velocity()
self.position = EllipsoidPosition(
) # navigation system position with Position()
self.isInitialized = False
def Initialze(self, acceleration, magneticField, position):
""" calculates attitutde and heading
sets position to given position vector
"""
self.quaternion = Quaternion(Euler(acceleration, magneticField).values)
self.position.values = position
self.isInitialized = True
def getOrientation(self):
return self.quaternion.getEulerAngles(
) # vector of euler angles in rad
def getVelocity(self):
return self.velocity.values
def getPosition(self):
return self.position.values
def test_getEulerAngles(self):
q = Quaternion(toVector(0.75, -0.51, pi))
vec = q.getEulerAngles()
phi, theta, psi = toValue(vec)
self.assertAlmostEqual(0.75, phi, delta=0.001)
self.assertAlmostEqual(-0.51, theta, delta=0.001)
self.assertAlmostEqual(pi, psi, delta=0.001)
class Strapdown(object):
def __init__(self):
""" creates Strapdown object which includes the current bearing, velocity and position
bearing (phi, theta, psi) in degrees, velocity (ax, ay, az) in m/s, position (x,y,z) in m
"""
self.quaternion = Quaternion()
self.velocity = Velocity() # vector (if known)
self.position = Position() # or GNSS.getPos()
# toVector(52.521918/180*pi, 13.413215\180*pi, 100.)
self.isInitialized = False
def Initialze(self, acceleration, magneticField):
self.quaternion = Quaternion(Euler(acceleration, magneticField).values)
self.isInitialized = True
#self.position = (toVector(n,e,d)) if GNSS measurement is icluded
def getOrientation(self):
return self.quaternion.getEulerAngles()
def getVelocity(self):
return self.velocity.values
def getPosition(self):
return self.position.values
