untested ...

Gernby · Dec 4, 2018 · dbdc422 · dbdc422
1 parent 46bf0aa
commit dbdc422
Showing 1 changed file with 11 additions and 10 deletions.
diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
@@ -44,11 +44,12 @@ def __init__(self, CP):
     self.steerpub = self.context.socket(zmq.PUB)
     self.steerpub.bind("tcp://*:8594")
     self.steerdata = ""
-    self.smooth_factor = CP.steerActuatorDelay / _DT
+    self.smooth_factor = 2.0 * CP.steerActuatorDelay / _DT
+    self.projection_factor = 5.0 * _DT
     self.feed_forward = 0.0
     self.angle_rate_desired = 0.0
     self.ff_angle_factor = 0.5
-    self.ff_rate_factor = 8.0
+    self.ff_rate_factor = 5.0
     self.accel_limit = 5.0
     self.curvature_factor = 0.0
     self.slip_factor = 0.0
@@ -144,28 +145,28 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly
     else:
       self.angle_steers_des = self.angle_steers_des_prev + self.angle_rate_desired * (cur_time - self.angle_steers_des_time)
       restricted_steer_rate = np.clip(self.angle_steers_des - float(angle_steers) , float(angle_rate) - self.accel_limit, float(angle_rate) + self.accel_limit)
-      future_angle_steers_des = float(angle_steers) + self.smooth_factor * _DT * restricted_steer_rate
-      future_angle_steers = float(angle_steers) + self.smooth_factor * _DT * float(angle_rate)
+      projected_angle_steers_des = self.angle_steers_des + self.projection_factor * restricted_steer_rate
+      projected_angle_steers = float(angle_steers) + self.projection_factor * float(angle_rate)
 
       steers_max = get_steer_max(CP, v_ego)
       self.pid.pos_limit = steers_max
       self.pid.neg_limit = -steers_max
       if CP.steerControlType == car.CarParams.SteerControlType.torque:
-        if abs(self.ff_rate_factor * float(restricted_steer_rate)) > abs(self.ff_angle_factor * float(future_angle_steers_des) - float(angle_offset)) - 0.5 \
+        if abs(self.ff_rate_factor * float(restricted_steer_rate)) > abs(self.ff_angle_factor * float(self.angle_steers_des) - float(angle_offset)) - 0.5 \
             and (abs(float(restricted_steer_rate)) > abs(angle_rate) or (float(restricted_steer_rate) < 0) != (angle_rate < 0)) \
-            and (float(restricted_steer_rate) < 0) == (float(future_angle_steers_des) - float(angle_offset) - 0.5 < 0):
+            and (float(restricted_steer_rate) < 0) == (float(self.angle_steers_des) - float(angle_offset) - 0.5 < 0):
           ff_type = "r"
           self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_rate_factor * v_ego**2 * float(restricted_steer_rate)) / self.smooth_factor  
-        elif abs(future_angle_steers_des - float(angle_offset)) > 0.5:
+        elif abs(self.angle_steers_des - float(angle_offset)) > 0.5:
           ff_type = "a"
-          self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_angle_factor * v_ego**2 * float(apply_deadzone(float(future_angle_steers_des) - float(angle_offset), 0.5))) / self.smooth_factor  
+          self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_angle_factor * v_ego**2 * float(apply_deadzone(float(self.angle_steers_des) - float(angle_offset), 0.5))) / self.smooth_factor  
         else:
           ff_type = "r"
           self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + 0.0) / self.smooth_factor  
       else:
         self.feed_forward = self.angle_steers_des_mpc   # feedforward desired angle
       deadzone = 0.0
-      output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override,
+      output_steer = self.pid.update(projected_angle_steers_des, projected_angle_steers, check_saturation=(v_ego > 10), override=steer_override,
                                      feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone)
 
       self.pCost = 0.0
@@ -186,7 +187,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly
 
       self.steerdata += ("%d,%s,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d|" % (self.isActive, \
       ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.cur_state[0].x, self.cur_state[0].y, self.cur_state[0].psi, self.cur_state[0].delta, self.cur_state[0].t, self.curvature_factor, self.slip_factor ,self.smooth_factor, self.accel_limit, float(restricted_steer_rate) ,self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, steer_motor, float(driver_torque), \
-      self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, future_angle_steers, float(angle_rate), self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \
+      self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, projected_angle_steers, float(angle_rate), self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \
       self.angle_steers_des_mpc, CP.steerRatio, CP.steerKf, CP.steerKpV[0], CP.steerKiV[0], CP.steerRateCost, PL.PP.l_prob, \
       PL.PP.r_prob, PL.PP.c_prob, PL.PP.p_prob, self.l_poly[0], self.l_poly[1], self.l_poly[2], self.l_poly[3], self.r_poly[0], self.r_poly[1], self.r_poly[2], self.r_poly[3], \
       self.p_poly[0], self.p_poly[1], self.p_poly[2], self.p_poly[3], PL.PP.c_poly[0], PL.PP.c_poly[1], PL.PP.c_poly[2], PL.PP.c_poly[3], PL.PP.d_poly[0], PL.PP.d_poly[1], \