Merge pull request commaai#464 from arne182/065-clean

Minor updates

panda/VERSION

@@ -1 +1 @@
-v2.4.7
+v1.5.2

panda/board/obj/gitversion.h

@@ -1 +1 @@
-const uint8_t gitversion[] = "v2.4.7-EON-unknown-DEBUG";
+const uint8_t gitversion[] = "v1.5.2-EON-unknown-DEBUG";

selfdrive/car/chrysler/interface.py

@@ -59,7 +59,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
     ret.steerActuatorDelay = 0.1
     ret.steerRateCost = 0.7
 
-    if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019):
+    if candidate in (CAR.JEEP_CHEROKEE_2017, CAR.JEEP_CHEROKEE_2018, CAR.JEEP_CHEROKEE_2019):
       ret.wheelbase = 2.91  # in meters
       ret.steerRatio = 12.7
       ret.steerActuatorDelay = 0.2  # in seconds

selfdrive/car/toyota/carcontroller.py

@@ -44,9 +44,9 @@ def accel_hysteresis(accel, accel_steady, enabled):
     # send 0 when disabled, otherwise acc faults
     accel_steady = 0.
   elif accel > accel_steady + ACCEL_HYST_GAP:
-    accel_steady = accel - ACCEL_HYST_GAP + 0.0111
+    accel_steady = accel - ACCEL_HYST_GAP
   elif accel < accel_steady - ACCEL_HYST_GAP:
-    accel_steady = accel + ACCEL_HYST_GAP + 0.0111
+    accel_steady = accel + ACCEL_HYST_GAP
   accel = accel_steady
 
   return accel, accel_steady

selfdrive/car/toyota/carstate.py

@@ -130,6 +130,7 @@ def __init__(self, CP):
     self.v_cruise_pcmlast = 41
     self.pcm_acc_status = False
     self.setspeedoffset = 34.0
+    self.setspeedcounter = 0
     self.Angles = np.zeros(250)
     self.Angles_later = np.zeros(250)
     self.Angle_counter = 0
@@ -251,12 +252,28 @@ def update(self, cp, cp_cam):
         self.setspeedoffset = 0
         self.v_cruise_pcmlast = self.v_cruise_pcm
     if self.v_cruise_pcm < self.v_cruise_pcmlast:
-      self.setspeedoffset = self.setspeedoffset + math.floor((int((-self.v_cruise_pcm)*34/128  + 169*34/128)-self.setspeedoffset)/(self.v_cruise_pcm-40))
+      if self.setspeedcounter > 0 and self.v_cruise_pcm > 41:
+        self.setspeedoffset = self.setspeedoffset + 4
+      else:
+        if math.floor((int((-self.v_cruise_pcm)*34/128  + 169*34/128)-self.setspeedoffset)/(self.v_cruise_pcm-40)) > 0:
+          self.setspeedoffset = self.setspeedoffset + math.floor((int((-self.v_cruise_pcm)*34/128  + 169*34/128)-self.setspeedoffset)/(self.v_cruise_pcm-40))
+      self.setspeedcounter = 50
     if self.v_cruise_pcmlast < self.v_cruise_pcm:
-      self.setspeedoffset = self.setspeedoffset + math.floor((int((-self.v_cruise_pcm)*34/128  + 169*34/128)-self.setspeedoffset)/(170-self.v_cruise_pcm))
-
+      if self.setspeedcounter > 0 and (self.setspeedoffset - 4) > 0:
+        self.setspeedoffset = self.setspeedoffset - 4
+      else:
+        self.setspeedoffset = self.setspeedoffset + math.floor((int((-self.v_cruise_pcm)*34/128  + 169*34/128)-self.setspeedoffset)/(170-self.v_cruise_pcm))
+      self.setspeedcounter = 50
+    if self.setspeedcounter > 0:
+      self.setspeedcounter = self.setspeedcounter - 1
     self.v_cruise_pcmlast = self.v_cruise_pcm
-    self.v_cruise_pcm = max(7, int(self.v_cruise_pcm) - self.setspeedoffset)
+    if int(self.v_cruise_pcm) - self.setspeedoffset < 7:
+      self.setspeedoffset = int(self.v_cruise_pcm) - 7
+    if int(self.v_cruise_pcm) - self.setspeedoffset > 169:
+      self.setspeedoffset = int(self.v_cruise_pcm) - 169
+
+
+    self.v_cruise_pcm = min(max(7, int(self.v_cruise_pcm) - self.setspeedoffset),169)
 
     if not self.left_blinker_on and not self.right_blinker_on:
       self.Angles[self.Angle_counter] = abs(self.angle_steers)

selfdrive/controls/lib/driver_monitor.py

@@ -7,7 +7,7 @@
 from common.travis_checker import travis
 
 op_params = opParams()
-factor = op_params.get('awareness_factor', 2.0)
+factor = op_params.get('awareness_factor', 10.0)
 
 if travis:
   factor = 1.0

selfdrive/controls/lib/latcontrol_lqr.py

@@ -69,7 +69,7 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste
           self.i_lqr = i
 
       self.output_steer = lqr_output + self.i_lqr
-      self.output_steer = clip(self.output_steer, -steers_max, steers_max)
+      self.output_steer = clip(self.output_steer*0.7, -steers_max, steers_max)
 
     lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
     lqr_log.i = self.i_lqr

selfdrive/controls/lib/long_mpc.py

@@ -136,7 +136,7 @@ def smooth_follow(self):  # in m/s
 
   def get_cost(self, TR):
     x = [.9, 1.8, 2.7]
-    y = [3.5, .8, .3]
+    y = [4.5, 2.8, 1.2]
     if self.x_lead is not None and self.v_ego is not None and self.v_ego != 0:
       real_TR = self.x_lead / float(self.v_ego)  # switched to cost generation using actual distance from lead car; should be safer
       if abs(real_TR - TR) >= .25:  # use real TR if diff is greater than x safety threshold

selfdrive/controls/lib/planner.py

@@ -33,7 +33,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.8, -0.7, -0.6, -0.5, -0.3]
+_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]
 
 # need fast accel at very low speed for stop and go

selfdrive/mapd/mapd.py

@@ -21,7 +21,7 @@
 
 from common.transformations.coordinates import geodetic2ecef
 import selfdrive.mapd.messaging as messaging
-from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points
+from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points, rate_curvature_points
 
 OVERPASS_API_URL = "https://z.overpass-api.de/api/interpreter"
 OVERPASS_API_URL2 = "https://lz4.overpass-api.de/api/interpreter"
@@ -204,7 +204,7 @@ def mapsd_thread():
 
     fix_ok = gps.flags & 1
 
-    if gps.accuracy > 2.0:
+    if gps.accuracy > 2.0 and not speedLimittrafficvalid:
       fix_ok = False
     if not fix_ok or last_query_result is None or not cache_valid:
       cur_way = None
@@ -245,7 +245,7 @@ def mapsd_thread():
         if curvature_valid:
           # Compute the curvature for each point
           with np.errstate(divide='ignore'):
-            circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])]
+            circles = [circle_through_points(*p, direction=True) for p in zip(pnts, pnts[1:], pnts[2:])]
             circles = np.asarray(circles)
             radii = np.nan_to_num(circles[:, 2])
             radii[radii < 15.] = 10000
@@ -256,7 +256,9 @@ def mapsd_thread():
               radii = radii*2.8
 
             curvature = 1. / radii
-
+          rate = [rate_curvature_points(*p) for p in zip(pnts[1:], pnts[2:],curvature[0:],curvature[1:])]
+          rate = ([0] + rate)
+          curvature = np.multiply(np.minimum(np.multiply(rate,4000)+0.7,1.1),curvature)
           # Index of closest point
           closest = np.argmin(np.linalg.norm(pnts, axis=1))
           dist_to_closest = pnts[closest, 0]  # We can use x distance here since it should be close
@@ -275,7 +277,6 @@ def mapsd_thread():
           curvature = curvature[close_idx]
 
           if len(curvature):
-            # TODO: Determine left or right turn
             curvature = np.nan_to_num(curvature)
 
 

selfdrive/mapd/mapd_helpers.py

@@ -19,7 +19,15 @@
 with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f:
   DEFAULT_SPEEDS_BY_REGION = json.loads(f.read())
 
-def circle_through_points(p1, p2, p3, force=False):
+def rate_curvature_points(p2,p3,curvature2,curvature3):
+  x2, y2, _ = p2
+  x3, y3, _ = p3
+  if abs(curvature3) > abs(curvature2):
+    return abs((curvature3-curvature2)/(np.sqrt((x3-x2)**2+(y3-y2)**2)))
+  else:
+    return 0
+
+def circle_through_points(p1, p2, p3, force=False, direction=False):
   """Fits a circle through three points
   Formulas from: http://www.ambrsoft.com/trigocalc/circle3d.htm"""
   x1, y1, _ = p1
@@ -32,7 +40,13 @@ def circle_through_points(p1, p2, p3, force=False):
   D = (x1**2 + y1**2) * (x3 * y2 - x2 * y3) + (x2**2 + y2**2) * (x1 * y3 - x3 * y1) + (x3**2 + y3**2) * (x2 * y1 - x1 * y2)
   try:
     if abs((y3-y1)*x2-(x3-x1)*y2+x3*y1-y3*x1)/np.sqrt((y3-y1)**2+(x3-x1)**2) > 0.1 or force:
-      return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2)))
+      if direction:
+        if (x2-x1)*(y3-y1)-(y2-y1)*(x3-x1)>0:
+          return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2)))
+        else:
+          return (-B / (2 * A), - C / (2 * A), -np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2)))
+      else:
+        return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2)))
     else:
       return (-B / (2 * A), - C / (2 * A), 10000)
   except RuntimeWarning:

selfdrive/test/test_car_models.py

@@ -81,7 +81,7 @@ def get_route_logs(route_name):
   #  'enableCamera': True,
   # },
   "9f7a7e50a51fb9db|2019-01-17--18-34-21": {
-    'carFingerprint': CHRYSLER.JEEP_CHEROKEE,
+    'carFingerprint': CHRYSLER.JEEP_CHEROKEE_2017,
     'enableCamera': True,
   },
   "192a598e34926b1e|2019-04-04--13-27-39": {
@@ -549,4 +549,4 @@ def get_route_logs(route_name):
     print("TEST FAILED")
     sys.exit(1)
   else:
-    print("TEST SUCESSFUL")
+    print("TEST SUCESSFUL")