# http://discussion.forum.nokia.com/forum/showthread.php?s=c9700af706de763105666122a37e1175&threadid=54857
import socket
import traceback
import operator # for cksum
import math # for bearing calc
def fix_gps_datestamp(s):
# 230503 UTC date (day, month, year)
d,m,y = s[0:2],s[2:4],s[4:6]
return "20%s%s%s" % (y,m,d)
class BTReader:
def connect(self):
self.sock=socket.socket(socket.AF_BT,socket.SOCK_STREAM)
# address,services=socket.bt_discover()
# print "Discovered: %s, %s"%(address,services)
# target=(address,services.values()[0])
# print "Connecting to "+str(target)
# and stash target?
# ('00:0e:6d:5e:cc:fb',1) is the socketcomm unit
target = ('00:0e:6d:5e:cc:fb',1) # is the socketcomm unit
self.sock.connect(target)
self.last = None
self.lat = None
self.lon = None
def readposition(self):
saw_gpgga = 0
saw_gprmc = 0
saw_gpgsa = 0
start_time = time.time()
self.samples = 0
self.badcksum = 0
self.nocksum = 0
while 1:
try:
buffer=""
ch=self.sock.recv(1)
while(ch!='$'):
ch=self.sock.recv(1)
while 1:
if (ch=='\r'):
break
buffer+=ch
ch=self.sock.recv(1)
if "*" not in buffer[1:]:
self.nocksum += 1
continue
if buffer.count("*") > 1:
self.extra_stars = buffer.count("*")
continue
sum_over, cksum = buffer[1:].split("*")
gen_cksum = "%02X" % reduce(operator.xor, map(ord,sum_over))
if gen_cksum != cksum:
self.badcksum += 1
continue
self.samples += 1
sentence = sum_over.split(",")
verb = sentence.pop(0)
if verb == "GPGGA":
saw_gpgga = 1
utc_timestamp, lat, lat_pol, lon, lon_pol, fix_qual, num_sats, HDOP, altitude, altitude_units, geoid_height, geoid_units, time_since_dgps, dgps_id = sentence
self.lat = float(lat)/100.0
if lat_pol == "S": self.lat = -self.lat
# lon_pol: W/E
self.lon = float(lon)/100.0
if lon_pol == "W": self.lon = -self.lon
self.timestamp = utc_timestamp
self.num_sats = num_sats
self.compute_bearing()
self.altitude = altitude + altitude_units
# return self.lat, self.lon, self.when
elif verb == "GPRMC":
saw_gprmc = 1
utc_timestamp, status, lat, lat_pol, lon, lon_pol, ground_speed_kts, track_angle_degs, datestamp, magnetic_variation, mag_var_pol, extra = sentence
# lat_pol: N/S
self.lat = float(lat)/100.0
if lat_pol == "S": self.lat = -self.lat
# lon_pol: W/E
self.lon = float(lon)/100.0
if lon_pol == "W": self.lon = -self.lon
self.datestamp = fix_gps_datestamp(datestamp)
self.timestamp = utc_timestamp
self.ground_speed_kts = ground_speed_kts
self.track_angle_degs = track_angle_degs
self.compute_bearing()
elif verb == "GPGSA":
saw_gpgsa = 1
auto, fixmode = sentence[:2]
self.fixmode = fixmode+"/"+auto
# self.auto = auto
except Exception, e:
self.last_traceback = "".join(traceback.format_exception(sys.exc_type, sys.exc_value, sys.exc_traceback))
print self.last_traceback
try:
print "sentence:", sentence
except UnboundLocalError:
pass
# continue
if saw_gpgga and saw_gprmc and saw_gpgsa and (time.time() - start_time >= 2):
# scan for at least 2 seconds, to "catch up"
return
def fields(self):
return self.lat, self.lon, "%sT%s" % (self.datestamp, self.timestamp), self.ground_speed_kts, self.track_angle_degs, self.altitude
def compute_bearing(self):
if not self.last: return
# most of these fail with nan, not traceback; let the outer catcher deal
oldlat, oldlon = self.last
deltalat = self.lat - oldlat # Y
deltalon = self.lon - oldlon # X
self.dist = math.hypot(deltalat, deltalon)
if deltalat == 0 and deltalon == 0:
self.bearing = None
return
self.bearing = 360.0*math.atan2(deltalat, deltalon)/(2*math.pi)
def close(self):
self.sock.close()
print "closed bt socket"
import time
import camera
imgbase = u"E:\\Images"
# bt=BTReader()
# bt.connect()
def take_pic_where():
global ss
img = camera.take_photo() # -> graphics.Image
#lat, lon, when, speed, angle = bt.readposition()
ss.bt.readposition()
ss.bt.last = (ss.bt.lat, ss.bt.lon)
imgpath = time.strftime("%Y%m%dt%H%M%S")
imgfile = os.path.join(imgbase, imgpath + ".jpg")
logfile = os.path.join(imgbase, imgpath + ".where")
img.save(imgfile)
print >> open(logfile, "w"), ss.bt.fields()
return imgfile
def btplay():
bt=BTReader()
bt.connect()
print bt.readposition()
print bt.readposition()
print bt.readposition()
print bt.fields()
print bt.auto, bt.fixmode
bt.close()
# from System/Apps/Python/imgviewer.py
import key_codes
import e32
lock=e32.Ao_lock()
running=1
def quit():
global running,lock
running=0
lock.signal()
import appuifw
appuifw.app.screen='full'
appuifw.app.exit_key_handler=quit
from appuifw import *
from graphics import *
c=Canvas()
app.body=c
draw_realscreen=Draw(c)
# draw_realscreen.clear(0)
# draw_realscreen.text((0,10),u"bluecamera",(0,0,255))
import thread
def btworker(bt):
global lock
while 1:
e32.ao_sleep(5)
# bt.readposition() # that really doesn't work...
lock.signal()
class screenstuff:
def __init__(self, draw):
self.title = u"bluecamera"
self.where = ""
self.what = ""
self.age = time.time()
self.draw = draw
self.baseline = 10
self.bt = BTReader()
self.bt.connect()
thread.start_new_thread(btworker, (self.bt,))
def close(self):
self.bt.close()
def render(self):
line1 = (0, 1*self.baseline)
line2 = (0, 2*self.baseline)
line3 = (0, 3*self.baseline)
line4 = (0, 4*self.baseline)
blue = (0,0,255)
green = (0,255,0)
yellow = (255,255,0)
red = (255,0,0)
black = 0
background_3d = (0,99,0)
background_2d = (99,99,0)
background_color = {
"2/A": background_2d,
"3/A": background_3d,
}.get(self.bt.fixmode, black)
self.draw.clear(background_color)
self.draw.text( line1, self.title, blue)
self.draw.text( line2, unicode(time.time()-self.age), red)
self.draw.text( line3, self.what, yellow)
n = 4
btkeys = self.bt.__dict__.keys()
btkeys.sort()
# consider pre-coding a set of these...
for linekey in btkeys:
if linekey == "sock": continue
lineval = self.bt.__dict__[linekey]
if not lineval: continue
line = u"%s: %s" % (linekey, lineval)
self.draw.text( (0, n*self.baseline), line, green)
n += 1
# next, draw a *line* representing the vector?
# how about reading the list and drawing a line of those?
ss = screenstuff(draw_realscreen)
def update_where():
global ss
ss.bt.readposition()
# ss.where = u"|".join(ss.bt.fields())
def makepic():
global lock,ss
f = take_pic_where()
ss.what = f
ss.age = time.time()
lock.signal()
def refresh():
global lock, ss
lock.signal()
c.bind(key_codes.EKey1,makepic)
c.bind(key_codes.EKey2,makepic)
c.bind(key_codes.EKey3,makepic)
c.bind(key_codes.EKey9,refresh)
while running:
# e32.ao_sleep(0.5)
update_where()
ss.render()
lock.wait()
ss.close()
# next: figure out bearing and distance from last hit...
#
# distance in degrees is easy - hypot(lat-lat, lon-lon)
# angle is easy if you cheat and assume the world is flat:
# atan(lat-lat, lon-lon)