Merge branch 'devel-en' into devel-zht

selfdrive/car/toyota/carstate.py

@@ -183,9 +183,9 @@ class CarState(object):
     self.a_ego = float(v_ego_x[1])
     self.standstill = not v_wheel > 0.001
 
-    if self.CP.carFingerprint in TSS2_CAR or self.CP.carFingerprint == CAR.LEXUS_ISH:
+    if self.CP.carFingerprint in TSS2_CAR:
       self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
-    elif self.CP.carFingerprint in NO_DSU_CAR:
+    elif self.CP.carFingerprint in NO_DSU_CAR or self.CP.carFingerprint == CAR.LEXUS_ISH:
       # cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
       # need to apply an offset as soon as the steering angle measurements are both received
       self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset

selfdrive/controls/lib/curvature_learner.py

@@ -10,7 +10,7 @@ import os
 # The learned curvature offsets will save and load automatically
 # If you still need help, check out how I have it implemented in the devel_curvaturefactorlearner branch
 # by Zorrobyte
-# version 2
+# version 4
 
 class CurvatureLearner:
     def __init__(self, debug=False):
@@ -19,17 +19,15 @@ class CurvatureLearner:
         self.frame = 0
         self.debug = debug
         try:
-            self.learned_offsets = pickle.load(open("/data/curvaturev2.p", "rb"))
-        except (OSError, IOError) as e:
+            self.learned_offsets = pickle.load(open("/data/curvaturev4.p", "rb"))
+        except (OSError, IOError):
             self.learned_offsets = {
                 "center": 0.,
-                "leftinner": 0.,
-                "rightinner": 0.,
-                "leftouter": 0.,
-                "rightouter": 0.
+                "inner": 0.,
+                "outer": 0.
             }
-            pickle.dump(self.learned_offsets, open("/data/curvaturev2.p", "wb"))
-            os.chmod("/data/curvaturev2.p", 0o777)
+            pickle.dump(self.learned_offsets, open("/data/curvaturev4.p", "wb"))
+            os.chmod("/data/curvaturev4.p", 0o777)
 
     def update(self, angle_steers=0., d_poly=None, v_ego=0.):
         if angle_steers > 0.1:
@@ -37,27 +35,27 @@ class CurvatureLearner:
                 self.learned_offsets["center"] -= d_poly[3] / self.learning_rate
                 self.offset = self.learned_offsets["center"]
             elif 2. < abs(angle_steers) < 5.:
-                self.learned_offsets["leftinner"] -= d_poly[3] / self.learning_rate
-                self.offset = self.learned_offsets["leftinner"]
+                self.learned_offsets["inner"] -= d_poly[3] / self.learning_rate
+                self.offset = self.learned_offsets["inner"]
             elif abs(angle_steers) > 5.:
-                self.learned_offsets["leftouter"] -= d_poly[3] / self.learning_rate
-                self.offset = self.learned_offsets["leftouter"]
+                self.learned_offsets["outer"] -= d_poly[3] / self.learning_rate
+                self.offset = self.learned_offsets["outer"]
         elif angle_steers < -0.1:
             if abs(angle_steers) < 2.:
                 self.learned_offsets["center"] += d_poly[3] / self.learning_rate
                 self.offset = self.learned_offsets["center"]
             elif 2. < abs(angle_steers) < 5.:
-                self.learned_offsets["rightinner"] += d_poly[3] / self.learning_rate
-                self.offset = self.learned_offsets["rightinner"]
+                self.learned_offsets["inner"] += d_poly[3] / self.learning_rate
+                self.offset = self.learned_offsets["inner"]
             elif abs(angle_steers) > 5.:
-                self.learned_offsets["rightouter"] += d_poly[3] / self.learning_rate
-                self.offset = self.learned_offsets["rightouter"]
+                self.learned_offsets["outer"] += d_poly[3] / self.learning_rate
+                self.offset = self.learned_offsets["outer"]
 
         self.offset = clip(self.offset, -0.3, 0.3)
         self.frame += 1
 
         if self.frame == 12000:  # every 2 mins
-            pickle.dump(self.learned_offsets, open("/data/curvaturev2.p", "wb"))
+            pickle.dump(self.learned_offsets, open("/data/curvaturev4.p", "wb"))
             self.frame = 0
         if self.debug:
             with open('/data/curvdebug.csv', 'a') as csv_file:

selfdrive/controls/lib/pathplanner.py

@@ -30,7 +30,7 @@ class PathPlanner(object):
     self.setup_mpc(CP.steerRateCost)
     self.solution_invalid_cnt = 0
     self.path_offset_i = 0.0
-    #self.curvature_offset = CurvatureLearner(debug=False)
+    self.curvature_offset = CurvatureLearner(debug=False)
 
   def setup_mpc(self, steer_rate_cost):
     self.libmpc = libmpc_py.libmpc
@@ -60,11 +60,11 @@ class PathPlanner(object):
     # Run MPC
     self.angle_steers_des_prev = self.angle_steers_des_mpc
     VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
-    curvature_factor = VM.curvature_factor(v_ego)
-    #if active:
-    #  curvfac = self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly, v_ego)
-    #else:
-    #  curvfac = 0.
+    #curvature_factor = VM.curvature_factor(v_ego)
+    if active:
+      curvfac = self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly, v_ego)
+    else:
+      curvfac = 0.
 
     #curvature_factor = VM.curvature_factor(v_ego) + curvfac