Merge pull request #93 from ShaneSmiskol/release2

update phantom with release2

selfdrive/car/chrysler/carstate.py

@@ -266,9 +266,7 @@ def update(self, cp, cp_cam):
     if self.cstm_btns.get_button_status("lka") == 0:
       self.lane_departure_toggle_on = False
     else:
-      self.lane_departure_toggle_on = True
-
-    if self.alcaMode == 3 and (self.left_blinker_on or self.right_blinker_on):
-      self.lane_departure_toggle_on = False
-    else:
-      self.lane_departure_toggle_on = True
+      if self.alcaMode == 3 and (self.left_blinker_on or self.right_blinker_on):
+        self.lane_departure_toggle_on = False
+      else:
+        self.lane_departure_toggle_on = True

selfdrive/car/gm/carstate.py

@@ -242,12 +242,10 @@ def update(self, pt_cp):
     if self.cstm_btns.get_button_status("lka") == 0:
       self.lane_departure_toggle_on = False
     else:
-      self.lane_departure_toggle_on = True
-
-    if self.alcaMode == 3 and (self.left_blinker_on or self.right_blinker_on):
-      self.lane_departure_toggle_on = False
-    else:
-      self.lane_departure_toggle_on = True
+      if self.alcaMode == 3 and (self.left_blinker_on or self.right_blinker_on):
+        self.lane_departure_toggle_on = False
+      else:
+        self.lane_departure_toggle_on = True
 
     if self.car_fingerprint in SUPERCRUISE_CARS:
       self.park_brake = False

selfdrive/car/honda/carstate.py

@@ -470,12 +470,10 @@ def update(self, cp, cp_cam):
     if self.cstm_btns.get_button_status("lka") == 0:
       self.lane_departure_toggle_on = False
     else:
-      self.lane_departure_toggle_on = True
-
-    if self.alcaMode == 3 and (self.left_blinker_on or self.right_blinker_on):
-      self.lane_departure_toggle_on = False
-    else:
-      self.lane_departure_toggle_on = True
+      if self.alcaMode == 3 and (self.left_blinker_on or self.right_blinker_on):
+        self.lane_departure_toggle_on = False
+      else:
+        self.lane_departure_toggle_on = True
 
     # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
     # TODO: this should be ok for all cars. Verify it.

selfdrive/car/honda/values.py

@@ -99,7 +99,7 @@ class CAR:
     57: 3, 145: 8, 316: 8, 340: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 6, 401: 8, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 493: 3, 506: 8, 507: 1, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 829: 5, 882: 2, 884: 7, 888: 8, 891: 8, 892: 8, 923: 2, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8,
   }],
   CAR.CRV_5G: [{
-    57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1093: 4, 1115: 4, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5
+    57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1093: 4, 1115: 4, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1626: 5, 1627: 5, 1633: 8, 1670: 5
   }],
   # 2018 Odyssey w/ Added Comma Pedal Support (512L & 513L)
   CAR.ODYSSEY: [{

selfdrive/car/toyota/carstate.py

@@ -439,11 +439,10 @@ def update(self, cp, cp_cam):
       if self.cstm_btns.get_button_status("lka") == 0:
         self.lane_departure_toggle_on = False
       else:
-        self.lane_departure_toggle_on = True
-    if self.alcaMode == 3 and (self.left_blinker_on or self.right_blinker_on):
-      self.lane_departure_toggle_on = False
-    else:
-      self.lane_departure_toggle_on = True
+        if self.alcaMode == 3 and (self.left_blinker_on or self.right_blinker_on):
+          self.lane_departure_toggle_on = False
+        else:
+          self.lane_departure_toggle_on = True
     self.distance_toggle = cp.vl["JOEL_ID"]['ACC_DISTANCE']
     if cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES'] == 2:
       self.trfix = True
@@ -465,8 +464,6 @@ def update(self, cp, cp_cam):
       if self.read_distance_lines == 3:
         self.UE.custom_alert_message(2,"Following distance set to 2.7s",200,3)
       self.read_distance_lines_prev = self.read_distance_lines
-    if cp.vl["EPS_STATUS"]['LKA_STATE'] == 17:
-      self.cstm_btns.set_button_status("lka", 0)
     if bool(cp.vl["JOEL_ID"]['ACC_SLOW']) <> self.acc_slow_on_prev:
       self.acc_slow_on = bool(cp.vl["JOEL_ID"]['ACC_SLOW'])
       if self.acc_slow_on:

selfdrive/car/toyota/interface.py

@@ -134,7 +134,7 @@ def get_params(candidate, fingerprint):
         ret.longitudinalKiV = [0.18, 0.12]
       else:
         ret.gasMaxV = [0.2, 0.5, 0.7]
-        ret.longitudinalKpV = [1.8, 0.8, 0.6]
+        ret.longitudinalKpV = [1.0, 0.5, 0.3]
         ret.longitudinalKiV = [0.20, 0.10]
     elif candidate == CAR.RAV4_2019:
       stop_and_go = True
@@ -152,11 +152,11 @@ def get_params(candidate, fingerprint):
       stop_and_go = False
       ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
       ret.wheelbase = 2.70
-      ret.steerRatio = 14.8 # 0.5.10
-      tire_stiffness_factor = 0.9443628344466842
+      ret.steerRatio = 17.84 # 0.5.10
+      tire_stiffness_factor = 1.01794
       ret.mass = 2860 * CV.LB_TO_KG + std_cargo  # mean between normal and hybrid
-      ret.steerKpV, ret.steerKiV = [[0.2], [0.125]]
-      ret.steerKf = 0.000038   # full torque for 20 deg at 80mph means 0.00007818594
+      ret.steerKpV, ret.steerKiV = [[0.2], [0.05]]
+      ret.steerKf = 0.00003909297   # full torque for 20 deg at 80mph means 0.00007818594
       if ret.enableGasInterceptor:
         ret.gasMaxV = [0.2, 0.5, 0.7]
         ret.longitudinalKpV = [1.2, 0.8, 0.5]

selfdrive/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.5.10.6-arne182"
+#define COMMA_VERSION "0.5.10.7-arne182"

selfdrive/controls/lib/long_mpc.py

@@ -32,18 +32,20 @@ def __init__(self, mpc_id, live_longitudinal_mpc):
     self.relative_velocity = None
     self.relative_distance = None
     self.stop_and_go = False
+    self.last_rate = None
+    self.new_frame = True
 
   def save_car_data(self, self_vel):
-    if len(self.dynamic_follow_dict["self_vels"]) >= 200:  # 100hz, so 200 items is 2 seconds
+    while len(self.dynamic_follow_dict["self_vels"]) >= self.calc_rate(2):  # 2 seconds
       del self.dynamic_follow_dict["self_vels"][0]
     self.dynamic_follow_dict["self_vels"].append(self_vel)
 
     if self.relative_velocity is not None:
-      if len(self.dynamic_follow_dict["lead_vels"]) >= 300:
+      while len(self.dynamic_follow_dict["lead_vels"]) >= self.calc_rate(3):  # 3 seconds
         del self.dynamic_follow_dict["lead_vels"][0]
       self.dynamic_follow_dict["lead_vels"].append(self_vel + self.relative_velocity)
 
-      if self.mpc_frame >= 50:  # add to traffic list every half second so we're not working with a huge list
+      if self.mpc_frame >= self.calc_rate(0.5):  # add to traffic list every half second so we're not working with a huge list
         if len(self.dynamic_follow_dict["traffic_vels"]) >= 360:  # 360 half seconds is 3 minutes of traffic logging
           del self.dynamic_follow_dict["traffic_vels"][0]
         self.dynamic_follow_dict["traffic_vels"].append(self_vel + self.relative_velocity)
@@ -53,6 +55,20 @@ def save_car_data(self, self_vel):
     else:  # if no car, reset lead car list; ignore for traffic
       self.dynamic_follow_dict["lead_vels"] = []
 
+  def calc_rate(self, seconds=1.0):  # return current rate of long_mpc in fps/hertz
+    current_time = time.time()
+    if self.last_rate is None or (current_time - self.last_rate) == 0:
+      rate = int(round(40.42 * seconds))
+    else:
+      rate = (1.0 / (current_time - self.last_rate)) * seconds
+
+    min_return = 20
+    max_return = seconds * 100
+    if self.new_frame:
+      self.last_rate = current_time
+      self.new_frame = False
+    return int(round(max(min(rate, max_return), min_return)))  # ensure we return a value between range, in hertz
+
   def calculate_tr(self, v_ego, car_state):
     """
     Returns a follow time gap in seconds based on car state values
@@ -86,6 +102,7 @@ def calculate_tr(self, v_ego, car_state):
       return 0.9  # 10m at 40km/hr
 
     if read_distance_lines == 2:
+      self.new_frame = True  # for rate calculation so it doesn't update time multiple times a frame
       self.save_car_data(v_ego)
       generatedTR = self.dynamic_follow(v_ego)
       generated_cost = self.generate_cost(generatedTR, v_ego)
@@ -148,21 +165,26 @@ def get_traffic_level(self, lead_vels):  # generate a value to modify TR by base
     return traffic
 
   def get_acceleration(self, velocity_list, is_self):  # calculate acceleration to generate more accurate following distances
+    a = 0.0
     if is_self:
-      a = (velocity_list[-1] - velocity_list[0]) / (len(velocity_list) / 100.0)
+      if sum(velocity_list) != 0:
+        a = (velocity_list[-1] - velocity_list[0]) / (len(velocity_list) / float(self.calc_rate(1)))
     else:
-      if len(velocity_list) >= 300:
-        a_short = (velocity_list[-1] - velocity_list[-150]) / 1.5  # calculate lead accel last 1.5 s
-        a_long = (velocity_list[-1] - velocity_list[-300]) / 3.0  # divide difference in velocity by how long in sec we're tracking velocity
+      if len(velocity_list) >= self.calc_rate(3) and sum(velocity_list) != 0:
+        a_short = (velocity_list[-1] - velocity_list[-self.calc_rate(1.5)]) / 1.5  # calculate lead accel last 1.5 s
+        a_long = (velocity_list[-1] - velocity_list[-self.calc_rate(3)]) / 3.0  # divide difference in velocity by how long in sec we're tracking velocity
 
         if abs(sum([a_short, a_long])) < 0.22352:  # if abs(sum) is less than .5 mph/s, average the two
           a = (a_short + a_long) / 2.0
         elif sum([a_short, a_long]) >= 0:  # return value furthest from 0
           a = max([a_short, a_long])
         else:
           a = min([a_short, a_long])
+      elif len(velocity_list) >= self.calc_rate(1.5) and sum(velocity_list) != 0:
+        a = (velocity_list[-1] - velocity_list[-self.calc_rate(1.5)]) / 1.5  # calculate lead accel last 1.5 s
       else:
-        a = (velocity_list[-1] - velocity_list[0]) / (len(velocity_list) / 100.0)
+        if sum(velocity_list) != 0:
+          a = (velocity_list[-1] - velocity_list[0]) / (len(velocity_list) / float(self.calc_rate(1)))
 
     return a
 

selfdrive/controls/lib/planner.py

@@ -26,7 +26,7 @@
 
 # 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  = [-0.7, -0.6125, -0.525, -0.4375, -0.285]
+_A_CRUISE_MIN_V  = [-.8, -.7, -.6, -.5, -.3]
 _A_CRUISE_MIN_BP = [0.0, 5.0, 10.0, 20.0, 55.0]
 
 # need fast accel at very low speed for stop and go
@@ -180,13 +180,13 @@ def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data):
       accel_limits = map(float, calc_cruise_accel_limits(v_ego, following, gasbuttonstatus))
       if gasbuttonstatus == 0:
         accellimitmaxdynamic = -0.0018*v_ego+0.2
-        jerk_limits = [min(-0.115, accel_limits[0]), max(accellimitmaxdynamic, accel_limits[1])]  # dynamic
+        jerk_limits = [min(-0.1, accel_limits[0] * 0.5), max(accellimitmaxdynamic, accel_limits[1])]  # dynamic
       elif gasbuttonstatus == 1:
         accellimitmaxsport = -0.002*v_ego+0.4
         jerk_limits = [min(-0.25, accel_limits[0]), max(accellimitmaxsport, accel_limits[1])]  # sport
       elif gasbuttonstatus == 2:
         accellimitmaxeco = -0.0015*v_ego+0.1
-        jerk_limits = [min(-0.1, accel_limits[0]), max(accellimitmaxeco, accel_limits[1])]  # eco
+        jerk_limits = [min(-0.1, accel_limits[0] * 0.5), max(accellimitmaxeco, accel_limits[1])]  # eco
 
       if not CS.carState.leftBlinker and not CS.carState.rightBlinker:
         steering_angle = CS.carState.steeringAngle