'dis is good'dis is good

selfdrive/controls/lib/latcontrol.py

@@ -59,15 +59,14 @@ def __init__(self, CP):
     self.total_actual_projection = max(0.0, CP.steerReactTime + CP.steerDampTime)
     self.actual_smoothing = max(1.0, CP.steerDampTime / _DT)
     self.desired_smoothing = max(1.0, CP.steerMPCDampTime / _DT)
-    self.ff_angle_factor = 1.0
-    self.ff_rate_factor = 1.0
+    self.ff_angle_factor = 5.0
+    self.ff_rate_factor = 0.1
     self.dampened_desired_angle = 0.0
     self.steer_counter = 1.0
     self.steer_counter_prev = 0.0
     self.rough_steers_rate = 0.0
     self.prev_angle_steers = 0.0
     self.calculate_rate = True
-    self.lane_prob_reset = False
     self.feed_forward = 0.0
     self.rate_mode = 0.0
     self.angle_mode = 0.0
@@ -151,52 +150,19 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM
           projected_angle_steers = float(angle_steers)
         self.dampened_angle_steers += ((projected_angle_steers - self.dampened_angle_steers) / self.actual_smoothing)
 
-      '''if path_plan.laneProb == 0.0 and self.lane_prob_reset == False:
-        if path_plan.lPoly[3] - path_plan.rPoly[3] > 3.9:
-          print(self.dampened_desired_angle, path_plan.angleSteers)
-          #self.dampened_desired_angle = path_plan.angleSteers
-        self.lane_prob_reset = True
-      elif path_plan.laneProb > 0.0:
-        self.lane_prob_reset = False
-      '''
-
       if CP.steerControlType == car.CarParams.SteerControlType.torque:
 
         steers_max = get_steer_max(CP, v_ego)
         self.pid.pos_limit = steers_max
         self.pid.neg_limit = -steers_max
         deadzone = 0.0
 
-        angle_feedforward = apply_deadzone(self.ff_angle_factor * (self.dampened_desired_angle - path_plan.angleOffset), 1.0) * v_ego**2
-        rate_feedforward = self.ff_rate_factor * self.dampened_desired_rate * v_ego**2
-
-        self.dampened_angle_ff += (angle_feedforward - self.dampened_angle_ff) / self.desired_smoothing
-        if angle_feedforward == 0 or (angle_feedforward < 0) == (rate_feedforward < 0):
-          self.dampened_rate_ff += (rate_feedforward - self.dampened_rate_ff) / self.desired_smoothing
-        else:
-          self.dampened_rate_ff += (0.0 - self.dampened_rate_ff) / self.desired_smoothing
-
-        feed_forward = self.dampened_angle_ff + self.dampened_rate_ff
-        if abs(feed_forward) > 0:
-          self.angle_mode = max(0.0, min(1.0, self.dampened_angle_ff / feed_forward))
-          self.rate_mode = 1.0 - self.angle_mode
-        else:
-          self.angle_mode = 0.0
-          self.rate_mode = 0.0
-
-        '''rate_more_significant = (abs(rate_feedforward) > abs(angle_feedforward))
-        rate_same_direction = (rate_feedforward > 0) == (angle_feedforward > 0)
-
-        if rate_more_significant and rate_same_direction: # and rate_away_from_zero:
-          self.feed_forward += ((rate_feedforward - self.feed_forward) / self.desired_smoothing)
-          self.rate_mode = 1
-          self.angle_mode = 0
-          #print(self.feed_forward)
-        else:
-          self.rate_mode = 0
-          self.angle_mode = 1
-          self.feed_forward += ((angle_feedforward - self.feed_forward) / self.desired_smoothing)
-        '''
+        ff_target_angle = self.dampened_desired_angle - path_plan.angleOffset
+        self.angle_mode = interp(abs(ff_target_angle), [1.5, 6.0], [0.0, 1.0])
+        self.rate_mode = 1.0 - self.angle_mode
+        angle_feedforward = self.angle_mode * self.ff_angle_factor * ff_target_angle
+        rate_feedforward = self.rate_mode * self.ff_rate_factor * self.dampened_desired_rate
+        feed_forward = v_ego**2 * (rate_feedforward + angle_feedforward)
 
         output_steer = self.pid.update(self.dampened_desired_angle, self.dampened_angle_steers, check_saturation=(v_ego > 10),
                                       override=steer_override, feedforward=feed_forward, speed=v_ego, deadzone=deadzone)

selfdrive/controls/lib/pathplanner.py

@@ -86,15 +86,12 @@ def update(self, CP, VM, CS, md, live100, live_parameters):
       for i in range(1,20):
         self.mpc_times[i] = self.mpc_times[i-1] + _DT_MPC
         self.mpc_angles[i] = float(math.degrees(self.mpc_solution[0].delta[i] * VM.sR) + angle_offset_bias)
-        self.mpc_rates[i] = (self.mpc_angles[i] - self.mpc_angles[i-1]) / (self.mpc_times[i] - self.mpc_times[i-1])
+        self.mpc_rates[i-1] = (self.mpc_angles[i] - self.mpc_angles[i-1]) / (self.mpc_times[i] - self.mpc_times[i-1])
         #self.mpc_rates[i] = math.degrees(self.mpc_solution[0].rate[i] * VM.sR)
 
       delta_desired = self.mpc_solution[0].delta[1]
       rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
-      rate_desired2 = math.degrees(self.mpc_solution[0].rate[1] * VM.sR)
-      print("%1.2f  %1.2f  %1.2f  %1.2f  %1.2f  %1.2f  " % (rate_desired, self.mpc_rates[1], rate_desired - self.mpc_rates[1], rate_desired2, self.mpc_rates[2], rate_desired2 - self.mpc_rates[2] ))
     else:
-      #print(CP.steerRateCost, VM.sR)
       delta_desired = math.radians(angle_steers - angle_offset_bias) / VM.sR
       rate_desired = 0.0