Removed waypoint stuff, get from TOSCo repo
This commit is contained in:
Binary file not shown.
@ -1,3 +0,0 @@
|
|||||||
#!/bin/sh
|
|
||||||
|
|
||||||
sudo -H pip3 install gps serial pynmea2
|
|
||||||
@ -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 +0,0 @@
|
|||||||
42.521985, -83.237700, 10, 0
|
|
||||||
|
@ -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:])
|
|
||||||
Reference in New Issue
Block a user