diff --git a/common/params.cc b/common/params.cc index 6ad1e3d9ee..e9651449e7 100644 --- a/common/params.cc +++ b/common/params.cc @@ -332,6 +332,7 @@ std::unordered_map keys = { {"TorquedOverride", PERSISTENT | BACKUP}, {"ToyotaAutoLockBySpeed", PERSISTENT | BACKUP}, {"ToyotaAutoUnlockByShifter", PERSISTENT | BACKUP}, + {"ToyotaDriveMode", PERSISTENT | BACKUP}, {"ToyotaEnhancedBsm", PERSISTENT | BACKUP}, {"ToyotaSnG", PERSISTENT | BACKUP}, {"ToyotaTSS2Long", PERSISTENT | BACKUP}, diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index e83d05b73b..f15288422e 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,18 +1,19 @@ import copy -from cereal import car +from cereal import car, custom from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import mean from openpilot.common.filter_simple import FirstOrderFilter from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car import DT_CTRL +from openpilot.common.params import Params from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.toyota.values import ToyotaFlags, ToyotaFlagsSP, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR SteerControlType = car.CarParams.SteerControlType - +AccelPersonality = custom.AccelerationPersonality # These steering fault definitions seem to be common across LKA (torque) and LTA (angle): # - high steer rate fault: goes to 21 or 25 for 1 frame, then 9 for 2 seconds # - lka/lta msg drop out: goes to 9 then 11 for a combined total of 2 seconds, then 3. @@ -79,6 +80,13 @@ class CarState(CarStateBase): self._right_blindspot_d2 = 0 self._right_blindspot_counter = 0 + self.signals_checked = False + self.sport_signal_seen = False + self.eco_signal_seen = False + self.accel_profile = None + self.prev_accel_profile = None + self.accel_profile_init = False + self.toyota_drive_mode = Params().get_bool('ToyotaDriveMode') self.frame = 0 def update(self, cp, cp_cam): @@ -176,6 +184,51 @@ class CarState(CarStateBase): ret.leftBlinker = ret.leftBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1 ret.rightBlinker = ret.rightBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2 + if self.toyota_drive_mode: + # Determine sport signal based on car model + sport_signal = 'SPORT_ON_2' if self.CP.carFingerprint in (CAR.TOYOTA_RAV4_TSS2, CAR.LEXUS_ES_TSS2, CAR.TOYOTA_HIGHLANDER_TSS2) else 'SPORT_ON' + + # Check signals once + if not self.signals_checked: + self.signals_checked = True + + # Try to detect sport mode signal, handle missing signal with a fallback + try: + sport_mode = cp.vl["GEAR_PACKET"][sport_signal] + self.sport_signal_seen = True + except KeyError: + sport_mode = 0 + self.sport_signal_seen = False + + # Try to detect eco mode signal, handle missing signal with a fallback + try: + eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON'] + self.eco_signal_seen = True + except KeyError: + eco_mode = 0 + self.eco_signal_seen = False + else: + # Always re-check the signals to account for mode changes + sport_mode = cp.vl["GEAR_PACKET"][sport_signal] if self.sport_signal_seen else 0 + eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON'] if self.eco_signal_seen else 0 + + # Set acceleration profile based on detected modes, with sport mode having higher priority + if sport_mode == 1: + self.accel_profile = AccelPersonality.sport + elif eco_mode == 1: + self.accel_profile = AccelPersonality.eco + else: + self.accel_profile = AccelPersonality.normal + + print(f"Accel profile set to: {self.accel_profile}") + + # If not initialized, sync profile with the current mode on the car + if not self.accel_profile_init or self.accel_profile != self.prev_accel_profile: + Params().put_nonblocking('AccelPersonality', str(self.accel_profile)) + self.accel_profile_init = True + # Update the previous profile to prevent unnecessary re-syncing + self.prev_accel_profile = self.accel_profile + if self.CP.carFingerprint != CAR.TOYOTA_MIRAI: ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"] diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_settings.cc index 50ee8463c1..bbef61845d 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_settings.cc @@ -191,6 +191,14 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge toyotaAutoUnlock->setConfirmation(true, false); addItem(toyotaAutoUnlock); + auto toyotaDriveMode = new ParamControlSP( + "ToyotaDriveMode", + tr("Enable Toyota Drive Mode Button"), + tr("Sunnypilot will link the Acceleration Personality to the car's physical drive mode selector.\nReboot Required."), + "../assets/offroad/icon_blank.png"); + toyotaDriveMode->setConfirmation(true, false); + addItem(toyotaDriveMode); + // Volkswagen addItem(new LabelControlSP(tr("Volkswagen"))); auto volkswagenCCOnly = new ParamControlSP( diff --git a/system/manager/manager.py b/system/manager/manager.py index fde1275a2e..c82b5cfcf3 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -106,6 +106,7 @@ def manager_init() -> None: ("TorqueMaxLatAccel", "250"), ("ToyotaAutoLockBySpeed", "0"), ("ToyotaAutoUnlockByShifter", "0"), + ("ToyotaDriveMode", "0"), ("ToyotaEnhancedBsm", "0"), ("TrueVEgoUi", "0"), ("TurnSpeedControl", "0"),