Merge pull request commaai#161 from arne182/release2

update to latest

selfdrive/car/toyota/carstate.py

@@ -34,7 +34,7 @@ def gps_distance(gpsLat, gpsLon, gpsAlt, gpsAcc):
   speedlimit = float(B[minindex,7])
 
   if abs(gpsAlt -B[minindex,3]) < altacc:
-    if gpsAcc<1.00001:
+    if gpsAcc<0.1:
       #dist = 6371010*acos(sin(radians(gpsLat))*sin(radians(lat))+cos(radians(gpsLat))*cos(radians(lat))*cos(radians(gpsLon-lon)))
       dist = (np.sum((B[minindex,[0,1,2]] - A)**2))**0.5
   #else:

selfdrive/controls/lib/planner.py

@@ -171,7 +171,13 @@ def update(self, rcv_times, CS, CP, VM, PP, live20, live100, md, live_map_data):
       if live_map_data.liveMapData.speedLimitValid:
         speed_limit = live_map_data.liveMapData.speedLimit
         v_speedlimit = speed_limit + offset
-      if live_map_data.liveMapData.speedLimitAheadValid and live_map_data.liveMapData.speedLimitAheadDistance < 200:
+      if gasbuttonstatus == 1:
+        speed_ahead_distance = 100
+      elif gasbuttonstatus == 2:
+        speed_ahead_distance = 300
+      else:
+        speed_ahead_distance = 200
+      if live_map_data.liveMapData.speedLimitAheadValid and live_map_data.liveMapData.speedLimitAheadDistance < speed_ahead_distance:
         speed_limit_ahead = live_map_data.liveMapData.speedLimitAhead
         #print "Speed Ahead found"
         #print speed_limit_ahead
@@ -215,13 +221,16 @@ def update(self, rcv_times, CS, CP, VM, PP, live20, live100, md, live_map_data):
         accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
         accel_limits[0] = min(accel_limits[0], accel_limits[1])
 
+      # Change accel limits based on time remaining to turn
+      if decel_for_turn:
+        time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max((v_ego + v_curvature)/2, 1.))
+        required_decel = min(0, (v_curvature - v_ego) / time_to_turn*0.85)
+        accel_limits[0] = max(accel_limits[0], required_decel)
+
       if v_speedlimit_ahead < v_speedlimit:
-        time_to_speedlimit = max(1.0, live_map_data.liveMapData.speedLimitAheadDistance / max(self.v_cruise, 1.))
-        #print "Decelerating in "
-        #print time_to_speedlimit
-        required_decel = min(0, (v_speedlimit_ahead - self.v_cruise) / time_to_speedlimit)*5
-        if live_map_data.liveMapData.speedLimitAheadDistance < 100.0:
-          max(required_decel*10.0, -3.0)
+        if live_map_data.liveMapData.speedLimitAheadDistance != 0:
+          required_decel = min(0, (v_speedlimit_ahead*v_speedlimit_ahead - v_ego*v_ego)/(live_map_data.liveMapData.speedLimitAheadDistance*2))
+        required_decel = max(required_decel*0.85, -3.0)
         #print "required_decel"
         #print required_decel
         #print "accel_limits 0"
@@ -230,13 +239,6 @@ def update(self, rcv_times, CS, CP, VM, PP, live20, live100, md, live_map_data):
         #print accel_limits[1]
         accel_limits[0] = min(accel_limits[0], required_decel)
 
-      # Change accel limits based on time remaining to turn
-      if decel_for_turn:
-        time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.))
-        required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn)
-        accel_limits[0] = max(accel_limits[0], required_decel)
-
-
       self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
                                                     v_cruise_setpoint,
                                                     accel_limits[1], accel_limits[0],

selfdrive/mapd/mapd_helpers.py

@@ -223,14 +223,39 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
     way = self
     for i in range(lookahead_ways):
       way_pts = way.points_in_car_frame(lat, lon, heading)
-
+      #print way_pts
       # Check current lookahead distance
-      max_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[-1, :]))
+      if way_pts[0,0] < 0:
+        max_dist = np.linalg.norm(way_pts[-1, :])
+      elif way_pts[-1,0] < 0:
+        max_dist = np.linalg.norm(way_pts[0, :])
+      else:
+        max_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :]))
+
 
       if max_dist > 2 * lookahead:
         #print "max_dist break"
         break
-
+      try:
+        if way.way.tags['junction']=='roundabout':
+          latmin = 181
+          lonmin = 181
+          latmax = -181
+          lonmax = -181
+          for n in way.way.nodes:
+            lonmax = max(n.lon,lonmax)
+            lonmin = min(n.lon,lonmin)
+            latmax = max(n.lat,latmax)
+            latmin = min(n.lat,latmin)
+          a = 111132.954*math.cos(float(latmax+latmin)/360*3.141592)*float(lonmax-lonmin)
+          speed_ahead = np.sqrt(1.6075*a)
+          min_dist = 999.9
+          for w in way_pts:
+            min_dist = min(min_dist, float(np.linalg.norm(w)))
+          speed_ahead_dist = min_dist
+          break
+      except KeyError:
+        pass
       if 'maxspeed' in way.way.tags:
         spd = parse_speed_tags(way.way.tags)
         #print "spd found"
@@ -242,9 +267,10 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
           #print spd
         if spd < current_speed_limit:
           speed_ahead = spd
-          min_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[-1, :]))
+          min_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :]))
           speed_ahead_dist = min_dist
           #print "slower speed found"
+          #print min_dist
 
           break
       # Find next way
@@ -310,7 +336,16 @@ def next_way(self, heading):
     #print self.way.nodes[-1].id
     #print "heading"
     #print heading
-    backwards = abs(heading - math.atan2(self.way.nodes[0].lat-self.way.nodes[-1].lat,self.way.nodes[0].lon-self.way.nodes[-1].lon)*180/3.14159265358979 - 180) < 90
+    angle=heading - math.atan2(self.way.nodes[0].lon-self.way.nodes[-1].lon,self.way.nodes[0].lat-self.way.nodes[-1].lat)*180/3.14159265358979 - 180
+    #print "angle before"
+    #print angle
+    if angle < -180:
+      angle = angle + 360
+    if angle > 180:
+      angle = angle - 360
+    #print "angle"
+    #print angle
+    backwards = abs(angle) > 90
     #print "backwards"
     #print backwards
     if backwards:
@@ -324,12 +359,23 @@ def next_way(self, heading):
     try:
       # Simple heuristic to find next way
       ways = [w for w in ways if w.id != self.id]
+      if len(ways) == 1:
+        way = Way(ways[0], self.query_results)
+        #print "only one way found"
+        return way
       ways = [w for w in ways if (w.nodes[0] == node or w.nodes[-1] == node)]
       if len(ways) == 1:
         way = Way(ways[0], self.query_results)
         #print "only one way found"
         return way
-
+      if len(ways) == 2:
+        try:
+          if ways[0].tags['junction']=='roundabout':
+            #print ("roundabout found")
+            way = Way(ways[0], self.query_results)
+            return way
+        except KeyError:
+          pass
       # Filter on highway tag
       acceptable_tags = list()
       cur_tag = self.way.tags['highway']
@@ -339,7 +385,10 @@ def next_way(self, heading):
         acceptable_tags.append('trunk')
         acceptable_tags.append('primary')
       ways = [w for w in ways if w.tags['highway'] in acceptable_tags]
-
+      if len(ways) == 1:
+        way = Way(ways[0], self.query_results)
+        #print "only one way found"
+        return way
       # Filter on number of lanes
       cur_num_lanes = int(self.way.tags['lanes'])
       if len(ways) > 1: