diff --git a/cereal/log.capnp b/cereal/log.capnp index 311421550..b3698aa74 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2209,4 +2209,5 @@ struct DragonConf { dpIsUpdating @72 :Bool; dpTimebombAssist @73 :Bool; dpDynamicGas @74 :Bool; + dpToyotaLowestCruiseOverrideVego @75 :Bool; } diff --git a/common/dp_conf.py b/common/dp_conf.py index dc3e6e211..b66dcb705 100644 --- a/common/dp_conf.py +++ b/common/dp_conf.py @@ -105,6 +105,7 @@ confs = [ {'name': 'dp_toyota_sng', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}, {'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']}, {'name': 'dp_toyota_zss', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}], 'conf_type': ['param']}, {'name': 'dp_toyota_lowest_cruise_override', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}, {'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']}, + {'name': 'dp_toyota_lowest_cruise_override_vego', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}, {'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']}, {'name': 'dp_toyota_lowest_cruise_override_at', 'default': 44, 'type': 'Float32', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}, {'name': 'dp_toyota_lowest_cruise_override', 'vals': [True]}], 'min': 0, 'max': 255., 'conf_type': ['param', 'struct']}, {'name': 'dp_toyota_lowest_cruise_override_speed', 'default': 32, 'type': 'Float32', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}, {'name': 'dp_toyota_lowest_cruise_override_speed', 'vals': [True]}], 'min': 0, 'max': 255., 'conf_type': ['param', 'struct']}, #misc diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 2b22f9176..cdeddead1 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -11,6 +11,12 @@ from common.params import Params EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): + def __init__(self, CP, CarController, CarState): + super().__init__(CP, CarController, CarState) + + # dp + self.dp_cruise_speed = 0. + @staticmethod def compute_gb(accel, speed): return float(accel) / 3.0 @@ -357,8 +363,16 @@ class CarInterface(CarInterfaceBase): # dp self.dragonconf = dragonconf ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl) - if dragonconf.dpToyotaLowestCruiseOverride and ret.cruiseState.speed < dragonconf.dpToyotaLowestCruiseOverrideAt * CV.KPH_TO_MS: - ret.cruiseState.speed = dragonconf.dpToyotaLowestCruiseOverrideSpeed * CV.KPH_TO_MS + if ret.cruiseState.enabled and dragonconf.dpToyotaLowestCruiseOverride and ret.cruiseState.speed < dragonconf.dpToyotaLowestCruiseOverrideAt * CV.KPH_TO_MS: + if dragonconf.dpToyotaLowestCruiseOverrideVego: + if self.dp_cruise_speed == 0.: + ret.cruiseState.speed = self.dp_cruise_speed = max( dragonconf.dpToyotaLowestCruiseOverrideSpeed * CV.KPH_TO_MS,ret.vEgo) + else: + ret.cruiseState.speed = self.dp_cruise_speed + else: + ret.cruiseState.speed = dragonconf.dpToyotaLowestCruiseOverrideSpeed * CV.KPH_TO_MS + else: + self.dp_cruise_speed = 0. ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False