diff --git a/README.md b/README.md index bdd695bd3c2a3b..f05c7886eb5d25 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ FrogPilot is a fully open-sourced fork of openpilot, featuring clear and concise ------ FrogPilot was last updated on: -**November 10th, 2024** +**November 23rd, 2024** Features ------ diff --git a/SConstruct b/SConstruct index 747cd09dd8a5fd..944d650d742d99 100644 --- a/SConstruct +++ b/SConstruct @@ -277,7 +277,6 @@ Export('envCython') # Qt build environment qt_env = env.Clone() - qt_modules = ["Widgets", "Gui", "Core", "Network", "Concurrent", "Multimedia", "Quick", "Qml", "QuickWidgets", "Location", "Positioning", "DBus", "Xml"] qt_libs = [] diff --git a/common/params.cc b/common/params.cc index bb50721f5a9d7a..da912794150e51 100644 --- a/common/params.cc +++ b/common/params.cc @@ -253,9 +253,7 @@ std::unordered_map keys = { {"CENavigationIntersections", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CENavigationLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CENavigationTurns", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"CertifiedHerbalistCalibrationParams", PERSISTENT}, {"CertifiedHerbalistDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"CertifiedHerbalistLiveTorqueParameters", PERSISTENT}, {"CertifiedHerbalistScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CESignalSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CESignalLaneDetection", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -294,9 +292,7 @@ std::unordered_map keys = { {"DisableCurveSpeedSmoothing", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DisableOnroadUploads", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DisableOpenpilotLongitudinal", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, - {"DissolvedOxygenCalibrationParams", PERSISTENT}, {"DissolvedOxygenDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"DissolvedOxygenLiveTorqueParameters", PERSISTENT}, {"DissolvedOxygenScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DistanceIconToDownload", PERSISTENT}, {"DisengageVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -308,11 +304,11 @@ std::unordered_map keys = { {"DownloadableSounds", PERSISTENT}, {"DownloadableWheels", PERSISTENT}, {"DownloadAllModels", PERSISTENT}, + {"DragonRiderDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DragonRiderScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DriveRated", CLEAR_ON_ONROAD_TRANSITION}, {"DriverCamera", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"DuckAmigoCalibrationParams", PERSISTENT}, {"DuckAmigoDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"DuckAmigoLiveTorqueParameters", PERSISTENT}, {"DuckAmigoScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DynamicPathWidth", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"DynamicPedalsOnUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -352,6 +348,7 @@ std::unordered_map keys = { {"HideMapIcon", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"HideMaxSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"HideSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"HideSpeedLimit", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"HolidayThemes", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"HumanAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"HumanFollowing", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -373,9 +370,7 @@ std::unordered_map keys = { {"LongitudinalMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"LongitudinalTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"LongPitch", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, - {"LosAngelesCalibrationParams", PERSISTENT}, {"LosAngelesDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"LosAngelesLiveTorqueParameters", PERSISTENT}, {"LosAngelesScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"LoudBlindspotAlert", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"LowVoltageShutdown", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -406,18 +401,13 @@ std::unordered_map keys = { {"NextMapSpeedLimit", PERSISTENT}, {"NewLongAPI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"NewLongAPIGM", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, - {"NewToyotaTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"NNFF", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NNFFLite", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NNFFModelName", CLEAR_ON_OFFROAD_TRANSITION}, {"NoLogging", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NorthDakotaCalibrationParams", PERSISTENT}, {"NorthDakotaDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NorthDakotaLiveTorqueParameters", PERSISTENT}, {"NorthDakotaScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NotreDameCalibrationParams", PERSISTENT}, {"NotreDameDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NotreDameLiveTorqueParameters", PERSISTENT}, {"NotreDameScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NoUploads", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NudgelessLaneChange", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -449,14 +439,10 @@ std::unordered_map keys = { {"QOLLongitudinal", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"QOLVisuals", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"RadarlessModels", PERSISTENT}, - {"RadicalTurtleCalibrationParams", PERSISTENT}, {"RadicalTurtleDrives", PERSISTENT | FROGPILOT_CONTROLS}, - {"RadicalTurtleLiveTorqueParameters", PERSISTENT}, {"RadicalTurtleScore", PERSISTENT | FROGPILOT_CONTROLS}, {"RandomEvents", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"RecertifiedHerbalistCalibrationParams", PERSISTENT}, {"RecertifiedHerbalistDrives", PERSISTENT | FROGPILOT_CONTROLS}, - {"RecertifiedHerbalistLiveTorqueParameters", PERSISTENT}, {"RecertifiedHerbalistScore", PERSISTENT | FROGPILOT_CONTROLS}, {"RefuseVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"RelaxedFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -479,9 +465,7 @@ std::unordered_map keys = { {"ScreenTimeout", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"ScreenTimeoutOnroad", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"SearchInput", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, - {"SecretGoodOpenpilotCalibrationParams", PERSISTENT}, {"SecretGoodOpenpilotDrives", PERSISTENT | FROGPILOT_CONTROLS}, - {"SecretGoodOpenpilotLiveTorqueParameters", PERSISTENT}, {"SecretGoodOpenpilotScore", PERSISTENT | FROGPILOT_CONTROLS}, {"SetSpeedLimit", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SetSpeedOffset", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -536,6 +520,7 @@ std::unordered_map keys = { {"SteerRatioStock", PERSISTENT}, {"StoppedTimer", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"TacoTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TestingSound", PERSISTENT}, {"TetheringEnabled", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, {"ThemeDownloadProgress", PERSISTENT}, {"ThemeUpdated", PERSISTENT}, @@ -562,9 +547,7 @@ std::unordered_map keys = { {"VoltSNG", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"WarningImmediateVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"WarningSoftVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"WD40CalibrationParams", PERSISTENT}, {"WD40Drives", PERSISTENT | FROGPILOT_CONTROLS}, - {"WD40LiveTorqueParameters", PERSISTENT}, {"WD40Score", PERSISTENT | FROGPILOT_CONTROLS}, {"WheelIcon", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"WheelSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, diff --git a/selfdrive/SConscript b/selfdrive/SConscript index 43296d6fdab16b..52898f758f2ae7 100644 --- a/selfdrive/SConscript +++ b/selfdrive/SConscript @@ -4,4 +4,4 @@ SConscript(['controls/lib/longitudinal_mpc_lib/SConscript']) SConscript(['locationd/SConscript']) SConscript(['navd/SConscript']) SConscript(['modeld/SConscript']) -SConscript(['ui/SConscript']) +SConscript(['ui/SConscript']) \ No newline at end of file diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 85cf41ec9ee488..eb4205d7249d81 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -16,8 +16,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.controls.lib.events import Events -from openpilot.selfdrive.frogpilot.frogpilot_utilities import update_frogpilot_toggles -from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles +from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles, update_frogpilot_toggles REPLAY = "REPLAY" in os.environ @@ -54,6 +53,12 @@ def __init__(self, CI=None): else: self.CI, self.CP = CI, CI.CP + # set alternative experiences from parameters + self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") + self.CP.alternativeExperience = 0 + if not self.disengage_on_accelerator: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly @@ -75,7 +80,7 @@ def __init__(self, CI=None): self.rk = Ratekeeper(100, print_delay_threshold=None) # FrogPilot variables - self.frogpilot_toggles = get_frogpilot_toggles(True) + self.frogpilot_toggles = get_frogpilot_toggles() if self.params.get_bool("AlwaysOnLateral"): self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 847240bbd9a0fd..d7c2696986c6f7 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -38,7 +38,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_toggles): + def get_pid_accel_limits(CP, current_speed, cruise_speed): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX # Determined by iteratively plotting and minimizing error for f(angle, speed) = steer. diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 9fb34a071049ef..f5d4f8a6e3e61f 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -22,7 +22,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_toggles): + def get_pid_accel_limits(CP, current_speed, cruise_speed): if CP.carFingerprint in HONDA_BOSCH: return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX elif CP.enableGasInterceptor: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 0de21000304a72..94f5950fff97d6 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -64,11 +64,9 @@ def calculate_speed_limit(self, cp, cp_cam): speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam return speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"] else: - if self.CP.flags & HyundaiFlags.LKAS12: - speed_limit = cp_cam.vl["LKAS12"]["CF_Lkas_TsrSpeed_Display_Clu"] - else: - speed_limit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"] - return speed_limit if speed_limit not in (0, 255) else 0 + speed_limit_nav = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"] if self.CP.flags & HyundaiFlags.NAV_MSG else 0 + speed_limit_cam = cp_cam.vl["LKAS12"]["CF_Lkas_TsrSpeed_Display_Clu"] if self.CP.flags & HyundaiFlags.LKAS12 else 0 + return speed_limit_cam if speed_limit_cam not in (0, 255) else speed_limit_nav def update(self, cp, cp_cam, frogpilot_toggles): if self.CP.carFingerprint in CANFD_CAR: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 62033b7dae25dc..125edce6d8f09e 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -14,7 +14,6 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.simple_kalman import KF1D, get_kalman_gain from openpilot.common.numpy_fast import clip -from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG from openpilot.selfdrive.car.values import PLATFORMS @@ -22,7 +21,7 @@ from openpilot.selfdrive.controls.lib.events import Events from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel -from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles +from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles, params, params_memory ButtonType = car.CarState.ButtonEvent.Type FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type @@ -225,10 +224,7 @@ def __init__(self, CP, CarController, CarState): self.CC: CarControllerBase = CarController(dbc_name, CP, self.VM) # FrogPilot variables - self.frogpilot_toggles = get_frogpilot_toggles(True) - - self.params = Params() - self.params_memory = Params("/dev/shm/params") + self.frogpilot_toggles = get_frogpilot_toggles() eps_firmware = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), "")) @@ -267,7 +263,7 @@ def apply(self, c: car.CarControl, now_nanos: int, frogpilot_toggles) -> tuple[c return self.CC.update(c, self.CS, now_nanos, frogpilot_toggles) @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_toggles): + def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX @classmethod @@ -278,7 +274,7 @@ def get_non_essential_params(cls, candidate: str): return cls.get_params(candidate, gen_empty_fingerprint(), list(), False, False, False) @classmethod - def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], disable_openpilot_long: bool, experimental_long: bool, params: Params, docs: bool): + def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], disable_openpilot_long: bool, experimental_long: bool, params: params, docs: bool): ret = CarInterfaceBase.get_std_params(candidate) platform = PLATFORMS[candidate] @@ -513,7 +509,7 @@ def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_ return events def frogpilot_distance_functions(self, frogpilot_toggles): - distance_button = self.CS.distance_button or self.params_memory.get_bool("OnroadDistanceButtonPressed") + distance_button = self.CS.distance_button or params_memory.get_bool("OnroadDistanceButtonPressed") if distance_button: self.gap_counter += 1 @@ -522,12 +518,12 @@ def frogpilot_distance_functions(self, frogpilot_toggles): if self.gap_counter == CRUISE_LONG_PRESS * (1.5 if self.is_gm else 1) and frogpilot_toggles.experimental_mode_via_distance or self.traffic_mode_changed: if frogpilot_toggles.conditional_experimental_mode: - conditional_status = self.params_memory.get_int("CEStatus") + conditional_status = params_memory.get_int("CEStatus") override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 1 if conditional_status >= 7 else 2 - self.params_memory.put_int("CEStatus", override_value) + params_memory.put_int("CEStatus", override_value) else: - experimental_mode = self.params.get_bool("ExperimentalMode") - self.params.put_bool("ExperimentalMode", not experimental_mode) + experimental_mode = params.get_bool("ExperimentalMode") + params.put_bool("ExperimentalMode", not experimental_mode) self.traffic_mode_changed = False if self.gap_counter == CRUISE_LONG_PRESS * 5: diff --git a/selfdrive/car/secoc.py b/selfdrive/car/secoc.py new file mode 100644 index 00000000000000..971ea36a192a90 --- /dev/null +++ b/selfdrive/car/secoc.py @@ -0,0 +1,47 @@ +import struct + +from Crypto.Hash import CMAC +from Crypto.Cipher import AES + +def add_mac(key, trip_cnt, reset_cnt, msg_cnt, msg): + # TODO: clean up conversion to and from hex + + addr, payload, bus = msg + reset_flag = reset_cnt & 0b11 + msg_cnt_flag = msg_cnt & 0b11 + payload = payload[:4] + + # Step 1: Build Freshness Value (48 bits) + # [Trip Counter (16 bit)][[Reset Counter (20 bit)][Message Counter (8 bit)][Reset Flag (2 bit)][Padding (2 bit)] + freshness_value = struct.pack('>HI', trip_cnt, (reset_cnt << 12) | ((msg_cnt & 0xff) << 4) | (reset_flag << 2)) + + # Step 2: Build data to authenticate (96 bits) + # [Message ID (16 bits)][Payload (32 bits)][Freshness Value (48 bits)] + to_auth = struct.pack('>H', addr) + payload + freshness_value + + # Step 3: Calculate CMAC (28 bit) + cmac = CMAC.new(key, ciphermod=AES) + cmac.update(to_auth) + mac = cmac.digest().hex()[:7] # truncated MAC + + # Step 4: Build message + # [Payload (32 bit)][Message Counter Flag (2 bit)][Reset Flag (2 bit)][Authenticator (28 bit)] + msg_cnt_rst_flag = struct.pack('>B', (msg_cnt_flag << 2) | reset_flag).hex()[1] + msg = payload.hex() + msg_cnt_rst_flag + mac + payload = bytes.fromhex(msg) + + return (addr, payload, bus) + +def build_sync_mac(key, trip_cnt, reset_cnt, id_=0xf): + id_ = struct.pack('>H', id_) # 16 + trip_cnt = struct.pack('>H', trip_cnt) # 16 + reset_cnt = struct.pack('>I', reset_cnt << 12)[:-1] # 20 + 4 padding + + to_auth = id_ + trip_cnt + reset_cnt # SecOC 11.4.1.1 page 138 + + cmac = CMAC.new(key, ciphermod=AES) + cmac.update(to_auth) + + msg = "0" + cmac.digest().hex()[:7] + msg = bytes.fromhex(msg) + return struct.unpack('>I', msg)[0] diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index f2d0544a46519f..64d8182963f9bf 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -3,11 +3,12 @@ from cereal import car from panda.python import uds -from openpilot.common.params import Params from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Tool, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 +from openpilot.selfdrive.frogpilot.frogpilot_variables import params + Ecu = car.CarParams.Ecu @@ -26,7 +27,7 @@ def __init__(self, CP): self.STEER_DELTA_DOWN = 40 elif CP.carFingerprint == CAR.SUBARU_IMPREZA_2020: self.STEER_MAX = 1439 - elif CP.carFingerprint == CAR.SUBARU_IMPREZA and Params().get_bool("CrosstrekTorque"): + elif CP.carFingerprint == CAR.SUBARU_IMPREZA and params.get_bool("CrosstrekTorque"): self.STEER_MAX = 3071 else: self.STEER_MAX = 2047 diff --git a/selfdrive/car/torque_data/lat_models/CHEVROLET_BOLT_EUV.json b/selfdrive/car/torque_data/lat_models/CHEVROLET_BOLT_EUV.json deleted file mode 100644 index f228dcfa17a0f6..00000000000000 --- a/selfdrive/car/torque_data/lat_models/CHEVROLET_BOLT_EUV.json +++ /dev/null @@ -1 +0,0 @@ -{"input_std":[[9.281861],[1.8477924],[0.7977224],[0.047467366],[1.7969038],[1.8168812],[1.8351218],[1.785757],[1.7335743],[1.6658221],[1.5893887],[0.047346078],[0.0473731],[0.047383286],[0.047291175],[0.047300573],[0.04712479],[0.046799928]],"model_test_loss":0.027355343103408813,"input_size":18,"current_date_and_time":"2023-08-05_05-11-44","input_mean":[[21.655252],[-0.07694559],[-0.006081294],[-0.007598456],[-0.07309746],[-0.075890236],[-0.07804942],[-0.076106496],[-0.07184982],[-0.06528668],[-0.060404416],[-0.0077262702],[-0.0077046235],[-0.0076850485],[-0.007687444],[-0.007708868],[-0.0077959923],[-0.008017422]],"input_vars":["v_ego","lateral_accel","lateral_jerk","roll","lateral_accel_m03","lateral_accel_m02","lateral_accel_m01","lateral_accel_p03","lateral_accel_p06","lateral_accel_p10","lateral_accel_p15","roll_m03","roll_m02","roll_m01","roll_p03","roll_p06","roll_p10","roll_p15"],"output_size":1,"layers":[{"dense_1_b":[[-0.35776415],[-0.126288],[0.007988413],[0.03850029],[-0.056796823],[0.0072075897],[0.17376427]],"dense_1_W":[[0.010538558,6.7293744,-0.007391298,-1.035045,-0.47446147,1.0940393,0.08225446,-0.5795131,0.573115,1.82923,-0.16999252,0.8818023,0.03560901,-0.9470653,-1.042151,1.1532398,1.9009485,-1.2265956],[0.009010456,-1.3618345,0.039026346,0.22943486,-0.6669799,-0.768271,-0.16828564,-0.13406865,0.19760236,-0.18953702,-0.019137766,0.1468623,0.26367718,0.4732375,0.73679143,0.62450147,0.30198866,-0.93340015],[-0.044318866,2.2138615,9.620889,-0.66480356,-0.06849887,-0.038568456,-0.30580127,-0.088089585,0.31187677,0.5968372,-1.1462088,0.13234706,0.29166338,-0.69978577,-0.01790655,-0.097928315,-0.56950736,0.7560642],[1.3758256,0.8700429,-0.24295154,0.20832618,-0.7735097,1.0379355,-0.60753095,-0.5457418,-1.0892137,-0.6708325,0.24982774,0.09251328,0.08596103,-0.58034134,-0.3046114,-0.19754426,0.006461508,0.5989185],[0.06314328,-2.492993,-0.28200585,0.49902886,0.8513198,-0.5704965,1.0960953,-0.08426956,-0.76074624,0.10669376,0.02775254,-0.42194095,-0.34657434,-0.10863868,0.17019278,0.0963825,-0.10169116,0.21243806],[0.007984091,-1.9276509,-0.0013926353,0.04057924,1.4438432,1.3440262,-2.506722,1.700801,0.72410774,0.13471895,-0.18753268,0.7303607,0.018133093,-0.71643597,0.07050904,-0.30161944,0.085108586,0.061202843],[0.9413167,-0.9954305,-0.19510143,-0.4711845,-0.12398665,-0.45175523,1.0616771,0.28550953,-0.55543137,-0.21576759,-0.2364377,-0.012782618,-0.050565187,0.257694,-0.20975965,0.00022543047,-0.08760746,0.4983852]],"activation":"σ"},{"dense_2_W":[[-0.71804935,0.017023304,-0.099854074,-0.3262651,-0.402829,-0.12073083,0.13537839],[-0.44002575,-0.1071093,-0.58886194,-0.17319413,-0.21134079,0.23135148,0.0006880979],[-0.55711204,-0.8693639,-0.6443761,-0.7382846,0.18980889,-0.6082511,-0.63103676],[0.11338112,-0.5327033,0.2856197,0.32239884,-0.72682667,0.5302305,-0.45094746],[-0.35197002,-0.14440043,-0.025249843,-0.48697743,0.3340018,-0.25992322,0.0050561456],[0.34757507,0.15670523,-0.14263277,0.50704384,-0.020171141,0.6408679,-0.3074123],[0.40258837,-0.59363264,0.35948923,-0.060853776,-0.0072541097,0.89743376,-0.49789146],[-0.63476,0.29580453,0.1689314,-0.5061521,0.24901053,-0.23218995,0.57968193],[-0.66173035,0.50861925,-0.5845035,-0.6602214,0.8341883,-0.31437424,0.8046359],[0.038493533,0.15464582,-0.04848341,-0.57820857,-0.25891733,-0.47527292,0.21441916],[-0.15464531,-0.07454202,-0.8215851,-0.12614948,-0.5924209,0.00017916794,0.24154592],[0.6091424,-0.112086505,0.1144111,-0.31035227,-0.9237534,0.041003596,-0.3542808],[0.2989792,-0.23780549,0.116059326,-0.6056522,0.5499526,-0.9001413,0.5200723]],"activation":"σ","dense_2_b":[[-0.2638964],[-0.24607328],[-0.15493082],[-0.0071187904],[-0.23515384],[-0.09098133],[-0.034066215],[0.03609036],[-0.03842933],[-0.042302527],[-0.2439947],[-0.16342077],[-0.01030317]]},{"dense_3_W":[[0.45771673,-0.2084603,0.3570187,0.35835707,0.13684571,-0.582958,0.29889587,0.43543494,0.11841301,-0.26185623,-0.4988437,0.5752996,-0.28057483],[-0.19594137,0.050280698,-0.29057446,-0.2829161,-0.16987291,-0.21278952,-0.59946907,0.21295123,0.7040468,0.53549695,-0.52553934,-0.19560973,0.6233473],[-0.19091003,0.08669418,-0.5792192,0.57799137,-0.36263424,0.6037143,0.27898273,-0.30951327,-0.3572644,0.46720102,-0.5403428,0.32415462,-0.60570025]],"activation":"identity","dense_3_b":[[-0.0047377176],[0.023170695],[-0.021546118]]},{"dense_4_W":[[-0.12453941,-1.006034,0.96776205]],"dense_4_b":[[-0.02351544]],"activation":"identity"}]} \ No newline at end of file diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 56c7d3f6ed11b6..a57bb6b71b0d01 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,13 +1,16 @@ import math from cereal import car +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.numpy_fast import clip, interp +from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, create_gas_interceptor_command, make_can_msg, rate_limit +from openpilot.selfdrive.car.secoc import add_mac, build_sync_mac 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, STOP_AND_GO_CAR + UNSUPPORTED_DSU_CAR, STOP_AND_GO_CAR, Ecu from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS from opendbc.can.packer import CANPacker @@ -19,7 +22,10 @@ ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2 +# The up limit allows the brakes/gas to unwind quickly leaving a stop, +# the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup ACCEL_WINDUP_LIMIT = 0.5 # m/s^2 / frame +ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3 # m/s^2 / frame # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long @@ -40,6 +46,7 @@ PARK = car.CarState.GearShifter.park + class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP @@ -50,14 +57,30 @@ def __init__(self, dbc_name, CP, VM): self.alert_active = False self.last_standstill = False self.standstill_req = False + self.permit_braking = True self.steer_rate_counter = 0 self.distance_button = 0 - self.pcm_accel_compensation = 0.0 - self.permit_braking = True + self.pcm_accel_compensation = FirstOrderFilter(0, 0.5, DT_CTRL * 3) + + # the PCM's reported acceleration request can sometimes mismatch aEgo, close the loop + self.pcm_accel_net_offset = FirstOrderFilter(0, 1.0, DT_CTRL * 3) + + # aEgo also often lags behind the PCM request due to physical brake lag which varies by car, + # so we error correct on the filtered PCM acceleration request using the actuator delay. + # TODO: move the delay into the interface + self.pcm_accel_net = FirstOrderFilter(0, self.CP.longitudinalActuatorDelay, DT_CTRL * 3) + if not any(fw.ecu == Ecu.hybrid for fw in self.CP.carFw): + self.pcm_accel_net.update_alpha(self.CP.longitudinalActuatorDelay + 0.2) self.packer = CANPacker(dbc_name) self.accel = 0 + self.prev_accel = 0 + + self.secoc_lka_message_counter = 0 + self.secoc_lta_message_counter = 0 + self.secoc_prev_reset_counter = 0 + self.secoc_mismatch_counter = 0 # FrogPilot variables self.doors_locked = False @@ -68,6 +91,7 @@ def __init__(self, dbc_name, CP, VM): def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators + stopping = actuators.longControlState == LongCtrlState.stopping hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE @@ -75,6 +99,17 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): # *** control msgs *** can_sends = [] + # *** handle secoc reset counter increase *** + if self.CP.flags & ToyotaFlags.SECOC.value: + if CS.secoc_synchronization['RESET_CNT'] != self.secoc_prev_reset_counter: + self.secoc_lka_message_counter = 0 + self.secoc_lta_message_counter = 0 + self.secoc_prev_reset_counter = CS.secoc_synchronization['RESET_CNT'] + + expected_mac = build_sync_mac(self.secoc_key, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT'])) + if int(CS.secoc_synchronization['AUTHENTICATOR']) != expected_mac and self.secoc_mismatch_counter < 100: + self.secoc_mismatch_counter += 1 + # *** steer torque *** new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params) @@ -108,7 +143,16 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): # toyota can trace shows STEERING_LKA at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages - can_sends.append(toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req)) + steer_command = toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req) + if self.CP.flags & ToyotaFlags.SECOC.value: + # TODO: check if this slow and needs to be done by the CANPacker + steer_command = add_mac(self.secoc_key, + int(CS.secoc_synchronization['TRIP_CNT']), + int(CS.secoc_synchronization['RESET_CNT']), + self.secoc_lka_message_counter, + steer_command) + self.secoc_lka_message_counter += 1 + can_sends.append(steer_command) # STEERING_LTA does not seem to allow more rate by sending faster, and may wind up easier if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR: @@ -123,6 +167,16 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.CP.steerControlType, self.last_angle, lta_active, self.frame // 2, torque_wind_down)) + if self.CP.flags & ToyotaFlags.SECOC.value: + lta_steer_2 = toyotacan.create_lta_steer_command_2(self.packer, self.frame // 2) + lta_steer_2 = add_mac(self.secoc_key, + int(CS.secoc_synchronization['TRIP_CNT']), + int(CS.secoc_synchronization['RESET_CNT']), + self.secoc_lta_message_counter, + lta_steer_2) + self.secoc_lta_message_counter += 1 + can_sends.append(lta_steer_2) + # *** gas and brake *** if self.CP.enableGasInterceptor and CC.longActive and self.CP.carFingerprint not in STOP_AND_GO_CAR: MAX_INTERCEPTOR_GAS = 0.5 @@ -142,44 +196,6 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): else: interceptor_gas_cmd = 0. - # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking - if (self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT or self.CP.flags & ToyotaFlags.NEW_TOYOTA_TUNE) and CC.longActive and not CS.out.cruiseState.standstill: - # calculate amount of acceleration PCM should apply to reach target, given pitch - if len(CC.orientationNED) == 3: - accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY - else: - accel_due_to_pitch = 0.0 - - net_acceleration_request = actuators.accel + accel_due_to_pitch - - # let PCM handle stopping for now - pcm_accel_compensation = 0.0 - if actuators.longControlState != LongCtrlState.stopping: - pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - net_acceleration_request) - - # prevent compensation windup - pcm_accel_compensation = clip(pcm_accel_compensation, actuators.accel - self.params.ACCEL_MAX, - actuators.accel - self.params.ACCEL_MIN) - - self.pcm_accel_compensation = rate_limit(pcm_accel_compensation, self.pcm_accel_compensation, -0.01, 0.01) - pcm_accel_cmd = actuators.accel - self.pcm_accel_compensation - - # Along with rate limiting positive jerk below, this greatly improves gas response time - # Consider the net acceleration request that the PCM should be applying (pitch included) - if net_acceleration_request < 0.1: - self.permit_braking = True - elif net_acceleration_request > 0.2: - self.permit_braking = False - else: - self.pcm_accel_compensation = 0.0 - pcm_accel_cmd = actuators.accel - self.permit_braking = True - - if frogpilot_toggles.sport_plus: - pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, get_max_allowed_accel(CS.out.vEgo))) - else: - pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, self.params.ACCEL_MAX)) - # on entering standstill, send standstill request if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor): self.standstill_req = True @@ -192,31 +208,86 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): # handle UI messages fcw_alert = hud_control.visualAlert == VisualAlert.fcw steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) + lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged + + if self.CP.openpilotLongitudinalControl: + if self.frame % 3 == 0: + # Press distance button until we are at the correct bar length. Only change while enabled to avoid skipping startup popup + if self.frame % 6 == 0 and self.CP.openpilotLongitudinalControl: + desired_distance = 4 - hud_control.leadDistanceBars + if CS.out.cruiseState.enabled and CS.pcm_follow_distance != desired_distance: + self.distance_button = not self.distance_button + else: + self.distance_button = 0 - # we can spam can to cancel the system even if we are using lat only control - if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: - lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged + # internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit + pcm_accel_cmd = actuators.accel + if CC.longActive: + pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT) + self.prev_accel = pcm_accel_cmd + + # calculate amount of acceleration PCM should apply to reach target, given pitch + accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY if len(CC.orientationNED) == 3 else 0.0 + net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch + + # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking + if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill: + # filter ACCEL_NET so it more closely matches aEgo delay for error correction + self.pcm_accel_net.update(CS.pcm_accel_net) + + # Our model of the PCM's acceleration request isn't perfect, so we learn the offset when moving + new_pcm_accel_net = CS.pcm_accel_net + if stopping or CS.out.standstill: + # TODO: check if maintaining the offset from before stopping is beneficial + self.pcm_accel_net_offset.x = 0.0 + else: + new_pcm_accel_net -= self.pcm_accel_net_offset.update((self.pcm_accel_net.x - accel_due_to_pitch) - CS.out.aEgo) + + # let PCM handle stopping for now + pcm_accel_compensation = 0.0 + if not stopping: + pcm_accel_compensation = 2.0 * (new_pcm_accel_net - net_acceleration_request) + + # prevent compensation windup + if frogpilot_toggles.sport_plus: + pcm_accel_compensation = clip(pcm_accel_compensation, pcm_accel_cmd - get_max_allowed_accel(CS.out.vEgo), + pcm_accel_cmd - self.params.ACCEL_MIN) + else: + pcm_accel_compensation = clip(pcm_accel_compensation, pcm_accel_cmd - self.params.ACCEL_MAX, + pcm_accel_cmd - self.params.ACCEL_MIN) + + pcm_accel_cmd = pcm_accel_cmd - self.pcm_accel_compensation.update(pcm_accel_compensation) - # Press distance button until we are at the correct bar length. Only change while enabled to avoid skipping startup popup - if self.frame % 6 == 0 and self.CP.openpilotLongitudinalControl: - desired_distance = 4 - hud_control.leadDistanceBars - if CS.out.cruiseState.enabled and CS.pcm_follow_distance != desired_distance: - self.distance_button = not self.distance_button else: - self.distance_button = 0 - - # Lexus IS uses a different cancellation message - if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: - can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) - elif self.CP.openpilotLongitudinalControl: - # internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit - pcm_accel_cmd = min(pcm_accel_cmd, self.accel + ACCEL_WINDUP_LIMIT) if CC.longActive else 0.0 + self.pcm_accel_compensation.x = 0.0 + self.pcm_accel_net_offset.x = 0.0 + self.pcm_accel_net.x = CS.pcm_accel_net + self.permit_braking = True + + # Along with rate limiting positive jerk above, this greatly improves gas response time + # Consider the net acceleration request that the PCM should be applying (pitch included) + net_acceleration_request_min = min(actuators.accel + accel_due_to_pitch, net_acceleration_request) + if net_acceleration_request_min < 0.1 or stopping or not CC.longActive: + self.permit_braking = True + elif net_acceleration_request_min > 0.2: + self.permit_braking = False + + if frogpilot_toggles.sport_plus: + pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, get_max_allowed_accel(CS.out.vEgo))) + else: + pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, self.params.ACCEL_MAX)) can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead, CS.acc_type, fcw_alert, self.distance_button, self.reverse_cruise_active)) self.accel = pcm_accel_cmd - else: - can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, True, False, lead, CS.acc_type, False, self.distance_button, self.reverse_cruise_active)) + + else: + # we can spam can to cancel the system even if we are using lat only control + if pcm_cancel_cmd: + if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: + can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) + else: + can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, True, False, lead, CS.acc_type, False, self.distance_button, self.reverse_cruise_active)) if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 3beace946ae201..6a722808e2b0f5 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -9,7 +9,7 @@ from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ - TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR + TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, SECOC_CAR SteerControlType = car.CarParams.SteerControlType @@ -24,197 +24,6 @@ # - prolonged high driver torque: 17 (permanent) PERM_STEER_FAULTS = (3, 17) -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1", None) - spdval1 = traffic_signals.get("SPDVAL1", None) - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - else: - return 0 - -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1") - spdval1 = traffic_signals.get("SPDVAL1") - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - return 0 - - -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1") - spdval1 = traffic_signals.get("SPDVAL1") - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - return 0 - - -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1") - spdval1 = traffic_signals.get("SPDVAL1") - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - return 0 - - -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1") - spdval1 = traffic_signals.get("SPDVAL1") - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - return 0 - - -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1") - spdval1 = traffic_signals.get("SPDVAL1") - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - return 0 - - -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1") - spdval1 = traffic_signals.get("SPDVAL1") - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - return 0 - - -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1") - spdval1 = traffic_signals.get("SPDVAL1") - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - return 0 - - -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1") - spdval1 = traffic_signals.get("SPDVAL1") - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - return 0 - - -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1") - spdval1 = traffic_signals.get("SPDVAL1") - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - return 0 - - -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1") - spdval1 = traffic_signals.get("SPDVAL1") - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - return 0 - - -# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! -@staticmethod -def calculate_speed_limit(cp_cam, frogpilot_toggles): - signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] - traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - - tsgn1 = traffic_signals.get("TSGN1") - spdval1 = traffic_signals.get("SPDVAL1") - - if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.KPH_TO_MS - elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: - return spdval1 * CV.MPH_TO_MS - return 0 - # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! @staticmethod @@ -241,6 +50,11 @@ def __init__(self, CP): self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2. self.cluster_min_speed = CV.KPH_TO_MS / 2. + if CP.flags & ToyotaFlags.SECOC.value: + self.shifter_values = can_define.dv["GEAR_PACKET_HYBRID"]["GEAR"] + else: + self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"] + # On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # the signal is zeroed to where the steering angle is at start. # Need to apply an offset as soon as the steering angle measurements are both received @@ -256,6 +70,7 @@ def __init__(self, CP): self.acc_type = 1 self.lkas_hud = {} self.pcm_accel_net = 0.0 + self.secoc_synchronization = None # FrogPilot variables self.latActive_previous = False @@ -267,15 +82,13 @@ def __init__(self, CP): def update(self, cp, cp_cam, CC, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() + cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp # Describes the acceleration request from the PCM if on flat ground, may be higher or lower if pitched # CLUTCH->ACCEL_NET is only accurate for gas, PCM_CRUISE->ACCEL_NET is only accurate for brake # These signals only have meaning when ACC is active - if self.CP.flags & ToyotaFlags.NEW_TOYOTA_TUNE or self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: - if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: - self.pcm_accel_net = max(cp.vl["CLUTCH"]["ACCEL_NET"], 0.0) - else: - self.pcm_accel_net = max(cp.vl["PCM_CRUISE"]["ACCEL_NET"], 0.0) + if "CLUTCH" in cp.vl: + self.pcm_accel_net = max(cp.vl["CLUTCH"]["ACCEL_NET"], 0.0) # Sometimes ACC_BRAKING can be 1 while showing we're applying gas already if cp.vl["PCM_CRUISE"]["ACC_BRAKING"]: @@ -294,12 +107,22 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0 ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1 - if self.CP.enableGasInterceptor: - ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2 - ret.gasPressed = ret.gas > 805 + if self.CP.flags & ToyotaFlags.SECOC.value: + self.secoc_synchronization = copy.copy(cp.vl["SECOC_SYNCHRONIZATION"]) + ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"] + ret.gasPressed = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"] > 0 + can_gear = int(cp.vl["GEAR_PACKET_HYBRID"]["GEAR"]) else: - # TODO: find a common gas pedal percentage signal - ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 + if self.CP.enableGasInterceptor: + ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2 + ret.gasPressed = ret.gas > 805 + else: + ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 # TODO: these also have GAS_PEDAL, come back and unify + can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"]) + if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: + ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5) + if self.CP.carFingerprint != CAR.TOYOTA_MIRAI: + ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"] ret.wheelSpeeds = self.get_wheel_speeds( cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"], @@ -330,14 +153,10 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): ret.steeringAngleOffsetDeg = self.angle_offset.x ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x - can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1 ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2 - if self.CP.carFingerprint != CAR.TOYOTA_MIRAI: - ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"] - ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale # we could use the override bit from dbc, but it's triggered at too high torque values @@ -351,6 +170,10 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): ret.steerFaultTemporary = ret.steerFaultTemporary or cp.vl["EPS_STATUS"]["LTA_STATE"] in TEMP_STEER_FAULTS ret.steerFaultPermanent = ret.steerFaultPermanent or cp.vl["EPS_STATUS"]["LTA_STATE"] in PERM_STEER_FAULTS + # Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until + # the more accurate angle sensor signal is initialized + ret.vehicleSensorsInvalid = not self.accurate_steer_angle_seen + if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: # TODO: find the bit likely in DSU_CRUISE that describes an ACC fault. one may also exist in CLUTCH ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0 @@ -368,8 +191,6 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): conversion_factor = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor - cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp - if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: if not (self.CP.flags & ToyotaFlags.SMART_DSU.value): self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"] @@ -381,7 +202,7 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): # send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1 if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR) or \ (self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1): - self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2 + ret.accFaulted = ret.accFaulted or cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2 self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] if self.CP.carFingerprint not in (NO_STOP_TIMER_CAR - TSS2_CAR): @@ -393,9 +214,6 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"]) ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0 - if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: - ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5) - if self.CP.enableBsm: ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1) ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1) @@ -458,7 +276,6 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): @staticmethod def get_can_parser(CP): messages = [ - ("GEAR_PACKET", 1), ("LIGHT_STALK", 1), ("BLINKERS_STATE", 0.15), ("BODY_CONTROL_STATE", 3), @@ -473,11 +290,22 @@ def get_can_parser(CP): ("STEER_TORQUE_SENSOR", 50), ] - if CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: - messages.append(("CLUTCH", 15)) + if CP.flags & ToyotaFlags.SECOC.value: + messages += [ + ("GEAR_PACKET_HYBRID", 60), + ("SECOC_SYNCHRONIZATION", 10), + ("GAS_PEDAL", 42), + ] + else: + if CP.carFingerprint not in [CAR.TOYOTA_MIRAI]: + messages.append(("ENGINE_RPM", 42)) - if CP.carFingerprint != CAR.TOYOTA_MIRAI: - messages.append(("ENGINE_RPM", 42)) + messages += [ + ("GEAR_PACKET", 1), + ] + + if CP.carFingerprint in (TSS2_CAR - SECOC_CAR - {CAR.LEXUS_NX_TSS2, CAR.TOYOTA_ALPHARD_TSS2, CAR.LEXUS_IS_TSS2}): + messages.append(("CLUTCH", 15)) if CP.carFingerprint in UNSUPPORTED_DSU_CAR: messages.append(("DSU_CRUISE", 5)) @@ -532,9 +360,14 @@ def get_cam_can_parser(CP): if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): messages += [ - ("PRE_COLLISION", 33), ("ACC_CONTROL", 33), ("PCS_HUD", 1), ] + # TODO: Figure out new layout of the PRE_COLLISION message + if not CP.flags & ToyotaFlags.SECOC.value: + messages += [ + ("PRE_COLLISION", 33), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2) diff --git a/selfdrive/car/toyota/fingerprints.py b/selfdrive/car/toyota/fingerprints.py index e3a918dac8eda0..338f72a54b36f6 100644 --- a/selfdrive/car/toyota/fingerprints.py +++ b/selfdrive/car/toyota/fingerprints.py @@ -394,6 +394,7 @@ (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646FF410200\x00\x00\x00\x008646GF408200\x00\x00\x00\x00', b'\x028646FF411100\x00\x00\x00\x008646GF409000\x00\x00\x00\x00', + b'\x028646FF413000\x00\x00\x00\x008646GF411000\x00\x00\x00\x00', b'\x028646FF413100\x00\x00\x00\x008646GF411100\x00\x00\x00\x00', ], }, @@ -723,11 +724,13 @@ b'\x01896630EE7000\x00\x00\x00\x00', b'\x01896630EF8000\x00\x00\x00\x00', b'\x01896630EG3000\x00\x00\x00\x00', + b'\x01896630EG3100\x00\x00\x00\x00', b'\x01896630EG5000\x00\x00\x00\x00', b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630EB3100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896630EB3200\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', @@ -1308,6 +1311,7 @@ b'\x01896633T07000\x00\x00\x00\x00', b'\x01896633T38000\x00\x00\x00\x00', b'\x01896633T58000\x00\x00\x00\x00', + b'\x01896633T63000\x00\x00\x00\x00', b'\x028966333S8000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', b'\x028966333S8000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x028966333T0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', @@ -1331,6 +1335,7 @@ b'8965B33252\x00\x00\x00\x00\x00\x00', b'8965B33590\x00\x00\x00\x00\x00\x00', b'8965B33690\x00\x00\x00\x00\x00\x00', + b'8965B33702\x00\x00\x00\x00\x00\x00', b'8965B33721\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ @@ -1352,6 +1357,7 @@ b'\x028646F3304300\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', b'\x028646F3309100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', b'\x028646F3309100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F3309400\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, CAR.LEXUS_ES: { diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 792b25ba24911f..3ac81cab7e4d7b 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -28,6 +28,10 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp if DBC[candidate]["pt"] == "toyota_new_mc_pt_generated": ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE + if ret.flags & ToyotaFlags.SECOC.value: + ret.secOcRequired = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_SECOC + if candidate in ANGLE_CONTROL_CAR: ret.steerControlType = SteerControlType.angle ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA @@ -59,9 +63,6 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp if candidate == CAR.LEXUS_ES_TSS2 and Ecu.hybrid not in found_ecus: ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value - if params.get_bool("NewToyotaTune"): - ret.flags |= ToyotaFlags.NEW_TOYOTA_TUNE.value - if candidate == CAR.TOYOTA_PRIUS: # Only give steer angle deadzone to for bad angle sensor prius for fw in car_fw: @@ -75,7 +76,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2): ret.wheelSpeedFactor = 1.035 - elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023): + elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023, CAR.TOYOTA_RAV4_PRIME, CAR.TOYOTA_SIENNA_4TH_GEN): ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP = [0.0] ret.lateralTuning.pid.kpBP = [0.0] @@ -108,6 +109,10 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp if candidate in (RADAR_ACC_CAR | NO_DSU_CAR): ret.experimentalLongitudinalAvailable = use_sdsu or candidate in RADAR_ACC_CAR + # Disabling radar is only supported on TSS2 radar-ACC cars + if experimental_long and candidate in RADAR_ACC_CAR: + ret.flags |= ToyotaFlags.DISABLE_RADAR.value + if not use_sdsu: # Disabling radar is only supported on TSS2 radar-ACC cars if experimental_long and candidate in RADAR_ACC_CAR: @@ -116,15 +121,19 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp use_sdsu = use_sdsu and experimental_long # openpilot longitudinal enabled by default: - # - non-(TSS2 radar ACC cars) w/ smartDSU installed # - cars w/ DSU disconnected # - TSS2 cars with camera sending ACC_CONTROL where we can block it # openpilot longitudinal behind experimental long toggle: - # - TSS2 radar ACC cars w/ smartDSU installed - # - TSS2 radar ACC cars w/o smartDSU installed (disables radar) - # - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet) - ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) + # - TSS2 radar ACC cars (disables radar) + + if ret.flags & ToyotaFlags.SECOC.value: + ret.openpilotLongitudinalControl = False + else: + ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or \ + candidate in (TSS2_CAR - RADAR_ACC_CAR) or \ + bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) ret.openpilotLongitudinalControl &= not disable_openpilot_long + ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR ret.enableGasInterceptor = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl @@ -139,17 +148,16 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp ret.minEnableSpeed = -1. if (candidate in STOP_AND_GO_CAR or ret.enableGasInterceptor) else MIN_ACC_SPEED tune = ret.longitudinalTuning - if candidate in TSS2_CAR or ret.flags & ToyotaFlags.NEW_TOYOTA_TUNE: + if candidate in TSS2_CAR: tune.kpV = [0.0] tune.kiV = [0.5] ret.vEgoStopping = 0.25 ret.vEgoStarting = 0.25 ret.stoppingDecelRate = 0.3 # reach stopping target smoothly - # Since we compensate for imprecise acceleration in carcontroller, we can be less aggressive with tuning - # This also prevents unnecessary request windup due to internal car jerk limits - if ret.flags & ToyotaFlags.NEW_TOYOTA_TUNE or ret.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: - tune.kiV = [0.25] + # Since we compensate for imprecise acceleration in carcontroller and error correct on aEgo, we can avoid using gains + if ret.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: + tune.kiV = [0.0] else: tune.kiBP = [0., 5., 35.] tune.kiV = [3.6, 2.4, 1.5] diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index fd3887efab3f49..5d633fc952aa00 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -33,10 +33,17 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, return packer.make_can_msg("STEERING_LTA", 0, values) +def create_lta_steer_command_2(packer, frame): + values = { + "COUNTER": frame + 128, + } + return packer.make_can_msg("STEERING_LTA_2", 0, values) + + def create_accel_command(packer, accel, pcm_cancel, permit_braking, standstill_req, lead, acc_type, fcw_alert, distance, reverse_cruise_active): # TODO: find the exact canceling bit that does not create a chime values = { - "ACCEL_CMD": accel, # compensated accel command + "ACCEL_CMD": accel, "ACC_TYPE": acc_type, "DISTANCE": distance, "MINI_CAR": lead, diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index e7e3ca0b24e022..1ae1effd05e238 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -64,10 +64,9 @@ class ToyotaFlags(IntFlag): SECOC = 2048 # FrogPilot Toyota flags - NEW_TOYOTA_TUNE = 4096 - RADAR_CAN_FILTER = 8192 - SMART_DSU = 16384 - ZSS = 32768 + RADAR_CAN_FILTER = 4096 + SMART_DSU = 8192 + ZSS = 16384 class Footnote(Enum): CAMRY = CarFootnote( diff --git a/selfdrive/classic_modeld/classic_modeld.py b/selfdrive/classic_modeld/classic_modeld.py index c285acd636d20e..541f6399688b03 100755 --- a/selfdrive/classic_modeld/classic_modeld.py +++ b/selfdrive/classic_modeld/classic_modeld.py @@ -25,7 +25,7 @@ from openpilot.selfdrive.classic_modeld.constants import ModelConstants from openpilot.selfdrive.classic_modeld.models.commonmodel_pyx import ModelFrame, CLContext -from openpilot.selfdrive.frogpilot.assets.model_manager import DEFAULT_MODEL +from openpilot.selfdrive.frogpilot.assets.model_manager import DEFAULT_CLASSIC_MODEL from openpilot.selfdrive.frogpilot.frogpilot_functions import MODELS_PATH from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles @@ -61,7 +61,7 @@ def __init__(self, context: CLContext, frogpilot_toggles: SimpleNamespace): self.radarless = frogpilot_toggles.radarless_model model_path = Path(__file__).parent / f'{MODELS_PATH}/{frogpilot_toggles.model}.thneed' - if frogpilot_toggles.model != DEFAULT_MODEL and model_path.exists(): + if frogpilot_toggles.model != DEFAULT_CLASSIC_MODEL and model_path.exists(): MODEL_PATHS[ModelRunner.THNEED] = model_path self.frame = ModelFrame(context) @@ -136,7 +136,7 @@ def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_ def main(demo=False): # FrogPilot variables - frogpilot_toggles = get_frogpilot_toggles(True) + frogpilot_toggles = get_frogpilot_toggles() enable_navigation = not frogpilot_toggles.navigationless_model radarless = frogpilot_toggles.radarless_model diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d2bf707df55136..e6a87b276093df 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -32,7 +32,7 @@ from openpilot.system.hardware import HARDWARE from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_acceleration import get_max_allowed_accel -from openpilot.selfdrive.frogpilot.frogpilot_variables import NON_DRIVING_GEARS, get_frogpilot_toggles +from openpilot.selfdrive.frogpilot.frogpilot_variables import NON_DRIVING_GEARS, get_frogpilot_toggles, params_memory SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS @@ -80,7 +80,6 @@ def __init__(self, CI=None): # Ensure the current branch is cached, otherwise the first iteration of controlsd lags self.branch = get_short_branch() - self.block_user = self.branch == "FrogPilot-Development" and self.params.get("DongleId", encoding='utf-8') != "FrogsGoMoo" # Setup sockets self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents', 'frogpilotCarControl']) @@ -99,7 +98,7 @@ def __init__(self, CI=None): if REPLAY: # no vipc in replay will make them ignored anyways ignore += ['roadCameraState', 'wideRoadCameraState'] - if get_frogpilot_toggles(True).radarless_model: + if get_frogpilot_toggles().radarless_model: ignore += ['radarState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', @@ -164,7 +163,7 @@ def __init__(self, CI=None): self.can_log_mono_time = 0 - self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0, self.block_user, get_frogpilot_toggles(True)) + self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0) if not sounds_available: self.events.add(EventName.soundsUnavailable, static=True) @@ -181,9 +180,7 @@ def __init__(self, CI=None): self.rk = Ratekeeper(100, print_delay_threshold=None) # FrogPilot variables - self.frogpilot_toggles = get_frogpilot_toggles(True) - - self.params_memory = Params("/dev/shm/params") + self.frogpilot_toggles = get_frogpilot_toggles() self.always_on_lateral_active = False self.always_on_lateral_active_previously = False @@ -400,7 +397,8 @@ def update_events(self, CS): planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled if (planner_fcw or model_fcw) and not (self.CP.notCar and self.joystick_mode): self.events.add(EventName.fcw) - self.fcw_event_triggered = True + if self.frogpilot_toggles.random_events: + self.fcw_event_triggered = True for m in messaging.drain_sock(self.log_sock, wait_for_one=False): try: @@ -546,7 +544,8 @@ def state_transition(self, CS): if self.events.contains(ET.ENABLE): if self.events.contains(ET.NO_ENTRY): self.current_alert_types.append(ET.NO_ENTRY) - self.no_entry_alert_triggered = True + if self.frogpilot_toggles.random_events: + self.no_entry_alert_triggered = True else: if self.events.contains(ET.PRE_ENABLE): @@ -673,7 +672,8 @@ def state_control(self, CS): max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99 if undershooting and turning and good_speed and max_torque: lac_log.active and self.events.add(EventName.goatSteerSaturated if self.frogpilot_toggles.goat_scream_alert else EventName.steerSaturated) - self.steer_saturated_event_triggered = True + if self.frogpilot_toggles.random_events: + self.steer_saturated_event_triggered = True else: self.steer_saturated_event_triggered = False elif lac_log.saturated: @@ -707,11 +707,11 @@ def state_control(self, CS): if self.CP.openpilotLongitudinalControl: if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents) or self.onroad_distance_pressed: menu_open = self.display_timer > 0 or not self.sm['frogpilotCarState'].hasMenu - if not (self.sm['frogpilotCarState'].distanceLongPressed or self.params_memory.get_bool("OnroadDistanceButtonPressed")) and menu_open: + if not (self.sm['frogpilotCarState'].distanceLongPressed or params_memory.get_bool("OnroadDistanceButtonPressed")) and menu_open: self.personality = (self.personality - 1) % 3 self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) self.display_timer = 350 - self.onroad_distance_pressed = self.params_memory.get_bool("OnroadDistanceButtonPressed") + self.onroad_distance_pressed = params_memory.get_bool("OnroadDistanceButtonPressed") self.display_timer -= 1 @@ -735,9 +735,9 @@ def update_frogpilot_variables(self, CS): if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents): if self.frogpilot_toggles.experimental_mode_via_lkas and self.enabled: if self.frogpilot_toggles.conditional_experimental_mode: - conditional_status = self.params_memory.get_int("CEStatus") + conditional_status = params_memory.get_int("CEStatus") override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 3 if conditional_status >= 7 else 4 - self.params_memory.put_int("CEStatus", override_value) + params_memory.put_int("CEStatus", override_value) else: self.experimental_mode = not self.experimental_mode self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 7fcf99a16e01b1..d39bd5d6c8f044 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -3,7 +3,6 @@ from cereal import car, log from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp -from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL # WARNING: this value was determined based on the model's training distribution, diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 4cc7572a007d12..d1b99957321fce 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -10,10 +10,11 @@ import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV from openpilot.common.git import get_short_branch -from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER +from openpilot.selfdrive.frogpilot.frogpilot_variables import params + AlertSize = log.ControlsState.AlertSize AlertStatus = log.ControlsState.AlertStatus VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -381,7 +382,7 @@ def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S def torque_nn_load_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: - model_name = Params().get("NNFFModelName", encoding='utf-8') + model_name = params.get("NNFFModelName", encoding='utf-8') if model_name == "": return Alert( "NNFF Torque Controller not available", diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 7e4a780263c449..dc1d9267af6532 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -89,7 +89,7 @@ def long_control_state_trans_old_long(CP, active, long_control_state, v_ego, v_t class LongControl: def __init__(self, CP): self.CP = CP - self.long_control_state = LongCtrlState.off # initialized to off + self.long_control_state = LongCtrlState.off self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 0280f3e936bde8..c710cb79ffc56a 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -162,8 +162,8 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): @staticmethod def parse_model(model_msg, model_error, v_ego, taco_tune): if (len(model_msg.position.x) == ModelConstants.IDX_N and - len(model_msg.velocity.x) == ModelConstants.IDX_N and - len(model_msg.acceleration.x) == ModelConstants.IDX_N): + len(model_msg.velocity.x) == ModelConstants.IDX_N and + len(model_msg.acceleration.x) == ModelConstants.IDX_N): x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index cfe4b2ab31340e..7c242e7c549878 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -59,15 +59,13 @@ def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0 if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) else: - i = self.i + error * self.k_i * self.i_rate - control = self.p + i + self.d + self.f - - # Update when changing i will move the control away from the limits - # or when i will move towards the sign of the error - if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or - (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ - not freeze_integrator: - self.i = i + if not freeze_integrator: + self.i = self.i + error * self.k_i * self.i_rate + + # Clip i to prevent exceeding control limits + control_no_i = self.p + self.d + self.f + control_no_i = clip(control_no_i, self.neg_limit, self.pos_limit) + self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i) control = self.p + self.i + self.d + self.f diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 4d306d4762b85f..c151c816076908 100644 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -35,7 +35,7 @@ def plannerd_thread(): poll='modelV2', ignore_avg_freq=['radarState']) # FrogPilot variables - frogpilot_toggles = get_frogpilot_toggles(True) + frogpilot_toggles = get_frogpilot_toggles() classic_model = frogpilot_toggles.classic_model radarless_model = frogpilot_toggles.radarless_model diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 4288a4ebe061a5..bdb986428abfa8 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -378,7 +378,7 @@ def main(): rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) # FrogPilot variables - frogpilot_toggles = get_frogpilot_toggles(True) + frogpilot_toggles = get_frogpilot_toggles() RD = RadarD(frogpilot_toggles, CP.radarTimeStep, RI.delay) diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/colors/colors.json b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/colors/colors.json new file mode 100644 index 00000000000000..6a91eb709410e1 --- /dev/null +++ b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/colors/colors.json @@ -0,0 +1,44 @@ +{ + "LaneLines": { + "red": 237, + "green": 116, + "blue": 46, + "alpha": 255 + }, + "LeadMarker": { + "red": 237, + "green": 116, + "blue": 46, + "alpha": 255 + }, + "Path": { + "red": 248, + "green": 177, + "blue": 44, + "alpha": 255 + }, + "PathEdge": { + "red": 198, + "green": 142, + "blue": 35, + "alpha": 255 + }, + "Sidebar1": { + "red": 153, + "green": 99, + "blue": 54, + "alpha": 255 + }, + "Sidebar2": { + "red": 248, + "green": 177, + "blue": 44, + "alpha": 255 + }, + "Sidebar3": { + "red": 237, + "green": 116, + "blue": 46, + "alpha": 255 + } +} diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.gif b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.gif new file mode 100644 index 00000000000000..8ba4c7704c167f Binary files /dev/null and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.gif differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.png b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.png deleted file mode 100644 index 627af8aff329cd..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.gif b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.gif new file mode 100644 index 00000000000000..8ba4c7704c167f Binary files /dev/null and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.gif differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.png b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.png deleted file mode 100644 index 627af8aff329cd..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_settings.png b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_settings.png index 55927599758a9b..125c4e54436dce 100644 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_settings.png and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_settings.png differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/signals/traditional_25 b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/signals/traditional_25 new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/signals/turn_signal.gif b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/signals/turn_signal.gif new file mode 100644 index 00000000000000..1561be8373ca8d Binary files /dev/null and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/signals/turn_signal.gif differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/disengage.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/disengage.wav index ba583c41f362d3..050f4f7eaacec0 100644 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/disengage.wav and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/disengage.wav differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/engage.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/engage.wav index 41e9b2d588d4aa..d3154356a3cdf6 100644 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/engage.wav and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/engage.wav differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt.wav deleted file mode 100644 index 1ae77051eb5722..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt_distracted.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt_distracted.wav deleted file mode 100644 index c3d4475caa70ce..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt_distracted.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/refuse.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/refuse.wav deleted file mode 100644 index 0e80f7d127dcd4..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/refuse.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_immediate.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_immediate.wav deleted file mode 100644 index b1815a95867b9e..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_immediate.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_soft.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_soft.wav deleted file mode 100644 index 261c7e1376c672..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_soft.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/steering_wheel/wheel.gif b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/steering_wheel/wheel.gif new file mode 100644 index 00000000000000..f1d730ddb2fa17 Binary files /dev/null and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/steering_wheel/wheel.gif differ diff --git a/selfdrive/frogpilot/assets/model_manager.py b/selfdrive/frogpilot/assets/model_manager.py index d85b1fa3373363..e426c0e187462e 100644 --- a/selfdrive/frogpilot/assets/model_manager.py +++ b/selfdrive/frogpilot/assets/model_manager.py @@ -16,14 +16,11 @@ VERSION = "v10" -DEFAULT_MODEL = "north-dakota" -DEFAULT_MODEL_NAME = "North Dakota (Default)" +DEFAULT_MODEL = "dragon-rider" +DEFAULT_MODEL_NAME = "Dragon Rider" - -def process_model_name(model_name): - cleaned_name = re.sub(r'[🗺️👀📡]', '', model_name) - cleaned_name = re.sub(r'[^a-zA-Z0-9()-]', '', cleaned_name) - return cleaned_name.replace(' ', '').replace('(Default)', '').replace('-', '') +DEFAULT_CLASSIC_MODEL = "north-dakota" +DEFAULT_CLASSIC_MODEL_NAME = "North Dakota (Default)" class ModelManager: @@ -73,19 +70,19 @@ def fetch_all_model_sizes(repo_url): else: return {file['name'].replace('.thneed', ''): file['size'] for file in thneed_files if 'size' in file} except Exception as e: - raise ConnectionError(f"Failed to fetch model sizes from {'GitHub' if 'github' in repo_url else 'GitLab'}: {e}") + handle_request_error(f"Failed to fetch model sizes from {'GitHub' if 'github' in repo_url else 'GitLab'}: {e}", None, None, None, None) return {} @staticmethod def copy_default_model(): - classic_default_model_path = os.path.join(MODELS_PATH, f"{DEFAULT_MODEL}.thneed") + classic_default_model_path = os.path.join(MODELS_PATH, f"{DEFAULT_CLASSIC_MODEL}.thneed") source_path = os.path.join(BASEDIR, "selfdrive", "classic_modeld", "models", "supercombo.thneed") if os.path.isfile(source_path): shutil.copyfile(source_path, classic_default_model_path) print(f"Copied the classic default model from {source_path} to {classic_default_model_path}") - default_model_path = os.path.join(MODELS_PATH, "secret-good-openpilot.thneed") + default_model_path = os.path.join(MODELS_PATH, f"{DEFAULT_MODEL}.thneed") source_path = os.path.join(BASEDIR, "selfdrive", "modeld", "models", "supercombo.thneed") if os.path.isfile(source_path): @@ -154,6 +151,8 @@ def update_model_params(self, model_info, repo_url): self.params.put_bool_nonblocking("ModelsDownloaded", models_downloaded) def are_all_models_downloaded(self, available_models, repo_url): + available_models = set(available_models) - {DEFAULT_MODEL, DEFAULT_CLASSIC_MODEL} + automatically_update_models = self.params.get_bool("AutomaticallyUpdateModels") all_models_downloaded = True @@ -190,7 +189,7 @@ def validate_models(self): current_model = self.params.get("Model", encoding='utf-8') current_model_name = self.params.get("ModelName", encoding='utf-8') - if "(Default)" in current_model_name and current_model_name != DEFAULT_MODEL_NAME: + if "(Default)" in current_model_name and current_model_name != DEFAULT_CLASSIC_MODEL_NAME: self.params.put_nonblocking("ModelName", current_model_name.replace(" (Default)", "")) available_models = self.params.get("AvailableModels", encoding='utf-8') @@ -206,8 +205,8 @@ def validate_models(self): model_name = model_file.replace(".thneed", "") if model_name not in available_models.split(','): if model_name == current_model: - self.params.put_nonblocking("Model", DEFAULT_MODEL) - self.params.put_nonblocking("ModelName", DEFAULT_MODEL_NAME) + self.params.put_nonblocking("Model", DEFAULT_CLASSIC_MODEL) + self.params.put_nonblocking("ModelName", DEFAULT_CLASSIC_MODEL_NAME) delete_file(os.path.join(MODELS_PATH, model_file)) print(f"Deleted model file: {model_file} - Reason: Model is not in the list of available models") diff --git a/selfdrive/frogpilot/assets/other_images/original_bg.jpg b/selfdrive/frogpilot/assets/other_images/original_bg.jpg new file mode 100644 index 00000000000000..13c36a7e240359 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/original_bg.jpg differ diff --git a/selfdrive/frogpilot/assets/theme_manager.py b/selfdrive/frogpilot/assets/theme_manager.py index c7676ce3cfff82..52498db67b6fe4 100644 --- a/selfdrive/frogpilot/assets/theme_manager.py +++ b/selfdrive/frogpilot/assets/theme_manager.py @@ -10,17 +10,16 @@ from dateutil import easter from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params from openpilot.selfdrive.frogpilot.assets.download_functions import GITHUB_URL, GITLAB_URL, download_file, get_repository_url, handle_error, handle_request_error, verify_download from openpilot.selfdrive.frogpilot.frogpilot_functions import ACTIVE_THEME_PATH, THEME_SAVE_PATH -from openpilot.selfdrive.frogpilot.frogpilot_utilities import update_frogpilot_toggles +from openpilot.selfdrive.frogpilot.frogpilot_variables import params, params_memory, update_frogpilot_toggles CANCEL_DOWNLOAD_PARAM = "CancelThemeDownload" DOWNLOAD_PROGRESS_PARAM = "ThemeDownloadProgress" -def update_theme_asset(asset_type, theme, holiday_theme, params): +def update_theme_asset(asset_type, theme, holiday_theme): save_location = os.path.join(ACTIVE_THEME_PATH, asset_type) if holiday_theme: @@ -81,9 +80,6 @@ def update_wheel_image(image, holiday_theme=None, random_event=True): class ThemeManager: def __init__(self): - self.params = Params() - self.params_memory = Params("/dev/shm/params") - self.previous_assets = {} @staticmethod @@ -195,30 +191,30 @@ def update_holiday(self): for holiday, holiday_date in holidays.items(): if (holiday.endswith("_week") and self.is_within_week_of(holiday_date, now)) or (now == holiday_date): if holiday != self.previous_assets.get("holiday_theme"): - self.params.put("CurrentHolidayTheme", holiday) - self.params_memory.put_bool("UpdateTheme", True) + params.put("CurrentHolidayTheme", holiday) + params_memory.put_bool("UpdateTheme", True) return if "holiday_theme" in self.previous_assets: - self.params.remove("CurrentHolidayTheme") - self.params_memory.put_bool("UpdateTheme", True) + params.remove("CurrentHolidayTheme") + params_memory.put_bool("UpdateTheme", True) self.previous_assets.pop("holiday_theme") def update_active_theme(self): if not os.path.exists(THEME_SAVE_PATH): return - holiday_themes = self.params.get_bool("HolidayThemes") - current_holiday_theme = self.params.get("CurrentHolidayTheme", encoding='utf-8') if holiday_themes else None + holiday_themes = params.get_bool("HolidayThemes") + current_holiday_theme = params.get("CurrentHolidayTheme", encoding='utf-8') if holiday_themes else None - if not current_holiday_theme and self.params.get_bool("PersonalizeOpenpilot"): + if not current_holiday_theme and params.get_bool("PersonalizeOpenpilot"): asset_mappings = { - "color_scheme": ("colors", self.params.get("CustomColors", encoding='utf-8')), - "distance_icons": ("distance_icons", self.params.get("CustomDistanceIcons", encoding='utf-8')), - "icon_pack": ("icons", self.params.get("CustomIcons", encoding='utf-8')), - "sound_pack": ("sounds", self.params.get("CustomSounds", encoding='utf-8')), - "turn_signal_pack": ("signals", self.params.get("CustomSignals", encoding='utf-8')), - "wheel_image": ("wheel_image", self.params.get("WheelIcon", encoding='utf-8')) + "color_scheme": ("colors", params.get("CustomColors", encoding='utf-8')), + "distance_icons": ("distance_icons", params.get("CustomDistanceIcons", encoding='utf-8')), + "icon_pack": ("icons", params.get("CustomIcons", encoding='utf-8')), + "sound_pack": ("sounds", params.get("CustomSounds", encoding='utf-8')), + "turn_signal_pack": ("signals", params.get("CustomSignals", encoding='utf-8')), + "wheel_image": ("wheel_image", params.get("WheelIcon", encoding='utf-8')) } else: asset_mappings = { @@ -238,7 +234,7 @@ def update_active_theme(self): if asset_type == "wheel_image": update_wheel_image(current_value, current_holiday_theme, random_event=False) else: - update_theme_asset(asset_type, current_value, current_holiday_theme, self.params) + update_theme_asset(asset_type, current_value, current_holiday_theme) self.previous_assets[asset] = current_value theme_changed = True @@ -246,7 +242,7 @@ def update_active_theme(self): if theme_changed: if current_holiday_theme: self.previous_assets["holiday_theme"] = current_holiday_theme - self.params_memory.put_bool("ThemeUpdated", True) + params_memory.put_bool("ThemeUpdated", True) update_frogpilot_toggles() def extract_zip(self, zip_file, extract_path): @@ -258,8 +254,8 @@ def extract_zip(self, zip_file, extract_path): def handle_existing_theme(self, theme_name, theme_param): print(f"Theme {theme_name} already exists, skipping download...") - self.params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Theme already exists...") - self.params_memory.remove(theme_param) + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Theme already exists...") + params_memory.remove(theme_param) def handle_verification_failure(self, extensions, theme_component, theme_name, theme_param, download_path): if theme_component == "distance_icons": @@ -274,24 +270,24 @@ def handle_verification_failure(self, extensions, theme_component, theme_name, t temp_theme_path = f"{os.path.splitext(theme_path)[0]}_temp{ext}" theme_url = download_link + ext print(f"Downloading theme from GitLab: {theme_name}") - download_file(CANCEL_DOWNLOAD_PARAM, theme_path, temp_theme_path, DOWNLOAD_PROGRESS_PARAM, theme_url, theme_param, self.params_memory) + download_file(CANCEL_DOWNLOAD_PARAM, theme_path, temp_theme_path, DOWNLOAD_PROGRESS_PARAM, theme_url, theme_param, params_memory) if verify_download(theme_path, temp_theme_path, theme_url): print(f"Theme {theme_name} downloaded and verified successfully from GitLab!") if ext == ".zip": - self.params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Unpacking theme...") + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Unpacking theme...") self.extract_zip(theme_path, os.path.join(THEME_SAVE_PATH, theme_name)) - self.params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") - self.params_memory.remove(theme_param) + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") + params_memory.remove(theme_param) return True - handle_error(download_path, "GitLab verification failed", "Verification failed", theme_param, DOWNLOAD_PROGRESS_PARAM, self.params_memory) + handle_error(download_path, "GitLab verification failed", "Verification failed", theme_param, DOWNLOAD_PROGRESS_PARAM, params_memory) return False def download_theme(self, theme_component, theme_name, theme_param): repo_url = get_repository_url() if not repo_url: - handle_error(None, "GitHub and GitLab are offline...", "Repository unavailable", theme_param, DOWNLOAD_PROGRESS_PARAM, self.params_memory) + handle_error(None, "GitHub and GitLab are offline...", "Repository unavailable", theme_param, DOWNLOAD_PROGRESS_PARAM, params_memory) return if theme_component == "distance_icons": @@ -312,20 +308,20 @@ def download_theme(self, theme_component, theme_name, theme_param): temp_theme_path = f"{os.path.splitext(theme_path)[0]}_temp{ext}" if os.path.isfile(theme_path): - handle_error(theme_path, "Theme already exists...", "Theme already exists...", theme_param, DOWNLOAD_PROGRESS_PARAM, self.params_memory) + handle_error(theme_path, "Theme already exists...", "Theme already exists...", theme_param, DOWNLOAD_PROGRESS_PARAM, params_memory) return theme_url = download_link + ext print(f"Downloading theme from GitHub: {theme_name}") - download_file(CANCEL_DOWNLOAD_PARAM, theme_path, temp_theme_path, DOWNLOAD_PROGRESS_PARAM, theme_url, theme_param, self.params_memory) + download_file(CANCEL_DOWNLOAD_PARAM, theme_path, temp_theme_path, DOWNLOAD_PROGRESS_PARAM, theme_url, theme_param, params_memory) if verify_download(theme_path, temp_theme_path, theme_url): print(f"Theme {theme_name} downloaded and verified successfully from GitHub!") if ext == ".zip": - self.params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Unpacking theme...") + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Unpacking theme...") self.extract_zip(theme_path, download_path) - self.params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") - self.params_memory.remove(theme_param) + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") + params_memory.remove(theme_param) return self.handle_verification_failure(extensions, theme_component, theme_name, theme_param, download_path) @@ -339,27 +335,27 @@ def filter_existing_assets(assets, subfolder): } return sorted(set(assets) - existing_themes) - self.params.put("DownloadableColors", ','.join(filter_existing_assets(downloadable_colors, "colors"))) + params.put("DownloadableColors", ','.join(filter_existing_assets(downloadable_colors, "colors"))) print("Colors list updated successfully.") distance_icons_directory = os.path.join(THEME_SAVE_PATH, "distance_icons") - self.params.put("DownloadableDistanceIcons", ','.join(sorted(set(downloadable_distance_icons) - { + params.put("DownloadableDistanceIcons", ','.join(sorted(set(downloadable_distance_icons) - { distance_icons.replace('_', ' ').split('.')[0].title() for distance_icons in os.listdir(distance_icons_directory) })) ) - self.params.put("DownloadableIcons", ','.join(filter_existing_assets(downloadable_icons, "icons"))) + params.put("DownloadableIcons", ','.join(filter_existing_assets(downloadable_icons, "icons"))) print("Icons list updated successfully.") - self.params.put("DownloadableSignals", ','.join(filter_existing_assets(downloadable_signals, "signals"))) + params.put("DownloadableSignals", ','.join(filter_existing_assets(downloadable_signals, "signals"))) print("Signals list updated successfully.") - self.params.put("DownloadableSounds", ','.join(filter_existing_assets(downloadable_sounds, "sounds"))) + params.put("DownloadableSounds", ','.join(filter_existing_assets(downloadable_sounds, "sounds"))) print("Sounds list updated successfully.") wheel_directory = os.path.join(THEME_SAVE_PATH, "steering_wheels") - self.params.put("DownloadableWheels", ','.join(sorted(set(downloadable_wheels) - { + params.put("DownloadableWheels", ','.join(sorted(set(downloadable_wheels) - { wheel.replace('_', ' ').split('.')[0].title() for wheel in os.listdir(wheel_directory) if wheel != "img_chffr_wheel.png" })) @@ -376,7 +372,7 @@ def validate_themes(self): } for theme_param, theme_component in asset_mappings.items(): - theme_name = self.params.get(theme_param, encoding='utf-8') + theme_name = params.get(theme_param, encoding='utf-8') if not theme_name or theme_name == "stock": continue diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 948b9c83a22836..313b3bf6fbbccd 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -1,7 +1,6 @@ import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV -from openpilot.common.params import Params from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_UNSET from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE @@ -17,8 +16,6 @@ class FrogPilotPlanner: def __init__(self): - self.params_memory = Params("/dev/shm/params") - self.cem = ConditionalExperimentalMode(self) self.frogpilot_acceleration = FrogPilotAcceleration(self) self.frogpilot_events = FrogPilotEvents(self) diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py index cf070263a8c559..2a1e292bde444d 100644 --- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -1,15 +1,12 @@ -from openpilot.common.params import Params - from openpilot.selfdrive.frogpilot.frogpilot_utilities import MovingAverageCalculator -from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD +from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory class ConditionalExperimentalMode: def __init__(self, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner - self.params_memory = Params("/dev/shm/params") - self.curvature_mac = MovingAverageCalculator() + self.slow_lead_mac = MovingAverageCalculator() self.stop_light_mac = MovingAverageCalculator() self.curve_detected = False @@ -18,14 +15,14 @@ def __init__(self, FrogPilotPlanner): def update(self, carState, frogpilotCarState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles): if frogpilot_toggles.experimental_mode_via_press: - self.status_value = self.params_memory.get_int("CEStatus") + self.status_value = params_memory.get_int("CEStatus") else: self.status_value = 0 if self.status_value not in {1, 2, 3, 4, 5, 6} and not carState.standstill: self.update_conditions(frogpilotCarState, self.frogpilot_planner.tracking_lead, v_ego, v_lead, frogpilot_toggles) self.experimental_mode = self.check_conditions(carState, frogpilotNavigation, modelData, self.frogpilot_planner.frogpilot_following.following_lead, v_ego, v_lead, frogpilot_toggles) - self.params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else 0) + params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else 0) else: self.experimental_mode = self.status_value in {2, 4, 6} or carState.standstill and self.experimental_mode self.stop_light_detected &= self.status_value not in {1, 2, 3, 4, 5, 6} @@ -63,7 +60,6 @@ def check_conditions(self, carState, frogpilotNavigation, modelData, following_l if self.frogpilot_planner.frogpilot_vcruise.slc.experimental_mode: self.status_value = 17 return True - return False def update_conditions(self, frogpilotCarState, tracking_lead, v_ego, v_lead, frogpilot_toggles): @@ -72,19 +68,25 @@ def update_conditions(self, frogpilotCarState, tracking_lead, v_ego, v_lead, fro self.stop_sign_and_light(frogpilotCarState, tracking_lead, v_ego, frogpilot_toggles) def curve_detection(self, tracking_lead, v_ego, frogpilot_toggles): - curve_detected = (1 / self.frogpilot_planner.road_curvature)**0.5 < v_ego - curve_active = (0.9 / self.frogpilot_planner.road_curvature)**0.5 < v_ego and self.curve_detected + if v_ego > CRUISING_SPEED: + curve_detected = (1 / self.frogpilot_planner.road_curvature)**0.5 < v_ego + curve_active = (0.9 / self.frogpilot_planner.road_curvature)**0.5 < v_ego and self.curve_detected - self.curvature_mac.add_data(curve_detected or curve_active) - self.curve_detected = self.curvature_mac.get_moving_average() >= THRESHOLD + self.curvature_mac.add_data(curve_detected or curve_active) + self.curve_detected = self.curvature_mac.get_moving_average() >= THRESHOLD + else: + self.curvature_mac.reset_data() + self.curve_detected = False def slow_lead(self, tracking_lead, v_lead, frogpilot_toggles): if tracking_lead: - slower_lead = self.frogpilot_planner.frogpilot_following.slower_lead and frogpilot_toggles.conditional_slower_lead + slower_lead = frogpilot_toggles.conditional_slower_lead and self.frogpilot_planner.frogpilot_following.slower_lead stopped_lead = frogpilot_toggles.conditional_stopped_lead and v_lead < 1 - self.slow_lead_detected = slower_lead or stopped_lead + self.slow_lead_mac.add_data(slower_lead or stopped_lead) + self.slow_lead_detected = self.slow_lead_mac.get_moving_average() >= THRESHOLD else: + self.slow_lead_mac.reset_data() self.slow_lead_detected = False def stop_sign_and_light(self, frogpilotCarState, tracking_lead, v_ego, frogpilot_toggles): diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_events.py b/selfdrive/frogpilot/controls/lib/frogpilot_events.py index 147a54fd32ef3b..d20c3d3db74299 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_events.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_events.py @@ -4,19 +4,15 @@ import openpilot.system.sentry as sentry from openpilot.common.conversions import Conversions as CV -from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.controlsd import Desire from openpilot.selfdrive.controls.lib.events import EventName, Events from openpilot.selfdrive.frogpilot.assets.theme_manager import update_wheel_image -from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED +from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, params, params_memory class FrogPilotEvents: def __init__(self, FrogPilotPlanner): - self.params = Params() - self.params_memory = Params("/dev/shm/params") - self.events = Events() self.frogpilot_planner = FrogPilotPlanner @@ -50,7 +46,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState self.random_event_timer += DT_MDL if self.random_event_timer >= 4: update_wheel_image(frogpilot_toggles.wheel_image, frogpilot_toggles.current_holiday_theme, False) - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.random_event_played = False self.random_event_timer = 0 @@ -98,7 +94,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState if not self.accel30_played and 3.5 > self.max_acceleration >= 3.0 and acceleration < 1.5: self.events.add(EventName.accel30) update_wheel_image("weeb_wheel") - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.accel30_played = True self.random_event_played = True self.max_acceleration = 0 @@ -106,7 +102,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState elif not self.accel35_played and 4.0 > self.max_acceleration >= 3.5 and acceleration < 1.5: self.events.add(EventName.accel35) update_wheel_image("tree_fiddy") - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.accel35_played = True self.random_event_played = True self.max_acceleration = 0 @@ -114,7 +110,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState elif not self.accel40_played and self.max_acceleration >= 4.0 and acceleration < 1.5: self.events.add(EventName.accel40) update_wheel_image("great_scott") - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.accel40_played = True self.random_event_played = True self.max_acceleration = 0 @@ -144,17 +140,17 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState if event_choice == "firefoxSteerSaturated": self.events.add(EventName.firefoxSteerSaturated) update_wheel_image("firefox") - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.firefox_played = True elif event_choice == "goatSteerSaturated": self.events.add(EventName.goatSteerSaturated) update_wheel_image("goat") - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.goat_played = True elif event_choice == "thisIsFineSteerSaturated": self.events.add(EventName.thisIsFineSteerSaturated) update_wheel_image("this_is_fine") - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.this_is_fine_played = True self.random_event_played = True @@ -178,7 +174,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState if frogpilot_toggles.speed_limit_changed_alert and self.frogpilot_planner.frogpilot_vcruise.speed_limit_changed: self.events.add(EventName.speedLimitChanged) - if self.frame == 4 and self.params.get("NNFFModelName", encoding='utf-8') is not None: + if self.frame == 4 and params.get("NNFFModelName", encoding='utf-8') is not None: self.events.add(EventName.torqueNNLoad) if frogpilotCarState.trafficModeActive != self.previous_traffic_mode: diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_tracking.py b/selfdrive/frogpilot/controls/lib/frogpilot_tracking.py index d23ce9665241c5..3606133993853e 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_tracking.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_tracking.py @@ -1,13 +1,12 @@ -from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.frogpilot.frogpilot_variables import params_tracking + class FrogPilotTracking: def __init__(self): - self.params_tracking = Params("/persist/tracking") - - self.total_drives = self.params_tracking.get_int("FrogPilotDrives") - self.total_kilometers = self.params_tracking.get_float("FrogPilotKilometers") - self.total_minutes = self.params_tracking.get_float("FrogPilotMinutes") + self.total_drives = params_tracking.get_int("FrogPilotDrives") + self.total_kilometers = params_tracking.get_float("FrogPilotKilometers") + self.total_minutes = params_tracking.get_float("FrogPilotMinutes") self.drive_added = False @@ -21,14 +20,14 @@ def update(self, carState): if self.drive_time > 60 and carState.standstill: self.total_kilometers += self.drive_distance / 1000 - self.params_tracking.put_float_nonblocking("FrogPilotKilometers", self.total_kilometers) + params_tracking.put_float_nonblocking("FrogPilotKilometers", self.total_kilometers) self.drive_distance = 0 self.total_minutes += self.drive_time / 60 - self.params_tracking.put_float_nonblocking("FrogPilotMinutes", self.total_minutes) + params_tracking.put_float_nonblocking("FrogPilotMinutes", self.total_minutes) self.drive_time = 0 if not self.drive_added and (self.total_minutes - self.starting_total_minutes > 15): self.total_drives += 1 - self.params_tracking.put_int_nonblocking("FrogPilotDrives", self.total_drives) + params_tracking.put_int_nonblocking("FrogPilotDrives", self.total_drives) self.drive_added = True diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py b/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py index 09a9bab4b64ed4..18219c9ff181d2 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py @@ -7,7 +7,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController -from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME +from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, params_memory TARGET_LAT_A = 1.9 @@ -15,8 +15,6 @@ class FrogPilotVCruise: def __init__(self, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner - self.params_memory = self.frogpilot_planner.params_memory - self.mtsc = MapTurnSpeedController() self.slc = SpeedLimitController() @@ -81,22 +79,22 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState self.slc.update(frogpilotCarState.dashboardSpeedLimit, controlsState.enabled, frogpilotNavigation.navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles) unconfirmed_slc_target = self.slc.desired_speed_limit - if (frogpilot_toggles.speed_limit_changed_alert or frogpilot_toggles.speed_limit_confirmation_lower or frogpilot_toggles.speed_limit_confirmation_higher) and self.slc_target != 0: + if (frogpilot_toggles.speed_limit_changed_alert or frogpilot_toggles.speed_limit_confirmation) and self.slc_target != 0: self.speed_limit_changed = unconfirmed_slc_target != self.previous_speed_limit and abs(self.slc_target - unconfirmed_slc_target) > 1 and unconfirmed_slc_target > 1 speed_limit_decreased = self.speed_limit_changed and self.slc_target > unconfirmed_slc_target speed_limit_increased = self.speed_limit_changed and self.slc_target < unconfirmed_slc_target - accepted_via_ui = self.params_memory.get_bool("SLCConfirmedPressed") and self.params_memory.get_bool("SLCConfirmed") - denied_via_ui = self.params_memory.get_bool("SLCConfirmedPressed") and not self.params_memory.get_bool("SLCConfirmed") + accepted_via_ui = params_memory.get_bool("SLCConfirmedPressed") and params_memory.get_bool("SLCConfirmed") + denied_via_ui = params_memory.get_bool("SLCConfirmedPressed") and not params_memory.get_bool("SLCConfirmed") - speed_limit_accepted = frogpilotCarControl.resumePressed or accepted_via_ui - speed_limit_denied = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents) or denied_via_ui or self.speed_limit_timer >= 10 + speed_limit_accepted = frogpilotCarControl.resumePressed and controlsState.enabled or accepted_via_ui + speed_limit_denied = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents) and controlsState.enabled or denied_via_ui or self.speed_limit_timer >= 10 if speed_limit_accepted or speed_limit_denied: self.previous_speed_limit = unconfirmed_slc_target - self.params_memory.put_bool("SLCConfirmed", False) - self.params_memory.put_bool("SLCConfirmedPressed", False) + params_memory.put_bool("SLCConfirmed", False) + params_memory.put_bool("SLCConfirmedPressed", False) if speed_limit_decreased: speed_limit_confirmed = not frogpilot_toggles.speed_limit_confirmation_lower or speed_limit_accepted diff --git a/selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py b/selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py index f72e374ebf5f0c..38623843fdfa17 100644 --- a/selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py +++ b/selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py @@ -3,12 +3,9 @@ import math from openpilot.common.numpy_fast import interp -from openpilot.common.params import Params from openpilot.selfdrive.frogpilot.frogpilot_utilities import calculate_distance_to_point -from openpilot.selfdrive.frogpilot.frogpilot_variables import TO_RADIANS - -params_memory = Params("/dev/shm/params") +from openpilot.selfdrive.frogpilot.frogpilot_variables import TO_RADIANS, params_memory TARGET_JERK = -0.6 # m/s^3 should match up with the long planner TARGET_ACCEL = -1.2 # m/s^2 should match up with the long planner diff --git a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py index 27a9da9e2ebca5..c7044ded094ffd 100644 --- a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py +++ b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py @@ -1,23 +1,18 @@ # PFEIFER - SLC - Modified by FrogAi for FrogPilot import json -from openpilot.common.params import Params - from openpilot.selfdrive.frogpilot.frogpilot_utilities import calculate_distance_to_point -from openpilot.selfdrive.frogpilot.frogpilot_variables import TO_RADIANS +from openpilot.selfdrive.frogpilot.frogpilot_variables import TO_RADIANS, params, params_memory class SpeedLimitController: def __init__(self): - self.params = Params() - self.params_memory = Params("/dev/shm/params") - self.experimental_mode = False self.desired_speed_limit = 0 self.offset = 0 self.speed_limit = 0 - self.previous_speed_limit = self.params.get_float("PreviousSpeedLimit") + self.previous_speed_limit = params.get_float("PreviousSpeedLimit") def update(self, dashboard_speed_limit, enabled, navigation_speed_limit, v_cruise, v_ego, frogpilot_toggles): map_speed_limit = self.get_map_speed_limit(v_ego, frogpilot_toggles) @@ -32,20 +27,20 @@ def update(self, dashboard_speed_limit, enabled, navigation_speed_limit, v_cruis def get_desired_speed_limit(self): if self.speed_limit > 1: if self.previous_speed_limit != self.speed_limit: - self.params.put_float_nonblocking("PreviousSpeedLimit", self.speed_limit) + params.put_float_nonblocking("PreviousSpeedLimit", self.speed_limit) self.previous_speed_limit = self.speed_limit return self.speed_limit + self.offset return 0 def get_map_speed_limit(self, v_ego, frogpilot_toggles): - map_speed_limit = self.params_memory.get_float("MapSpeedLimit") + map_speed_limit = params_memory.get_float("MapSpeedLimit") - next_map_speed_limit = json.loads(self.params_memory.get("NextMapSpeedLimit", "{}")) + next_map_speed_limit = json.loads(params_memory.get("NextMapSpeedLimit", "{}")) next_map_speed_limit_lat = next_map_speed_limit.get("latitude", 0) next_map_speed_limit_lon = next_map_speed_limit.get("longitude", 0) next_map_speed_limit_value = next_map_speed_limit.get("speedlimit", 0) - position = json.loads(self.params_memory.get("LastGPSPosition", "{}")) + position = json.loads(params_memory.get("LastGPSPosition", "{}")) lat = position.get("latitude", 0) lon = position.get("longitude", 0) diff --git a/selfdrive/frogpilot/fleetmanager/helpers.py b/selfdrive/frogpilot/fleetmanager/helpers.py index d20700ac46cdee..cbc9cfac6a3f27 100644 --- a/selfdrive/frogpilot/fleetmanager/helpers.py +++ b/selfdrive/frogpilot/fleetmanager/helpers.py @@ -20,6 +20,7 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +import base64 import json import math import os @@ -43,17 +44,15 @@ from urllib.parse import parse_qs, quote import openpilot.system.sentry as sentry -from openpilot.selfdrive.frogpilot.frogpilot_utilities import update_frogpilot_toggles +from openpilot.selfdrive.frogpilot.frogpilot_variables import params, params_storage, update_frogpilot_toggles + +XOR_KEY = "s8#pL3*Xj!aZ@dWq" pi = 3.1415926535897932384626 x_pi = 3.14159265358979324 * 3000.0 / 180.0 a = 6378245.0 ee = 0.00669342162296594323 -params = Params() -params_memory = Params("/dev/shm/params") -params_storage = Params("/persist/params") - PRESERVE_ATTR_NAME = 'user.preserve' PRESERVE_ATTR_VALUE = b'1' PRESERVE_COUNT = 5 @@ -438,6 +437,20 @@ def transform_lng(lng, lat): ret += (150.0 * math.sin(lng / 12.0 * pi) + 300.0 * math.sin(lng / 30.0 * pi)) * 2.0 / 3.0 return ret +def xor_encrypt_decrypt(data: str, key: str) -> str: + return ''.join(chr(ord(c) ^ ord(key[i % len(key)])) for i, c in enumerate(data)) + +def encode_parameters(params_dict): + serialized_data = json.dumps(params_dict) + obfuscated_data = xor_encrypt_decrypt(serialized_data, XOR_KEY) + encoded_data = base64.b64encode(obfuscated_data.encode('utf-8')).decode('utf-8') + return encoded_data + +def decode_parameters(encoded_string): + obfuscated_data = base64.b64decode(encoded_string.encode('utf-8')).decode('utf-8') + decrypted_data = xor_encrypt_decrypt(obfuscated_data, XOR_KEY) + return json.loads(decrypted_data) + def get_all_toggle_values(): toggle_values = {} @@ -453,16 +466,29 @@ def get_all_toggle_values(): value = "0" toggle_values[key] = value if value is not None else "0" - return toggle_values + return encode_parameters(toggle_values) + +def store_toggle_values(request_data): + current_parameters = { + key.decode('utf-8') if isinstance(key, bytes) else key: None + for key in params.all_keys() if params.get_key_type(key) & ParamKeyType.FROGPILOT_STORAGE + } + decoded_values = decode_parameters(request_data['data']) -def store_toggle_values(updated_values): - for key, value in updated_values.items(): + for key in current_parameters: + print(f"Processing key: {key}") + value = decoded_values.get(key, "0") try: if isinstance(value, (int, float)): value = str(value) - params.put(key, value.encode('utf-8')) - params_storage.put(key, value.encode('utf-8')) + print(f"value: {value}") + params.put(key, value) + params_storage.put(key, value) except Exception as e: print(f"Failed to update {key}: {e}") + extra_keys = set(decoded_values.keys()) - set(current_parameters.keys()) + if extra_keys: + print(f"Warning: Ignoring extra keys: {extra_keys}") + update_frogpilot_toggles() diff --git a/selfdrive/frogpilot/fleetmanager/templates/tools.html b/selfdrive/frogpilot/fleetmanager/templates/tools.html index 12ac34798190c2..04347d0c1d93d0 100644 --- a/selfdrive/frogpilot/fleetmanager/templates/tools.html +++ b/selfdrive/frogpilot/fleetmanager/templates/tools.html @@ -29,40 +29,33 @@

Toggle Values

- + - +