diff --git a/common/params.cc b/common/params.cc index d456cee48..15697f7c7 100644 --- a/common/params.cc +++ b/common/params.cc @@ -272,6 +272,7 @@ std::unordered_map keys = { {"FPSCounter", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"FrogsGoMoo", PERSISTENT}, + {"FrogsGoMooTune", PERSISTENT}, {"GoatScream", PERSISTENT}, {"HideAOLStatusBar", PERSISTENT}, {"HideCEMStatusBar", PERSISTENT}, diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index ebcaab114..878d00af2 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -7,7 +7,7 @@ from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.toyota import toyotacan from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \ - UNSUPPORTED_DSU_CAR + UNSUPPORTED_DSU_CAR, TSS2_CAR from opendbc.can.packer import CANPacker LongCtrlState = car.CarControl.Actuators.LongControlState @@ -33,6 +33,17 @@ MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows COMPENSATORY_CALCULATION_THRESHOLD_V = [-0.3, -0.25, 0.] # m/s^2 COMPENSATORY_CALCULATION_THRESHOLD_BP = [0., 11., 23.] # m/s + +def compute_gb_toyota(accel, speed): + creep_brake = 0.0 + creep_speed = 2.3 + creep_brake_value = 0.15 + if speed < creep_speed: + creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value + gb = accel - creep_brake + return gb + + class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP @@ -55,6 +66,9 @@ class CarController(CarControllerBase): params = Params() self.cydia_tune = params.get_bool("CydiaTune") + self.frogs_go_moo_tune = params.get_bool("FrogsGoMooTune") + + self.pcm_accel_comp = 0 def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators @@ -141,7 +155,7 @@ class CarController(CarControllerBase): self.prohibit_neg_calculation = False # limit minimum to only positive until first positive is reached after engagement, don't calculate when long isn't active - if CC.longActive and not self.prohibit_neg_calculation and self.cydia_tune: + if CC.longActive and not self.prohibit_neg_calculation and (self.cydia_tune or self.frogs_go_moo_tune and self.CP.carFingerprint not in TSS2_CAR): accel_offset = CS.pcm_neutral_force / self.CP.mass else: accel_offset = 0. @@ -149,9 +163,33 @@ class CarController(CarControllerBase): # only calculate pcm_accel_cmd when long is active to prevent disengagement from accelerator depression if CC.longActive: if frogpilot_variables.sport_plus: - pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS) + if self.frogs_go_moo_tune: + wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) + + gas_accel = compute_gb_toyota(actuators.accel, CS.out.vEgo) + wind_brake + self.pcm_accel_comp = clip(gas_accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.03, self.pcm_accel_comp + 0.03) + pcm_accel_cmd = gas_accel + self.pcm_accel_comp + + if not CC.longActive: + pcm_accel_cmd = 0.0 + + pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS) + else: + pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS) else: - pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX) + if self.frogs_go_moo_tune: + wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) + + gas_accel = compute_gb_toyota(actuators.accel, CS.out.vEgo) + wind_brake + self.pcm_accel_comp = clip(gas_accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.03, self.pcm_accel_comp + 0.03) + pcm_accel_cmd = gas_accel + self.pcm_accel_comp + + if not CC.longActive: + pcm_accel_cmd = 0.0 + + pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX) + else: + pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX) else: pcm_accel_cmd = 0. diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 84e3a6980..5d15bde00 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -50,6 +50,8 @@ class CarState(CarStateBase): self.lkas_hud = {} # FrogPilot variables + self.pcm_accel_net = 0.0 + self.pcm_neutral_force = 0.0 def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() @@ -187,6 +189,9 @@ class CarState(CarStateBase): message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"] self.lkas_enabled = any(self.lkas_hud.get(key) == 1 for key in message_keys) + self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"] + self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] + return ret @staticmethod diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 48d5c2317..b17364e54 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -163,6 +163,22 @@ class CarInterface(CarInterfaceBase): tune.kpV = [0.8, 1.] tune.kiBP = [0., 5.] tune.kiV = [0.3, 1.] + elif params.get_bool("FrogsGoMooTune"): + tune.deadzoneBP = [0., 16., 20., 30.] + tune.deadzoneV = [0., .03, .06, .15] + + tune.kpBP = [0., 5., 20.] + tune.kpV = [1.3, 1.0, 0.7] + + # In MPH = [ 0, 27, 45, 60, 89] + tune.kiBP = [ 0., 12., 20., 27., 40.] + tune.kiV = [.35, .215, .195, .10, .01] + + if candidate in TSS2_CAR: + ret.vEgoStopping = 0.15 # car is near 0.1 to 0.2 when car starts requesting stopping accel + ret.vEgoStarting = 0.15 # needs to be > or == vEgoStopping + ret.stopAccel = -0.40 # Toyota requests -0.4 when stopped + ret.stoppingDecelRate = 0.09 # reach stopping target smoothly elif (candidate in TSS2_CAR or ret.enableGasInterceptor) and params.get_bool("DragonPilotTune"): # Credit goes to the DragonPilot team! tune.deadzoneBP = [0., 16., 20., 30.] diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc index 2928e1544..a1518f800 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc @@ -123,6 +123,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil {"StockTune", tr("Stock")}, {"CydiaTune", tr("Cydia")}, {"DragonPilotTune", tr("DragonPilot")}, + {"FrogsGoMooTune", tr("FrogPilot")}, }; toggle = new FrogPilotButtonsParamControl(param, title, desc, icon, tuneOptions);