except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
displayDeviceInfo()
print("Press Enter to quit....")
try:
print("GPS Current Time: %s" % (gps.getTime().toString()))
print("GPS Current Date: %s" % (gps.getDate().toString()))
print("GPS Current Latitude: %F" % (gps.getLatitude()))
print("GPS Current Longitude: %F" % (gps.getLongitude()))
print("GPS Current Altitude: %F" % (gps.getAltitude()))
print("GPS Current Heading: %F" % (gps.getHeading()))
print("GPS Current Velocity: %F" % (gps.getVelocity()))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
chr = sys.stdin.read(1)
print("Closing...")
try:
gps.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
compass = CMPS03(port="COM8")
### Variable Init
latitude = 0.0
longitude = 0.0
heading = 0.0
headings_gps = []
headings_compass = []
bearings = []
### MAIN LOOP
while True:
try:
latitude = float(gps.getLatitude())
longitude = float(gps.getLongitude())
heading = float(gps.getHeading())
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
curr_location_tuple = (latitude, longitude)
curr_location_point = point.Point(latitude, longitude)
curr_waypoint_point = point.Point(WAYPOINTS[curr_waypoint][0],WAYPOINTS[curr_waypoint][1])
feet_to_waypoint = distance.distance(curr_location_tuple, WAYPOINTS[curr_waypoint]).feet
bearing_to_waypoint = curr_location_point.bearing(curr_waypoint_point)
headings_gps.append(heading)
if len(headings_gps) > 6:
headings_gps.pop(0)
heading_gps_avg = mean(headings_gps)
atexit.register(clean_up)
gps = None
try:
gps = GPS()
gps.openPhidget()
gps.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
exit(1)
print("Date,Time,Lat,Long,Altitude,Velocity,Heading")
while True:
try:
print("{0},{1},{2},{3},{4},{5},{6}".format(
gps.getDate().toString(),
gps.getTime().toString(),
gps.getLatitude(),
gps.getLongitude(),
gps.getAltitude(),
gps.getVelocity(),
gps.getHeading(),
))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
time.sleep(.02)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
displayDeviceInfo()
print("Press Enter to quit....")
try:
print("GPS Current Time: %s" % (gps.getTime().toString()))
print("GPS Current Date: %s" % (gps.getDate().toString()))
print("GPS Current Latitude: %F" % (gps.getLatitude()))
print("GPS Current Longitude: %F" % (gps.getLongitude()))
print("GPS Current Altitude: %F" % (gps.getAltitude()))
print("GPS Current Heading: %F" % (gps.getHeading()))
print("GPS Current Velocity: %F" % (gps.getVelocity()))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
chr = sys.stdin.read(1)
print("Closing...")
try:
gps.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
header = 'imgName,curTime,elpTime,gps_time,gps_vel, gps_heading, gps_lon,gps_lat,gps_alt\n'
logFile.write(header)
for i in range(2000):
#print 1
curTime = str(datetime.datetime.now())
imgName = 'testImg' + str(i).zfill(5) + '.jpg'
img = cv.QueryFrame(capture)
#print 2
#cv.ShowImage("camera", img)
cv.SaveImage(saveDir + '/' + imgName, img)
#print 3
#gps stuff...
gps_time = gps.getTime().toString()
gps_lat = gps.getLatitude()
gps_lon = gps.getLongitude()
gps_alt = gps.getAltitude()
gps_heading = gps.getHeading()
gps_vel = gps.getVelocity()
elpTime = str(datetime.datetime.now())
outList = [
imgName, curTime, elpTime, gps_time, gps_vel, gps_heading, gps_lon,
gps_lat, gps_alt
]
outList = [str(x) for x in outList]
outStr = ','.join(outList) + '\n'
print 'Iteration:', i, str(curTime), gps_lon, gps_lat, gps_alt
logFile.write(outStr)
#time.sleep(0.1)
logFile.close()
imgName = 'TestImg'+str(i).zfill(6)+'.jpg'
if i%2==0:
imgA = cv.QueryFrame(captureA)
cv.SaveImage(saveDir+'/'+imgName, imgA)
else:
imgB = cv.QueryFrame(captureB)
cv.SaveImage(saveDir+'/'+imgName, imgB)
#print 2
#cv.ShowImage("camera", img)
#print 3
#gps stuff...
gps_time = gps.getTime().toString()
gps_lat = gps.getLatitude()
gps_lon = gps.getLongitude()
gps_alt = gps.getAltitude()
gps_heading = gps.getHeading()
gps_vel = gps.getVelocity()
elpTime = str(datetime.datetime.now())
outList = [imgName,curTime,elpTime,gps_time,gps_vel, gps_heading, gps_lon,gps_lat,gps_alt]
outList = [str(x) for x in outList]
outStr = ','.join(outList)+'\n'
logFile.write(outStr)
curFrame = curFrame + 1
if curFrame %10 ==0:
logFile.flush()
print 'Iteration:', i, str(curTime), gps_lon,gps_lat,gps_alt
#time.sleep(0.1)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
displayDeviceInfo()
print("Press Enter to quit....")
try:
print("GPS Current Time: %s" % (gps.getTime().toString()))
print("GPS Current Date: %s" % (gps.getDate().toString()))
print("GPS Current Latitude: %F" % (gps.getLatitude()))
print("GPS Current Longitude: %F" % (gps.getLongitude()))
print("GPS Current Altitude: %F" % (gps.getAltitude()))
print("GPS Current Heading: %F" % (gps.getHeading()))
print("GPS Current Velocity: %F" % (gps.getVelocity()))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Press Enter to quit....")
# DO SOMETHING HERE #
# Set digital output port 0 to be on
#interfaceKit.setOutputState(0, 1)
#import time
#time.sleep(3)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
displayDeviceInfo()
print("Press Enter to quit....")
try:
print("GPS Current Time: %s" %(gps.getTime().toString()))
print("GPS Current Date: %s" %(gps.getDate().toString()))
print("GPS Current Latitude: %F" %(gps.getLatitude()))
print("GPS Current Longitude: %F" %(gps.getLongitude()))
print("GPS Current Altitude: %F" %(gps.getAltitude()))
print("GPS Current Heading: %F" %(gps.getHeading()))
print("GPS Current Velocity: %F" % (gps.getVelocity()))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Press Enter to quit....")
# DO SOMETHING HERE #
# Set digital output port 0 to be on
#interfaceKit.setOutputState(0, 1)
#import time
