class IMU(object):
device = None
def __init__(self):
self.spatial = Spatial()
self.spatial.setOnSpatialDataHandler(self.getDataHandler)
self.spatial.setOnAttachHandler(self.attachDeviceHandler)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(16)
def attachDeviceHandler(self, e):
self.device = e.device
def getDataHandler(self,e):
for data in e.spatialData:
if len(data.Acceleration) > 0:
print "Acc",
print data.Acceleration[0],
print data.Acceleration[1],
print data.Acceleration[2],
if len(data.AngularRate) > 0:
print "Gyr",
print data.AngularRate[0],
print data.AngularRate[1],
print data.AngularRate[2],
if len(data.MagneticField) > 0:
print "Mag",
print data.MagneticField[0],
print data.MagneticField[1],
print data.MagneticField[2]
def __init__(self):
"""
"""
# update from the IMU.
self.spatial = Spatial()
imu_handlers = Controller.IMU_Handlers()
self.prev = 0
# attach the event handlers.
try:
self.spatial.setOnAttachHandler(imu_handlers.on_attach)
self.spatial.setOnDetachHandler(imu_handlers.on_detach)
self.spatial.setOnErrorhandler(imu_handlers.on_error)
self.spatial.setOnSpatialDataHandler(imu_handlers.on_data)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(4)
except:
print("Error connecting to IMU, I cannot handle this. " + \
"I will just go die now!")
exit(1)
samplePeriod = 1 / 256
self.ahrs = MadgwickAHRS(samplePeriod)
#t = Thread(target=self.update_head)
#t.daemon = True
#t.start()
self.x = 0
self.y = 0
self.z = 0
self.update_head()
def __init__(self):
self.spatial = Spatial()
self.spatial.setOnSpatialDataHandler(self.getDataHandler)
self.spatial.setOnAttachHandler(self.attachDeviceHandler)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(16)
def StartPhidget(self):
# Initialise the Phidgets sensor
self.phidget_attached = False
self.time_start = None
try:
if self.accelerometer:
self.accelerometer.closePhidget()
self.accelerometer = Spatial()
self.accelerometer.setOnAttachHandler(self.AccelerometerAttached)
self.accelerometer.setOnDetachHandler(self.AccelerometerDetached)
self.accelerometer.setOnErrorhandler(self.AccelerometerError)
self.accelerometer.setOnSpatialDataHandler(self.SpatialData)
#self.accelerometer.enableLogging(Phidget.PhidgetLogLevel.PHIDGET_LOG_WARNING, None)
#self.accelerometer.enableLogging(Phidget.PhidgetLogLevel.PHIDGET_LOG_VERBOSE, None)
self.accelerometer.openPhidget()
self.accelerometer.waitForAttach(10000)
# set data rate in milliseconds (we will decimate from 4ms to 20ms later)
# we now do this in the Attach handler
#self.accelerometer.setDataRate(PHIDGETS_NOMINAL_DATA_INTERVAL_MS)
except RuntimeError as e:
logging.error("Runtime Exception: %s", e.details)
return
except PhidgetException as e:
logging.error("Phidget Exception: %s. Is the Phidget not connected?", e.details)
return
self.phidget_attached = True
class IMU(object):
device = None
def __init__(self):
self.spatial = Spatial()
self.spatial.setOnSpatialDataHandler(self.getDataHandler)
self.spatial.setOnAttachHandler(self.attachDeviceHandler)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(16)
def attachDeviceHandler(self, e):
self.device = e.device
def getDataHandler(self, e):
for data in e.spatialData:
if len(data.Acceleration) > 0:
print "Acc",
print data.Acceleration[0],
print data.Acceleration[1],
print data.Acceleration[2],
if len(data.AngularRate) > 0:
print "Gyr",
print data.AngularRate[0],
print data.AngularRate[1],
print data.AngularRate[2],
if len(data.MagneticField) > 0:
print "Mag",
print data.MagneticField[0],
print data.MagneticField[1],
print data.MagneticField[2]
def __init__(self):
"""
"""
# update from the IMU.
self.spatial = Spatial()
imu_handlers = Controller.IMU_Handlers()
self.prev = 0
# attach the event handlers.
try:
self.spatial.setOnAttachHandler(imu_handlers.on_attach)
self.spatial.setOnDetachHandler(imu_handlers.on_detach)
self.spatial.setOnErrorhandler(imu_handlers.on_error)
self.spatial.setOnSpatialDataHandler(imu_handlers.on_data)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(4)
except:
print("Error connecting to IMU, I cannot handle this. " + \
"I will just go die now!")
exit(1)
#t = Thread(target=self.update_head)
#t.daemon = True
#t.start()
self.x = 0
self.y = 0
self.z = 0
data = copy.copy(Controller.imu_measurements)
while (len(data[2]) != 3):
data = copy.copy(Controller.imu_measurements)
print("here")
mag_v = np.array(data[2])
norm = np.linalg.norm(mag_v)
mag_v = mag_v / norm
x = mag_v[0]
y = mag_v[1]
z = mag_v[2]
# intial reference frame
self.state = [x, y, z]
self.update_head()
def __init__(self):
self.spatial = Spatial()
self.spatial.setOnSpatialDataHandler(self.getDataHandler)
self.spatial.setOnAttachHandler(self.attachDeviceHandler)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(16)
def StartPhidget(self):
# Initialise the Phidgets sensor
self.phidget_attached = False
self.time_start = None
try:
if self.accelerometer:
self.accelerometer.closePhidget()
self.accelerometer = Spatial()
self.accelerometer.setOnAttachHandler(self.AccelerometerAttached)
self.accelerometer.setOnDetachHandler(self.AccelerometerDetached)
self.accelerometer.setOnErrorhandler(self.AccelerometerError)
self.accelerometer.setOnSpatialDataHandler(self.SpatialData)
self.accelerometer.openPhidget()
self.accelerometer.waitForAttach(10000)
# set data rate in milliseconds (we will decimate from 4ms to 20ms later)
self.accelerometer.setDataRate(PHIDGETS_NOMINAL_DATA_INTERVAL_MS)
except RuntimeError as e:
logging.error("Runtime Exception: %s", e.details)
return
except PhidgetException as e:
logging.error("Phidget Exception: %s. Is the Phidget not connected?", e.details)
return
self.phidget_attached = True
def __init__(self):
############################
# lets introduce and init the main variables
#self.com = None
self.isInitialized = False
#Create an accelerometer object
try:
self = Spatial()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
self.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
self.waitForAttach(4000)
print "is attached = ", self.isAttached()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
self.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
self.setDataRate(1000)
def __init__(self):
"""
"""
# update from the IMU.
self.spatial = Spatial()
imu_handlers = Controller.IMU_Handlers()
self.prev = 0
# attach the event handlers.
try:
self.spatial.setOnAttachHandler(imu_handlers.on_attach)
self.spatial.setOnDetachHandler(imu_handlers.on_detach)
self.spatial.setOnErrorhandler(imu_handlers.on_error)
self.spatial.setOnSpatialDataHandler(imu_handlers.on_data)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(4)
except:
print("Error connecting to IMU, I cannot handle this. " + \
"I will just go die now!")
exit(1)
samplePeriod = 1/256
self.ahrs = MadgwickAHRS(samplePeriod)
#t = Thread(target=self.update_head)
#t.daemon = True
#t.start()
self.x = 0
self.y = 0
self.z = 0
self.update_head()
def create_spatial(self):
try:
spatial = Spatial()
self.status = "connected"
return spatial
except RuntimeError as e:
self.status = "not connected"
print("Runtime Exception: %s" % e.details)
def __init__(self):
"""
"""
# update from the IMU.
self.spatial = Spatial()
imu_handlers = Controller.IMU_Handlers()
self.prev = 0
# attach the event handlers.
try:
self.spatial.setOnAttachHandler(imu_handlers.on_attach)
self.spatial.setOnDetachHandler(imu_handlers.on_detach)
self.spatial.setOnErrorhandler(imu_handlers.on_error)
self.spatial.setOnSpatialDataHandler(imu_handlers.on_data)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(4)
except:
print("Error connecting to IMU, I cannot handle this. " + "I will just go die now!")
exit(1)
# t = Thread(target=self.update_head)
# t.daemon = True
# t.start()
self.x = 0
self.y = 0
self.z = 0
data = copy.copy(Controller.imu_measurements)
while len(data[2]) != 3:
data = copy.copy(Controller.imu_measurements)
print("here")
mag_v = np.array(data[2])
norm = np.linalg.norm(mag_v)
mag_v = mag_v / norm
x = mag_v[0]
y = mag_v[1]
z = mag_v[2]
# intial reference frame
self.state = [x, y, z]
self.update_head()
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 __init__(self):
# Build the Phidget Object
try:
self.spatial = Spatial()
except Exception as e:
print("Could not build object %s" %e)
exit(0)
self.last_angles = [0,0,0] # The last set of bearing angles
self.compass_bearing_filter = [] # bearing store for filtering
self.bearing_filter_size = 10 # Max size of bearing store
self.compass_bearing = 0 # Init bearing value
# Register signal capture to end programme
signal.signal(signal.SIGINT, self.signal_handler)
def __init__(self):
try:
self.spatial = Spatial()
except Exception as e:
print("Could not build object %s" %e)
exit(0)
self.RegisterHandlers()
self.last_angles = [0,0,0]
self.compass_bearing_filter = []
self.bearing_filter_size = 10
self.compass_bearing = 0
# Register singal capture
signal.signal(signal.SIGINT, self.signal_handler)
def AttachSpatial(databasepath, serialNumber):
def onAttachHandler(event):
logString = "Spatial Attached " + str(event.device.getSerialNum())
#print(logString)
DisplayAttachedDeviceInfo(event.device)
def onDetachHandler(event):
logString = "Spatial Detached " + str(event.device.getSerialNum())
#print(logString)
DisplayDetachedDeviceInfo(event.device)
event.device.closePhidget()
def onErrorHandler(event):
logString = "Spatial Error " + str(event.device.getSerialNum()) + ", Error: " + event.description
#print(logString)
DisplayErrorDeviceInfo(event.device)
def onServerConnectHandler(event):
logString = "Spatial Server Connect " + str(event.device.getSerialNum())
#print(logString)
def onServerDisconnectHandler(event):
logString = "Spatial Server Disconnect " + str(event.device.getSerialNum())
#print(logString)
def spatialDataHandler(event):
logString = "Spatial Changed " + str(event.device.getSerialNum())
#print(logString)
try:
conn = sqlite3.connect(databasepath)
index = 0
for spatialData in enumerate(event.spatialData):
accelX = 0
accelY = 0
accelZ = 0
if len(spatialData[1].Acceleration) > 0:
accelX = spatialData[1].Acceleration[0]
accelY = spatialData[1].Acceleration[1]
if len(spatialData[1].Acceleration) > 2:
accelZ = spatialData[1].Acceleration[2]
angularX = 0
angularY = 0
angularZ = 0
if len(spatialData[1].AngularRate) > 0:
angularX = spatialData[1].AngularRate[0]
angularY = spatialData[1].AngularRate[1]
if len(spatialData[1].AngularRate) > 2:
angularZ = spatialData[1].AngularRate[2]
magneticX = 0
magneticY = 0
magneticZ = 0
if len(spatialData[1].MagneticField) > 0:
magneticX = spatialData[1].MagneticField[0]
magneticY = spatialData[1].MagneticField[1]
if len(spatialData[1].AngularRate) > 2:
magneticZ = spatialData[1].MagneticField[2]
conn.execute("INSERT INTO SPATIAL_DATACHANGE VALUES(NULL, DateTime('now'), ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)",
(event.device.getSerialNum(), index, accelX, accelY, accelZ, angularX, angularY, angularZ, magneticX, magneticY, magneticZ))
index += 1
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
try:
p = Spatial()
p.setOnAttachHandler(onAttachHandler)
p.setOnDetachHandler(onDetachHandler)
p.setOnErrorhandler(onErrorHandler)
p.setOnServerConnectHandler(onServerConnectHandler)
p.setOnServerDisconnectHandler(onServerDisconnectHandler)
p.setOnSpatialDataHandler(spatialDataHandler)
p.openPhidget(serialNumber)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting...")
exit(1)
def startAccel(trialId, graph):
global accelFilename
global outPutTxt_a
global orientation
global fA
global totalTimeElapsed
global totalSamplesTaken_a
global totalAbsZAccel
global totalAVec
global totalAvgAVec
totalAvgAVec = 0
totalAVec = 0
totalAbsZAccel = 0
totalSamplesTaken_a = 0
totalTimeElapsed = 0
# Setutp graphing ability
outPutTxt_a = graph
orientation = True # True corresponds to "right-side up"
if graph:
accelFilename = str(trialId) + "_accelOutput.txt"
fA = open(accelFilename, 'w') #WRITES OVER PREVIOUS DATA
header = "#Trial: " + str(
trialId) + " (x[g], y[g], z[g], 3D A_Vec, samples) \n"
fA.write(header)
#Create an accelerometer object
try:
global spatial
spatial = Spatial()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
spatial.setOnAttachHandler(SpatialAttached)
spatial.setOnDetachHandler(SpatialDetached)
spatial.setOnErrorhandler(SpatialError)
spatial.setOnSpatialDataHandler(SpatialData)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
spatial.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
spatial.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
spatial.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
spatial.setDataRate(24) ############### Sample rate is 24ms averaged
def AttachSpatial(databasepath, serialNumber):
def onAttachHandler(event):
logString = "Spatial Attached " + str(event.device.getSerialNum())
#print(logString)
DisplayAttachedDeviceInfo(event.device)
def onDetachHandler(event):
logString = "Spatial Detached " + str(event.device.getSerialNum())
#print(logString)
DisplayDetachedDeviceInfo(event.device)
event.device.closePhidget()
def onErrorHandler(event):
logString = "Spatial Error " + str(
event.device.getSerialNum()) + ", Error: " + event.description
#print(logString)
DisplayErrorDeviceInfo(event.device)
def onServerConnectHandler(event):
logString = "Spatial Server Connect " + str(
event.device.getSerialNum())
#print(logString)
def onServerDisconnectHandler(event):
logString = "Spatial Server Disconnect " + str(
event.device.getSerialNum())
#print(logString)
def spatialDataHandler(event):
logString = "Spatial Changed " + str(event.device.getSerialNum())
#print(logString)
try:
conn = sqlite3.connect(databasepath)
index = 0
for spatialData in enumerate(event.spatialData):
accelX = 0
accelY = 0
accelZ = 0
if len(spatialData[1].Acceleration) > 0:
accelX = spatialData[1].Acceleration[0]
accelY = spatialData[1].Acceleration[1]
if len(spatialData[1].Acceleration) > 2:
accelZ = spatialData[1].Acceleration[2]
angularX = 0
angularY = 0
angularZ = 0
if len(spatialData[1].AngularRate) > 0:
angularX = spatialData[1].AngularRate[0]
angularY = spatialData[1].AngularRate[1]
if len(spatialData[1].AngularRate) > 2:
angularZ = spatialData[1].AngularRate[2]
magneticX = 0
magneticY = 0
magneticZ = 0
if len(spatialData[1].MagneticField) > 0:
magneticX = spatialData[1].MagneticField[0]
magneticY = spatialData[1].MagneticField[1]
if len(spatialData[1].AngularRate) > 2:
magneticZ = spatialData[1].MagneticField[2]
conn.execute(
"INSERT INTO SPATIAL_DATACHANGE VALUES(NULL, DateTime('now'), ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)",
(event.device.getSerialNum(), index, accelX, accelY,
accelZ, angularX, angularY, angularZ, magneticX,
magneticY, magneticZ))
index += 1
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
try:
p = Spatial()
p.setOnAttachHandler(onAttachHandler)
p.setOnDetachHandler(onDetachHandler)
p.setOnErrorhandler(onErrorHandler)
p.setOnServerConnectHandler(onServerConnectHandler)
p.setOnServerDisconnectHandler(onServerDisconnectHandler)
p.setOnSpatialDataHandler(spatialDataHandler)
p.openPhidget(serialNumber)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting...")
exit(1)
class Compass():
def __init__(self):
try:
self.spatial = Spatial()
except Exception as e:
print("Could not build object %s" %e)
exit(0)
self.RegisterHandlers()
self.last_angles = [0,0,0]
self.compass_bearing_filter = []
self.bearing_filter_size = 10
self.compass_bearing = 0
# Register singal capture
signal.signal(signal.SIGINT, self.signal_handler)
def DeviceInfo(self):
"""
Get and print all the information about this device
"""
self.is_attached = self.spatial.isAttached()
self.device_name = self.spatial.getDeviceName()
self.seriel_number = self.spatial.getSerialNum()
self.device_version= self.spatial.getDeviceVersion()
self.accel_axis_count = self.spatial.getAccelerationAxisCount()
self.gyro_axis_count = self.spatial.getGyroAxisCount()
self.comp_axis_count = self.spatial.getCompassAxisCount()
print("Start of Device Info")
print self.is_attached
print self.device_name
print self.seriel_number
print self.device_version
print self.accel_axis_count
print self.gyro_axis_count
print self.comp_axis_count
print("End of Device Info")
def RegisterHandlers(self):
"""
Register our event handlers - these functions will be called at the set event
"""
try:
self.spatial.setOnAttachHandler(self.DeviceAttached)
self.spatial.setOnDetachHandler(self.DeviceDetached)
self.spatial.setOnErrorhandler(self.DeviceError)
self.spatial.setOnSpatialDataHandler(self.CalculateBearing)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(0)
def DeviceAttached(self, e):
"""
Device has been attached (event)
"""
self.attached = e.device
print("Device %i Attached!" % (self.attached.getSerialNum()))
self.DeviceInfo()
def DeviceDetached(self, e):
"""
Device has been detached event
"""
self.detached = e.device
print("Spatial %i Detached!" % (self.detached.getSerialNum()))
def DeviceError(self, e):
"""
Device has encountered an error
"""
try:
source = e.device
print("Device %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def ReadData(self, e):
"""
Reads and prints data off of each sensor
"""
source = e.device
print("Spatial %i: Amount of data %i" % (source.getSerialNum(), len(e.spatialData)))
for index, spatialData in enumerate(e.spatialData):
print("=== Data Set: %i ===" % (index))
if len(spatialData.Acceleration) > 0:
print("Acceleration> x: %6f y: %6f z: %6f" % (spatialData.Acceleration[0], spatialData.Acceleration[1], spatialData.Acceleration[2]))
if len(spatialData.AngularRate) > 0:
print("Angular Rate> x: %6f y: %6f z: %6f" % (spatialData.AngularRate[0], spatialData.AngularRate[1], spatialData.AngularRate[2]))
if len(spatialData.MagneticField) > 0:
print("Magnetic Field> x: %6f y: %6f z: %6f" % (spatialData.MagneticField[0], spatialData.MagneticField[1], spatialData.MagneticField[2]))
print("Time Span> Seconds Elapsed: %i microseconds since last packet: %i" % (spatialData.Timestamp.seconds, spatialData.Timestamp.microSeconds))
print("------------------------------------------")
def CalculateBearing(self, e):
"""
Calculates a bearing value using the accelerometer and magnetometer
"""
gravity = []
mag_field = []
angles = []
# 0 = x axis, 1 = y axis, 2 = z axis
gravity.append(self.spatial.getAcceleration(0))
gravity.append(self.spatial.getAcceleration(1))
gravity.append(self.spatial.getAcceleration(2))
mag_field.append(self.spatial.getMagneticField(0))
mag_field.append(self.spatial.getMagneticField(1))
mag_field.append(self.spatial.getMagneticField(2))
# Roll angle about axis 0
# tan(roll angle) = gy/gz
# Atan2 gives us output as (-180 - 180) deg
roll_angle = math.atan2(gravity[1], gravity[2])
# Pitch Angle - about axis 1
# tan(pitch angle) = -gx / ((gy * sin(roll angle)) + (gz * cos(roll angle)))
# Pitch angle range is (-90 - 90) degrees
pitch_angle = math.atan( -gravity[0] / (gravity[1] * math.sin(roll_angle) + gravity[2] * math.cos(roll_angle)))
# Yaw Angle - about axis 2
# tan(yaw angle) = (mz * sin(roll) \96 my * cos(roll)) /
# (mx * cos(pitch) + my * sin(pitch) * sin(roll) + mz * sin(pitch) * cos(roll))
# Use Atan2 to get our range in (-180 - 180)
# Yaw angle == 0 degrees when axis 0 is pointing at magnetic north
yaw_angle = math.atan2( mag_field[2] * math.sin(roll_angle) - mag_field[1] * math.cos(roll_angle),
mag_field[0] * math.cos(pitch_angle) + mag_field[1] * math.sin(pitch_angle) * math.sin(roll_angle) + mag_field[2] * math.sin(pitch_angle) * math.cos(roll_angle))
# Add angles to our list
angles.append(roll_angle)
angles.append(pitch_angle)
angles.append(yaw_angle)
# This is our low pass filter to make the values look nicer on screen
try:
# Make sure the filter bugger doesn't have values passing the -180<->180 mark
# This only applies to the Roll and Yaw - Pitch will never have a sudden switch like that
for i in xrange(0,3,2):
if( math.fabs( angles[i] - self.last_angles[i] > 3 )):
for stuff in self.compass_bearing_filter:
if(angles[i] > self.last_angles[i]):
stuff[i] += 360 * math.pi / 180.0
else:
stuff[i] -= 360 * math.pi / 180.0
self.last_angles = angles
self.compass_bearing_filter.append(angles)
if(len(self.compass_bearing_filter) > self.bearing_filter_size):
self.compass_bearing_filter.pop(0)
yaw_angle = pitch_angle = roll_angle = 0
for stuff in self.compass_bearing_filter:
roll_angle += stuff[0]
pitch_angle += stuff[1]
yaw_angle += stuff[2]
yaw_angle = yaw_angle / len(self.compass_bearing_filter)
pitch_angle = pitch_angle / len(self.compass_bearing_filter)
roll_angle = roll_angle / len(self.compass_bearing_filter)
# Convert radians to degrees for display
self.compass_bearing = yaw_angle * (180.0 / math.pi)
# Set a directional string (one of 8 compass directions
# based on the angle i.e. North, North-East, South-West etc.
# Split by checking main 4 compass point angles +/- 22.5 deg
if(self.compass_bearing >=0 and self.compass_bearing < 22.5 ):
string_bearing = "North: "
elif(self.compass_bearing >=22.5 and self.compass_bearing < 67.5 ):
string_bearing = "North East: "
elif(self.compass_bearing >=67.5 and self.compass_bearing < 112.5 ):
string_bearing = "East: "
elif(self.compass_bearing >=112.5 and self.compass_bearing < 157.5 ):
string_bearing = "South East: "
elif(self.compass_bearing >=157.5 and self.compass_bearing <= 180 ):
string_bearing = "South: "
elif(self.compass_bearing <= -157.5 and self.compass_bearing > -180 ):
string_bearing = "South: "
elif(self.compass_bearing <= -112.5 and self.compass_bearing > -157.5 ):
string_bearing = "South West: "
elif(self.compass_bearing <= -67.5 and self.compass_bearing > -112.5 ):
string_bearing = "West: "
elif(self.compass_bearing <= -22.5 and self.compass_bearing > -67.5 ):
string_bearing = "North West: "
elif(self.compass_bearing <= 0.0 and self.compass_bearing > -22.5 ):
string_bearing = "North: "
# Add the actual bearing value to the string then print
string_bearing = string_bearing + " %.1f" % self.compass_bearing
print string_bearing
except Exception as e:
print e
def Main(self):
try:
self.spatial.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(0)
print ("Looking for device...")
try:
self.spatial.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
self.spatial.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(0)
print("Exiting....")
exit(0)
self.spatial.setDataRate(1000)
self.DeviceInfo()
print("Closing...")
try:
self.spatial.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
def signal_handler(self, signal, frame):
print("Ctrl+C pressed")
try:
self.spatial.closePhidget()
print("Compass Closed")
print("Exiting....")
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(0)
exit(1)
from geometry_msgs.msg import Vector3
import roslib
roslib.load_manifest('tf')
import tf
#Basic imports
from ctypes import *
import sys
#Phidget specific imports
from Phidgets.Phidget import Phidget
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import SpatialDataEventArgs, AttachEventArgs, DetachEventArgs, ErrorEventArgs
from Phidgets.Devices.Spatial import Spatial, SpatialEventData, TimeSpan
#Create an accelerometer object
try:
spatial = Spatial()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Information Display Function
def DisplayDeviceInfo():
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (spatial.isAttached(), spatial.getDeviceName(), spatial.getSerialNum(), spatial.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Acceleration Axes: %i" % (spatial.getAccelerationAxisCount()))
print("Number of Gyro Axes: %i" % (spatial.getGyroAxisCount()))
print("Number of Compass Axes: %i" % (spatial.getCompassAxisCount()))
from Phidgets.Events.Events import SpatialDataEventArgs, AttachEventArgs, DetachEventArgs, ErrorEventArgs, CurrentChangeEventArgs, PositionChangeEventArgs, VelocityChangeEventArgs
from Phidgets.Devices.AdvancedServo import AdvancedServo
from Phidgets.Devices.Servo import ServoTypes
from Phidgets.Phidget import PhidgetLogLevel
#Crear un objeto AdvancedServo
try:
advancedServo = AdvancedServo()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Crear un objeto Spatial
try:
spatial = Spatial()
timeSpan = TimeSpan(0, 0)
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Valores de los mototres
currentList = [0, 0, 0, 0, 0, 0, 0, 0]
velocityList = [0, 0, 0, 0, 0, 0, 0, 0]
# Define una funcion para manejar errores y asi usarla en los "try...except"
def LocalErrorCatcher(event):
print("Phidget Exception: " + str(e.code) + " - " + str(e.details))
class Controller(object):
"""
"""
# stores acceleration gyroscope, magnetic, time.
imu_measurements = [[], [], [], 0]
class IMU_Handlers(object):
"""
"""
def on_data(self, e):
"""
"""
source = e.device
for index, spatialData in enumerate(e.spatialData):
if (len(spatialData.Acceleration) == 3) and (len(spatialData.AngularRate) == 3) \
and (len(spatialData.MagneticField) == 3):
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]
]
Controller.imu_measurements[0] = gyr
Controller.imu_measurements[1] = acc
Controller.imu_measurements[2] = mag
Controller.imu_measurements[
3] = spatialData.Timestamp.seconds
break
def on_attach(self, e):
"""
"""
return
def on_detach(self, e):
"""
"""
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)))
def __init__(self):
"""
"""
# update from the IMU.
self.spatial = Spatial()
imu_handlers = Controller.IMU_Handlers()
self.prev = 0
# attach the event handlers.
try:
self.spatial.setOnAttachHandler(imu_handlers.on_attach)
self.spatial.setOnDetachHandler(imu_handlers.on_detach)
self.spatial.setOnErrorhandler(imu_handlers.on_error)
self.spatial.setOnSpatialDataHandler(imu_handlers.on_data)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(4)
except:
print("Error connecting to IMU, I cannot handle this. " + \
"I will just go die now!")
exit(1)
samplePeriod = 1 / 256
self.ahrs = MadgwickAHRS(samplePeriod)
#t = Thread(target=self.update_head)
#t.daemon = True
#t.start()
self.x = 0
self.y = 0
self.z = 0
self.update_head()
def update_head(self):
"""
"""
while True:
data = copy.copy(Controller.imu_measurements)
if (len(data[0]) + len(data[1]) + len(data[2]) == 9):
self.ahrs.update(data[0][0], data[0][1], data[0][2],
data[1][0], data[1][1], data[1][2],
data[2][0], data[2][1], data[2][2])
x, y, z = self.ahrs.getEulerAngles()
self.x = x
self.y = y
self.z = z
if self.prev != data[3]:
print("")
print(self.x, self.y, self.z)
#print (self.ahrs.quatern2rotMat())
print(data)
print("")
#print (data[3])
self.prev = data[3]
class PhidgetsSensor(object):
def __init__(self, config=None):
if not config:
config = {}
self.phidget_attached = False
self.collect_samples = True
self.picker = True
self.sensor_data_lock = threading.Lock()
self.delta_fields = set()
self.sensor_data = {}
self.sensor_data['Type'] = 'ACCELEROMETER_3_AXIS'
self.sensor_data['Calibrated'] = True
self.sensor_data['Model'] = 'Phidgets 1056'
self.decimation = PHIDGETS_DECIMATION
if 'software_version' in config:
self.software_version = config.get('software_version')
else:
self.software_version = 'PyCSN Unknown'
if 'decimation' in config:
self.decimation = config.get('decimation')
if 'picker' in config:
self.picker = config.get('picker')
if 'pick_threshold' in config:
self.pick_threshold = config.get('pick_threshold')
else:
self.pick_threshold = PICKER_THRESHOLD
if 'serial' in config:
self.sensor_data['Serial'] = config.get('serial')
else:
self.sensor_data['Serial'] = ''
if 'datastore' in config:
self.datastore = config.get('datastore')
else:
self.datastore = '/var/tmp/phidgetsdata'
#if 'pick_queue' in config:
# self.pick_queue = config.get('pick_queue')
#else:
# self.pick_queue = None
if 'latitude' in config:
self.latitude = config.get('latitude')
else:
self.latitude = 0.0
if 'longitude' in config:
self.longitude = config.get('longitude')
else:
self.longitude = 0.0
if 'floor' in config:
self.floor = config.get('floor')
else:
self.floor = '1'
if 'client_id' in config:
self.client_id = config.get('client_id')
else:
self.client_id = 'Unknown'
if 'stomp' in config:
self.stomp = config.get('stomp')
else:
self.stomp = None
if 'stomp_topic' in config:
self.stomp_topic = config.get('stomp_topic')
else:
self.stomp_topic = None
if 'connect_stomp' in config:
self.connect_stomp = config.get('connect_stomp')
else:
self.connect_stomp = None
self.datastore_uploaded = self.datastore + '/uploaded'
self.datastore_corrupted = self.datastore + '/corrupted'
logging.info('Sensor datastore directory is %s',self.datastore)
# Make sure data directory exists
try:
os.makedirs(self.datastore)
except OSError as exception:
if exception.errno != errno.EEXIST:
logging.error('Error making directory %s', self.datastore)
raise exception
try:
os.makedirs(self.datastore_uploaded)
except OSError as exception:
if exception.errno != errno.EEXIST:
logging.error('Error making directory %s', self.datastore_uploaded)
raise exception
try:
os.makedirs(self.datastore_corrupted)
except OSError as exception:
if exception.errno != errno.EEXIST:
logging.error('Error making directory %s', self.datastore_corrupted)
raise exception
self.sensor_data['Units'] = 'g'
self.sensor_data['Num_samples'] = 50
self.sensor_data['Sample_window_size'] = 1
self.accelerometer = None
self.time_start = None
self.reported_clock_drift = 0.0
self.file_store_interval = 600.0
self.last_phidgets_timestamp = None
self.last_datastore_filename = None
self.last_datastore_filename_uploaded = None
self.writing_errors = 0
self.last_sample_seconds = time.time()
# a lock for updating the timing variables
self.timing_lock = threading.Lock()
self.timing_gradient = 0.0
self.timing_intercept = 0.0
self.timing_base_time = time.time()
self.recent_picks = []
# Everything looks OK, start the collect samples and picking threads and the Phidget
self.sensor_raw_data_queue = Queue.Queue()
raw_data_thread = threading.Thread(target=self.ProcessSensorReading, args=['raw'])
raw_data_thread.setDaemon(True)
raw_data_thread.start()
self.sensor_raw_data_queue.join()
logging.info('Raw Data Thread started')
# start the Picker thread, if this is required
if self.picker:
self.sensor_readings_queue = Queue.Queue()
thread = threading.Thread(target=self.Picker, args=['Simple Threshold'])
thread.setDaemon(True)
thread.start()
logging.info('Picker started')
self.sensor_readings_queue.join()
logging.info('Sensor readings thread started')
else:
logging.info('This Phidget will not calculate or send picks')
logging.info('Client initialised: now starting Phidget')
self.data_buffer = []
# Start the Phidget
self.StartPhidget()
if not self.phidget_attached:
raise
self.sensor_data['Serial'] = str(self.accelerometer.getSerialNum())
self.DisplayDeviceInfo()
def StartPhidget(self):
# Initialise the Phidgets sensor
self.phidget_attached = False
self.time_start = None
try:
if self.accelerometer:
self.accelerometer.closePhidget()
self.accelerometer = Spatial()
self.accelerometer.setOnAttachHandler(self.AccelerometerAttached)
self.accelerometer.setOnDetachHandler(self.AccelerometerDetached)
self.accelerometer.setOnErrorhandler(self.AccelerometerError)
self.accelerometer.setOnSpatialDataHandler(self.SpatialData)
#self.accelerometer.enableLogging(Phidget.PhidgetLogLevel.PHIDGET_LOG_WARNING, None)
#self.accelerometer.enableLogging(Phidget.PhidgetLogLevel.PHIDGET_LOG_VERBOSE, None)
self.accelerometer.openPhidget()
self.accelerometer.waitForAttach(10000)
# set data rate in milliseconds (we will decimate from 4ms to 20ms later)
# we now do this in the Attach handler
#self.accelerometer.setDataRate(PHIDGETS_NOMINAL_DATA_INTERVAL_MS)
except RuntimeError as e:
logging.error("Runtime Exception: %s", e.details)
return
except PhidgetException as e:
logging.error("Phidget Exception: %s. Is the Phidget not connected?", e.details)
return
self.phidget_attached = True
def ProcessSensorReading(self, args):
# handles incoming samples from the Phidgets sensor
logging.info('Initialise ProcessSensorReading')
while True:
e, sample_timestamp = self.sensor_raw_data_queue.get()
if self.collect_samples:
if self.time_start == None:
self.time_start = sample_timestamp
self.last_timestamp_sent_to_picker = 0
self.last_sample_seconds = self.time_start
self.sample_count = 0
self.accelerations = []
self.datastore_filename = self.MakeFilename(sample_timestamp)
logging.info('Storing samples to %s',self.datastore_filename)
self.datastore_file = open(self.datastore_filename, 'w')
self.first_sample_timestamp_in_file = None
self.last_phidgets_timestamp = 0.0
for index, spatialData in enumerate(e.spatialData):
phidgets_timestamp = spatialData.Timestamp.seconds + (spatialData.Timestamp.microSeconds * 0.000001)
# the accelerations are scaled and reordered to conform to the SAF standard of [N, E, Z]
accs = [PHIDGETS_ACCELERATION_TO_G*spatialData.Acceleration[1],\
PHIDGETS_ACCELERATION_TO_G*spatialData.Acceleration[0],\
PHIDGETS_ACCELERATION_TO_G*spatialData.Acceleration[2]]
# we keep a running sum of accelerations in self.accelerations, which we will use to average later
if len(self.accelerations) == 0:
self.accelerations = accs
else:
for i in range(len(self.accelerations)):
self.accelerations[i] += accs[i]
sample_increment = 1
if self.last_phidgets_timestamp:
sample_increment = int(round( (phidgets_timestamp - self.last_phidgets_timestamp) / PHIDGETS_NOMINAL_DATA_INTERVAL))
if sample_increment > 4*self.decimation:
logging.warn('Missing >3 samples: last sample %s current sample %s missing samples %s',\
self.last_phidgets_timestamp, phidgets_timestamp, sample_increment)
elif sample_increment == 0:
logging.warn('Excess samples: last sample %s current sample %s equiv samples %s',\
self.last_phidgets_timestamp, phidgets_timestamp, sample_increment)
self.last_phidgets_timestamp = phidgets_timestamp
self.last_sample_seconds = sample_timestamp
self.sample_count += 1
if self.sample_count >= self.decimation:
# we average the last self.decimation samples
accelerations = [acc / float(self.decimation) for acc in self.accelerations]
#invert channel 1 (E-W) - this results in +1g being reported when the sensor is resting on its E side
accelerations[1] = -accelerations[1]
data_entry = [sample_timestamp, accelerations]
if self.picker:
if sample_timestamp - self.last_timestamp_sent_to_picker >= PICKER_INTERVAL:
# put this reading in the queue for the Picker
self.last_timestamp_sent_to_picker = sample_timestamp
self.sensor_readings_queue.put(data_entry)
# append this reading to the data buffer
# @TODO add lock
self.data_buffer.append(data_entry)
if self.datastore_file:
if self.first_sample_timestamp_in_file == None:
self.first_sample_timestamp_in_file = sample_timestamp
try:
self.datastore_file.write("%20.5f" % sample_timestamp + ' ' +
' '.join("%10.7f" % x for x in accelerations)+'\n')
self.writing_errors = 0
except Exception, e:
self.writing_errors += 1
if self.writing_errors <= 10:
logging.error('Error %s writing sample to file %s with timestamp %s',e,self.datastore_filename,sample_timestamp)
self.sample_count = 0
self.accelerations = []
if sample_timestamp - self.first_sample_timestamp_in_file >= self.file_store_interval:
logging.info('File %s store interval elapsed with %s samples, rate %s samples/second',self.datastore_filename,len(self.data_buffer),float(len(self.data_buffer))/self.file_store_interval)
# we will change to a new file in the datastore
if self.datastore_file:
self.datastore_file.close()
#logging.info('File closed')
self.last_datastore_filename = self.datastore_filename
self.data_buffer = []
self.time_start = None
logging.error('Raw Data Processing Thread terminated')
__date__ = 'May 17 2010'
#Basic imports
from ctypes import *
import sys
import math
#from ROOT import TGraph, TCanvas
#Phidget specific imports
from Phidgets.Phidget import Phidget
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import SpatialDataEventArgs, AttachEventArgs, DetachEventArgs, ErrorEventArgs
from Phidgets.Devices.Spatial import Spatial, SpatialEventData, TimeSpan
#Create an accelerometer object
try:
spatial = Spatial()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
sys.exit(1)
#Information Display Function
def DisplayDeviceInfo():
print(
"|------------|----------------------------------|--------------|------------|"
)
print(
"|- Attached -|- Type -|- Serial No. -|- Version -|"
)
print(
class Sensor(object):
"""
Phidgets sensor class obeying the expected Sensor contract.
Expected functions for a Sensor are:
run
A function which takes a threading.Event object which is set when the
sensor should halt. The body of run should be used for processing
sensor data, and is run in its own thread by the controller.
get_current_data
Used to respond to requests for ongoing sensor data. Must be thread
safe. The result is a tuple of metadata and a block of data to be
uploaded, if desired. To opt not to return data, the sensor can
simply return `(None, None)`. The function can block to request data
from the sensor thread, if necessary.
set_sensor_id
Set the sensor_id for this sensor. Must be thread safe. Without a
sensor id, events cannot be sent.
to_dict
Return a dictionary containing all fields that need persisted and the
special fields 'module' and 'class', which tell the client how to
instantiate the sensor. The dictionary will be passed as the second
argmuent to __init__ when deserializing a client. Must be thread safe.
"""
def __init__(self, send_event_fn=None, config=None):
if not config:
config = {}
if send_event_fn:
self._send_event = send_event_fn
self.phidget_attached = False
self.sensor_data_lock = threading.Lock()
# Set of fields that have changed since the last update.
self.delta_fields = set()
self.sensor_data = client_messages_pb2.SensorMetadata()
self.sensor_data.sensor_type = common_pb2.ACCELEROMETER_3_AXIS
self.sensor_data.calibrated = True
self.sensor_data.model = 'Phidgets 1056'
if 'serial' in config:
self.sensor_data.serial = config.get('serial')
else:
self.sensor_data.serial = ''
if 'datastore' in config:
self.datastore = config.get('datastore')
else:
self.datastore = '/tmp/phidgetsdata'
if 'time_server' in config:
self.time_server = config.get('time_server')
else:
self.time_server = None
self.datastore_uploaded = self.datastore + '/uploaded'
self.datastore_corrupted = self.datastore + '/corrupted'
logging.info('Sensor datastore directory is %s',self.datastore)
self.sensor_data.units = 'g'
self.sensor_data.num_samples = 50
self.sensor_data.sample_window_size = 1
self.accelerometer = None
self.time_start = None
self.reported_clock_drift = datetime.timedelta(seconds=0)
self.file_store_interval = datetime.timedelta(seconds=600)
self.last_phidgets_timestamp = None
self.last_datastore_filename = None
self.last_datastore_filename_uploaded = None
# a lock for updating the timing variables
self.timing_lock = threading.Lock()
self.timing_gradient = 0.0
self.timing_intercept = 0.0
self.timing_base_time = datetime.datetime.utcnow()
self.recent_picks = []
# Make sure data directory exists
try:
os.makedirs(self.datastore)
except OSError as exception:
logging.info('Notification for directories %s', exception)
try:
os.makedirs(self.datastore_uploaded)
except OSError as exception:
logging.info('Notification for directories %s', exception)
try:
os.makedirs(self.datastore_corrupted)
except OSError as exception:
logging.info('Notification for directories %s', exception)
# start the Picker thread
self.sensor_readings_queue = Queue.Queue()
thread = threading.Thread(target=self.Picker, args=['algorithm'])
thread.setDaemon(True)
thread.start()
self.sensor_readings_queue.join()
logging.info('Client initialised: now starting Phidget')
self.data_buffer = []
# Start the Phidget
self.StartPhidget()
self.sensor_data.serial = str(self.accelerometer.getSerialNum())
self.DisplayDeviceInfo()
def StartPhidget(self):
# Initialise the Phidgets sensor
self.phidget_attached = False
self.time_start = None
try:
if self.accelerometer:
self.accelerometer.closePhidget()
self.accelerometer = Spatial()
self.accelerometer.setOnAttachHandler(self.AccelerometerAttached)
self.accelerometer.setOnDetachHandler(self.AccelerometerDetached)
self.accelerometer.setOnErrorhandler(self.AccelerometerError)
self.accelerometer.setOnSpatialDataHandler(self.SpatialData)
self.accelerometer.openPhidget()
self.accelerometer.waitForAttach(10000)
# set data rate in milliseconds (we will decimate from 4ms to 20ms later)
self.accelerometer.setDataRate(PHIDGETS_NOMINAL_DATA_INTERVAL_MS)
except RuntimeError as e:
logging.error("Runtime Exception: %s", e.details)
return
except PhidgetException as e:
logging.error("Phidget Exception: %s. Is the Phidget not connected?", e.details)
return
self.phidget_attached = True
def Picker(self, args):
logging.info('Initialise picker with args %s', args)
# LTA = 10 seconds
# Gap = 1 seconds
# STA = 0.5 seconds
delta = PHIDGETS_NOMINAL_DATA_INTERVAL_MS * PHIDGETS_DECIMATION * 0.001
LTA_count = int(LTA / delta)
STA_count = int(STA / delta)
Gap_count = int(Gap / delta)
ksigma_count = LTA_count + Gap_count + STA_count
horizontal_accelerations = []
vertical_accelerations = []
last_horizontal_pick_timestamp = last_vertical_pick_timestamp = self.GetNtpCorrectedTimestamp()
in_pick_horizontal = False
in_pick_vertical = False
count_readings = 0
timestamp_reading = datetime.datetime.utcnow()
while True:
(timestamp, accelerations) = self.sensor_readings_queue.get()
count_readings += 1
"""
if count_readings == 5*PHIDGETS_DECIMATION:
timestamp_now = datetime.datetime.utcnow()
logging.info("Picker: %s readings in %s seconds", count_readings, (timestamp_now-timestamp_reading).total_seconds())
timestamp_reading = timestamp_now
count_readings = 0
"""
horizontal = math.sqrt(accelerations[0]*accelerations[0] + accelerations[1]*accelerations[1])
north = accelerations[0]
east = accelerations[1]
vertical = accelerations[2]
horizontal_accelerations.append(horizontal)
vertical_accelerations.append(vertical)
number_in_list = len(horizontal_accelerations)
if number_in_list < ksigma_count:
continue
lta_horizontal_std = numpy.std(horizontal_accelerations[:LTA_count])
lta_vertical_std = numpy.std(vertical_accelerations[:LTA_count])
lta_horizontal_lta = numpy.mean(horizontal_accelerations[:LTA_count])
lta_vertical_lta = numpy.mean(vertical_accelerations[:LTA_count])
#sta_horizontal_std = numpy.std(horizontal_accelerations[LTA_count+Gap_count:])
#sta_vertical_std = numpy.std(vertical_accelerations[LTA_count+Gap_count:])
sta_horizontal_mean = sum([abs(a-lta_horizontal_lta) for a in horizontal_accelerations[LTA_count+Gap_count:]]) / STA_count
ksigma_horizontal = sta_horizontal_mean / lta_horizontal_std
sta_vertical_mean = sum([abs(a-lta_vertical_lta) for a in vertical_accelerations[LTA_count+Gap_count:]]) / STA_count
ksigma_vertical = sta_vertical_mean / lta_vertical_std
if not in_pick_horizontal and timestamp-last_horizontal_pick_timestamp > MINIMUM_REPICK_INTERVAL_SECONDS:
if ksigma_horizontal > KSIGMA_THRESHOLD:
in_pick_horizontal = True
maximum_acceleration_north = abs(north)
maximum_acceleration_east = abs(east)
last_horizontal_pick_timestamp = timestamp
ksigma_horizontal_at_pick = ksigma_horizontal
logging.info('Horizontal pick at %s ksigma=%s',timestamp,ksigma_horizontal_at_pick)
if not in_pick_vertical and timestamp-last_vertical_pick_timestamp > MINIMUM_REPICK_INTERVAL_SECONDS:
if ksigma_vertical > KSIGMA_THRESHOLD:
in_pick_vertical = True
maximum_acceleration_vertical = abs(vertical)
last_vertical_pick_timestamp = timestamp
ksigma_vertical_at_pick = ksigma_vertical
logging.info('Vertical pick at %s ksigma=%s',timestamp,ksigma_vertical_at_pick)
if in_pick_horizontal:
if ksigma_horizontal < ksigma_horizontal_at_pick:
self.recent_picks.append((last_horizontal_pick_timestamp, [maximum_acceleration_north, maximum_acceleration_east, 0.0]))
while len(self.recent_picks) > RECENT_PICKS_COUNT:
self.recent_picks = self.recent_picks[1:]
self.send_event(last_horizontal_pick_timestamp, [maximum_acceleration_north, maximum_acceleration_east, 0.0])
in_pick_horizontal = False
else:
if abs(north) > maximum_acceleration_north:
maximum_acceleration_north = abs(north)
if abs(east) > maximum_acceleration_east:
maximum_acceleration_east = abs(east)
if in_pick_vertical:
if ksigma_vertical < ksigma_vertical_at_pick:
self.recent_picks.append((last_vertical_pick_timestamp, [0.0, 0.0, maximum_acceleration_vertical]))
while len(self.recent_picks) > RECENT_PICKS_COUNT:
self.recent_picks = self.recent_picks[1:]
# pick has ended: send the message
self.send_event(last_vertical_pick_timestamp, [0.0, 0.0, maximum_acceleration_vertical])
in_pick_vertical = False
else:
if abs(vertical) > maximum_acceleration_vertical:
maximum_acceleration_vertical = abs(vertical)
vertical_accelerations = vertical_accelerations[1:]
horizontal_accelerations = horizontal_accelerations[1:]
def DisplayDeviceInfo(self):
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (self.accelerometer.isAttached(), self.accelerometer.getDeviceName(), self.accelerometer.getSerialNum(), self.accelerometer.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Axes: %i" % (self.accelerometer.getAccelerationAxisCount()))
print('Max Acceleration Axis 0: {} Min Acceleration Axis 0: {}'.format(self.accelerometer.getAccelerationMax(0), self.accelerometer.getAccelerationMin(0)))
print('Max Acceleration Axis 1: {} Min Acceleration Axis 1: {}'.format(self.accelerometer.getAccelerationMax(1), self.accelerometer.getAccelerationMin(1)))
print('Max Acceleration Axis 2: {} Min Acceleration Axis 2: {}'.format(self.accelerometer.getAccelerationMax(2), self.accelerometer.getAccelerationMin(2)))
def setFileStoreInterval(self, file_store_interval):
# sets the interval for writing the data to a new file
self.file_store_interval = file_store_interval
#Event Handler Callback Functions
def AccelerometerAttached(self, e):
attached = e.device
logging.info("Accelerometer %s Attached!", attached.getSerialNum())
def AccelerometerDetached(self, e):
detached = e.device
self.phidget_attached = False
logging.warn('Accelerometer %s Detached!',(detached.getSerialNum()))
def AccelerometerError(self, e):
try:
source = e.device
print("Accelerometer %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def AccelerometerAccelerationChanged(self, e):
source = e.device
print("Accelerometer %i: Axis %i: %6f" % (source.getSerialNum(), e.index, e.acceleration))
def MakeFilename(self, timestamp):
return self.datastore + '/' + str(self.accelerometer.getSerialNum()) + '_' + timestamp.strftime(FILESTORE_NAMING) + '.dat'
def setTimingFitVariables(self, base_time, gradient, intercept):
# this is called by the main client which is monitoring the system clock compared with NTP
with self.timing_lock:
self.timing_base_time = base_time
self.timing_gradient = gradient
self.timing_intercept = intercept
def GetNtpCorrectedTimestamp(self):
# from the current system time we use the NTP thread's line fit to estimate the true time
time_now = datetime.datetime.utcnow()
offset_estimate = (time_now-self.timing_base_time).total_seconds() * self.timing_gradient + self.timing_intercept
return time_now + datetime.timedelta(microseconds = offset_estimate*1000000.0)
def SpatialData(self, e):
if not self.phidget_attached:
return
sample_timestamp = self.GetNtpCorrectedTimestamp()
if self.time_start == None:
self.time_start = sample_timestamp
self.last_sample_datetime = self.time_start
self.sample_count = 0
self.accelerations = []
if self.datastore:
self.datastore_filename = self.MakeFilename(sample_timestamp)
logging.info('Storing samples to %s',self.datastore_filename)
self.datastore_file = open(self.datastore_filename, 'w')
self.first_sample_timestamp_in_file = None
self.last_phidgets_timestamp = 0.0
for index, spatialData in enumerate(e.spatialData):
phidgets_timestamp = spatialData.Timestamp.seconds + (spatialData.Timestamp.microSeconds * 0.000001)
# the accelerations are scaled and reordered to conform to the SAF standard of [N, E, Z]
accs = [PHIDGETS_ACCELERATION_TO_G*spatialData.Acceleration[1],\
PHIDGETS_ACCELERATION_TO_G*spatialData.Acceleration[0],\
PHIDGETS_ACCELERATION_TO_G*spatialData.Acceleration[2]]
#accs = [a*PHIDGETS_ACCELERATION_TO_G for a in spatialData.Acceleration]
#print spatialData.Acceleration
#print accs
if len(self.accelerations) == 0:
self.accelerations = accs
else:
for i in range(len(self.accelerations)):
self.accelerations[i] += accs[i]
if self.last_phidgets_timestamp:
if phidgets_timestamp - self.last_phidgets_timestamp > 1.5 * PHIDGETS_NOMINAL_DATA_INTERVAL_MS:
logging.warn('Missing samples: last sample %s current sample %s equiv samples %s',\
self.last_phidgets_timestamp, phidgets_timestamp, \
(phidgets_timestamp-self.last_phidgets_timestamp)/PHIDGETS_NOMINAL_DATA_INTERVAL_MS)
self.last_phidgets_timestamp = phidgets_timestamp
self.last_sample_datetime = sample_timestamp
self.sample_count += 1
if self.sample_count == PHIDGETS_DECIMATION:
# we average the last PHIDGETS_DECIMATION samples
accelerations = [acc / float(PHIDGETS_DECIMATION) for acc in self.accelerations]
#invert channel 1 (E-W) - this results in +1g being reported when the sensor is resting on its E side
accelerations[1] = -accelerations[1]
data_entry = [sample_timestamp, accelerations]
# put this reading in the queue for the Picker
self.sensor_readings_queue.put(data_entry)
self.data_buffer.append(data_entry)
if self.datastore_file:
if self.first_sample_timestamp_in_file == None:
self.first_sample_timestamp_in_file = sample_timestamp
self.datastore_file.write(sample_timestamp.strftime(PRECISION_NAMING) + ' ' +
' '.join("%10.7f" % x for x in accelerations)+'\n')
self.sample_count = 0
self.accelerations = []
if sample_timestamp - self.first_sample_timestamp_in_file >= self.file_store_interval:
logging.info('File store interval elapsed with %s samples, rate %s samples/second',len(self.data_buffer),float(len(self.data_buffer))/self.file_store_interval.seconds)
if self.datastore:
# we will change to a new file in the datastore
self.datastore_file.close()
self.last_datastore_filename = self.datastore_filename
self.data_buffer = []
self.time_start = None
def to_dict(self):
# No need to persist anything that doesn't change.
details = {}
details['module'] = 'PhidgetsSensor'
details['class'] = 'Sensor'
with self.sensor_data_lock:
details['sensor_id'] = self.sensor_data.sensor_id
details['serial'] = self.sensor_data.serial
details['datastore'] = self.datastore
if self.last_datastore_filename_uploaded:
details['last_upload'] = self.last_datastore_filename_uploaded
else:
details['last_upload'] = ''
if self.time_server:
details['time_server'] = self.time_server
return details
def get_metadata(self):
data_copy = client_messages_pb2.SensorMetadata()
with self.sensor_data_lock:
data_copy.CopyFrom(self.sensor_data)
return data_copy
def datafiles_not_yet_uploaded(self):
# Returns a list of files that are older than last_datastore_filename (or current time) in the
# data file directory that have not yet been uploaded
# we subtract 10 minutes off the time to avoid finding the currently open file (although we could in
# principle trap that)
file_list = []
last_file_date = self.GetNtpCorrectedTimestamp() - datetime.timedelta(seconds=600)
logging.info('Searching for files older than %s',last_file_date)
for f in os.listdir(self.datastore):
filename = self.datastore + '/' + f
if filename == self.datastore_filename:
logging.info('Will not add currently opened file %s', filename)
continue
if os.path.isfile(filename):
try:
t = os.path.getctime(filename)
file_date = datetime.datetime.fromtimestamp(t)
if file_date < last_file_date:
logging.info('Not yet uploaded: %s',filename)
file_list.append(filename)
except:
logging.error('Error getting file time for %s', filename)
return file_list
def datafile_to_streams(self, filename):
# opens the datafile, reads the accelerometer readings, and adjusts the samples to match
# the required delta time interval.
# Places the adjusted samples in the returned serialized DataRecord object
data = heartbeat_pb2.DataRecord()
data.datapoint_spacing = PHIDGETS_RESAMPLED_DATA_INTERVAL_MS
with open(filename,'r') as f:
lines = f.readlines()
# the time stamp of the first entry in the file
first_entry = lines[0].split()
first_entry_time = datetime.datetime.strptime(first_entry[0], PRECISION_NAMING)
start_time = first_entry_time
# the time stamp of the last entry
last_entry = lines[-1].split()
last_entry_time = datetime.datetime.strptime(last_entry[0], PRECISION_NAMING)
delta_step_seconds = PHIDGETS_RESAMPLED_DATA_INTERVAL_MS * 0.001
# given the first and last entries, we calculate the number of steps of delta interval to span the difference
num_steps = 1 + int(round((last_entry_time - first_entry_time).total_seconds() / delta_step_seconds))
# we initialise our three acceleration component arrays each of length num_steps to "None"
streams_values = [None,None,None]
for i in range(3):
streams_values[i] = [None]*num_steps
# now we read each timestamped line in the file
for line in lines:
# each line entry has a NTP corrected timestamp and (typically) 3 acceleration values
entry = line.split()
entry_time = datetime.datetime.strptime(entry[0], PRECISION_NAMING)
#we calculate the location (step) of this time stamp in the acceleration array
step = int(round((entry_time-first_entry_time).total_seconds() / delta_step_seconds))
if step >= num_steps:
logging.error('Resampling at step %s line %s',step, line)
continue
# and we calculate the absolute time of this location (step)
step_time = first_entry_time + datetime.timedelta(microseconds=delta_step_seconds*step*1000000)
# we calculate the difference between the step's absolute time, and the timestamp in the file
difference = (step_time-entry_time).total_seconds()
# if the difference is zero (i.e. the timestamp is exactly in the right position) we add the accelerations
if difference == 0.0 or step == 0:
for i in range(3):
streams_values[i][step] = float(entry[i+1])
else:
# otherwise, we calculate the acceleration at the step by linear interpolation
# As = d/(d+delta)(A1-A0) + A0
# where As is the acceleration at the step
# d is the step interval (seconds)
# delta is the difference between the step time and the time stamp
# A1 and A0 are the accelerations at the timestamp and at the previous step, respectively
correction = delta_step_seconds / (delta_step_seconds + difference)
for i in range(3):
this_accel = float(entry[i+1])
last_accel = streams_values[i][step-1]
if last_accel == None:
# missing data for the previous step: assign the current acceleration
streams_values[i][step-1] = this_accel
last_accel = this_accel
streams_values[i][step] = correction * (this_accel-last_accel) + last_accel
# the end time is defined as the start time plus delta times (the number of steps minus one)
end_time = start_time + datetime.timedelta(microseconds=delta_step_seconds*(num_steps-1)*1000000)
#logging.info('File %s %s Samples created %s Start %s End %s',filename,len(lines),num_steps,start_time,end_time)
# there may be positions in the acceleration arrays that are not yet filled in (i.e. == None)
# if so we assign them to the most recent filled in accelerations
# add the data streams to the message
for stream_values in streams_values:
for j in range(1,len(stream_values)):
if stream_values[j] == None:
stream_values[j] = stream_values[j-1]
data_stream = data.streams.add()
data_stream.values.extend(stream_values)
data = data.SerializeToString()
return (start_time, end_time, num_steps, data)
def get_filename_data(self, filename):
# prepares the data in filename into an offer protocol buffer
# ensure that this is not the file currently being written
if filename == self.datastore_filename:
return None, None
offer_pb = heartbeat_pb2.SensorReadingDescription()
offer_pb.data_format = heartbeat_pb2.SensorReadingDescription.DATA_RECORD
offer_pb.current_data = True
offer_pb.sensor_id = self.sensor_data.sensor_id
try:
(start_time, end_time, num_samples, data) = self.datafile_to_streams(filename)
except Exception,e:
logging.error('Error %s converting datafile %s to streams', str(e), filename)
# move the file to a directory for corrupt files
self.mark_file_corrupted(filename)
return None, None
logging.info('Data file %s %s elapsed seconds, nsamples %s',filename, (end_time-start_time).total_seconds(), num_samples)
offer_pb.start_date = util.date_format(start_time)
offer_pb.end_date = util.date_format(end_time)
return offer_pb, data
def startAccel(trialId, graph):
global accelFilename
global outPutTxt_a
global orientation
global fA
global totalTimeElapsed
global totalSamplesTaken_a
global totalAbsZAccel
global totalAVec
global totalAvgAVec
totalAvgAVec = 0;
totalAVec = 0;
totalAbsZAccel = 0;
totalSamplesTaken_a = 0;
totalTimeElapsed = 0;
# Setutp graphing ability
outPutTxt_a = graph
orientation = True # True corresponds to "right-side up"
if graph:
accelFilename = str(trialId) + "_accelOutput.txt"
fA = open(accelFilename, 'w') #WRITES OVER PREVIOUS DATA
header = "#Trial: " + str(trialId) + " (x[g], y[g], z[g], 3D A_Vec, samples) \n"
fA.write(header)
#Create an accelerometer object
try:
global spatial
spatial = Spatial()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
spatial.setOnAttachHandler(SpatialAttached)
spatial.setOnDetachHandler(SpatialDetached)
spatial.setOnErrorhandler(SpatialError)
spatial.setOnSpatialDataHandler(SpatialData)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
spatial.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
spatial.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
spatial.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
spatial.setDataRate(24) ############### Sample rate is 24ms averaged
class Controller(object):
"""
"""
# stores acceleration gyroscope, magnetic, time.
imu_measurements = [[], [], [], 0]
_AXES2TUPLE = {
'sxyz': (0, 0, 0, 0),
'sxyx': (0, 0, 1, 0),
'sxzy': (0, 1, 0, 0),
'sxzx': (0, 1, 1, 0),
'syzx': (1, 0, 0, 0),
'syzy': (1, 0, 1, 0),
'syxz': (1, 1, 0, 0),
'syxy': (1, 1, 1, 0),
'szxy': (2, 0, 0, 0),
'szxz': (2, 0, 1, 0),
'szyx': (2, 1, 0, 0),
'szyz': (2, 1, 1, 0),
'rzyx': (0, 0, 0, 1),
'rxyx': (0, 0, 1, 1),
'ryzx': (0, 1, 0, 1),
'rxzx': (0, 1, 1, 1),
'rxzy': (1, 0, 0, 1),
'ryzy': (1, 0, 1, 1),
'rzxy': (1, 1, 0, 1),
'ryxy': (1, 1, 1, 1),
'ryxz': (2, 0, 0, 1),
'rzxz': (2, 0, 1, 1),
'rxyz': (2, 1, 0, 1),
'rzyz': (2, 1, 1, 1)
}
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
_NEXT_AXIS = [1, 2, 0, 1]
_EPS = np.finfo(float).eps * 4.0
class IMU_Handlers(object):
"""
"""
def on_data(self, e):
"""
"""
source = e.device
for index, spatialData in enumerate(e.spatialData):
if (len(spatialData.Acceleration) == 3) and (len(spatialData.AngularRate) == 3) \
and (len(spatialData.MagneticField) == 3):
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]
]
Controller.imu_measurements[0] = gyr
Controller.imu_measurements[1] = acc
Controller.imu_measurements[2] = mag
Controller.imu_measurements[
3] = spatialData.Timestamp.seconds
break
def on_attach(self, e):
"""
"""
return
def on_detach(self, e):
"""
"""
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)))
def __init__(self):
"""
"""
# update from the IMU.
self.spatial = Spatial()
imu_handlers = Controller.IMU_Handlers()
self.prev = 0
# attach the event handlers.
try:
self.spatial.setOnAttachHandler(imu_handlers.on_attach)
self.spatial.setOnDetachHandler(imu_handlers.on_detach)
self.spatial.setOnErrorhandler(imu_handlers.on_error)
self.spatial.setOnSpatialDataHandler(imu_handlers.on_data)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(4)
except:
print("Error connecting to IMU, I cannot handle this. " + \
"I will just go die now!")
exit(1)
#t = Thread(target=self.update_head)
#t.daemon = True
#t.start()
self.x = 0
self.y = 0
self.z = 0
data = copy.copy(Controller.imu_measurements)
while (len(data[2]) != 3):
data = copy.copy(Controller.imu_measurements)
print("here")
mag_v = np.array(data[2])
norm = np.linalg.norm(mag_v)
mag_v = mag_v / norm
x = mag_v[0]
y = mag_v[1]
z = mag_v[2]
# intial reference frame
self.state = [x, y, z]
self.update_head()
def update_head(self):
"""
"""
while True:
data = copy.copy(Controller.imu_measurements)
if (len(data[0]) + len(data[1]) + len(data[2]) == 9):
#self.ahrs.update(
# data[0][0], data[0][1], data[0][2],
# data[1][0], data[1][1], data[1][2],
# data[2][0], data[2][1], data[2][2]
#)
mag_v = np.array(data[2])
norm = np.linalg.norm(mag_v)
mag_v = mag_v / norm
x = mag_v[0]
y = mag_v[1]
z = mag_v[2]
if self.prev != data[3]:
print("")
print([x, y, z])
print(self.state)
print(math.acos(np.dot(self.state, [x, y, z])))
print(self.state[0] - x, self.state[1] - y,
self.state[2] - z)
#print (self.ahrs.quatern2rotMat())
#print (data)
print("")
#print (data[3])
self.x = x
self.y = y
self.z = z
self.prev = data[3]
import serial
import time
import math
#Basic imports
from ctypes import *
import sys
#Phidget specific imports
from Phidgets.Phidget import Phidget
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import SpatialDataEventArgs, AttachEventArgs, DetachEventArgs, ErrorEventArgs
from Phidgets.Devices.Spatial import Spatial, SpatialEventData, TimeSpan
sp = Spatial()
print("Opening phidget object....")
try:
sp.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
sp.waitForAttach(6000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
class PhidgetsSensor(object):
def __init__(self, config=None):
if not config:
config = {}
self.phidget_attached = False
self.collect_samples = True
self.picker = True
self.sensor_data_lock = threading.Lock()
self.delta_fields = set()
self.sensor_data = {}
self.sensor_data['Type'] = 'ACCELEROMETER_3_AXIS'
self.sensor_data['Calibrated'] = True
self.sensor_data['Model'] = 'Phidgets 1056'
self.decimation = PHIDGETS_DECIMATION
if 'software_version' in config:
self.software_version = config.get('software_version')
else:
self.software_version = 'PyCSN Unknown'
if 'decimation' in config:
self.decimation = config.get('decimation')
if 'picker' in config:
self.picker = config.get('picker')
if 'pick_threshold' in config:
self.pick_threshold = config.get('pick_threshold')
else:
self.pick_threshold = PICKER_THRESHOLD
if 'serial' in config:
self.sensor_data['Serial'] = config.get('serial')
else:
self.sensor_data['Serial'] = ''
if 'datastore' in config:
self.datastore = config.get('datastore')
else:
self.datastore = '/var/tmp/phidgetsdata'
#if 'pick_queue' in config:
# self.pick_queue = config.get('pick_queue')
#else:
# self.pick_queue = None
if 'latitude' in config:
self.latitude = config.get('latitude')
else:
self.latitude = 0.0
if 'longitude' in config:
self.longitude = config.get('longitude')
else:
self.longitude = 0.0
if 'floor' in config:
self.floor = config.get('floor')
else:
self.floor = '1'
if 'client_id' in config:
self.client_id = config.get('client_id')
else:
self.client_id = 'Unknown'
if 'stomp' in config:
self.stomp = config.get('stomp')
else:
self.stomp = None
if 'stomp_topic' in config:
self.stomp_topic = config.get('stomp_topic')
else:
self.stomp_topic = None
if 'connect_stomp' in config:
self.connect_stomp = config.get('connect_stomp')
else:
self.connect_stomp = None
self.datastore_uploaded = self.datastore + '/uploaded'
self.datastore_corrupted = self.datastore + '/corrupted'
logging.info('Sensor datastore directory is %s',self.datastore)
# Make sure data directory exists
try:
os.makedirs(self.datastore)
except OSError as exception:
if exception.errno != errno.EEXIST:
logging.error('Error making directory %s', self.datastore)
raise exception
try:
os.makedirs(self.datastore_uploaded)
except OSError as exception:
if exception.errno != errno.EEXIST:
logging.error('Error making directory %s', self.datastore_uploaded)
raise exception
try:
os.makedirs(self.datastore_corrupted)
except OSError as exception:
if exception.errno != errno.EEXIST:
logging.error('Error making directory %s', self.datastore_corrupted)
raise exception
self.sensor_data['Units'] = 'g'
self.sensor_data['Num_samples'] = 50
self.sensor_data['Sample_window_size'] = 1
self.accelerometer = None
self.time_start = None
self.reported_clock_drift = 0.0
self.file_store_interval = 600.0
self.last_phidgets_timestamp = None
self.last_datastore_filename = None
self.last_datastore_filename_uploaded = None
self.writing_errors = 0
self.last_sample_seconds = time.time()
# a lock for updating the timing variables
self.timing_lock = threading.Lock()
self.timing_gradient = 0.0
self.timing_intercept = 0.0
self.timing_base_time = time.time()
self.recent_picks = []
# Everything looks OK, start the collect samples and picking threads and the Phidget
"""
self.sensor_raw_data_queue = Queue.Queue()
raw_data_thread = threading.Thread(target=self.ProcessSensorReading, args=['raw'])
raw_data_thread.setDaemon(True)
raw_data_thread.start()
self.sensor_raw_data_queue.join()
logging.info('Raw Data Thread started')
# start the Picker thread, if this is required
if self.picker:
self.sensor_readings_queue = Queue.Queue()
thread = threading.Thread(target=self.Picker, args=['Simple Threshold'])
thread.setDaemon(True)
thread.start()
logging.info('Picker started')
self.sensor_readings_queue.join()
logging.info('Sensor readings thread started')
else:
logging.info('This Phidget will not calculate or send picks')
"""
self.sensor_raw_data_queue = Queue.Queue()
raw_data_thread = threading.Thread(target=self.Picker, args=['Simple Threshold'])
raw_data_thread.setDaemon(True)
raw_data_thread.start()
logging.info('Picker started')
self.sensor_raw_data_queue.join()
logging.info('Client initialised: now starting Phidget')
self.data_buffer = []
# Start the Phidget
self.StartPhidget()
if not self.phidget_attached:
raise
self.sensor_data['Serial'] = str(self.accelerometer.getSerialNum())
self.DisplayDeviceInfo()
def StartPhidget(self):
# Initialise the Phidgets sensor
self.phidget_attached = False
self.time_start = None
try:
if self.accelerometer:
self.accelerometer.closePhidget()
self.accelerometer = Spatial()
self.accelerometer.setOnAttachHandler(self.AccelerometerAttached)
self.accelerometer.setOnDetachHandler(self.AccelerometerDetached)
self.accelerometer.setOnErrorhandler(self.AccelerometerError)
self.accelerometer.setOnSpatialDataHandler(self.SpatialData)
#self.accelerometer.enableLogging(Phidget.PhidgetLogLevel.PHIDGET_LOG_WARNING, None)
#self.accelerometer.enableLogging(Phidget.PhidgetLogLevel.PHIDGET_LOG_VERBOSE, None)
self.accelerometer.openPhidget()
self.accelerometer.waitForAttach(10000)
# set data rate in milliseconds (we will decimate from 4ms to 20ms later)
# we now do this in the Attach handler
#self.accelerometer.setDataRate(PHIDGETS_NOMINAL_DATA_INTERVAL_MS)
except RuntimeError as e:
logging.error("Runtime Exception: %s", e.details)
return
except PhidgetException as e:
logging.error("Phidget Exception: %s. Is the Phidget not connected?", e.details)
return
self.phidget_attached = True
def Picker(self, args):
logging.info('Initialise picker %s', args)
delta = PHIDGETS_NOMINAL_DATA_INTERVAL_MS * self.decimation * 0.001
LTA_count = int(LTA / delta)
def pick_orientation(scaled, timestamps, orientation):
""" Sends picks on a single orientation, either 'n', 'e', or 'z'. """
# ---------------------------------------------------------------
# CREATE AGENTS AND STREAMS
# ---------------------------------------------------------------
# 1. DECIMATE SCALED DATA.
# Window of size DECIMATION is decimated to its average.
decimated = Stream('decimated')
map_window(lambda v: sum(v) / float(len(v)), scaled, decimated,
window_size=self.decimation, step_size=self.decimation)
# 2. DECIMATE TIMESTAMPS.
# Window of size DECIMATION is decimated to its last value.
decimated_timestamps = Stream('decimated_timestamps')
map_window(lambda window: window[-1],
timestamps, decimated_timestamps,
window_size=self.decimation, step_size=self.decimation)
# 3. DEMEAN (subtract mean from) DECIMATED STREAM.
# Subtract mean of window from the window's last value.
# Move sliding window forward by 1 step.
demeaned = Stream('demeaned', initial_value=[0.0] * (LTA_count - 1))
map_window(lambda window: window[-1] - sum(window) / float(len(window)),
decimated, demeaned,
window_size=LTA_count, step_size=1)
# 4. MERGE TIMESTAMPS WITH DEMEANED ACCELERATIONS.
# Merges decimated_timestamps and demeaned to get timestamped_data.
timestamped_data = Stream('timestamped_data')
zip_streams(in_streams=[decimated_timestamps, demeaned], out_stream=timestamped_data)
# 5. DETECT PICKS.
# Output a pick if the value part of the time_value (t_v) exceeds threshold.
picks = Stream('picks')
filter_element(lambda t_v: abs(t_v[1]) > self.pick_threshold, timestamped_data, picks)
# 6. QUENCH PICKS.
# An element is a (timestamp, value).
# Start a new quench when timestamp > QUENCH_PERIOD + last_quench.
# Update the last quench when a new quench is initiated.
# Initially the last_quench (i.e. state) is 0.
quenched_picks = Stream('quenched_picks')
# f is the filtering function
def f(timestamped_value, last_quench, QUENCH_PERIOD):
timestamp, value = timestamped_value
new_quench = timestamp > QUENCH_PERIOD + last_quench
last_quench = timestamp if new_quench else last_quench
# return filter condition (new_quench) and next state (last_quench)
return new_quench, last_quench
filter_element(f, picks, quenched_picks, state=0, QUENCH_PERIOD=2)
# 7. SEND QUENCHED PICKS.
self.send_event(quenched_picks)
def picker_agent(nezt):
# nezt is a stream of 'data_entry' in the original code.
# Declare acceleration streams in n, e, z orientations and timestamp stream.
n, e, z, t = (Stream('raw_north'), Stream('raw_east'), Stream('raw_vertical'),
Stream('timestamps'))
# split the nezt stream into its components
split_element(lambda nezt: [nezt[0], nezt[1], nezt[2], nezt[3]],
in_stream=nezt, out_streams=[n, e, z, t])
# Determine picks for each orientation after scaling. Note negative scale for East.
# Parameters of pick_orientation are:
# (0) stream of accelerations in a single orientation, scaled by multiplying by SCALE.
# (1) timestamps
# (2) The orientation 'n', 'e', or 'z'
pick_orientation(f_mul(n, PHIDGETS_ACCELERATION_TO_G), t, 'n')
pick_orientation(f_mul(e, -PHIDGETS_ACCELERATION_TO_G), t, 'e')
pick_orientation(f_mul(z, PHIDGETS_ACCELERATION_TO_G), t, 'z')
def datastore_file_agent(in_streams, out_streams):
""" Agent that manages writing to and creating datastore files
@in_streams - Stream of (n, e, z, t)
@out_streams - Stream of (n, e, z, t) - stream itself is not modified
"""
def write_to_datastore_file(nezt, file_ptr):
""" Writes the input stream to the given file_ptr """
accelerations = nezt[:3]
sample_timestamp = nezt[3]
file_ptr.write("%20.5f" % sample_timestamp + ' ' +
' '.join("%10.7f" % x for x in accelerations) + '\n')
def write_to_file(stream, state):
""" Writes stream to file. Creates a new file after FILE_STORE_INTERVAL passes.
@param stream - input stream of acceleration data.
@param state - tuple of (timestamp, curr_file_pointer, n_writing_error)
"""
timestamp = stream[3]
latest_timestamp, curr_file_ptr, curr_filename, n_sample_cnt, n_writing_error = state
# update the datastore file if FILE_STORE_INTERVAL passed
if timestamp >= latest_timestamp + self.file_store_interval:
logging.info('File %s store interval elapsed with %s samples, rate %s samples/second',
curr_filename, n_sample_cnt, float(n_sample_cnt) / self.file_store_interval)
if curr_file_ptr is not None:
curr_file_ptr.close()
curr_filename = self.MakeFilename(timestamp)
curr_file_ptr = open(curr_filename, 'w')
latest_timestamp = timestamp
n_sample_cnt = 0
try:
write_to_datastore_file(stream, curr_file_ptr)
n_sample_cnt += 1
except:
n_writing_error += 1
if n_writing_error <= 10:
print('Error writing sample to file {} with timestamp {}'.format(curr_filename, timestamp))
return stream, (latest_timestamp, curr_file_ptr, curr_filename, n_sample_cnt, n_writing_error)
sink_element(func=write_to_file, in_stream=in_streams, state=(-self.file_store_interval, None, None, 0, 0))
nezt = Stream('nezt')
picker_agent(nezt)
datastore_file_agent(nezt)
while True:
e, sample_timestamp = self.sensor_raw_data_queue.get()
if self.collect_samples:
if self.time_start is None:
self.time_start = sample_timestamp
self.last_phidgets_timestamp = 0.0
for index, spatialData in enumerate(e.spatialData):
phidgets_timestamp = spatialData.Timestamp.seconds + (spatialData.Timestamp.microSeconds * 0.000001)
stream_data = (spatialData.Acceleration[1], spatialData.Acceleration[0],
spatialData.Acceleration[2], sample_timestamp)
nezt.extend([stream_data])
if self.last_phidgets_timestamp:
sample_increment = int(round( (phidgets_timestamp - self.last_phidgets_timestamp) / PHIDGETS_NOMINAL_DATA_INTERVAL))
if sample_increment > 4*self.decimation:
logging.warn('Missing >3 samples: last sample %s current sample %s missing samples %s',\
self.last_phidgets_timestamp, phidgets_timestamp, sample_increment)
elif sample_increment == 0:
logging.warn('Excess samples: last sample %s current sample %s equiv samples %s',\
self.last_phidgets_timestamp, phidgets_timestamp, sample_increment)
self.last_phidgets_timestamp = phidgets_timestamp
def DisplayDeviceInfo(self):
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (self.accelerometer.isAttached(), self.accelerometer.getDeviceName(), self.accelerometer.getSerialNum(), self.accelerometer.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Axes: %i" % (self.accelerometer.getAccelerationAxisCount()))
print('Max Acceleration Axis 0: {} Min Acceleration Axis 0: {}'.format(self.accelerometer.getAccelerationMax(0), self.accelerometer.getAccelerationMin(0)))
print('Max Acceleration Axis 1: {} Min Acceleration Axis 1: {}'.format(self.accelerometer.getAccelerationMax(1), self.accelerometer.getAccelerationMin(1)))
print('Max Acceleration Axis 2: {} Min Acceleration Axis 2: {}'.format(self.accelerometer.getAccelerationMax(2), self.accelerometer.getAccelerationMin(2)))
def setFileStoreInterval(self, file_store_interval):
# sets the interval for writing the data to a new file
self.file_store_interval = file_store_interval
#Event Handler Callback Functions
def AccelerometerAttached(self, e):
attached = e.device
self.phidget_attached = True
logging.info("Accelerometer %s Attached!", attached.getSerialNum())
# set data rate in milliseconds (we will decimate from 4ms to 20ms later)
self.accelerometer.setDataRate(PHIDGETS_NOMINAL_DATA_INTERVAL_MS)
logging.info("Phidget data rate interval set to %s milliseconds", PHIDGETS_NOMINAL_DATA_INTERVAL_MS)
def AccelerometerDetached(self, e):
detached = e.device
self.phidget_attached = False
logging.error('Accelerometer %s Detached!',(detached.getSerialNum()))
def AccelerometerError(self, e):
try:
source = e.device
logging.error("Accelerometer %s: Phidget Error %s: %s", source.getSerialNum(), e.eCode, e.description)
except PhidgetException as e:
logging.error("Phidget Exception %s: %s", e.code, e.details)
def AccelerometerAccelerationChanged(self, e):
source = e.device
logging.error("Accelerometer %s: Axis %s: %s", source.getSerialNum(), e.index, e.acceleration)
def MakeFilename(self, timestamp):
timestamp_datetime = datetime.datetime.fromtimestamp(timestamp).strftime(FILESTORE_NAMING)
sps = int (1000 / (PHIDGETS_NOMINAL_DATA_INTERVAL_MS * self.decimation))
return self.datastore + '/' + str(sps) + '_' + timestamp_datetime + '.dat'
def setTimingFitVariables(self, base_time, gradient, intercept):
# this is called by the main client which is monitoring the system clock compared with NTP
# we acquire a lock on the timing variables to update them
with self.timing_lock:
self.timing_base_time = base_time
self.timing_gradient = gradient
self.timing_intercept = intercept
def GetNtpCorrectedTimestamp(self):
# from the current system time we use the NTP thread's line fit to estimate the true time
time_now = time.time()
# we ensure that the timing variables are not being updated concurrently by acquiring a lock on them
with self.timing_lock:
offset_estimate = (time_now-self.timing_base_time) * self.timing_gradient + self.timing_intercept
return time_now + offset_estimate
def StopSampleCollection(self):
# This stops more samples being collected, and closes the current samples file
self.collect_samples = False
self.datastore_file.close()
def StartSampleCollection(self):
# This restarts sample collection (into files)
self.collect_samples = True
def SpatialData(self, e):
if not self.phidget_attached:
return
sample_timestamp = self.GetNtpCorrectedTimestamp()
self.sensor_raw_data_queue.put((e, sample_timestamp))
def to_dict(self):
# No need to persist anything that doesn't change.
details = {}
details['module'] = 'PhidgetsSensor'
details['class'] = 'PhidgetsSensor'
with self.sensor_data_lock:
details['sensor_id'] = self.sensor_data
details['serial'] = self.sensor_data['Serial']
details['datastore'] = self.datastore
details['decimation'] = self.decimation
details['picker'] = self.picker
details['pick_threshold'] = self.pick_threshold
if self.last_datastore_filename_uploaded:
details['last_upload'] = self.last_datastore_filename_uploaded
else:
details['last_upload'] = ''
return details
def get_metadata(self):
logging.error('PhidgetSensor get_metadata Not implemented')
def datafiles_not_yet_uploaded(self):
# Returns a list of files that are older than last_datastore_filename (or current time) in the
# data file directory that have not yet been uploaded
# we subtract 10 minutes off the time to avoid finding the currently open file (although we could in
# principle trap that)
file_list = []
last_file_date = self.GetNtpCorrectedTimestamp() - 600.0
logging.info('Searching for files older than %s',last_file_date)
for f in os.listdir(self.datastore):
filename = self.datastore + '/' + f
if filename == self.datastore_filename:
logging.info('Will not add currently opened file %s', filename)
continue
if os.path.isfile(filename):
try:
#t = os.path.getctime(filename)
file_date = os.path.getctime(filename)
if file_date < last_file_date:
if len(file_list) < 20:
logging.info('Not yet uploaded: %s',filename)
elif len(file_list) == 20:
logging.info('Not yet uploaded: %s (will not show more)', filename)
file_list.append(filename)
except:
logging.error('Error getting file time for %s', filename)
return file_list
def mark_file_uploaded(self, filename):
# when a sensor data file has been successfully uploaded to the server we move it to the uploaded directory
try:
shutil.copy2(filename, self.datastore_uploaded)
self.last_datastore_filename_uploaded = filename
os.remove(filename)
except:
logging.warning('Failed to move %s to %s', filename, self.datastore_uploaded)
def mark_file_corrupted(self, filename):
# when a sensor data file fails to convert to stream we move it to the corrupt directory
try:
shutil.copy2(filename, self.datastore_corrupted)
except:
logging.warning('Failed to move %s to %s', filename, self.datastore_corrupted)
# always remove the corrupt file from the main directory
try:
os.remove(filename)
except:
logging.error('Failed to delete %s', filename)
def set_sensor_id(self, sensor_id):
with self.sensor_data_lock:
self.sensor_data['sensor_id'] = sensor_id
def send_event(self, in_stream):
"""
pick_message = {'timestamp': event_time,
'station': self.client_id,
'latitude': self.latitude,
'longitude': self.longitude,
'demeaned_accelerations': values,
'floor': self.floor,
}
try:
message_body = json.dumps(pick_message)
m = Message()
m.set_body(message_body)
self.pick_queue.write(m)
#logging.info('Sent pick to Amazon SQS: %s', pick_message)
except Exception, e:
logging.error('Failed to send pick message: %s', e)
"""
def send_pick_to_finder(values):
# values: (time stamp, accs)
# Send pick to FinDer
station_name = self.client_id
station_name = station_name[0:1] + station_name[3:]
finder_accs = [acc*G_TO_CMS2 for acc in values[1:]]
channel = 'HNN'
if finder_accs[1] > 0.0: channel = 'HNE'
if finder_accs[2] > 0.0: channel = 'HNZ'
pick_datetime = datetime.datetime.utcfromtimestamp(float(values[0]))
pick_time = pick_datetime.strftime(TIMESTAMP_NAMING)
finder_location = str(self.latitude) + ' ' + str(self.longitude)
timenow_timestamp = self.GetNtpCorrectedTimestamp()
timenow = datetime.datetime.utcfromtimestamp(timenow_timestamp)
activemq_message = '1 ' + timenow.strftime(TIMESTAMP_NAMING)[:-3] + ' '+self.software_version+'\n'
line = '%s CSN.%s.%s.-- %s %s %s %s\n' % \
(finder_location, station_name, channel, pick_time, abs(finder_accs[0]), abs(finder_accs[1]), abs(finder_accs[2]))
activemq_message += line
activemq_message += 'ENDOFDATA\n'
logging.info("ActiveMQ message to send:\n%s", activemq_message[:-1])
try:
self.stomp.put(activemq_message, destination=self.stomp_topic)
except Exception, err:
logging.error('Error sending pick to ActiveMQ %s', err)
logging.info('Trying to reconnect to ActiveMQ broker')
self.stomp = self.connect_stomp()
sink_element(func=send_pick_to_finder, in_stream=in_stream)
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)))
class Controller(object):
"""
"""
# stores acceleration gyroscope, magnetic, time.
imu_measurements = [[], [], [], 0]
class IMU_Handlers(object):
"""
"""
def on_data(self, e):
"""
"""
source = e.device
for index, spatialData in enumerate(e.spatialData):
if (len(spatialData.Acceleration) == 3) and (len(spatialData.AngularRate) == 3) \
and (len(spatialData.MagneticField) == 3):
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]]
Controller.imu_measurements[0] = gyr
Controller.imu_measurements[1] = acc
Controller.imu_measurements[2] = mag
Controller.imu_measurements[3] = spatialData.Timestamp.seconds
break;
def on_attach(self, e):
"""
"""
return
def on_detach(self, e):
"""
"""
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)))
def __init__(self):
"""
"""
# update from the IMU.
self.spatial = Spatial()
imu_handlers = Controller.IMU_Handlers()
self.prev = 0
# attach the event handlers.
try:
self.spatial.setOnAttachHandler(imu_handlers.on_attach)
self.spatial.setOnDetachHandler(imu_handlers.on_detach)
self.spatial.setOnErrorhandler(imu_handlers.on_error)
self.spatial.setOnSpatialDataHandler(imu_handlers.on_data)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(4)
except:
print("Error connecting to IMU, I cannot handle this. " + \
"I will just go die now!")
exit(1)
samplePeriod = 1/256
self.ahrs = MadgwickAHRS(samplePeriod)
#t = Thread(target=self.update_head)
#t.daemon = True
#t.start()
self.x = 0
self.y = 0
self.z = 0
self.update_head()
def update_head(self):
"""
"""
while True:
data = copy.copy(Controller.imu_measurements)
if (len(data[0]) + len(data[1]) + len(data[2]) == 9):
self.ahrs.update(
data[0][0], data[0][1], data[0][2],
data[1][0], data[1][1], data[1][2],
data[2][0], data[2][1], data[2][2]
)
x, y, z = self.ahrs.getEulerAngles()
self.x = x
self.y = y
self.z = z
if self.prev != data[3]:
print ("")
print(self.x , self.y , self.z)
#print (self.ahrs.quatern2rotMat())
print (data)
print ("")
#print (data[3])
self.prev = data[3]
#!/usr/bin/env python
import serial
import time
import math
#Basic imports
from ctypes import *
import sys
#Phidget specific imports
from Phidgets.Phidget import Phidget
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import SpatialDataEventArgs, AttachEventArgs, DetachEventArgs, ErrorEventArgs
from Phidgets.Devices.Spatial import Spatial, SpatialEventData, TimeSpan
sp = Spatial()
print("Opening phidget object....")
try:
sp.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
sp.waitForAttach(6000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
__author__ = 'Adam Stelmack'
__version__ = '2.1.8'
__date__ = 'May 17 2010'
#Basic imports
from ctypes import *
import sys
#Phidget specific imports
from Phidgets.Phidget import Phidget
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import SpatialDataEventArgs, AttachEventArgs, DetachEventArgs, ErrorEventArgs
from Phidgets.Devices.Spatial import Spatial, SpatialEventData, TimeSpan
#Create an accelerometer object
try:
spatial = Spatial()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Information Display Function
def DisplayDeviceInfo():
print(
"|------------|----------------------------------|--------------|------------|"
)
print(
"|- Attached -|- Type -|- Serial No. -|- Version -|"
)
print(
__date__ = 'May 17 2010'
#Basic imports
from ctypes import *
import sys
#Phidget specific imports
from Phidgets.Phidget import Phidget
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import SpatialDataEventArgs, AttachEventArgs, DetachEventArgs, ErrorEventArgs
from Phidgets.Devices.Spatial import Spatial, SpatialEventData, TimeSpan
from Phidgets.Phidget import PhidgetLogLevel
#Create an accelerometer object
try:
spatial = Spatial()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Information Display Function
def DisplayDeviceInfo():
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (spatial.isAttached(), spatial.getDeviceName(), spatial.getSerialNum(), spatial.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Acceleration Axes: %i" % (spatial.getAccelerationAxisCount()))
print("Number of Gyro Axes: %i" % (spatial.getGyroAxisCount()))
print("Number of Compass Axes: %i" % (spatial.getCompassAxisCount()))
class Controller(object):
"""
"""
# stores acceleration gyroscope, magnetic, time.
imu_measurements = [[], [], [], 0]
_AXES2TUPLE = {
"sxyz": (0, 0, 0, 0),
"sxyx": (0, 0, 1, 0),
"sxzy": (0, 1, 0, 0),
"sxzx": (0, 1, 1, 0),
"syzx": (1, 0, 0, 0),
"syzy": (1, 0, 1, 0),
"syxz": (1, 1, 0, 0),
"syxy": (1, 1, 1, 0),
"szxy": (2, 0, 0, 0),
"szxz": (2, 0, 1, 0),
"szyx": (2, 1, 0, 0),
"szyz": (2, 1, 1, 0),
"rzyx": (0, 0, 0, 1),
"rxyx": (0, 0, 1, 1),
"ryzx": (0, 1, 0, 1),
"rxzx": (0, 1, 1, 1),
"rxzy": (1, 0, 0, 1),
"ryzy": (1, 0, 1, 1),
"rzxy": (1, 1, 0, 1),
"ryxy": (1, 1, 1, 1),
"ryxz": (2, 0, 0, 1),
"rzxz": (2, 0, 1, 1),
"rxyz": (2, 1, 0, 1),
"rzyz": (2, 1, 1, 1),
}
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
_NEXT_AXIS = [1, 2, 0, 1]
_EPS = np.finfo(float).eps * 4.0
class IMU_Handlers(object):
"""
"""
def on_data(self, e):
"""
"""
source = e.device
for index, spatialData in enumerate(e.spatialData):
if (
(len(spatialData.Acceleration) == 3)
and (len(spatialData.AngularRate) == 3)
and (len(spatialData.MagneticField) == 3)
):
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]]
Controller.imu_measurements[0] = gyr
Controller.imu_measurements[1] = acc
Controller.imu_measurements[2] = mag
Controller.imu_measurements[3] = spatialData.Timestamp.seconds
break
def on_attach(self, e):
"""
"""
return
def on_detach(self, e):
"""
"""
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)))
def __init__(self):
"""
"""
# update from the IMU.
self.spatial = Spatial()
imu_handlers = Controller.IMU_Handlers()
self.prev = 0
# attach the event handlers.
try:
self.spatial.setOnAttachHandler(imu_handlers.on_attach)
self.spatial.setOnDetachHandler(imu_handlers.on_detach)
self.spatial.setOnErrorhandler(imu_handlers.on_error)
self.spatial.setOnSpatialDataHandler(imu_handlers.on_data)
self.spatial.openPhidget()
self.spatial.waitForAttach(10000)
self.spatial.setDataRate(4)
except:
print("Error connecting to IMU, I cannot handle this. " + "I will just go die now!")
exit(1)
# t = Thread(target=self.update_head)
# t.daemon = True
# t.start()
self.x = 0
self.y = 0
self.z = 0
data = copy.copy(Controller.imu_measurements)
while len(data[2]) != 3:
data = copy.copy(Controller.imu_measurements)
print("here")
mag_v = np.array(data[2])
norm = np.linalg.norm(mag_v)
mag_v = mag_v / norm
x = mag_v[0]
y = mag_v[1]
z = mag_v[2]
# intial reference frame
self.state = [x, y, z]
self.update_head()
def update_head(self):
"""
"""
while True:
data = copy.copy(Controller.imu_measurements)
if len(data[0]) + len(data[1]) + len(data[2]) == 9:
# self.ahrs.update(
# data[0][0], data[0][1], data[0][2],
# data[1][0], data[1][1], data[1][2],
# data[2][0], data[2][1], data[2][2]
# )
mag_v = np.array(data[2])
norm = np.linalg.norm(mag_v)
mag_v = mag_v / norm
x = mag_v[0]
y = mag_v[1]
z = mag_v[2]
if self.prev != data[3]:
print("")
print([x, y, z])
print(self.state)
print(math.acos(np.dot(self.state, [x, y, z])))
print(self.state[0] - x, self.state[1] - y, self.state[2] - z)
# print (self.ahrs.quatern2rotMat())
# print (data)
print("")
# print (data[3])
self.x = x
self.y = y
self.z = z
self.prev = data[3]
__date__ = 'May 17 2010'
#Basic imports
from ctypes import *
import sys
import math
#from ROOT import TGraph, TCanvas
#Phidget specific imports
from Phidgets.Phidget import Phidget
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import SpatialDataEventArgs, AttachEventArgs, DetachEventArgs, ErrorEventArgs
from Phidgets.Devices.Spatial import Spatial, SpatialEventData, TimeSpan
#Create an accelerometer object
try:
spatial = Spatial()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
sys.exit(1)
#Information Display Function
def DisplayDeviceInfo():
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (spatial.isAttached(), spatial.getDeviceName(), spatial.getSerialNum(), spatial.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Acceleration Axes: %i" % (spatial.getAccelerationAxisCount()))
print("Number of Gyro Axes: %i" % (spatial.getGyroAxisCount()))
print("Number of Compass Axes: %i" % (spatial.getCompassAxisCount()))
