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

FrogPilot's Toyota tune

Browse Source
This commit is contained in:
FrogAi
2024-04-01 10:37:08 -07:00
parent a30eaf473b
commit b873bf848a
5 changed files with 65 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params.cc
+1
View File
@@ -272,6 +272,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"FPSCounter", PERSISTENT},
{"FrogPilotTogglesUpdated", PERSISTENT},
{"FrogsGoMoo", PERSISTENT},
{"FrogsGoMooTune", PERSISTENT},
{"GoatScream", PERSISTENT},
{"HideAOLStatusBar", PERSISTENT},
{"HideCEMStatusBar", PERSISTENT},
selfdrive/car/toyota/carcontroller.py
+42 -4
View File
@@ -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.
selfdrive/car/toyota/carstate.py
+5
View File
@@ -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
selfdrive/car/toyota/interface.py
+16
View File
@@ -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.]
selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc
+1
View File
@@ -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);