working awesome!

daehahn · Mar 27, 2019 · ecb67bd · ecb67bd
1 parent 78890d6
commit ecb67bd
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 13 deletions.
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
@@ -343,7 +343,6 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
     "vEgoRaw": CS.vEgoRaw,
     "angleSteers": CS.steeringAngle,
     "dampAngleSteers": float(LaC.dampened_angle_steers),
-    "dampAngleRateDes": float(LaC.dampened_desired_rate),
     "curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo),
     "steerOverride": CS.steeringPressed,
     "state": state,

diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
@@ -52,7 +52,6 @@ def __init__(self, CP):
     self.desired_smoothing = max(1.0, CP.steerMPCDampTime / _DT)
     self.dampened_angle_steers = 0.0
     self.dampened_desired_angle = 0.0
-    self.dampened_desired_rate = 0.0
     self.steer_counter = 1.0
     self.steer_counter_prev = 0.0
     self.rough_steers_rate = 0.0
@@ -115,19 +114,15 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM
       self.pid.reset()
       self.dampened_angle_steers = float(angle_steers)
       self.dampened_desired_angle = float(angle_steers)
-      self.dampened_desired_rate = 0.0
     else:
 
       if self.gernbySteer == False:
         self.dampened_angle_steers = float(angle_steers)
         self.dampened_desired_angle = float(path_plan.angleSteers)
-        self.dampened_desired_rate = 0.0
 
       else:
         projected_desired_angle = interp(sec_since_boot() + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcAngles)
-        projected_desired_rate = interp(sec_since_boot(), path_plan.mpcTimes, path_plan.mpcRates)
         self.dampened_desired_angle = (((self.desired_smoothing - 1.) * self.dampened_desired_angle) + projected_desired_angle) / self.desired_smoothing
-        self.dampened_desired_rate = (((self.desired_smoothing - 1.) * self.dampened_desired_rate) + projected_desired_rate) / self.desired_smoothing
 
         projected_angle_steers = float(angle_steers) + self.total_actual_projection * float(angle_rate)
         if not steer_override:
@@ -140,12 +135,8 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM
         self.pid.neg_limit = -steers_max
         deadzone = 0.0
 
-        if abs(self.dampened_desired_rate) > abs((self.dampened_desired_angle - path_plan.angleOffset)):
-          steer_feedforward = v_ego**2 * (self.dampened_desired_rate)
-        else:
-          steer_feedforward = v_ego**2 * (self.dampened_desired_angle - path_plan.angleOffset)
-
-        print("sr %1.3f ff  %1.3f offset  %1.3f  damp_des_angle  %1.3f  damp_des_rate %1.3f " % ( VM.sR, steer_feedforward, path_plan.angleOffset, self.dampened_desired_angle, self.dampened_desired_rate))
+        steer_feedforward = (self.dampened_desired_angle - path_plan.angleOffset) * v_ego**2
+        #print("sr %1.3f ff  %1.3f offset  %1.3f  damp_des_angle  %1.3f  damp_des_rate %1.3f " % ( VM.sR, steer_feedforward, path_plan.angleOffset, self.dampened_desired_angle))
 
         output_steer = self.pid.update(self.dampened_desired_angle, self.dampened_angle_steers, check_saturation=(v_ego > 10),
                                       override=steer_override, feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)

diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
@@ -89,7 +89,7 @@ def update(self, CP, VM, CS, md, live100, live_parameters):
       delta_desired = self.mpc_solution[0].delta[1]
       rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
     else:
-      print(CP.steerRateCost, VM.sR)
+      #print(CP.steerRateCost, VM.sR)
       delta_desired = math.radians(angle_steers - angle_offset_bias) / VM.sR
       rate_desired = 0.0