Code: Select all
#!/usr/bin/env python
import os
import pygame, sys
import datetime
import time
import math
import shutil
from pygame.locals import *
import serial
from decimal import *
getcontext().prec = 8
# initial values
Frame = 0
fix = 0
gps_con = 0
start = 0
lat = 0
lon = 0
speed = 0
angle = 0
oldX = 0
oldY = 0
# set screen size
width = 640
height = 480
Frame = 1
# set SCALE
J = 100000
#move old log.txt if exists
try:
if os.path.exists('/run/shm/log.txt') == True:
now = datetime.datetime.now()
timestamp = now.strftime("%y%m%d%H%M%S")
shutil.copy('/run/shm/log.txt','/home/pi/' + str(timestamp) + '_log.txt')
os.remove('/run/shm/log.txt')
except OSError:
pass
pygame.init()
blackColor = pygame.Color( 0, 0, 0)
yellowColor = pygame.Color(255,255, 0)
redColor = pygame.Color(255, 0, 0)
greenColor = pygame.Color( 0,255, 0)
if not Frame:
windowSurfaceObj = pygame.display.set_mode((width,height), pygame.NOFRAME, 24)
else:
windowSurfaceObj = pygame.display.set_mode((width,height), 1, 24)
pygame.display.set_caption('Pi-GPSPLot')
def print_text (msg,color,width,height):
fontObj = pygame.font.Font('freesansbold.ttf', height / 32)
blackColor = pygame.Color( 0, 0, 0)
pygame.draw.rect(windowSurfaceObj, blackColor, Rect(0,height - (height/10),width,height - (height/10)))
msgSurfaceObj = fontObj.render(msg, False, color)
msgRectobj = msgSurfaceObj.get_rect()
msgRectobj.topleft = (width / 20, height - 30)
windowSurfaceObj.blit(msgSurfaceObj, msgRectobj)
def fileplot(J):
# replot coordinates when zooming in or out.
q = 0
oldX = 0
try:
pygame.draw.rect(windowSurfaceObj, blackColor, Rect(0,0,width,height))
pygame.display.update()
with open("/run/shm/log.txt", "r") as file:
while q < 9999:
inputx = file.readline()
inp = inputx.split(',',10)
if q == 0:
A = float(inp[3])
B = float(inp[5])
if q > 0:
X = float(inp[3])
Y = float(inp[5])
pygame.draw.rect(windowSurfaceObj, yellowColor,Rect(((A-X)* J)+(width/2),((B-Y)*J)+ (height/2),3,3))
if oldX != 0:
pygame.draw.line(windowSurfaceObj, yellowColor, (((A-oldX)* J)+(width/2),((B-oldY)*J)+ (height/2)), (((A-X)* J)+(width/2),((B-Y)*J)+ (height/2)))
oldX = X
oldY = Y
pygame.draw.rect(windowSurfaceObj, redColor,Rect((width/2)- 4,(height/2) - 4,10,10),1)
pygame.display.update()
q +=1
except:
pass
while True:
# check for gps connected on USB0 or USB1
if gps_con == 0 and os.path.exists('/dev/ttyUSB0') == True:
ser = serial.Serial('/dev/ttyUSB0',4800,timeout = 10)
gps_con = 1
print "connected on USB0"
elif gps_con == 0 and os.path.exists('/dev/ttyUSB1') == True:
ser = serial.Serial('/dev/ttyUSB1',4800,timeout = 10)
gps_con = 1
print "connected on USB1"
if gps_con == 1:
gps = ser.readline()
if gps[1 : 6] == "GPGGA":
gps1 = gps.split(',',14)
if gps[1:6] == "GPGSA":
fix = int(gps[9:10])
if gps[1 : 6] == "GPGGA" and len(gps) > 68 and (gps1[3] == "N" or gps1[3] == "S")and fix > 1:
lat = int(gps[18:20]) + (Decimal(int(gps[20:22]))/(Decimal(60))) + (Decimal(int(gps[23:27]))/(Decimal(360000)))
if gps[28:29] == "S":
lat = 0 - lat
lon = int(gps[30:33]) + (Decimal(int(gps[33:35]))/(Decimal(60))) + (Decimal(int(gps[36:40]))/(Decimal(360000)))
if gps[41:42] == "W":
lon = 0 - lon
if start < 4:
A = lat
B = lon
pygame.draw.rect(windowSurfaceObj, redColor,Rect((width/2)- 4,(height/2) - 4,10,10),1)
start += 1
if start == 4:
now = datetime.datetime.now()
timestamp = now.strftime("%y/%m/%d-%H:%M:%S")
timp = "TIME:, " + str(timestamp) + ", LAT:, " + str(lat) + ", LON:, " + str(lon) + ", SPEED:, " + str(speed) + ", ANGLE:, " + str(angle) + "\n"
with open("/run/shm/log.txt", "a") as file:
file.write(timp)
start +=1
else:
start += 1
X = lat
Y = lon
pygame.draw.rect(windowSurfaceObj, redColor,Rect(((A-X)* J)+(width/2),((B-Y)*J)+ (height/2),3,3))
pygame.display.update()
time.sleep(.25)
pygame.draw.rect(windowSurfaceObj, yellowColor,Rect(((A-X)* J)+(width/2),((B-Y)*J)+ (height/2),3,3))
if oldX != 0:
pygame.draw.line(windowSurfaceObj, yellowColor, (((A-oldX)* J)+(width/2),((B-oldY)*J)+ (height/2)), (((A-X)* J)+(width/2),((B-Y)*J)+ (height/2)))
msg = "LAT: " + str(lat) + " LON: " + str(lon) + " SPEED: " + str(speed) + " ANGLE: " + str(angle) + " SCALE: " + str(((Decimal(1)/Decimal(J))*Decimal(10000)))[:4] + " Km/Px"
print_text(msg,greenColor,width,height)
pygame.display.update()
now = datetime.datetime.now()
timestamp = now.strftime("%y/%m/%d-%H:%M:%S")
timp = "TIME:, " + str(timestamp) + ", LAT:, " + str(lat) + ", LON:, " + str(lon) + ", SPEED:, " + str(speed) + ", ANGLE:, " + str(angle) + "\n"
with open("/run/shm/log.txt", "a") as file:
file.write(timp)
if start > 4:
oldX = X
oldY = Y
if gps[1 : 6] == "GPRMC" and fix > 1:
gps2 = gps.split(',',14)
speed = gps2[7]
angle = gps2[8]
msg = "LAT: " + str(lat) + " LON: " + str(lon) + " SPEED: " + str(speed) + " ANGLE: " + str(angle) + " SCALE: " + str(((Decimal(1)/Decimal(J))*Decimal(10000)))[:4] + " Km/Px"
print_text(msg,greenColor,width,height)
pygame.display.update()
if fix <= 1:
msg = "LAT: " + str(lat) + " LON: " + str(lon) + " SPEED: " + str(speed) + " ANGLE: " + str(angle) + " SCALE: " + str(((Decimal(1)/Decimal(J))*Decimal(10000)))[:4] + " Km/Px"
print_text(msg,redColor,width,height)
pygame.display.update()
print gps
# mouse and keyboard
for event in pygame.event.get():
if event.type == QUIT:
pygame.quit()
sys.exit()
elif event.type == MOUSEBUTTONUP or event.type == KEYDOWN:
z = 0
kz = 0
if event.type == KEYDOWN:
kz = event.key
# screen capture
if kz == K_s:
now = datetime.datetime.now()
timestamp = now.strftime("%y%m%d%H%M%S")
pygame.image.save(windowSurfaceObj, '/home/pi/scr' + str(timestamp) + '.bmp')
if event.type == MOUSEBUTTONUP:
mousex, mousey = event.pos
# click bottom right to zoom in
if mousex > width * 0.9 and mousey > height * 0.9:
J = J * 2
fileplot(J)
# click bottom left to zoom out
if mousex < width * 0.1 and mousey > height * 0.9:
J = J / 2
fileplot(J)