|
|
|
@ -16,7 +16,7 @@ from system.version import get_short_branch |
|
|
|
|
from selfdrive.boardd.boardd import can_list_to_can_capnp |
|
|
|
|
from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can |
|
|
|
|
from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET |
|
|
|
|
from selfdrive.controls.lib.drive_helpers import V_CRUISE_INITIAL, update_v_cruise, initialize_v_cruise |
|
|
|
|
from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise |
|
|
|
|
from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature |
|
|
|
|
from selfdrive.controls.lib.latcontrol import LatControl |
|
|
|
|
from selfdrive.controls.lib.longcontrol import LongControl |
|
|
|
@ -162,8 +162,8 @@ class Controls: |
|
|
|
|
self.active = False |
|
|
|
|
self.can_rcv_error = False |
|
|
|
|
self.soft_disable_timer = 0 |
|
|
|
|
self.v_cruise_kph = V_CRUISE_INITIAL |
|
|
|
|
self.v_cruise_cluster_kph = V_CRUISE_INITIAL |
|
|
|
|
self.v_cruise_kph = 0 |
|
|
|
|
self.v_cruise_cluster_kph = 0 |
|
|
|
|
self.v_cruise_kph_last = 0 |
|
|
|
|
self.mismatch_counter = 0 |
|
|
|
|
self.cruise_mismatch_counter = 0 |
|
|
|
@ -452,19 +452,16 @@ class Controls: |
|
|
|
|
|
|
|
|
|
self.v_cruise_kph_last = self.v_cruise_kph |
|
|
|
|
|
|
|
|
|
if CS.cruiseState.available: |
|
|
|
|
# if stock cruise is completely disabled, then we can use our own set speed logic |
|
|
|
|
if not self.CP.pcmCruise: |
|
|
|
|
if CS.cruiseState.available: |
|
|
|
|
self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.vEgo, CS.gasPressed, CS.buttonEvents, |
|
|
|
|
self.button_timers, self.enabled, self.is_metric) |
|
|
|
|
self.v_cruise_cluster_kph = self.v_cruise_kph |
|
|
|
|
else: |
|
|
|
|
self.v_cruise_kph = V_CRUISE_INITIAL |
|
|
|
|
self.v_cruise_cluster_kph = V_CRUISE_INITIAL |
|
|
|
|
else: |
|
|
|
|
if CS.cruiseState.available: |
|
|
|
|
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH |
|
|
|
|
self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH |
|
|
|
|
|
|
|
|
|
else: |
|
|
|
|
self.v_cruise_kph = 0 |
|
|
|
|
self.v_cruise_cluster_kph = 0 |
|
|
|
|