Merge pull request commaai#214 from ShaneSmiskol/release2

stop and go smoothness v2

selfdrive/controls/controlsd.py

@@ -475,7 +475,7 @@ def controlsd_thread(gctx=None, rate=100):
     with open("/data/params/d/ControlsParams", "r+") as f:
       controls_params = json.loads(f.read())
       if "angle_model_bias" in controls_params and "angle_offset" not in controls_params:
-        controls_params["angle_offset"] = controls_params["angle_model_bias"]
+        controls_params["angle_offset"] = float(controls_params["angle_model_bias"])
         del controls_params["angle_model_bias"]
       f.seek(0)
       f.write(json.dumps(controls_params))

selfdrive/controls/lib/long_mpc.py

@@ -31,6 +31,7 @@ def __init__(self, mpc_id, live_longitudinal_mpc):
     self.mpc_frame = 0  # idea thanks to kegman
     self.relative_velocity = None
     self.relative_distance = None
+    self.stop_and_go = False
     #self.dict_builder = {}
 
   def save_car_data(self, self_vel):
@@ -63,7 +64,7 @@ def calculate_tr(self, v_ego, car_state):
     """
 
     read_distance_lines = car_state.readdistancelines
-    if v_ego < 2.0:  # if under 2m/s
+    if v_ego < 2.0 and read_distance_lines != 2:  # if under 2m/s and not dynamic follow
       return 1.8  # under 7km/hr use a TR of 1.8 seconds
 
     if car_state.leftBlinker or car_state.rightBlinker:  # if car is changing lanes and not already .9s
@@ -163,9 +164,22 @@ def get_acceleration(self, velocity_list, is_self):  # calculate acceleration to
     return a
 
   def dynamic_follow(self, velocity):  # in m/s
-    x = [0.0, 1.86267, 3.72533, 5.588, 7.45067, 9.31333, 11.55978, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocity
-    y = [1.03, 1.05363, 1.07879, 1.11493, 1.16969, 1.25071, 1.36325, 1.43, 1.6, 1.7, 1.75618, 1.85, 2.0]  # distances
-    TR = interpolate.interp1d(x, y, fill_value='extrapolate')(velocity)[()]  # extrapolate above 90 mph
+    x_vel = [0.0, 1.86267, 3.72533, 5.588, 7.45067, 9.31333, 11.55978, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocity
+    y_mod = [1.03, 1.05363, 1.07879, 1.11493, 1.16969, 1.25071, 1.36325, 1.43, 1.6, 1.7, 1.75618, 1.85, 2.0]  # distances
+
+    stop_and_go_magic_number = 4.4704  # 10 mph
+
+    if velocity <= 0.0044704:  # .01 mph
+      self.stop_and_go = True
+    elif velocity >= stop_and_go_magic_number:
+      self.stop_and_go = False
+
+    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 5 to 10 mph, ramp 1.8 sng distance to regular dynamic follow value
+      y = [1.8, interp(x[1], 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
 
     if self.relative_velocity is not None:
       x = [-11.62304, -7.84277, -5.45001, -4.37005, -2.98368, -2.49073, -1.96698, -1.13517, 0.0, 0.12799, 0.77499, 1.85325, 2.68511]  # relative velocity values
@@ -280,4 +294,4 @@ def update(self, CS, lead, v_cruise_setpoint):
       self.cur_state[0].a_ego = 0.0
       self.v_mpc = v_ego
       self.a_mpc = CS.carState.aEgo
-      self.prev_lead_status = False
+      self.prev_lead_status = False