diff --git a/selfdrive/controls/lib/disturbance_controller.py b/selfdrive/controls/lib/disturbance_controller.py index c52a0f568..a37850960 100644 --- a/selfdrive/controls/lib/disturbance_controller.py +++ b/selfdrive/controls/lib/disturbance_controller.py @@ -15,15 +15,15 @@ class DisturbanceController: self.alpha_prev = ALPHA_MIN self.desired_curvature_prev = 0.0 self.pid = PIDController(1, 0, k_f=0, pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE) - self.reaction_hist = deque([0.0], maxlen=int(round(CP.steerActuatorDelay / DT_CTRL))+1) + #self.reaction_hist = deque([0.0], maxlen=int(round(CP.steerActuatorDelay / DT_CTRL))+1) def reset(self): self.lowpass_filtered = 0.0 self.alpha_prev = ALPHA_MIN self.desired_curvature_prev = 0.0 self.pid.reset() - self.reaction_hist.clear() - self.reaction_hist.append(0.0) + #self.reaction_hist.clear() + #self.reaction_hist.append(0.0) def compute_dynamic_alpha(self, desired_curvature, dt=DT_CTRL, A=0.02, n=2.0, beta=3.0, k=2.0): d_desired = abs(desired_curvature - self.desired_curvature_prev) / dt @@ -49,20 +49,20 @@ class DisturbanceController: if calibrated_pose is None or CS.vEgo < 0.1: return desired_curvature - #steering_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg) - #actual_curvature = -VM.calc_curvature_3dof(calibrated_pose.acceleration.y, calibrated_pose.acceleration.x, - # calibrated_pose.angular_velocity.yaw, CS.vEgo, steering_angle_without_offset, - # 0.) + steering_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg) + actual_curvature = -VM.calc_curvature_3dof(calibrated_pose.acceleration.y, calibrated_pose.acceleration.x, + calibrated_pose.angular_velocity.yaw, CS.vEgo, steering_angle_without_offset, + 0.) - actual_curvature = calibrated_pose.acceleration.y / (CS.vEgo ** 2) + #actual_curvature = calibrated_pose.acceleration.y / (CS.vEgo ** 2) alpha = self.compute_dynamic_alpha(desired_curvature) reaction = self.lowpass_filter(actual_curvature, alpha) - self.reaction_hist.append(reaction) + #self.reaction_hist.append(reaction) disturbance = self.highpass_filter(actual_curvature, reaction) - reaction_delayed = self.reaction_hist[0] - error = desired_curvature - (reaction_delayed + disturbance) + #reaction_delayed = self.reaction_hist[0] + error = desired_curvature - disturbance #(reaction_delayed + disturbance) output_curvature = self.pid.update(error, feedforward=desired_curvature, speed=CS.vEgo)