066 clean

README_arne182.md

@@ -8,7 +8,6 @@ For a demo of this version of openpilot check the video below:
 [![demo of openpilot with this branch](https://img.youtube.com/vi/WKwSq8TPdpo/0.jpg)](https://www.youtube.com/watch?v=WKwSq8TPdpo)
 
 # Installation
-
 ## Panda flashing
 
 To get this branch to work, it is required to flash your Panda because:

selfdrive/car/toyota/interface.py

@@ -286,7 +286,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
     ret.longitudinalTuning.kpBP = [0., 5., 35.]
     ret.longitudinalTuning.kiBP = [0., 35.]
     ret.stoppingControl = False
-    ret.startAccel = 0.0
+    ret.startAccel = 0.01
 
     return ret
 

selfdrive/controls/controlsd.py

@@ -347,7 +347,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
                                               CS.cruiseState.standstill,
                                               v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP,
                                               gas_button_status, plan.decelForTurn, plan.longitudinalPlanSource,
-                                              radar_state.leadOne, CS.gasPressed, plan.fcw)
+                                              radar_state.leadOne, CS.gasPressed, plan.fcw, plan.hasLead)
   # Steering PID loop and lateral MPC
   actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, path_plan)
 

selfdrive/controls/lib/long_mpc.py

@@ -124,11 +124,12 @@ def dynamic_follow(self):  # in m/s
 
     if self.lead_data['v_lead'] is not None:  # if lead
       x = [-15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68]  # relative velocity values
-      y = [.504, 0.34, 0.29, 0.25, 0.22, 0.19, 0.13, 0.053, 0.017, 0, -0.015, -0.042, -0.108, -0.163]  # modification values
+      y = [.504, 0.34, 0.29, 0.25, 0.22, 0.15, 0.0, 0.0, 0.0, 0.0, -0.015, -0.042, -0.108, -0.163]  # modification values
       TR_mod = interp(self.lead_data['v_lead'] - self.v_ego, x, y)
 
       x = [-2.235, -1.49, -1.1, -0.67, -0.224, 0.0, 0.67, 1.1, 1.49]  # lead acceleration values
-      y = [0.26, 0.182, 0.104, 0.052, 0.039, 0.0, -0.016, -0.032, -0.056]  # modification values
+      y = [0.26, 0.182, 0.104, 0.0, 0.0, 0.0, -0.016, -0.032, -0.056]  # modification values
+
       TR_mod += interp(self.lead_data['a_lead'], x, y)
       # TR_mod += interp(self.get_acceleration(), x, y)  # todo: when lead car has been braking over the past 3 seconds, slightly increase TR
 

selfdrive/controls/lib/longcontrol.py

@@ -116,18 +116,19 @@ def dynamic_gas(self, v_ego, gas_interceptor, gas_button_status):
     max_return = 1.0
     return round(max(min(accel, max_return), min_return), 5)  # ensure we return a value between range
 
-  def process_lead(self, lead_one):
+  def process_lead(self, lead_one,has_lead):
     self.lead_data['vRel'] = lead_one.vRel
     self.lead_data['a_lead'] = lead_one.aLeadK
     self.lead_data['x_lead'] = lead_one.dRel
-    self.lead_data['status'] = lead_one.status
+    self.lead_data['status'] = has_lead
+
 
 
   def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP,
-             gas_button_status, decelForTurn, longitudinalPlanSource, lead_one, gas_pressed, fcw):
+             gas_button_status, decelForTurn, longitudinalPlanSource, lead_one, gas_pressed, fcw, has_lead):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
     # Actuation limits
-    self.process_lead(lead_one)
+    self.process_lead(lead_one,has_lead)
     try:
       gas_interceptor = CP.enableGasInterceptor
     except AttributeError:
@@ -189,7 +190,10 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
     elif self.long_control_state == LongCtrlState.stopping:
       # Keep applying brakes until the car is stopped
       if not standstill or output_gb > -BRAKE_STOPPING_TARGET:
-        output_gb -= STOPPING_BRAKE_RATE / RATE
+        factor = 1
+        if self.lead_data['status']:
+          factor = interp(self.lead_data['x_lead'], [0.0,1.0,3.0,5.0], [1000.0,100.0,10.0,1.0])
+        output_gb -= STOPPING_BRAKE_RATE / RATE * factor
       output_gb = clip(output_gb, -brake_max, gas_max)
 
       self.v_pid = v_ego
@@ -198,7 +202,10 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
     # Intention is to move again, release brake fast before handing control to PID
     elif self.long_control_state == LongCtrlState.starting:
       if output_gb < -0.2:
-        output_gb += STARTING_BRAKE_RATE / RATE
+        factor = 1
+        if self.lead_data['status']:
+          factor = interp(self.lead_data['x_lead'], [0.0,2.0,4.0], [0.0,0.5,1.0])
+        output_gb += STARTING_BRAKE_RATE / RATE * factor
       self.v_pid = v_ego
       self.pid.reset()
 

selfdrive/controls/lib/planner.py

@@ -31,6 +31,8 @@
 
 # lookup tables VS speed to determine min and max accels in cruise
 # make sure these accelerations are smaller than mpc limits
+_A_CRUISE_MIN_V_ECO = [-1.0, -1.5, -1.0, -0.3, -0.1]
+_A_CRUISE_MIN_V_SPORT = [-3.0, -3.5, -4.0, -4.0, -4.0]
 _A_CRUISE_MIN_V_FOLLOWING = [-4.0, -3.5, -3.0, -2.5, -2.0]
 _A_CRUISE_MIN_V = [-1.6, -0.7, -0.6, -0.5, -0.3]
 _A_CRUISE_MIN_BP = [0.0, 5.0, 10.0, 20.0, 55.0]
@@ -62,8 +64,13 @@ def calc_cruise_accel_limits(v_ego, following, gas_button_status):
   if following:
     a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_FOLLOWING)
   else:
-    a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
-
+    if gas_button_status == 1:
+      a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_SPORT)
+    elif gas_button_status == 2:
+      a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_ECO)
+    else:
+      a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
+
   if following:
     a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING)
   else:

selfdrive/controls/lib/speed_smoother.py

@@ -18,9 +18,9 @@ def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts):
 
   # recover quickly if dV is positive and aEgo is negative or viceversa
   if dV > 0. and aEgo < 0.:
-    jMax *= 3.
+    jMax *= 10.
   elif dV < 0. and aEgo > 0.:
-    jMin *= 3.
+    jMin *= 5.
 
   tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin)
 

selfdrive/mapd/mapd_helpers.py

@@ -428,7 +428,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
               if backwards and (n.tags['traffic_signals:direction']=='backward' or n.tags['traffic_signals:direction']=='both'):
                 print("backward")
                 if way_pts[count, 0] > 0:
-                  speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0)
+                  speed_ahead_dist = max(0. , way_pts[count, 0] - 4.0)
                   print(speed_ahead_dist)
                   speed_ahead = 5/3.6
                   if n.tags['highway']=='traffic_signals':
@@ -438,7 +438,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
               elif not backwards and (n.tags['traffic_signals:direction']=='forward' or n.tags['traffic_signals:direction']=='both'):
                 print("forward")
                 if way_pts[count, 0] > 0:
-                  speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0)
+                  speed_ahead_dist = max(0. , way_pts[count, 0] - 4.0)
                   print(speed_ahead_dist)
                   speed_ahead = 5/3.6
                   if n.tags['highway']=='traffic_signals':
@@ -454,7 +454,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
                   if direction > 180:
                     direction = direction - 360
                   if abs(direction) > 135:
-                    speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0)
+                    speed_ahead_dist = max(0. , way_pts[count, 0] - 4.0)
                     print(speed_ahead_dist)
                     speed_ahead = 5/3.6
                     if n.tags['highway']=='traffic_signals':