mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-03 12:32:06 +08:00
Chrysler: clean up CarController (#24337)
* clean up * max range is 15 * use wheel buttons counter and clean up * no * try spamming 10 times with next counter value * try 50 hz * this is dead code * revert behavior changes, just clean up * revert more * also that * no multiline can creation * sort imports old-commit-hash: 3f0b06b16e638036adb7fe5110b6b5c38d30f1fe
This commit is contained in:
@@ -1,16 +1,16 @@
|
||||
from cereal import car
|
||||
from selfdrive.car import apply_toyota_steer_torque_limits
|
||||
from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \
|
||||
create_wheel_buttons
|
||||
from selfdrive.car.chrysler.values import CAR, CarControllerParams
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car import apply_toyota_steer_torque_limits
|
||||
from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, create_wheel_buttons
|
||||
from selfdrive.car.chrysler.values import CAR, CarControllerParams
|
||||
|
||||
class CarController():
|
||||
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.ccframe = 0
|
||||
self.prev_frame = -1
|
||||
self.frame = 0
|
||||
self.prev_lkas_frame = -1
|
||||
self.hud_count = 0
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.gone_fast_yet = False
|
||||
@@ -18,12 +18,13 @@ class CarController():
|
||||
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
|
||||
def update(self, CC, CS):
|
||||
# this seems needed to avoid steering faults and to force the sync with the EPS counter
|
||||
frame = CS.lkas_counter
|
||||
if self.prev_frame == frame:
|
||||
if self.prev_lkas_frame == CS.lkas_counter:
|
||||
return car.CarControl.Actuators.new_message(), []
|
||||
|
||||
actuators = CC.actuators
|
||||
|
||||
# steer torque
|
||||
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
|
||||
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last,
|
||||
@@ -36,7 +37,7 @@ class CarController():
|
||||
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019):
|
||||
if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0):
|
||||
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
|
||||
lkas_active = moving_fast and enabled
|
||||
lkas_active = moving_fast and CC.enabled
|
||||
|
||||
if not lkas_active:
|
||||
apply_steer = 0
|
||||
@@ -45,28 +46,24 @@ class CarController():
|
||||
|
||||
can_sends = []
|
||||
|
||||
#*** control msgs ***
|
||||
# *** control msgs ***
|
||||
|
||||
if pcm_cancel_cmd:
|
||||
if CC.cruiseControl.cancel:
|
||||
# TODO: would be better to start from frame_2b3
|
||||
new_msg = create_wheel_buttons(self.packer, self.ccframe, cancel=True)
|
||||
can_sends.append(new_msg)
|
||||
can_sends.append(create_wheel_buttons(self.packer, self.frame, cancel=True))
|
||||
|
||||
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
|
||||
# frame is 100Hz (0.01s period)
|
||||
if (self.ccframe % 25 == 0): # 0.25s period
|
||||
if (CS.lkas_car_model != -1):
|
||||
new_msg = create_lkas_hud(
|
||||
self.packer, CS.out.gearShifter, lkas_active, hud_alert,
|
||||
self.hud_count, CS.lkas_car_model)
|
||||
can_sends.append(new_msg)
|
||||
if self.frame % 25 == 0: # 0.25s period
|
||||
if CS.lkas_car_model != -1:
|
||||
can_sends.append(create_lkas_hud(self.packer, CS.out.gearShifter, lkas_active,
|
||||
CC.hudControl.visualAlert, self.hud_count, CS.lkas_car_model))
|
||||
self.hud_count += 1
|
||||
|
||||
new_msg = create_lkas_command(self.packer, int(apply_steer), self.gone_fast_yet, frame)
|
||||
can_sends.append(new_msg)
|
||||
can_sends.append(create_lkas_command(self.packer, int(apply_steer), self.gone_fast_yet, CS.lkas_counter))
|
||||
|
||||
self.ccframe += 1
|
||||
self.prev_frame = frame
|
||||
self.frame += 1
|
||||
self.prev_lkas_frame = CS.lkas_counter
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
|
||||
|
||||
@@ -52,6 +52,6 @@ def create_wheel_buttons(packer, frame, cancel=False):
|
||||
# WHEEL_BUTTONS (571) Message sent to cancel ACC.
|
||||
values = {
|
||||
"ACC_CANCEL": cancel,
|
||||
"COUNTER": frame % 10
|
||||
"COUNTER": frame % 10 # FIXME: this counter is wrong
|
||||
}
|
||||
return packer.make_can_msg("WHEEL_BUTTONS", 0, values)
|
||||
|
||||
@@ -65,8 +65,4 @@ class CarInterface(CarInterfaceBase):
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
|
||||
if (self.CS.frame == -1):
|
||||
return car.CarControl.Actuators.new_message(), [] # if we haven't seen a frame 220, then do not update.
|
||||
|
||||
return self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
|
||||
return self.CC.update(c, self.CS)
|
||||
|
||||
Reference in New Issue
Block a user