nprobert/pi_tools
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Removed waypoint stuff, get from TOSCo repo

Browse Source
This commit is contained in:
Neal Probert
2021-01-26 09:34:04 -05:00
parent a8c19a5709
commit 17950d862d
5 changed files with 0 additions and 262 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
Projects/GPS/python/alert.wav
View File

Binary file not shown.

3
Projects/GPS/python/setup.sh
View File

@ -1,3 +0,0 @@
#!/bin/sh
sudo -H pip3 install gps serial pynmea2

24
Projects/GPS/python/test.py
View File

@ -1,24 +0,0 @@
#! /usr/bin/python3
from gps import *
import time
gpsd = gps(mode=WATCH_ENABLE|WATCH_NEWSTYLE)
print('latitude\tlongitude\ttime utc\t\taltitude\tepv\tept\tspeed\tclimb') # '\t' = TAB to try and output the data in columns.
try:
while True:
report = gpsd.next() #
if report['class'] == 'TPV':
out = ""
for i in ['lat','lon','time','alt','epv','ept','speed','climb']:
out += str(getattr(report,i,0.0)) + "\t"
print(out)
time.sleep(1)
except (KeyboardInterrupt, SystemExit): #when you press ctrl+c
print("Done.\nExiting.")

1
Projects/GPS/python/test1.csv
View File

@ -1 +0,0 @@
42.521985, -83.237700, 10, 0
1 42.521985 -83.237700 10 0

234
Projects/GPS/python/waypoint_alert.py
View File

@ -1,234 +0,0 @@
#! /usr/bin/python3
import os
import sys
import getopt
import csv
from math import *
from gps import *
from time import strftime, sleep
import RPi.GPIO as GPIO
##############################################################################
# sound stuff
##############################################################################
import os
from sys import platform as _platform
sound_args = ""
def play_sound(file):
if _platform == "win32" or _platform == "win64" or _platform == "cygwin":
cmd = "wmplayer " + '"' + file + '"'
else:
cmd = "aplay -N " + sound_args + " " + file + " &"
os.system(cmd)
##############################################################################
# buzzer stuff
##############################################################################
# https://www.instructables.com/Raspberry-Pi-Tutorial-How-to-Use-a-Buzzer/
buzzer=23
seconds=3
def buzzer_init():
GPIO.setwarnings(0)
GPIO.setmode(GPIO.BCM)
GPIO.setup(buzzer, GPIO.OUT)
def buzzer_on():
GPIO.output(buzzer, GPIO.HIGH)
play_sound("alert.wav")
def buzzer_off():
GPIO.output(buzzer, GPIO.LOW)
def buzzer_run():
global seconds
sec = seconds
while (sec):
buzzer_on()
sleep(0.5)
buzzer_off()
sleep(0.5)
sec = sec - 1
buzzer_init()
##############################################################################
# gps stuff
##############################################################################
# https://ozzmaker.com/using-python-with-a-gps-receiver-on-a-raspberry-pi/
gpsd = gps(mode=WATCH_ENABLE|WATCH_NEWSTYLE)
#
# GPS receiver is ublox from V2V-MD at 10x seconds
#
# http://www.movable-type.co.uk/scripts/latlong.html
earth_flatening = 1.0/298.257223563
earth_radius = 6378137.0
def haversine(lat1, lon1, lat2, lon2):
"""
Calculate the great circle distance between two points
on the earth (specified in decimal degrees)
"""
# convert decimal degrees to radians
lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
# haversine formula
dlon = lon2 - lon1
dlat = lat2 - lat1
a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
c = 2 * asin(sqrt(a))
# 6367 km is the radius of the Earth
return earth_radius * c
def roydistance(lat1, lon1, lat2, lon2):
"""
Calculate the great circle distance between two points
on the earth (specified in decimal degrees)
"""
# convert decimal degrees to radians
lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
# Roy's method
f1 = (1.0 - earth_flatening)
top = (pow((lon2-lon1), 2) * pow(cos(lat1), 2)) + pow(lat2-lat1, 2)
bot = pow(sin(lat1), 2) + (pow(f1, 2) * pow(cos(lat1), 2))
dist = f1 * earth_radius * sqrt(top/bot)
return dist
##############################################################################
# virtual cones
##############################################################################
# node from csv (each line is lat, long, distance, next code# (optional)):
# 42.522040, -83.237594, 5, 1
def read_csv(csv_file):
file_csv = open(csv_file)
data_csv = csv.reader(file_csv)
virtual_cones = list(data_csv)
total_cones = len(virtual_cones)
print(csv_file, total_cones)
# print(virtual_cones)
return virtual_cones
##############################################################################
# main
##############################################################################
def main(argv):
virtual_list = [[],[],[],[],[],[],[],[],[]]
virtual_count = 0
current_cone = [0,0,0,0,0,0,0,0]
total_cones = [0,0,0,0,0,0,0,0]
try:
os.mkdir("logs")
except:
pass
log = open(strftime("logs/way-%Y%m%d-%H%M.log"), 'a')
log.write("Time,Latitude,Longitude,Heading,Speed,Description\n")
try:
opts, args = getopt.getopt(argv,"",["",""])
except getopt.GetoptError:
print(sys.argv[0], '<csvfile>')
sys.exit(2)
# for opt, arg in opts:
# nop
# slurp in files for each list
for arg in args:
virtual_cones = read_csv(arg)
current_cone[virtual_count] = 0
total_cones[virtual_count] = 0
virtual_list[virtual_count] = virtual_cones
virtual_count = virtual_count + 1
# print(virtual_list)
buzz_no = 0
max_buzz = 3
try:
while True:
report = gpsd.next() #
if report['class'] == 'TPV':
time = getattr(report, 'time', 0.0)
lat = getattr(report, 'lat', 0.0)
lon = getattr(report, 'lon', 0.0)
heading = getattr(report, 'track', 0.0)
speed = getattr(report, 'speed', 0.0)
log.write(time+","+str(lat)+","+str(lon)+","+str(heading)+","+str(speed))
# print("Position: %f, %f, %d" % (lat, lon, speed))
# walk through each list
for i in range(0, virtual_count):
# end of list?
if current_cone[i] >= total_cones[i]:
current_cone[i] = 0
# check first node for reset
if current_cone[i]:
# node from csv (each line is lat, long, distance, next code#)
row = virtual_list[i][0]
cone_lat = float(row[0])
cone_lon = float(row[1])
cone_dist = float(row[2])
dist = haversine(lat, lon, cone_lat, cone_lon) / 1000.0
if dist < cone_dist:
current_cone[i] = 0
# node from csv (each line is lat, long, distance, next code#)
row = virtual_list[i][current_cone[i]]
cone_lat = float(row[0])
cone_lon = float(row[1])
cone_dist = float(row[2])
# next cone
cone_next = current_cone[i] + 1
try:
cone_next = int(row[3])
except:
cone_next = 0
# distance between vehicle and cone
# dist = haversine(lat, lon, cone_lat, cone_lon)
dist = roydistance(lat, lon, cone_lat, cone_lon)
# check distance trigger
if (dist < cone_dist and buzz_no < max_buzz):
log.write(".Waypoint="+str(i)+":"+str(current_cone[i]))
print("Waypoint %d:%d reached: %f, %f, %d" % (i, current_cone[i], lat, lon, speed))
buzzer_run()
current_cone[i] = cone_next
buzz_no = buzz_no + 1
else:
buzz_no = 0
log.write("\n")
# time.sleep(0.2)
except (KeyboardInterrupt, SystemExit): #when you press ctrl+c
log.close()
print("Done.\nExiting.")
main(sys.argv[1:])