diff --git a/classes/gpsmath.py b/classes/gpsmath.py index 6c6516f..117d1e8 100644 --- a/classes/gpsmath.py +++ b/classes/gpsmath.py @@ -1,8 +1,63 @@ -from math import sin, cos, asin, sqrt, radians +from math import sin, cos, asin, sqrt, radians, atan2 earth_flatening = 1.0/298.257223563 earth_radius = 6378137.0 +############################################################################# +# GPS stuff +############################################################################# + +PI = 4 * atan2(1, 1) +MPERDEG = (111.13285 * 1000.0) +RADIUS = earth_radius # meters + +def deg2rad(deg): + global PI + return ((deg)*PI/180.0) + +def rad2deg(rad): + global PI + return ((rad)*180.0/PI) + +def gps_bearing(lat1, lon1, lat2, lon2): + lat1 = deg2rad(lat1) + lon1 = deg2rad(lon1) + lat2 = deg2rad(lat2) + lon2 = deg2rad(lon2) + y = sin(lon2-lon1) * cos(lat2) + x = cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(lon2-lon1) + bearing = rad2deg( atan2(y, x) ) + return (bearing + 360.0) % 360.0 + +def gps_distance(lat1, lon1, lat2, lon2): + global RADIUS + lat1 = deg2rad(lat1) + lon1 = deg2rad(lon1) + lat2 = deg2rad(lat2) + lon2 = deg2rad(lon2) + a = sin((lat2-lat1)/2)**2 + cos(lat1) * cos(lat2) * sin((lon2-lon1)/2)**2 + c = 2 * atan2(sqrt(a), sqrt(1 - a)) + return RADIUS * c + +def gps_xy(lat1, lon1, lat2, lon2): + global MPERDEG + mperdeg = MPERDEG * cos(deg2rad(lat2+lat1)/2) + x = (lon2 - lon1) * mperdeg + y = (lat2 - lat1) * MPERDEG + return (x, y) + +def gps_xydistance(lat1, lon1, lat2, lon2): + global MPERDEG + mperdeg = MPERDEG * cos(deg2rad(lat2+lat1)/2) + x = (lon2 - lon1) * mperdeg + y = (lat2 - lat1) * MPERDEG + dist = sqrt(x*x + y*y) + return (x, y, dist) + +# +# +# + def haversine(lat1, lon1, lat2, lon2): """ Calculate the great circle distance between two points diff --git a/gps_vcones/alert.wav b/gps_vcones/alert.wav index 268ffc0..92c757e 100644 Binary files a/gps_vcones/alert.wav and b/gps_vcones/alert.wav differ diff --git a/gps_vcones/vcones_gui.py b/gps_vcones/vcones_gui.py index d002d93..f352be6 100755 --- a/gps_vcones/vcones_gui.py +++ b/gps_vcones/vcones_gui.py @@ -25,8 +25,10 @@ config_file = "vcones.yaml" # audio stuff ############################################################################## +vc_path = os.path.realpath(__file__) + def play_sound(file): - wave_obj = sa.WaveObject.from_wave_file(file) + wave_obj = sa.WaveObject.from_wave_file(vc_path + "/" + file) play_obj = wave_obj.play() play_obj.wait_done() @@ -124,46 +126,77 @@ class VirtualCones(): if self.num_cones == 0: return (0, 999.0, 0) - # end of list? - if self.current_cone >= self.num_cones: - self.current_cone = 0 - - # check first node for reset - if self.current_cone: - # node from csv (each line is lat, long, distance) - row = self.cones[0] + # find node in list + for i in range(self.num_cones): + # node from csv (each line is lat, long, distance, next code#) + row = self.cones[i] cone_lat = row[0] cone_lon = row[1] cone_dist = row[2] - dist = roydistance(lat, lon, cone_lat, cone_lon) - if dist < rng: - self.current_cone = 0 - - # node from csv (each line is lat, long, distance, next code#) - row = self.cones[self.current_cone] - cone_lat = row[0] - cone_lon = row[1] - cone_dist = row[2] - - # next cone - cone_next = self.current_cone + 1 - try: - cone_next = int(row[3]) - except: - pass - + # distance between vehicle and cone - self.distance = roydistance(lat, lon, cone_lat, cone_lon) - - # check distance trigger - if self.distance < rng: -# out = "Waypoint %s:%d reached: %f, %f, %f, %d" % (self.filename, self.current_cone, lat, lon, heading, speed) -# print(out) - play_sound("alert.wav") - self.current_cone = cone_next - trig = 1 + self.distance = roydistance(lat, lon, cone_lat, cone_lon) + + # check distance trigger + if self.distance < rng: +# out = "Waypoint %s:%d reached: %f, %f, %f, %d" % (self.filename, self.current_cone, lat, lon, heading, speed) +# print(out) + self.current_cone = i + trig = 1 + if self.buzz_no < self.max_buzz: + play_sound("alert.wav") + self.buzz_no = self.buzz_no + 1 + break + else: + self.buzz_no = 0 return (self.current_cone, self.distance, trig) + +# def check_list(self, lat, lon, heading, speed, rng): +# trig = 0 +# if self.num_cones == 0: +# return (0, 999.0, 0) + +# # end of list? +# if self.current_cone >= self.num_cones: +# self.current_cone = 0 + + # check first node for reset +# if self.current_cone: + # node from csv (each line is lat, long, distance) +# row = self.cones[0] +# cone_lat = row[0] +# cone_lon = row[1] +# cone_dist = row[2] +# dist = roydistance(lat, lon, cone_lat, cone_lon) +# if dist < rng: +# self.current_cone = 0 + + # node from csv (each line is lat, long, distance, next code#) +# row = self.cones[self.current_cone] +# cone_lat = row[0] +# cone_lon = row[1] +# cone_dist = row[2] + + # next cone +# cone_next = self.current_cone + 1 +# try: +# cone_next = int(row[3]) +# except: +# pass + + # distance between vehicle and cone +# self.distance = roydistance(lat, lon, cone_lat, cone_lon) + + # check distance trigger +# if self.distance < rng: +# out = "Waypoint %s:%d reached: %f, %f, %f, %d" % (self.filename, self.current_cone, lat, lon, heading, speed) +# print(out) +# play_sound("alert.wav") +# self.current_cone = cone_next +# trig = 1 + +# return (self.current_cone, self.distance, trig) ############################################################################## # thread