mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 08:14:00 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tesla: half the DAS_steeringControl message rate (#27503)

Browse Source 
* half the freq, double the limit

* fix bug

* bump submodule
old-commit-hash: 7ab9569a0815022ef5abadc6a3641bd93e4a8f6d
This commit is contained in:
Robbe Derks
2023-06-01 23:25:55 +02:00
committed by GitHub
parent edc8384d6f
commit 0db0c890a5
4 changed files with 16 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
panda
+1 -1
Submodule panda updated: 00c2689487...5847d7dbc0
selfdrive/car/tesla/carcontroller.py
+11 -10
View File
@@ -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
selfdrive/car/tesla/teslacan.py
+2 -2
View File
@@ -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]
selfdrive/car/tesla/values.py
+2 -2
View File
@@ -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