def start_accelerometer(self):
"""
Creates Phidget22 accelerometer and polls it at a fixed rate using a QTimer.
"""
# set up the target image
self.setup_target()
# create the accelerometer, and wait 1 sec for connection
# TODO does the open wait for attachment throw an error
try:
self.accelerometer = Accelerometer()
self.accelerometer.openWaitForAttachment(1000)
log.info("Created accelerometer")
except:
log.warning("There is no accelerometer connected")
self.accelerometer = None
return
# create QTimer to periodically monitor the acceleration
self.accelerometer_timer = QtCore.QTimer()
self.accelerometer_timer.setTimerType(QtCore.Qt.PreciseTimer)
self.accelerometer_timer.timeout.connect(self.update_target)
self.accelerometer_timer.setInterval(ACCELEROMETER_PERIOD_MS)
self.accelerometer_timer.start()
log.debug("Timer limiting accelerometer update frequency to %f Hz" %
(1000.0 / ACCELEROMETER_PERIOD_MS))
def main():
try:
#Create your Phidget channels
accelerometer0 = Accelerometer()
gyroscope0 = Gyroscope()
magnetometer0 = Magnetometer()
#Set addressing parameters to specify which channel to open (if any)
#Assign any event handlers you need before calling open so that no events are missed.
accelerometer0.setOnAccelerationChangeHandler(onAccelerationChange)
gyroscope0.setOnAngularRateUpdateHandler(onAngularRateUpdate)
magnetometer0.setOnMagneticFieldChangeHandler(onMagneticFieldChange)
#Open your Phidgets and wait for attachment
accelerometer0.openWaitForAttachment(5000)
gyroscope0.openWaitForAttachment(5000)
magnetometer0.openWaitForAttachment(5000)
#Do stuff with your Phidgets here or in your event handlers.
time.sleep(5)
#Close your Phidgets once the program is done.
accelerometer0.close()
gyroscope0.close()
magnetometer0.close()
except PhidgetException as ex:
#We will catch Phidget Exceptions here, and print the error informaiton.
traceback.print_exc()
print("")
print("PhidgetException " + str(ex.code) + " (" + ex.description +
"): " + ex.details)
def __init__(self):
self.kalmanX = KalmanAngle()
self.kalmanY = KalmanAngle()
self.RestrictPitch = True #Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
self.radToDeg = 57.2957786
self.kalAngleX = 0
self.kalAngleY = 0
try:
self.accel = Accelerometer() #ch.getAcceleration()
self.gyro = Gyroscope() #ch.getAngularRate()
except Exception as e:
print(
"Cannot initalize, try checking you have the Phidget22 library installed."
)
print("Error:", e)
sys.exit()
def imu_talker():
# Create new phidget22 accelerometer object
accelerometer = Accelerometer()
# Check if imu can be found by OS
accelerometer.openWaitForAttachment(1000)
try:
pub = rospy.Publisher('crashed', Bool, queue_size=10)
rospy.init_node('crash_detector', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# Get current acceleration
acceleration = accelerometer.getAcceleration()
# Check if y acceleration has a large negative value in gs
if acceleration[1] < -0.2:
# In this case we have probably crashed
crashed = True
else:
# In this case we probably didn't crash
crashed = False
pub.publish(crashed)
rate.sleep()
except rospy.ROSInterruptException:
accelerometer.close()
def initialize_sensors():
print("\n--- Sensors initializing...")
try:
#For a raspberry pi, for example, to set up pins 4 and 5, you would add
#GPIO.setmode(GPIO.BCM)
#GPIO.setup(4, GPIO.IN)
#GPIO.setup(5, GPIO.IN)
print("--- Waiting 10 seconds for sensors to warm up...")
time.sleep(10)
# Activate the accelerometer
global ch
ch = Accelerometer()
# Assign event handlers
ch.setOnAttachHandler(AccelerometerAttached)
ch.setOnDetachHandler(AccelerometerDetached)
ch.setOnErrorHandler(ErrorEvent)
# Wait for the sensor to be attached
print(
"--- Waiting for the Phidget Accelerometer Object to be attached..."
)
ch.openWaitForAttachment(5000)
print("--- Sensors initialized!")
# Sync the time on this device to an internet time server
try:
print('\n--- Syncing time...')
os.system('sudo service ntpd stop')
time.sleep(1)
os.system('sudo ntpd -gq')
time.sleep(1)
os.system('sudo service ntpd start')
print('--- Success! Time is ' + str(datetime.datetime.now()))
except:
print('Error syncing time!')
except Exception as ex:
# Log any error, if it occurs
print(
str(datetime.datetime.now()) +
" Error when initializing sensors: " + str(ex))
def main():
accelerometer0 = Accelerometer()
accelerometer0.setOnAccelerationChangeHandler(onAccelerationChange)
accelerometer0.openWaitForAttachment(5000)
try:
input("Press Enter to Stop\n")
except (Exception, KeyboardInterrupt):
pass
accelerometer0.close()
def initialize_sensors():
print("\n--- Sensors initializing...")
try:
#For a raspberry pi, for example, to set up pins 4 and 5, you would add
#GPIO.setmode(GPIO.BCM)
#GPIO.setup(4, GPIO.IN)
#GPIO.setup(5, GPIO.IN)
print("--- Waiting 10 seconds for sensors to warm up...")
time.sleep(10)
# Activate the accelerometer
global ch
ch = Accelerometer()
# Assign event handlers
ch.setOnAttachHandler(AccelerometerAttached)
ch.setOnDetachHandler(AccelerometerDetached)
ch.setOnErrorHandler(ErrorEvent)
# Wait for the sensor to be attached
print("--- Waiting for the Phidget Accelerometer Object to be attached...")
ch.openWaitForAttachment(5000)
print("--- Sensors initialized!")
# Sync the time on this device to an internet time server
try:
print('\n--- Syncing time...')
os.system('sudo service ntpd stop')
time.sleep(1)
os.system('sudo ntpd -gq')
time.sleep(1)
os.system('sudo service ntpd start')
print('--- Success! Time is ' + str(datetime.datetime.now()))
except:
print('Error syncing time!')
except Exception as ex:
# Log any error, if it occurs
print(str(datetime.datetime.now()) + " Error when initializing sensors: " + str(ex))
class TiltData:
_all = set()
_logger = None
_accelerometer = Accelerometer()
_waitTimeForConnect = 5000
def __init__(self,
config={},
positionChange=0,
elapsedtime=0.0,
position=0):
TiltData._all.add(self)
self.gestureProcessor = TiltGestureProcessor(self, config)
self.queueLength = config['accelerometerQueueLength']
self.components = [
Queue(self.queueLength),
Queue(self.queueLength),
Queue(self.queueLength)
]
self.variances = [
Queue(self.queueLength),
Queue(self.queueLength),
Queue(self.queueLength)
]
self.magnitude = 0.0
self.zeros = [0.0, 0.0, 0.0]
self.serialNumber = ''
if (TiltData._logger == None):
TiltData._logger = logging.getLogger('tiltsensorserver')
try:
TiltData._accelerometer.setOnAttachHandler(
TiltData._accelerometerAttached)
TiltData._accelerometer.setOnDetachHandler(
TiltData._accelerometerDetached)
TiltData._accelerometer.setOnErrorHandler(
TiltData._accelerometerError)
TiltData._accelerometer.setOnAccelerationChangeHandler(
TiltData._accelerometerAccelerationChanged)
except PhidgetException as e:
_accelerometer = None
d = {'clientip': "tilter", 'user': "******"}
TiltData._logger.critical('Tilter init failed: %s',
e.details,
extra=d)
try:
TiltData._accelerometer.openWaitForAttachment(
TiltData._waitTimeForConnect)
except PhidgetException as e:
d = {'clientip': "tilter", 'user': "******"}
TiltData._logger.critical('Tilter connect failed: %s',
e.details,
extra=d)
TiltData._accelerometer = None
def setZeros(self, x0, y0, z0):
self.zeros = [x0, y0, z0]
def set_accelerometerZero(self, index, newZero):
self.zeros[index] = newZero
def level_table(self):
self.components = [
Queue(self.queueLength),
Queue(self.queueLength),
Queue(self.queueLength)
]
self.variances = [
Queue(self.queueLength),
Queue(self.queueLength),
Queue(self.queueLength)
]
def ingestSpatialData(self, sensorData):
if self.components[0].size() == 0:
self.setZeros(sensorData.Acceleration[0],
sensorData.Acceleration[1],
sensorData.Acceleration[2])
newX = config['flipX'] * (sensorData.Acceleration[0] - self.zeros[0])
newY = config['flipY'] * (sensorData.Acceleration[1] - self.zeros[1])
newZ = sensorData.Acceleration[2] - self.zeros[2]
self.variances[0].enqueue(newX - self.components[0].head())
self.variances[1].enqueue(newY - self.components[1].head())
self.variances[2].enqueue(newZ - self.components[2].head())
self.components[0].enqueue(newX)
self.components[1].enqueue(newY)
self.components[2].enqueue(newZ)
def ingest_accelerometerData(self, sensorData):
for index in range(3):
if self.components[index].size() == 0:
self.set_accelerometerZero(index, sensorData[index])
newX = sensorData[index] - self.zeros[index]
self.variances[index].enqueue(newX - self.components[index].head())
self.components[index].enqueue(newX)
def getJSON(self):
jsonBundle = {
'type': 'tilt',
'packet': {
'sensorID': '',
'tiltX': 0.0,
'tiltY': 0.0
}
}
return (JSON.dumps(jsonBundle))
def _accelerometerAttached(e):
attached = e
for tilter in TiltData._all:
tilter.serialNumber = attached.getDeviceSerialNumber()
d = {
'clientip': "tilter",
'user': "******",
'foo': "accelerometer attached"
}
TiltData._logger.info('accelerometer Attached! %s', 'yay', extra=d)
def _accelerometerDetached(e):
detached = e
d = {
'clientip':
"tilter",
'user':
"******",
'foo':
"accelerometer Detached! %s" % (detached.getDeviceSerialNumber())
}
TiltData._logger.warning('accelerometer Detached! %s',
'connection reset',
extra=d)
def _accelerometerError(e, eCode, description):
d = {'clientip': "tilter", 'user': "******"}
TiltData._logger.error('_accelerometerError %s', description, extra=d)
def _accelerometerAccelerationChanged(e, acceleration, timestamp):
# print("Acceleration: %f %f %f" % (acceleration[0], acceleration[1], acceleration[2]))
# print("Timestamp: %f\n" % timestamp)
#TiltData._logger.info('accelerometer data %s', "checking", extra=d)
for tilter in TiltData._all:
#print(tilter.serialNumber, e.getDeviceSerialNumber(), len(TiltData._all ))
if tilter.serialNumber == e.getDeviceSerialNumber():
#print(repr(tilter), len(TiltData._all ))
if tilter:
tilter.ingest_accelerometerData(acceleration)
class PhidgetKalman(object):
#Read the gyro and acceleromater values from MPU6050
def __init__(self):
self.kalmanX = KalmanAngle()
self.kalmanY = KalmanAngle()
self.RestrictPitch = True #Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
self.radToDeg = 57.2957786
self.kalAngleX = 0
self.kalAngleY = 0
try:
self.accel = Accelerometer() #ch.getAcceleration()
self.gyro = Gyroscope() #ch.getAngularRate()
except Exception as e:
print(
"Cannot initalize, try checking you have the Phidget22 library installed."
)
print("Error:", e)
sys.exit()
# return self.accel, self.gyro
def start(self):
try:
self.accel.openWaitForAttachment(1000)
self.gyro.openWaitForAttachment(1000)
except Exception as e:
print(
"Issue with attaching to IMU. The IMU maybe connected to something else right now.."
)
print("Error:", e)
sys.exit()
def stop(self):
self.accel.close()
self.gyro.close()
print("Stopping")
def get_angles(self, measure_time=5):
#Keep going
time.sleep(1)
accX, accY, accZ = self.accel.getAcceleration()
if (self.RestrictPitch):
roll = math.atan2(accY, accZ) * self.radToDeg
pitch = math.atan(-accX / math.sqrt((accY**2) +
(accZ**2))) * self.radToDeg
else:
roll = math.atan(accY / math.sqrt((accX**2) +
(accZ**2))) * self.radToDeg
pitch = math.atan2(-accX, accZ) * self.radToDeg
# print(roll)
self.kalmanX.setAngle(roll)
self.kalmanX.setAngle(pitch)
gyroXAngle = roll
gyroYAngle = pitch
compAngleX = roll
compAngleY = pitch
timer = time.time()
compare_timer = time.time()
flag = 0
try:
while int(time.time()) < int(measure_time) + int(
compare_timer
): #Should only run for about 5 seconds <- Could make lower
#Read Accelerometer raw value
accX, accY, accZ = self.accel.getAcceleration()
#Read Gyroscope raw value
gyroX, gyroY, gyroZ = self.gyro.getAngularRate()
dt = time.time() - timer
timer = time.time()
if (self.RestrictPitch):
roll = math.atan2(accY, accZ) * self.radToDeg
pitch = math.atan(
-accX / math.sqrt((accY**2) +
(accZ**2))) * self.radToDeg
else:
roll = math.atan(
accY / math.sqrt((accX**2) +
(accZ**2))) * self.radToDeg
pitch = math.atan2(-accX, accZ) * self.radToDeg
gyroXRate = gyroX / 131
gyroYRate = gyroY / 131
if (self.RestrictPitch):
if ((roll < -90 and self.kalAngleX > 90)
or (roll > 90 and self.kalAngleX < -90)):
self.kalmanX.setAngle(roll)
complAngleX = roll
self.kalAngleX = roll
gyroXAngle = roll
else:
self.kalAngleX = self.kalmanX.getAngle(
roll, gyroXRate, dt)
if (abs(self.kalAngleX) > 90):
gyroYRate = -gyroYRate
self.kalAngleY = self.kalmanX.getAngle(
pitch, gyroYRate, dt)
else:
if ((pitch < -90 and self.kalAngleY > 90)
or (pitch > 90 and self.kalAngleY < -90)):
self.kalmanX.setAngle(pitch)
complAngleY = pitch
self.kalAngleY = pitch
gyroYAngle = pitch
else:
self.kalAngleY = self.kalmanX.getAngle(
pitch, gyroYRate, dt)
if (abs(self.kalAngleY) > 90):
gyroXRate = -gyroXRate
self.kalAngleX = self.kalmanX.getAngle(
roll, gyroXRate, dt)
#angle = (rate of change of angle) * change in time
gyroXAngle = gyroXRate * dt
gyroYAngle = gyroYAngle * dt
#compAngle = constant * (old_compAngle + angle_obtained_from_gyro) + constant * angle_obtained from accelerometer
compAngleX = 0.93 * (compAngleX + gyroXRate * dt) + 0.07 * roll
compAngleY = 0.93 * (compAngleY +
gyroYRate * dt) + 0.07 * pitch
if ((gyroXAngle < -180) or (gyroXAngle > 180)):
gyroXAngle = self.kalAngleX
if ((gyroYAngle < -180) or (gyroYAngle > 180)):
gyroYAngle = self.kalAngleY
self.X_angle = self.kalAngleX
self.Y_angle = self.kalAngleY
#print("Angle X: " + str(self.kalAngleX)+" " +"Angle Y: " + str(self.kalAngleY))
#print(str(roll)+" "+str(gyroXAngle)+" "+str(compAngleX)+" "+str(self.kalAngleX)+" "+str(pitch)+" "+str(gyroYAngle)+" "+str(compAngleY)+" "+str(self.kalAngleY))
time.sleep(0.005)
except KeyboardInterrupt:
print("test")
return self.X_angle, self.Y_angle
def plugin_reconfigure(handle, new_config):
""" Reconfigures the plugin
Args:
handle: handle returned by the plugin initialisation call
new_config: JSON object representing the new configuration category for the category
Returns:
new_handle: new handle to be used in the future calls
"""
_LOGGER.info("Old config for wind_turbine plugin {} \n new config {}".format(handle, new_config))
# Shutdown sensors
try:
handle['humidity'].close()
handle['temperature'].close()
handle['current'].close()
handle['encoder'].close()
handle['accelerometer'].close()
handle['gyroscope'].close()
handle['magnetometer'].close()
except Exception as ex:
_LOGGER.exception("wind_turbine exception: {}".format(str(ex)))
raise ex
time.sleep(5)
new_handle = copy.deepcopy(new_config)
try:
# check if temp/humidity sensor is enabled. If so restart it
if new_handle['tempHumEnable']['value'] == 'true':
new_handle['humidity'] = HumiditySensor()
new_handle['humidity'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['humidity'].setHubPort(int(new_handle['tempHumPort']['value']))
new_handle['humidity'].setIsHubPortDevice(False)
new_handle['humidity'].setChannel(0)
new_handle['humidity'].openWaitForAttachment(5000)
try:
new_handle['humidity'].getHumidity()
except Exception:
pass
new_handle['temperature'] = TemperatureSensor()
new_handle['temperature'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['temperature'].setHubPort(int(new_handle['tempHumPort']['value']))
new_handle['temperature'].setIsHubPortDevice(False)
new_handle['temperature'].setChannel(0)
new_handle['temperature'].openWaitForAttachment(5000)
try:
new_handle['temperature'].getTemperature()
except Exception:
pass
# check if current sensor is enabled, if so restart it
if new_handle['currentEnable']['value'] == 'true':
new_handle['current'] = CurrentInput()
new_handle['current'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['current'].setHubPort(int(new_handle['currentPort']['value']))
new_handle['current'].setIsHubPortDevice(False)
new_handle['current'].setChannel(0)
new_handle['current'].openWaitForAttachment(5000)
try:
new_handle['current'].getCurrent()
except Exception:
pass
# check if encoder sensor is enabled
if new_handle['encoderEnable']['value'] == 'true':
new_handle['encoder'] = Encoder()
new_handle['encoder'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['encoder'].setHubPort(int(new_handle['encoderPort']['value']))
new_handle['encoder'].setIsHubPortDevice(False)
new_handle['encoder'].setChannel(0)
new_handle['encoder'].openWaitForAttachment(5000)
new_handle['encoder'].setDataInterval(20)
i = 0
while i < 120:
try:
new_handle['encoder'].getPosition()
except Exception:
time.sleep(1)
i += 1
else:
break
# check if accelerometer is enabled
if new_handle['accelerometerEnable']['value'] == 'true':
new_handle['accelerometer'] = Accelerometer()
new_handle['accelerometer'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['accelerometer'].setHubPort(int(new_handle['spatialPort']['value']))
new_handle['accelerometer'].setIsHubPortDevice(False)
new_handle['accelerometer'].setChannel(0)
new_handle['accelerometer'].openWaitForAttachment(5000)
new_handle['accelerometer'].setDataInterval(20)
i = 0
while i < 120:
try:
new_handle['accelerometer'].getAcceleration()
except Exception:
time.sleep(1)
i += 1
else:
break
# check if gyroscope is enabled
if new_handle['gyroscopeEnable']['value'] == 'true':
new_handle['gyroscope'] = Gyroscope()
new_handle['gyroscope'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['gyroscope'].setHubPort(int(new_handle['spatialPort']['value']))
new_handle['gyroscope'].setIsHubPortDevice(False)
new_handle['gyroscope'].setChannel(0)
new_handle['gyroscope'].openWaitForAttachment(5000)
new_handle['gyroscope'].setDataInterval(20)
i = 0
while i < 120:
try:
new_handle['gyroscope'].getAngularRate()
except Exception:
time.sleep(1)
i += 1
else:
break
# check if magnetometer enable is enabled
if new_handle['magnetometerEnable']['value'] == 'true':
new_handle['magnetometer'] = Magnetometer()
new_handle['magnetometer'].setDeviceSerialNumber(int(new_handle['hubSN']['value']))
new_handle['magnetometer'].setHubPort(int(new_handle['spatialPort']['value']))
new_handle['magnetometer'].setIsHubPortDevice(False)
new_handle['magnetometer'].setChannel(0)
new_handle['magnetometer'].openWaitForAttachment(5000)
new_handle['magnetometer'].setDataInterval(20)
i = 0
while i < 120:
try:
new_handle['magnetometer'].getMagneticField()
except Exception:
time.sleep(1)
i += 1
else:
break
# check if hub has changed, if so init restart
if new_handle['hubSN']['value'] != handle['hubSN']['value']:
new_handle['restart'] = 'yes'
else:
new_handle['restart'] = 'no'
except Exception as ex:
_LOGGER.exception("wind_turbine exception: {}".format(str(ex)))
raise ex
# counter to know when to run process
new_handle['tempHumCount'] = 0
new_handle['currentCount'] = 0
new_handle['encoderCount'] = 0
new_handle['accelerometerCount'] = 0
new_handle['gyroscopeCount'] = 0
new_handle['magnetometerCount'] = 0
# counter of last encoder value
new_handle['encoderPreviousValue'] = handle['encoderPreviousValue']
return new_handle
def plugin_init(config):
""" Initialise the plugin.
Args:
config: JSON configuration document for the South plugin configuration category
Returns:
data: JSON object to be used in future calls to the plugin
Raises:
"""
try:
data = copy.deepcopy(config)
if data['tempHumEnable']['value'] == 'true':
data['humidity'] = HumiditySensor()
data['humidity'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['humidity'].setHubPort(int(data['tempHumPort']['value']))
data['humidity'].setIsHubPortDevice(False)
data['humidity'].setChannel(0)
data['humidity'].openWaitForAttachment(5000)
try:
data['humidity'].getHumidity()
except Exception:
pass
data['temperature'] = TemperatureSensor()
data['temperature'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['temperature'].setHubPort(int(data['tempHumPort']['value']))
data['temperature'].setIsHubPortDevice(False)
data['temperature'].setChannel(0)
data['temperature'].openWaitForAttachment(5000)
try:
data['temperature'].getTemperature()
except Exception:
pass
if data['currentEnable']['value'] == 'true':
data['current'] = CurrentInput()
data['current'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['current'].setHubPort(int(data['currentPort']['value']))
data['current'].setIsHubPortDevice(False)
data['current'].setChannel(0)
data['current'].openWaitForAttachment(5000)
try:
data['current'].getCurrent()
except Exception:
pass
if data['encoderEnable']['value'] == 'true':
data['encoder'] = Encoder()
data['encoder'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['encoder'].setHubPort(int(data['encoderPort']['value']))
data['encoder'].setIsHubPortDevice(False)
data['encoder'].setChannel(0)
data['encoder'].openWaitForAttachment(5000)
data['encoder'].setDataInterval(20)
i = 0
while i < 120:
try:
data['encoder'].getPosition()
except Exception:
time.sleep(1)
i += 1
else:
break
if data['accelerometerEnable']['value'] == 'true':
data['accelerometer'] = Accelerometer()
data['accelerometer'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['accelerometer'].setHubPort(int(data['spatialPort']['value']))
data['accelerometer'].setIsHubPortDevice(False)
data['accelerometer'].setChannel(0)
data['accelerometer'].openWaitForAttachment(5000)
data['accelerometer'].setDataInterval(20)
i = 0
while i < 120:
try:
data['accelerometer'].getAcceleration()
except Exception:
time.sleep(1)
i += 1
else:
break
if data['gyroscopeEnable']['value'] == 'true':
data['gyroscope'] = Gyroscope()
data['gyroscope'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['gyroscope'].setHubPort(int(data['spatialPort']['value']))
data['gyroscope'].setIsHubPortDevice(False)
data['gyroscope'].setChannel(0)
data['gyroscope'].openWaitForAttachment(5000)
data['gyroscope'].setDataInterval(20)
i = 0
while i < 120:
try:
data['gyroscope'].getAngularRate()
except Exception:
time.sleep(1)
i += 1
else:
break
if data['magnetometerEnable']['value'] == 'true':
data['magnetometer'] = Magnetometer()
data['magnetometer'].setDeviceSerialNumber(int(data['hubSN']['value']))
data['magnetometer'].setHubPort(int(data['spatialPort']['value']))
data['magnetometer'].setIsHubPortDevice(False)
data['magnetometer'].setChannel(0)
data['magnetometer'].openWaitForAttachment(5000)
data['magnetometer'].setDataInterval(20)
i = 0
while i < 120:
try:
data['magnetometer'].getMagneticField()
except Exception:
time.sleep(1)
i += 1
else:
break
except Exception as ex:
_LOGGER.exception("wind_turbine exception: {}".format(str(ex)))
raise ex
# counter to know when to run process
data['tempHumCount'] = 0
data['currentCount'] = 0
data['encoderCount'] = 0
data['accelerometerCount'] = 0
data['gyroscopeCount'] = 0
data['magnetometerCount'] = 0
# counter of last encoder value
data['encoderPreviousValue'] = 0
data['encoderPreviousTime'] = 0
return data
class PlotCamLiteWindow_Monitor(QMainWindow):
def __init__(self):
super().__init__()
# init variables
self.depth_cam_proc = None
self.accelerometer = None
self.metadata = None
self.experiment_path = None
self.is_streaming = multiprocessing.Value('i', False)
self.current_plot_number = 0
self.x = 0.0
self.y = 0.0
self.init_ui()
self.title = "PlotCam Lite - %s and PyQt5" % PLATFORM
def init_ui(self):
"""
Initalizes the UI.
Connects methods to the buttons and begins the background processes.
"""
if not pcl_config["monitor"]:
self.setFixedSize(850, 630)
with disable_logging(logging.DEBUG):
loadUi(pcl_config["main_window_ui_path"], self)
# Set Window Title
self.setWindowTitle(PROGRAM_TITLE)
# Set Window Icon
self.setWindowIcon(QIcon(ICON_IMAGE_PATH))
# Add actions to menu bar
self.add_actions()
# Start the camera stream
self.start_stream()
# Start the accelerometer
self.start_accelerometer()
# Connect buttons to methods
self.take_picture_button.clicked.connect(self.take_picture)
self.exit_button.clicked.connect(self.close)
self.new_file_button.clicked.connect(self.new_experiment_dialog)
self.load_experiment_button.clicked.connect(self.browse_files)
# disable the take pic btn until an experiment is set
self.take_picture_button.setEnabled(False)
self.alert = QSound(ALERT_AUDIO_PATH)
def add_actions(self):
"""
Connects the menu bar actions to their respective functions.
"""
# File Menu
self.actionClose.triggered.connect(self.close)
# Edit Menu
self.action640x480.triggered.connect(lambda: self.set_res(480, 640))
self.action1280x720.triggered.connect(lambda: self.set_res(720, 1280))
self.action15.triggered.connect(lambda: self.set_fps(15))
self.action30.triggered.connect(lambda: self.set_fps(30))
self.actionMonitor.triggered.connect(lambda: self.set_view("view"))
self.actionVR.triggered.connect(lambda: self.set_fps(30))
self.resGroup = QActionGroup(self)
self.resGroup.addAction(self.action640x480)
self.resGroup.addAction(self.action1280x720)
self.resGroup.setExclusive(True)
self.action640x480.setCheckable(True)
self.action1280x720.setCheckable(True)
if pcl_config["stream_height"] == 640:
self.action640x480.setChecked(True)
else:
self.action1280x720.setChecked(True)
self.fpsGroup = QActionGroup(self)
self.fpsGroup.addAction(self.action15)
self.fpsGroup.addAction(self.action30)
self.fpsGroup.setExclusive(True)
self.action15.setCheckable(True)
self.action30.setCheckable(True)
if pcl_config["stream_fps"] == 15:
self.action15.setChecked(True)
else:
self.action30.setChecked(True)
# Help Menu
self.actionAbout.triggered.connect(self.about_dialog)
self.actionDocumentation.triggered.connect(
lambda: self.open_URL(HELP_DOCUMENTATION_URL))
def open_URL(self, url):
"""
Opens specified URL
Args:
url ([string]): [The URL to open, can be formatted using QUrl]
"""
webbrowser.open(url)
def set_res(self, width, height):
"""
Set resolution of the stream
Args:
width (int): Width of the stream
height (int): Height of the stream
"""
self.configure_stream(width, height, pcl_config["stream_fps"])
def set_fps(self, fps):
"""
Set FPS of the stream
Args:
fps (int): Frames per Second of the stream
"""
self.configure_stream(pcl_config["stream_width"],
pcl_config["stream_height"], fps)
def set_view(self, view_type):
"""
Set View Type of the window
Args:
view_type (string): View type of the window, monitor or vr
"""
log.info("View Type Changed to " + view_type)
def start_stream(self):
"""
Start the camera stream.
Creates a seperate process for the stream, and interacts with the frames through shared memory.
Uses a QTimer to maintain a stable frame rate.
"""
# grab config variables
STREAM_HEIGHT = pcl_config["stream_height"]
STREAM_WIDTH = pcl_config["stream_width"]
STREAM_FPS = pcl_config["stream_fps"]
# set up the label
camera_size_policy = QSizePolicy(STREAM_WIDTH, STREAM_HEIGHT)
self.camera_label.setSizePolicy(camera_size_policy)
# create shared memory to hold a single frame for inter process communication,
# wrap it in a numpy array
nbyte_per_frame = STREAM_WIDTH * STREAM_HEIGHT * FRAME_NCHANNELS # bytes in a frame
log.debug("Allocating %i x %i bytes of shared memory" %
(SM_BUF_SIZE, nbyte_per_frame))
self.frame_shm = shared_memory.SharedMemory(create=True,
size=nbyte_per_frame *
SM_BUF_SIZE)
shm_shape = (SM_BUF_SIZE, STREAM_HEIGHT, STREAM_WIDTH, FRAME_NCHANNELS)
self.frame_buffer = np.ndarray(shm_shape,
dtype=np.uint8,
buffer=self.frame_shm.buf)
# spawn the child depth cam process
self.frame_in_use = Value("i", False)
self.pending_frame = Value("i", False)
read_pipe, write_pipe = multiprocessing.Pipe()
self.camera_communication_pipe = write_pipe
self.depth_cam_proc = Process(
target=generate_frames,
args=(self.frame_shm.name, shm_shape, STREAM_HEIGHT, STREAM_WIDTH,
STREAM_FPS, self.frame_in_use, self.pending_frame, read_pipe,
self.is_streaming),
)
self.depth_cam_proc.start()
log.info("Created depth camera process, pId = %i" %
self.depth_cam_proc.pid)
# create a QTimer to manage frame updates
self.fps_timer = QtCore.QTimer()
self.fps_timer.setTimerType(QtCore.Qt.PreciseTimer)
self.fps_timer.timeout.connect(self.update_stream)
self.fps_timer.setInterval(round(1000.0 / STREAM_FPS))
self.fps_timer.start()
log.debug("Timer limiting FPS to %i" % STREAM_FPS)
# just for some stats
self.frameUpdateCount = 0
self.stream_start_time = time.time()
# for camera depending on accelerometer
self.waitingForLevel = False
def configure_stream(self, width, height, fps):
# End exisiting stream
pcl_config["stream_height"] = height
pcl_config["stream_width"] = width
pcl_config["stream_fps"] = fps
# Tear down camera process
self.end_stream()
# Start stream with new values
self.start_stream()
log.info("The stream configuration is as follows:\n Resolution: " +
str(height) + "x" + str(width) + "\nFPS: " + str(fps))
def update_stream(self):
"""
Updates the GUI to display the current video frame.
"""
self.frame_in_use.value = True
pix = frame_to_pixmap(self.frame_buffer[0])
self.camera_label.setPixmap(pix)
self.frame_in_use.value = False
self.frameUpdateCount += 1
def take_picture(self):
"""
Notifies camera process to save the next frame.
Disables the take picture button & posts the image name and experiment path onto the shared pipe.
(SUS) Waits till image is saved to enable the button again.
"""
if not self.is_streaming.value:
log.info("Cant save pic - no stream !!")
return
# disable take pic btn until its done
self.take_picture_button.setEnabled(False)
if not within_tolerance(self.x, self.y, LEVEL_TOLERANCE):
log.info("Waiting till camera is level to take the picture")
self.waitingForLevel = True
return
# write save image info to pipe
plot_num_str = str(self.current_plot_number).zfill(PLOT_NUMBER_PADDING)
img_name = "%s_%s" % (self.experiment_name, plot_num_str)
log.debug("Queueing image <%s> to be saved..." % img_name)
self.pending_frame.value = True
self.camera_communication_pipe.send((self.experiment_path, img_name))
# wait for image to be saved
while (self.pending_frame.value):
pass
self.update_metadata()
self.save_metadata()
# resume normal operations
log.debug("Image <%s> successfully saved" % img_name)
self.update_plot_number(self.current_plot_number + 1)
self.take_picture_button.setEnabled(True)
self.alert.play()
self.waitingForLevel = False
def start_accelerometer(self):
"""
Creates Phidget22 accelerometer and polls it at a fixed rate using a QTimer.
"""
# set up the target image
self.setup_target()
# create the accelerometer, and wait 1 sec for connection
# TODO does the open wait for attachment throw an error
try:
self.accelerometer = Accelerometer()
self.accelerometer.openWaitForAttachment(1000)
log.info("Created accelerometer")
except:
log.warning("There is no accelerometer connected")
self.accelerometer = None
return
# create QTimer to periodically monitor the acceleration
self.accelerometer_timer = QtCore.QTimer()
self.accelerometer_timer.setTimerType(QtCore.Qt.PreciseTimer)
self.accelerometer_timer.timeout.connect(self.update_target)
self.accelerometer_timer.setInterval(ACCELEROMETER_PERIOD_MS)
self.accelerometer_timer.start()
log.debug("Timer limiting accelerometer update frequency to %f Hz" %
(1000.0 / ACCELEROMETER_PERIOD_MS))
def setup_target(self):
"""
Sets up the target graphic of the GUI.
"""
self.wrapper = QWidget()
self.target_layout = QGridLayout()
self.target_layout.addWidget(self.wrapper)
self.target_widget.setLayout(self.target_layout)
self.target = Target()
self.target.pixmap = QPixmap(TARGET_ICON_PATH)
self.target.setGeometry(40, 0, 150, 150)
self.target.setParent(self.wrapper)
self.target.show()
def update_target(self):
"""
Update the coordinate on the GUI's target.
"""
# TODO try-catch error
acceleration = self.accelerometer.getAcceleration()
self.x = acceleration[0]
self.y = acceleration[1]
self.target.coordinate = QPointF(self.x, self.y)
if within_tolerance(self.x, self.y, LEVEL_TOLERANCE):
if self.waitingForLevel:
self.take_picture()
self.camera_level_label.setText("Camera is Level")
self.camera_level_label.setStyleSheet("color: green;")
else:
self.camera_level_label.setText("Camera is not Level")
self.camera_level_label.setStyleSheet("color: red;")
def about_dialog(self):
"""
Open the "New Experiment" Dialog and connect methods to update file name and plot number labels.
"""
about_page = AboutPage()
about_page.exec_()
about_page.show()
def new_experiment_dialog(self):
"""
Open the "New Experiment" Dialog and connect methods to update file name and plot number labels.
"""
new_exp_page = NewExperimentPage()
new_exp_page.expirementCreated.connect(self.update_experiment)
new_exp_page.changePlotNumber.connect(self.update_plot_number)
new_exp_page.exec_()
new_exp_page.show()
def browse_files(self):
"""
Opens up experiment folder directory so that an existing experiment can be chosen
"""
loaded_filename = QFileDialog.getExistingDirectory(
self, 'Select Experiment Folder', PCL_EXP_PATH)
self.update_experiment(basename(loaded_filename))
@pyqtSlot(str)
def update_experiment(self, new_exp_name):
"""
Updates the working experiment.
Modifys all relevant variables and paths.
Sets up metadata to be periodically saved to file.
Enables button if experiment is set.
Args:
new_exp_name (str): The new experiment name
"""
# make sure this is a valid experiment before updating
new_exp_path = os.path.join(PCL_EXP_PATH, new_exp_name)
if not self.validate_experiment(new_exp_path):
return
self.experiment_name = new_exp_name
self.file_name_label.setText(self.experiment_name)
self.experiment_path = new_exp_path
# save old metadata before opening new experiment
self.save_metadata()
metadata_path = os.path.join(self.experiment_path, "Metadata",
"%s.json" % self.experiment_name)
self.metadata = Metadata(metadata_path)
last_index = self.metadata.get_last_index()
if last_index != -1:
self.update_plot_number(last_index + 1)
self.take_picture_button.setEnabled(True)
log.info("Opened experiment %s" % self.experiment_name)
def validate_experiment(self, exp_path):
"""
Validates experiment at the given path.
If no experiment exists or the experiment structure is faulty,
Displays an error message dialog.
Args:
exp_path (path): path to a plotcam experiment
Returns:
bool: true if experiment is valid
"""
# TODO would u want to create RGB and depth folders if they r missing
valid = True
errorMsg = ""
if not os.path.exists(exp_path):
valid = False
errorMsg = "No experiment at %s" % exp_path
elif not os.path.exists(os.path.join(exp_path, "Depth")):
valid = False
errorMsg = "Experiment missing depth data directory"
elif not os.path.exists(os.path.join(exp_path, "RGB")):
valid = False
errorMsg = "Experiment missing rgb data directory"
# dont need to validate metadata, the path will be created if it doenst exist on update exp
if not valid:
# error pop up
log.info(errorMsg)
pass
return valid
@pyqtSlot(int)
def update_plot_number(self, new_plot_number):
"""
Updates plot number.
Args:
new_plot_number (int): The new plot number
"""
self.plot_number_label.setText(
"Plot #: " + str(new_plot_number).zfill(PLOT_NUMBER_PADDING))
self.current_plot_number = new_plot_number
def update_metadata(self):
"""
Updates metadata with new entry.
"""
num = str(self.current_plot_number).zfill(PLOT_NUMBER_PADDING)
t = datetime.now().strftime('%H:%M:%S')
date = datetime.now().strftime('%d/%m/%Y')
name = self.experiment_name
self.metadata.add_entry(num, t, date, self.x, self.y, name)
def save_metadata(self):
"""
Saves the metadata file to disk.
"""
if self.metadata is None:
return
log.info("Saving metadata")
self.metadata.save()
def end_stream(self):
if self.depth_cam_proc:
log.debug("Stream's Average FPS: %.2f" %
(self.frameUpdateCount /
(time.time() - self.stream_start_time)))
# end depth cam process and wait for it to complete
# TODO end the fps Qtimer
self.is_streaming.value = False
self.frame_in_use.value = False
self.depth_cam_proc.join()
# free shared memory
self.frame_shm.close()
self.frame_shm.unlink()
log.info("Terminated camera process")
def closeEvent(self, event):
"""
Overrides systems "self.close" method so that the program can terminate properly.
Teardown all running threads and processes.
"""
# tear down accelerometer
if self.accelerometer:
pass
# tear down camera process
self.end_stream()
log.info("PlotCamLite says goodbye :[")
from Phidget22.Devices.Accelerometer import *
from Phidget22.PhidgetException import *
from Phidget22.Phidget import *
from Phidget22.Net import *
#########################################################################
# Event Managers Definitions
#########################################################################
t = []
ac0 = []
ac1 = []
ac2 = []
try:
ch = Accelerometer()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
def AccelerometerAttached(e):
try:
attached = e
print("\nAttach Event Detected (Information Below)")
print("===========================================")
print("Library Version: %s" % attached.getLibraryVersion())
print("Serial Number: %d" % attached.getDeviceSerialNumber())
print("Channel: %d" % attached.getChannel())