Tracking changes from TOSCo

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

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