Merge pull request commaai#345 from ShaneSmiskol/release2

dynamic follow low speed tuning and fix hanging on startup

selfdrive/controls/lib/long_mpc.py

@@ -162,7 +162,7 @@ def get_traffic_level(self, lead_vels):  # generate a value to modify TR by base
     x = [0, len(lead_vels)]
     y = [1.35, 1.0]  # min and max values to modify TR by
     traffic = interp(sum(lead_vel_diffs), x, y)
-    return traffic
+    return float(traffic)
 
   def get_acceleration(self, velocity_list, is_self):  # calculate acceleration to generate more accurate following distances
     a = 0.0
@@ -201,7 +201,7 @@ def dynamic_follow(self, velocity):  # in m/s
 
     if self.stop_and_go:  # this allows a smooth deceleration to a stop, while being able to have smooth stop and go
       x = [stop_and_go_magic_number / 2.0, stop_and_go_magic_number]  # from 10 to 20 mph, ramp 1.8 sng distance to regular dynamic follow value
-      y = [1.8, interp(x[1], x_vel, y_mod)]
+      y = [1.8, interp(stop_and_go_magic_number, x_vel, y_mod)]
       TR = interp(velocity, x, y)
     else:
       TR = interpolate.interp1d(x_vel, y_mod, fill_value='extrapolate')(velocity)[()]  # extrapolate above 90 mph
@@ -219,16 +219,16 @@ def dynamic_follow(self, velocity):  # in m/s
       y = [0.37909, 0.30045, 0.20378, 0.04158, 0, -0.115, -0.195]  # modification values
       TR_mod += interp(self.get_acceleration(self.dynamic_follow_dict["lead_vels"], False), x, y)  # factor in lead car's acceleration; should perform better
 
-      x = [0, 37.6085, 50.3843, 54.6429, 65.3908, 83.0336, 93.1731]  # distance in meters
-      y = [0.95, 1.06, 1.1215, 1.1845, 1.2568, 1.313, 1.34]
+      x = [0, 6.9128, 16.0047, 27.163, 37.6085, 50.3843, 54.6429, 65.3908, 83.0336, 93.1731]  # distance in meters
+      y = [1.0175, 1.0079, 1.0045, 1.0083, 1.0176, 1.0547, 1.0911, 1.1454, 1.1838, 1.195]
       TR_mod *= interp(self.relative_distance, x, y)  # factor in distance from lead car to try and brake quicker
 
-      x = [0, 2.2352, 22.352, 33.528]  # 0, 5, 50, 75 mph
-      y = [.25, 1.0, 1.0, .90]  # multiply sum of all TR modifications by this
-      TR += (float(TR_mod) * interp(velocity, x, y))  # lower TR modification for stop and go, and at higher speeds
-
-      TR = float(TR) * self.get_traffic_level(self.dynamic_follow_dict["traffic_vels"])  # modify TR based on last minute of traffic data
+      x = [1.1594, 2.7298, 6.1562, 10.5105, 22.352, 33.528]  # speed in m/s
+      y = [0.7, 0.885, 1.0, 1.024, 1.0, 0.9]
+      TR_mod *= float(interp(velocity, x, y))  # lower TR modification for stop and go, and at higher speeds
 
+      TR += TR_mod
+      TR *= self.get_traffic_level(self.dynamic_follow_dict["traffic_vels"])  # modify TR based on last minute of traffic data
     if TR < 0.65:
       return 0.65
     else:

selfdrive/kegman_conf.py

@@ -101,7 +101,6 @@ def write_config(conf):  # never to be called outside kegman_conf
       json.dump(conf, f, indent=2, sort_keys=True)
       os.chmod(kegman_file, 0o764)
 
-
 def save(data):  # allows for writing multiple key/value pairs
   global conf
   global thread_counter

selfdrive/manager.py

@@ -353,11 +353,12 @@ def manager_thread():
   while 1:
     # get health of board, log this in "thermal"
     msg = messaging.recv_sock(thermal_sock, wait=True)
-    gps = messaging.recv_one(gps_sock)
-    if 47.3024876979 < gps.gpsLocation.latitude and 54.983104153 > gps.gpsLocation.latitude and gps.gpsLocation.longitude > 5.98865807458 and gps.gpsLocation.longitude < 15.0169958839: 
-      logger_dead = True
-    else:
-      logger_dead = False
+    gps = messaging.recv_one_or_none(gps_sock)
+    if gps:
+      if 47.3024876979 < gps.gpsLocation.latitude < 54.983104153 and 5.98865807458 < gps.gpsLocation.longitude < 15.0169958839:
+        logger_dead = True
+      else:
+        logger_dead = False
     # uploader is gated based on the phone temperature
     if msg.thermal.thermalStatus >= ThermalStatus.yellow:
       kill_managed_process("uploader")