class PhidgetWrapper(object):
def __init__(self, data_callback):
self.spatial = Spatial()
self.callback = data_callback;
# attach the event handlers.
try:
self.spatial.setOnAttachHandler(self.on_attach)
self.spatial.setOnDetachHandler(self.on_detach)
self.spatial.setOnErrorhandler(self.on_error)
self.spatial.setOnSpatialDataHandler(self.on_data)
self.spatial.openPhidget()
self.spatial.waitForAttach(1000)
self.spatial.setDataRate(4)
self.spatial.setCompassCorrectionParameters(0.51236, 0.02523, 0.16216, 0.07254, 1.88718, 1.82735, 2.14068, -0.06096, -0.04644, -0.05897, 0.00783, -0.05211, 0.00870);
except e:
print("Error connecting to IMU, I cannot handle this. I will just go die now!", e)
exit(1)
def on_data(self, e):
source = e.device
for index, spatialData in enumerate(e.spatialData):
if len(spatialData.Acceleration) > 0 and len(spatialData.AngularRate) > 0 and len(spatialData.MagneticField) > 0:
acc = [spatialData.Acceleration[0], spatialData.Acceleration[1], spatialData.Acceleration[2]]
gyr = [spatialData.AngularRate[0], spatialData.AngularRate[1], spatialData.AngularRate[2]]
mag = [spatialData.MagneticField[0], spatialData.MagneticField[1], spatialData.MagneticField[2]]
self.callback(acc, gyr, mag, spatialData.Timestamp.microSeconds)
def on_attach(self, e):
print('Phidget attached!')
return
def on_detach(self, e):
print('Phidget detached!')
return
def on_error(self, e):
try:
source = e.device
print(("Spatial %i: Phidget Error %i: %s" % \
(source.getSerialNum(), e.eCode, e.description)))
except PhidgetException as e:
print(("Phidget Exception %i: %s" % (e.code, e.details)))
try:
spatial.resetCompassCorrectionParameters()
# From Coop Report Ben Krupicz (UBC) Sept 2014, Table 1 + appendix C
# Parameters: magField = 1, offset0, offset1, offset2, A0' = Gain0 * A0, A4' = Gain1 * A4, A8' = Gain2 * A8, A1' = Gain0 * A1,
# A2' = Gain0 * A2, A3' = Gain1 * A3, A5' = Gain1 * A5, A6' = Gain2 * A6, A7' = Gain2 * A7
# SetParameters(magField, offset0, offset1, offset2, A0', A4', A8', A1', A2', A3', A5', A6', A7')
if spatial.getSerialNum() == 301816:
#spatial.setCompassCorrectionParameters(1, 0.0084, 0.1937, 0.0517,
# 0.8877*0.9617, 0.8991*0.9395, 1.0452*1.0988,
# 0.8877*-0.0067, 0.8877*-0.0133,
# 0.8991*-0.0066, 0.8991*-0.00033789,
# 1.0452*-0.0152, 1.0452*-0.00034909)
# M.Walters' recalibration (errors under discussion)
spatial.setCompassCorrectionParameters(1, 0.0247, 0.1831, -0.0506,
1.6537, 1.7979, 1.9713,
-0.0102, -0.0107,
-0.0110, -0.0044,
-0.0127, -0.0048)
print "Calibrated phidget %i" %spatial.getSerialNum()
elif spatial.getSerialNum() == 302229:
#spatial.setCompassCorrectionParameters(1, 0.0394, 0.2194, -0.2528,
# 0.0633*0.9723, 0.8860*0.9388, 0.9329*1.0880,
# 0.0633*-0.0078, 0.0633*-0.0110,
# 0.8860*-0.0075, 0.8860*0.00035813,
# 0.9329*0.0122, 0.9329*0.00035841)
spatial.setCompassCorrectionParameters(1, 0.0535, 0.2105, -0.2624,
1.7464, 1.6831, 1.9148,
-0.0151, -0.0107,
-0.0144, -0.0019,
-0.0116, -0.0021)
print("Reset calibration constants and set new constants")
try:
spatial.resetCompassCorrectionParameters()
# From Coop Report Ben Krupicz (UBC) Sept 2014, Table 1 + appendix C
# Parameters: magField = 1, offset0, offset1, offset2, A0' = Gain0 * A0, A4' = Gain1 * A4, A8' = Gain2 * A8, A1' = Gain0 * A1,
# A2' = Gain0 * A2, A3' = Gain1 * A3, A5' = Gain1 * A5, A6' = Gain2 * A6, A7' = Gain2 * A7
# SetParameters(magField, offset0, offset1, offset2, A0', A4', A8', A1', A2', A3', A5', A6', A7')
if spatial.getSerialNum() == 301816:
#spatial.setCompassCorrectionParameters(1, 0.0084, 0.1937, 0.0517,
# 0.8877*0.9617, 0.8991*0.9395, 1.0452*1.0988,
# 0.8877*-0.0067, 0.8877*-0.0133,
# 0.8991*-0.0066, 0.8991*-0.00033789,
# 1.0452*-0.0152, 1.0452*-0.00034909)
# M.Walters' recalibration (errors under discussion)
spatial.setCompassCorrectionParameters(1, 0.0247, 0.1831, -0.0506,
1.6537, 1.7979, 1.9713, -0.0102,
-0.0107, -0.0110, -0.0044,
-0.0127, -0.0048)
print "Calibrated phidget %i" % spatial.getSerialNum()
elif spatial.getSerialNum() == 302229:
#spatial.setCompassCorrectionParameters(1, 0.0394, 0.2194, -0.2528,
# 0.0633*0.9723, 0.8860*0.9388, 0.9329*1.0880,
# 0.0633*-0.0078, 0.0633*-0.0110,
# 0.8860*-0.0075, 0.8860*0.00035813,
# 0.9329*0.0122, 0.9329*0.00035841)
spatial.setCompassCorrectionParameters(1, 0.0535, 0.2105, -0.2624,
1.7464, 1.6831, 1.9148, -0.0151,
-0.0107, -0.0144, -0.0019,
-0.0116, -0.0021)
print "Calibrated phidget %i" % spatial.getSerialNum()
# phidget01