diff --git a/panda b/panda index 00c2689487..5847d7dbc0 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 00c26894876484d2c5a5b63a1c33c28f0f9b15dd +Subproject commit 5847d7dbc05df3bc76243b6704b9d98544eb53df diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 1f18c3d0ea..aeaaba88e7 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -24,17 +24,18 @@ class CarController: hands_on_fault = CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3 lkas_enabled = CC.latActive and not hands_on_fault - if lkas_enabled: - # Angular rate limit based on speed - apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams) + if self.frame % 2 == 0: + if lkas_enabled: + # Angular rate limit based on speed + apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams) - # To not fault the EPS - apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20) - else: - apply_angle = CS.out.steeringAngleDeg + # To not fault the EPS + apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20) + else: + apply_angle = CS.out.steeringAngleDeg - self.apply_angle_last = apply_angle - can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame)) + self.apply_angle_last = apply_angle + can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, (self.frame // 2) % 16)) # Longitudinal control (in sync with stock message, about 40Hz) if self.CP.openpilotLongitudinalControl: @@ -59,7 +60,7 @@ class CarController: # TODO: HUD control new_actuators = actuators.copy() - new_actuators.steeringAngleDeg = apply_angle + new_actuators.steeringAngleDeg = self.apply_angle_last self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index c84f2c45be..a491c030f8 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -17,12 +17,12 @@ class TeslaCAN: ret += sum(dat) return ret & 0xFF - def create_steering_control(self, angle, enabled, frame): + def create_steering_control(self, angle, enabled, counter): values = { "DAS_steeringAngleRequest": -angle, "DAS_steeringHapticRequest": 0, "DAS_steeringControlType": 1 if enabled else 0, - "DAS_steeringControlCounter": (frame % 16), + "DAS_steeringControlCounter": counter, } data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)[2] diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index bac025a9c5..f79e5f0097 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -103,8 +103,8 @@ BUTTONS = [ ] class CarControllerParams: - ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15]) - ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4]) + ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[10., 1.6, .3]) + ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[10., 7.0, 0.8]) JERK_LIMIT_MAX = 8 JERK_LIMIT_MIN = -8 ACCEL_TO_SPEED_MULTIPLIER = 3