diff --git a/cereal/log.capnp b/cereal/log.capnp index 9a944eac3429f6..77ddeb835aa923 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -622,6 +622,7 @@ struct RadarState @0x9a185389d6fdd05f { leadRight @14 :LeadData; leadLeftFar @15 :LeadData; leadRightFar @16 :LeadData; + leadsLead @17 :LeadData; cumLagMs @5 :Float32; struct LeadData { diff --git a/common/params.cc b/common/params.cc index 6d64a1d120ccfa..55478afad90fb4 100644 --- a/common/params.cc +++ b/common/params.cc @@ -132,8 +132,8 @@ std::unordered_map keys = { {"GithubUsername", PERSISTENT}, {"GitRemote", PERSISTENT}, {"GsmApn", PERSISTENT}, - {"GsmMetered", PERSISTENT}, - {"GsmRoaming", PERSISTENT}, + {"GsmMetered", PERSISTENT | FROGPILOT_STORAGE}, + {"GsmRoaming", PERSISTENT | FROGPILOT_STORAGE}, {"HardwareSerial", PERSISTENT}, {"HasAcceptedTerms", PERSISTENT}, {"IMEI", PERSISTENT}, @@ -163,7 +163,7 @@ std::unordered_map keys = { {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"NavPastDestinations", PERSISTENT}, - {"NavSettingLeftSide", PERSISTENT}, + {"NavSettingLeftSide", PERSISTENT | FROGPILOT_STORAGE}, {"NavSettingTime24h", PERSISTENT}, {"NetworkMetered", PERSISTENT}, {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, @@ -180,18 +180,18 @@ std::unordered_map keys = { {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START}, {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START}, {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, - {"OpenpilotEnabledToggle", PERSISTENT}, + {"OpenpilotEnabledToggle", PERSISTENT | FROGPILOT_STORAGE}, {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"PandaSomResetTriggered", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"PandaSignatures", CLEAR_ON_MANAGER_START}, {"PrimeType", PERSISTENT}, - {"RecordFront", PERSISTENT}, + {"RecordFront", PERSISTENT | FROGPILOT_STORAGE}, {"RecordFrontLock", PERSISTENT}, // for the internal fleet {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"RouteCount", PERSISTENT}, {"SecOCKey", PERSISTENT | DONT_LOG}, {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, - {"SshEnabled", PERSISTENT}, + {"SshEnabled", PERSISTENT | FROGPILOT_STORAGE}, {"TermsVersion", PERSISTENT}, {"Timezone", PERSISTENT}, {"TrainingVersion", PERSISTENT}, @@ -217,12 +217,12 @@ std::unordered_map keys = { {"AdjacentPathMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"AdvancedCustomUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"AdvancedLateralTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"AggressiveFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"AggressiveJerkAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"AggressiveJerkDanger", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"AggressiveJerkDeceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"AggressiveJerkSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"AggressiveJerkSpeedDecrease", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AggressiveFollow", PERSISTENT | FROGPILOT_CONTROLS}, + {"AggressiveJerkAcceleration", PERSISTENT | FROGPILOT_CONTROLS}, + {"AggressiveJerkDanger", PERSISTENT | FROGPILOT_CONTROLS}, + {"AggressiveJerkDeceleration", PERSISTENT | FROGPILOT_CONTROLS}, + {"AggressiveJerkSpeed", PERSISTENT | FROGPILOT_CONTROLS}, + {"AggressiveJerkSpeedDecrease", PERSISTENT | FROGPILOT_CONTROLS}, {"AggressivePersonalityProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"AlertVolumeControl", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"AlwaysOnLateral", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -236,7 +236,7 @@ std::unordered_map keys = { {"AvailableModels", PERSISTENT}, {"AvailableModelsNames", PERSISTENT}, {"BigMap", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"BlacklistedModels", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"BlacklistedModels", PERSISTENT | FROGPILOT_CONTROLS}, {"BlindSpotMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"BlindSpotPath", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"BorderMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -254,8 +254,8 @@ std::unordered_map keys = { {"CENavigationIntersections", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CENavigationLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CENavigationTurns", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"CertifiedHerbalistDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"CertifiedHerbalistScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CertifiedHerbalistDrives", PERSISTENT | FROGPILOT_CONTROLS}, + {"CertifiedHerbalistScore", PERSISTENT | FROGPILOT_CONTROLS}, {"CESignalSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CESignalLaneDetection", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CESlowerLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -271,7 +271,7 @@ std::unordered_map keys = { {"CrosstrekTorque", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"CurrentHolidayTheme", CLEAR_ON_MANAGER_START}, {"CurrentModelName", CLEAR_ON_MANAGER_START}, - {"CurveSensitivity", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CurveSensitivity", PERSISTENT | FROGPILOT_CONTROLS}, {"CurveSpeedControl", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CustomAlerts", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"CustomColors", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -279,8 +279,6 @@ std::unordered_map keys = { {"CustomCruiseLong", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CustomDistanceIcons", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CustomIcons", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"CustomizationLevel", PERSISTENT}, - {"CustomizationLevelConfirmed", PERSISTENT}, {"CustomPersonalities", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CustomSignals", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"CustomSounds", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -292,8 +290,8 @@ std::unordered_map keys = { {"DeviceShutdown", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DisableOnroadUploads", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DisableOpenpilotLongitudinal", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, - {"DissolvedOxygenDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"DissolvedOxygenScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DissolvedOxygenDrives", PERSISTENT | FROGPILOT_CONTROLS}, + {"DissolvedOxygenScore", PERSISTENT | FROGPILOT_CONTROLS}, {"DistanceIconToDownload", PERSISTENT}, {"DisengageVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"DoToggleReset", PERSISTENT}, @@ -304,14 +302,14 @@ std::unordered_map keys = { {"DownloadableSounds", PERSISTENT}, {"DownloadableWheels", PERSISTENT}, {"DownloadAllModels", PERSISTENT}, - {"DragonRiderDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"DragonRiderScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DragonRiderDrives", PERSISTENT | FROGPILOT_CONTROLS}, + {"DragonRiderScore", PERSISTENT | FROGPILOT_CONTROLS}, {"DriverCamera", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"DuckAmigoDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"DuckAmigoScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DuckAmigoDrives", PERSISTENT | FROGPILOT_CONTROLS}, + {"DuckAmigoScore", PERSISTENT | FROGPILOT_CONTROLS}, {"DynamicPathWidth", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"DynamicPedalsOnUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"EngageVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"EngageVolume", PERSISTENT | FROGPILOT_VISUALS}, {"ExperimentalGMTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"ExperimentalModeActivation", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ExperimentalModels", PERSISTENT}, @@ -332,8 +330,9 @@ std::unordered_map keys = { {"FrogPilotDrives", PERSISTENT | FROGPILOT_TRACKING}, {"FrogPilotKilometers", PERSISTENT | FROGPILOT_TRACKING}, {"FrogPilotMinutes", PERSISTENT | FROGPILOT_TRACKING}, - {"FrogPilotToggles", PERSISTENT}, + {"FrogPilotToggles", CLEAR_ON_MANAGER_START}, {"FrogPilotTogglesUpdated", PERSISTENT}, + {"FrogPilotTuningLevels", CLEAR_ON_MANAGER_START}, {"FrogsGoMoosTweak", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"FullMap", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"GasRegenCmd", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, @@ -364,27 +363,27 @@ std::unordered_map keys = { {"LateralMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"LateralTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"LeadDepartingAlert", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"LeadDetectionThreshold", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LeadDetectionThreshold", PERSISTENT | FROGPILOT_CONTROLS}, {"LeadInfo", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"LockDoors", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"LongitudinalMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"LongitudinalTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"LongPitch", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, - {"LosAngelesDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"LosAngelesScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LosAngelesDrives", PERSISTENT | FROGPILOT_CONTROLS}, + {"LosAngelesScore", PERSISTENT | FROGPILOT_CONTROLS}, {"LoudBlindspotAlert", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"LowVoltageShutdown", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LowVoltageShutdown", PERSISTENT | FROGPILOT_CONTROLS}, {"ManualUpdateInitiated", PERSISTENT}, {"MapAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"MapboxPublicKey", PERSISTENT}, {"MapboxSecretKey", PERSISTENT}, {"MapDeceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"MapGears", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"MapsSelected", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, + {"MapsSelected", PERSISTENT | FROGPILOT_OTHER}, {"MapSpeedLimit", PERSISTENT}, {"MapStyle", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"MapTargetVelocities", PERSISTENT}, - {"MaxDesiredAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"MaxDesiredAcceleration", PERSISTENT | FROGPILOT_CONTROLS}, {"MinimumBackupSize", PERSISTENT}, {"MinimumLaneChangeSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"Model", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -405,10 +404,10 @@ std::unordered_map keys = { {"NNFFLite", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NNFFModelName", CLEAR_ON_OFFROAD_TRANSITION}, {"NoLogging", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NorthDakotaDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NorthDakotaScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NotreDameDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NotreDameScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NorthDakotaDrives", PERSISTENT | FROGPILOT_CONTROLS}, + {"NorthDakotaScore", PERSISTENT | FROGPILOT_CONTROLS}, + {"NotreDameDrives", PERSISTENT | FROGPILOT_CONTROLS}, + {"NotreDameScore", PERSISTENT | FROGPILOT_CONTROLS}, {"NoUploads", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NudgelessLaneChange", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NumericalTemp", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -433,8 +432,8 @@ std::unordered_map keys = { {"PersonalizeOpenpilot", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"PreferredSchedule", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, {"PreviousSpeedLimit", PERSISTENT}, - {"PromptDistractedVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"PromptVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"PromptDistractedVolume", PERSISTENT | FROGPILOT_VISUALS}, + {"PromptVolume", PERSISTENT | FROGPILOT_VISUALS}, {"QOLLateral", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"QOLLongitudinal", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"QOLVisuals", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -445,13 +444,13 @@ std::unordered_map keys = { {"RandomEvents", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"RecertifiedHerbalistDrives", PERSISTENT | FROGPILOT_CONTROLS}, {"RecertifiedHerbalistScore", PERSISTENT | FROGPILOT_CONTROLS}, - {"RefuseVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"RelaxedFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"RelaxedJerkAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"RelaxedJerkDanger", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"RelaxedJerkDeceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"RelaxedJerkSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"RelaxedJerkSpeedDecrease", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"RefuseVolume", PERSISTENT | FROGPILOT_VISUALS}, + {"RelaxedFollow", PERSISTENT | FROGPILOT_CONTROLS}, + {"RelaxedJerkAcceleration", PERSISTENT | FROGPILOT_CONTROLS}, + {"RelaxedJerkDanger", PERSISTENT | FROGPILOT_CONTROLS}, + {"RelaxedJerkDeceleration", PERSISTENT | FROGPILOT_CONTROLS}, + {"RelaxedJerkSpeed", PERSISTENT | FROGPILOT_CONTROLS}, + {"RelaxedJerkSpeedDecrease", PERSISTENT | FROGPILOT_CONTROLS}, {"RelaxedPersonalityProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ResetFrogTheme", PERSISTENT}, {"ReverseCruise", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -488,7 +487,6 @@ std::unordered_map keys = { {"SLCConfirmationHigher", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SLCConfirmationLower", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SLCConfirmed", PERSISTENT}, - {"SLCConfirmedPressed", PERSISTENT}, {"SLCLookaheadHigher", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SLCLookaheadLower", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SLCFallback", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -502,23 +500,23 @@ std::unordered_map keys = { {"SpeedLimitController", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SpeedLimitSources", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"StandardFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"StandardJerkAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"StandardJerkDanger", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"StandardJerkDeceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"StandardJerkSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"StandardJerkSpeedDecrease", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"StandardJerkAcceleration", PERSISTENT | FROGPILOT_CONTROLS}, + {"StandardJerkDanger", PERSISTENT | FROGPILOT_CONTROLS}, + {"StandardJerkDeceleration", PERSISTENT | FROGPILOT_CONTROLS}, + {"StandardJerkSpeed", PERSISTENT | FROGPILOT_CONTROLS}, + {"StandardJerkSpeedDecrease", PERSISTENT | FROGPILOT_CONTROLS}, {"StandardPersonalityProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"StandbyMode", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"StartupMessageBottom", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"StartupMessageTop", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"StartupMessageBottom", PERSISTENT | FROGPILOT_VISUALS}, + {"StartupMessageTop", PERSISTENT | FROGPILOT_VISUALS}, {"StaticPedalsOnUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"SteerFriction", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SteerFriction", PERSISTENT | FROGPILOT_CONTROLS}, {"SteerFrictionStock", PERSISTENT}, - {"SteerLatAccel", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SteerLatAccel", PERSISTENT | FROGPILOT_CONTROLS}, {"SteerLatAccelStock", PERSISTENT}, - {"SteerKP", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SteerKP", PERSISTENT | FROGPILOT_CONTROLS}, {"SteerKPStock", PERSISTENT}, - {"SteerRatio", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SteerRatio", PERSISTENT | FROGPILOT_CONTROLS}, {"SteerRatioStock", PERSISTENT}, {"StoppedTimer", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"TacoTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -526,15 +524,17 @@ std::unordered_map keys = { {"TetheringEnabled", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, {"ThemeDownloadProgress", PERSISTENT}, {"ToyotaDoors", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, - {"TrafficFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"TrafficJerkAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"TrafficJerkDanger", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"TrafficJerkDeceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"TrafficJerkSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"TrafficJerkSpeedDecrease", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TrafficFollow", PERSISTENT | FROGPILOT_CONTROLS}, + {"TrafficJerkAcceleration", PERSISTENT | FROGPILOT_CONTROLS}, + {"TrafficJerkDanger", PERSISTENT | FROGPILOT_CONTROLS}, + {"TrafficJerkDeceleration", PERSISTENT | FROGPILOT_CONTROLS}, + {"TrafficJerkSpeed", PERSISTENT | FROGPILOT_CONTROLS}, + {"TrafficJerkSpeedDecrease", PERSISTENT | FROGPILOT_CONTROLS}, {"TrafficPersonalityProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"TuningInfo", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"TurnAggressiveness", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TuningLevel", PERSISTENT}, + {"TuningLevelConfirmed", PERSISTENT}, + {"TurnAggressiveness", PERSISTENT | FROGPILOT_CONTROLS}, {"TurnDesires", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"UnlimitedLength", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"UnlockDoors", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, @@ -545,8 +545,8 @@ std::unordered_map keys = { {"UseVienna", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"VisionTurnControl", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"VoltSNG", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, - {"WarningImmediateVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"WarningSoftVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"WarningImmediateVolume", PERSISTENT | FROGPILOT_VISUALS}, + {"WarningSoftVolume", PERSISTENT | FROGPILOT_VISUALS}, {"WD40Drives", PERSISTENT | FROGPILOT_CONTROLS}, {"WD40Score", PERSISTENT | FROGPILOT_CONTROLS}, {"WheelIcon", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 208d0e08980efb..36b236ceabcfdd 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -49,10 +49,18 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805; #define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks // Stock longitudinal -#define TOYOTA_COMMON_TX_MSGS \ - {0x2E4, 0, 5}, {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \ +#define TOYOTA_BASE_TX_MSGS \ + {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \ {0x750, 0, 8}, /* white list 0x750 for Enhanced Diagnostic Request */ \ +#define TOYOTA_COMMON_TX_MSGS \ + TOYOTA_BASE_TX_MSGS \ + {0x2E4, 0, 5}, \ + +#define TOYOTA_COMMON_SECOC_TX_MSGS \ + TOYOTA_BASE_TX_MSGS \ + {0x2E4, 0, 8}, {0x131, 0, 8}, \ + #define TOYOTA_COMMON_LONG_TX_MSGS \ TOYOTA_COMMON_TX_MSGS \ {0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8}, /* DSU bus 0 */ \ @@ -66,7 +74,7 @@ const CanMsg TOYOTA_TX_MSGS[] = { }; const CanMsg TOYOTA_SECOC_TX_MSGS[] = { - TOYOTA_COMMON_TX_MSGS + TOYOTA_COMMON_SECOC_TX_MSGS }; const CanMsg TOYOTA_LONG_TX_MSGS[] = { @@ -81,9 +89,11 @@ const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = { #define TOYOTA_COMMON_RX_CHECKS(lta) \ {.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}}, \ {.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \ - {.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}}, \ - {.msg = {{0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \ - {0x226, 0, 8, .check_checksum = false, .frequency = 40U}, { 0 }}}, \ + {.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, \ + {0x176, 0, 8, .check_checksum = true, .frequency = 32U}, { 0 }}}, \ + {.msg = {{0x101, 0, 8, .check_checksum = false, .frequency = 50U}, \ + {0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \ + {0x226, 0, 8, .check_checksum = false, .frequency = 40U}}}, \ RxCheck toyota_lka_rx_checks[] = { TOYOTA_COMMON_RX_CHECKS(false) diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 307b5df0c3468e..b918b684abc5a3 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -1,4 +1,5 @@ # functions common among cars +import logging from collections import namedtuple from dataclasses import dataclass from enum import IntFlag, ReprEnum, EnumType @@ -11,6 +12,10 @@ from openpilot.common.utils import Freezable from openpilot.selfdrive.car.docs_definitions import CarDocs +# set up logging +carlog = logging.getLogger('carlog') +carlog.setLevel(logging.INFO) +carlog.propagate = False # kg of standard extra cargo to count for drive, gas, etc... STD_CARGO_KG = 136. diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index c9f30f6e3de616..45cfcafe937327 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -206,7 +206,7 @@ def get_car(logcan, sendcan, disable_openpilot_long, experimental_long_allowed, if frogpilot_toggles.block_user: candidate = "MOCK" threading.Thread(target=sentry.capture_fingerprint, args=(candidate, params, True,)).start() - elif not params.get_bool("FingerprintLogged"): + elif candidate != "MOCK" and not params.get_bool("FingerprintLogged"): threading.Thread(target=sentry.capture_fingerprint, args=(candidate, params,)).start() CarInterface, _, _ = interfaces[candidate] diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 8ab60d786ecbed..4871ff5a426171 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -295,7 +295,7 @@ def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_ eps_firmware = str(next((fw.fwVersion for fw in car_fw if fw.ecu == "eps"), "")) model = get_nn_model_path(candidate, eps_firmware) if model is not None: - params.put_nonblocking("NNFFModelName", candidate.replace("_", " ")) + params.put("NNFFModelName", candidate.replace("_", " ")) # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload if not ret.notCar: diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 4c56efaed430b9..795fafd4cd4ae8 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,14 +1,18 @@ import math from cereal import car +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.numpy_fast import clip, interp -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.common.realtime import DT_CTRL +from openpilot.selfdrive.car import carlog, 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 from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS +from openpilot.selfdrive.controls.lib.pid import PIDController from opendbc.can.packer import CANPacker from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_acceleration import get_max_allowed_accel @@ -19,7 +23,10 @@ ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2 -ACCEL_WINDUP_LIMIT = 0.5 # m/s^2 / frame +# 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 = 4.0 * DT_CTRL * 3 # 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 +47,23 @@ PARK = car.CarState.GearShifter.park +def get_long_tune(CP, params): + kiBP = [0.] + kdBP = [0.] + kdV = [0.] + if CP.carFingerprint in TSS2_CAR: + kiV = [0.25] + kdV = [0.25 / 4] + + else: + kiBP = [0., 5., 35.] + kiV = [3.6, 2.4, 1.5] + + return PIDController(0.0, (kiBP, kiV), k_f=1.0, k_d=(kdBP, kdV), + pos_limit=params.ACCEL_MAX, neg_limit=params.ACCEL_MIN, + rate=1 / (DT_CTRL * 3)) + + class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP @@ -50,31 +74,81 @@ 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 + # *** start long control state *** + self.long_pid = get_long_tune(self.CP, self.params) + + self.error_rate = FirstOrderFilter(0.0, 0.5, DT_CTRL * 3) + self.prev_error = 0.0 + + self.aego = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3) + + self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL) - self.packer = CANPacker(dbc_name) self.accel = 0 + self.prev_accel = 0 + # *** end long control state *** + + self.packer = CANPacker(dbc_name) + + 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.stock_max_accel = self.params.ACCEL_MAX + self.doors_locked = False self.reverse_cruise_active = False + self.updated_pid = False self.cruise_timer = 0 self.previous_set_speed = 0 def update(self, CC, CS, now_nanos, frogpilot_toggles): + if frogpilot_toggles.sport_plus: + if not self.updated_pid: + self.params.ACCEL_MAX = get_max_allowed_accel(0) + self.long_pid = get_long_tune(self.CP, self.params) + self.updated_pid = True + + self.params.ACCEL_MAX = min(frogpilot_toggles.max_desired_acceleration, get_max_allowed_accel(CS.out.vEgo)) + else: + if self.updated_pid: + self.params.ACCEL_MAX = self.stock_max_accel + self.long_pid = get_long_tune(self.CP, self.params) + self.updated_pid = False + + self.params.ACCEL_MAX = min(frogpilot_toggles.max_desired_acceleration, self.stock_max_accel) + 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 + if len(CC.orientationNED) == 3: + self.pitch.update(CC.orientationNED[1]) + # *** 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: + carlog.error("SecOC synchronization MAC mismatch, wrong key?") + 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 +182,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 +206,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,43 +235,10 @@ 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 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)) + 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. + # This prevents unexpected pedal range rescaling + can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2)) # 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): @@ -192,36 +252,76 @@ 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 - - # 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 + # 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(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY + # TODO: on uphills this sometimes sets PERMIT_BRAKING low not considering the creep force + net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch + + # GVC does not overshoot ego acceleration when starting from stop, but still has a similar delay + if not self.CP.flags & ToyotaFlags.SECOC.value: + a_ego_blended = interp(CS.out.vEgo, [1.0, 2.0], [CS.gvc, CS.out.aEgo]) + else: + a_ego_blended = CS.out.aEgo + + # wind down integral when approaching target for step changes and smooth ramps to reduce overshoot + prev_aego = self.aego.x + self.aego.update(a_ego_blended) + j_ego = (self.aego.x - prev_aego) / (DT_CTRL * 3) + a_ego_future = a_ego_blended + j_ego * 0.5 + + if actuators.longControlState == LongCtrlState.pid: + error = pcm_accel_cmd - a_ego_blended + self.error_rate.update((error - self.prev_error) / (DT_CTRL * 3)) + self.prev_error = error + + error_future = pcm_accel_cmd - a_ego_future + pcm_accel_cmd = self.long_pid.update(error_future, error_rate=self.error_rate.x, + speed=CS.out.vEgo, + feedforward=pcm_accel_cmd) else: - self.distance_button = 0 + self.long_pid.reset() + self.error_rate.x = 0.0 + self.prev_error = 0.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 + # 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.2 or stopping or not CC.longActive: + self.permit_braking = True + elif net_acceleration_request_min > 0.3: + self.permit_braking = False + + pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, 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)) - 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. - # This prevents unexpected pedal range rescaling - can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2)) + 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)) # *** hud ui *** if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 55a8dedbb4f42a..716b7db63c1f4e 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 @@ -45,11 +45,15 @@ class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) - self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"] self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. 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 @@ -64,7 +68,8 @@ def __init__(self, CP): self.low_speed_lockout = False self.acc_type = 1 self.lkas_hud = {} - self.pcm_accel_net = 0.0 + self.gvc = 0.0 + self.secoc_synchronization = None # FrogPilot variables self.latActive_previous = False @@ -76,21 +81,10 @@ 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.RAISED_ACCEL_LIMIT: - 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"]: - self.pcm_accel_net += min(cp.vl["PCM_CRUISE"]["ACCEL_NET"], 0.0) - - # add creeping force at low speeds only for braking, CLUTCH->ACCEL_NET already shows this - neutral_accel = max(cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] / self.CP.mass, 0.0) - if self.pcm_accel_net + neutral_accel < 0.0: - self.pcm_accel_net += neutral_accel + if not self.CP.flags & ToyotaFlags.SECOC.value: + self.gvc = cp.vl["VSC1S07"]["GVC"] ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]]) @@ -100,12 +94,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"], @@ -136,14 +140,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 @@ -174,8 +174,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"] @@ -199,9 +197,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) @@ -264,7 +259,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), @@ -279,11 +273,20 @@ 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: + messages.append(("VSC1S07", 20)) + 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 UNSUPPORTED_DSU_CAR: messages.append(("DSU_CRUISE", 5)) @@ -338,9 +341,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 b8942cd59d3df9..ec9cdbb073e8df 100644 --- a/selfdrive/car/toyota/fingerprints.py +++ b/selfdrive/car/toyota/fingerprints.py @@ -110,6 +110,7 @@ b'\x018966306Q6000\x00\x00\x00\x00', b'\x018966333N1100\x00\x00\x00\x00', b'\x018966333N4300\x00\x00\x00\x00', + b'\x018966333P3000\x00\x00\x00\x00', b'\x018966333P3100\x00\x00\x00\x00', b'\x018966333P3200\x00\x00\x00\x00', b'\x018966333P4200\x00\x00\x00\x00', @@ -160,6 +161,7 @@ b'8821F0605200 ', b'8821F0606200 ', b'8821F0607200 ', + b'8821F0607300 ', b'8821F0608000 ', b'8821F0608200 ', b'8821F0608300 ', @@ -192,6 +194,7 @@ b'8965B33581\x00\x00\x00\x00\x00\x00', b'8965B33611\x00\x00\x00\x00\x00\x00', b'8965B33621\x00\x00\x00\x00\x00\x00', + b'8965B33630\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F0601200 ', @@ -207,6 +210,7 @@ b'8821F0605200 ', b'8821F0606200 ', b'8821F0607200 ', + b'8821F0607300 ', b'8821F0608000 ', b'8821F0608200 ', b'8821F0608300 ', @@ -754,6 +758,7 @@ b'\x018966353Q2300\x00\x00\x00\x00', b'\x018966353Q4000\x00\x00\x00\x00', b'\x018966353R1100\x00\x00\x00\x00', + b'\x018966353R7000\x00\x00\x00\x00', b'\x018966353R7100\x00\x00\x00\x00', b'\x018966353R8000\x00\x00\x00\x00', b'\x018966353R8100\x00\x00\x00\x00', @@ -1353,6 +1358,7 @@ ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F0610000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', + b'\x028646F0610100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', @@ -1550,6 +1556,7 @@ b'\x01896630E36100\x00\x00\x00\x00', b'\x01896630E36200\x00\x00\x00\x00', b'\x01896630E36300\x00\x00\x00\x00', + b'\x01896630E36600\x00\x00\x00\x00', b'\x01896630E37100\x00\x00\x00\x00', b'\x01896630E37200\x00\x00\x00\x00', b'\x01896630E37300\x00\x00\x00\x00', @@ -1656,6 +1663,7 @@ b'\x02348X8000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348Y3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0234D14000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0234D15000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0234D16000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 084bcb14bc52d1..172e0d65bfcb01 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -29,6 +29,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 @@ -57,7 +61,11 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \ and not (ret.flags & ToyotaFlags.SMART_DSU) - if candidate == CAR.LEXUS_ES_TSS2 and Ecu.hybrid not in found_ecus: + if Ecu.hybrid in found_ecus: + ret.flags |= ToyotaFlags.HYBRID.value + + # TODO: expand to the rest of the cars + if candidate in (CAR.LEXUS_ES_TSS2,) and not (ret.flags & ToyotaFlags.HYBRID.value): ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value if candidate == CAR.TOYOTA_PRIUS: @@ -73,7 +81,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] @@ -121,8 +129,13 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp # - 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) + + 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 @@ -136,27 +149,21 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp # to a negative value, so it won't matter. ret.minEnableSpeed = -1. if (candidate in STOP_AND_GO_CAR or ret.enableGasInterceptor) else MIN_ACC_SPEED - tune = ret.longitudinalTuning if ret.enableGasInterceptor: + tune = ret.longitudinalTuning tune.kiBP = [0., 5., 20.] tune.kiV = [1.3, 1.0, 0.7] ret.vEgoStopping = 0.25 ret.vEgoStarting = 0.25 ret.stoppingDecelRate = 0.3 # reach stopping target smoothly elif 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.RAISED_ACCEL_LIMIT: - tune.kiV = [0.25] - else: - tune.kiBP = [0., 5., 35.] - tune.kiV = [3.6, 2.4, 1.5] + # Hybrids have much quicker longitudinal actuator response + if ret.flags & ToyotaFlags.HYBRID.value: + ret.longitudinalActuatorDelay = 0.05 if params.get_bool("FrogsGoMoosTweak"): ret.stoppingDecelRate = 0.1 # reach stopping target smoothly 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 1ae1effd05e238..6fd0885c696550 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -298,7 +298,7 @@ class CAR(Platforms): LEXUS_ES_TSS2 = ToyotaTSS2PlatformConfig( [ ToyotaCarDocs("Lexus ES 2019-24"), - ToyotaCarDocs("Lexus ES Hybrid 2019-24", video_link="https://youtu.be/BZ29osRVJeg?t=12"), + ToyotaCarDocs("Lexus ES Hybrid 2019-25", video_link="https://youtu.be/BZ29osRVJeg?t=12"), ], LEXUS_ES.specs, ) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e46a7bb828b37d..f182040008f498 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -15,7 +15,7 @@ from openpilot.common.git import get_short_branch from openpilot.common.numpy_fast import clip from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL +from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL, DT_MDL from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event @@ -579,7 +579,7 @@ def state_control(self, CS): torque_params = self.sm['liveTorqueParameters'] friction = self.frogpilot_toggles.steer_friction if self.frogpilot_toggles.use_custom_steer_friction else torque_params.frictionCoefficientFiltered lat_accel_factor = self.frogpilot_toggles.steer_lat_accel_factor if self.frogpilot_toggles.use_custom_lat_accel_factor else torque_params.latAccelFactorFiltered - if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.frogpilot_toggles.force_auto_tune) and not self.frogpilot_toggles.force_auto_tune_off: + if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.frogpilot_toggles.force_auto_tune): self.LaC.update_live_torque_params(lat_accel_factor, torque_params.latAccelOffsetFiltered, friction) @@ -744,7 +744,7 @@ def update_frogpilot_variables(self, CS): self.experimental_mode = not self.experimental_mode self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode) - if self.sm.frame % 10 == 0 or self.resume_pressed: + if self.sm.frame * DT_CTRL % DT_MDL == 0 or self.resume_pressed: self.resume_previously_pressed = self.resume_pressed FPCC = custom.FrogPilotCarControl.new_message() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index bdb986428abfa8..cae152992a6cb1 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -221,6 +221,18 @@ def get_lead_adjacent(tracks: dict[int, Track], model_data: capnp._DynamicStruct return lead_dict +def get_leads_lead(tracks: dict[int, Track], leadOne) -> dict[str, Any]: + lead_dict = {'status': False} + + toyota_corolla_length = 4.6355 + leads_lead_tracks = [c for c in tracks.values() if leadOne.status and c.dRel > leadOne.dRel + toyota_corolla_length] + if len(leads_lead_tracks) > 0: + closest_track = min(leads_lead_tracks, key=lambda c: c.dRel) + lead_dict = closest_track.get_RadarState() + + return lead_dict + + class RadarD: def __init__(self, frogpilot_toggles, radar_ts: float, delay: int = 0): self.points: dict[int, tuple[float, float, float]] = {} @@ -304,6 +316,7 @@ def update(self, sm: messaging.SubMaster, rr): self.radar_state.leadLeftFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True, far=True) self.radar_state.leadRight = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthRight, left=False) self.radar_state.leadRightFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthRight, left=False, far=True) + self.radar_state.leadsLead = get_leads_lead(self.tracks, self.radar_state.leadOne) # Update FrogPilot parameters if sm['frogpilotPlan'].togglesUpdated: diff --git a/selfdrive/frogpilot/assets/active_theme/colors/colors.json b/selfdrive/frogpilot/assets/active_theme/colors/colors.json deleted file mode 100644 index bf527d51eb243e..00000000000000 --- a/selfdrive/frogpilot/assets/active_theme/colors/colors.json +++ /dev/null @@ -1,44 +0,0 @@ -{ - "LaneLines": { - "red": 23, - "green": 134, - "blue": 68, - "alpha": 242 - }, - "LeadMarker": { - "red": 23, - "green": 134, - "blue": 68, - "alpha": 242 - }, - "Path": { - "red": 23, - "green": 134, - "blue": 68, - "alpha": 242 - }, - "PathEdge": { - "red": 18, - "green": 107, - "blue": 54, - "alpha": 242 - }, - "Sidebar1": { - "red": 23, - "green": 134, - "blue": 68, - "alpha": 242 - }, - "Sidebar2": { - "red": 23, - "green": 134, - "blue": 68, - "alpha": 242 - }, - "Sidebar3": { - "red": 23, - "green": 134, - "blue": 68, - "alpha": 242 - } -} diff --git a/selfdrive/frogpilot/assets/active_theme/distance_icons/aggressive.gif b/selfdrive/frogpilot/assets/active_theme/distance_icons/aggressive.gif deleted file mode 100644 index 05cd8d01c4e7ec..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/distance_icons/aggressive.gif and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/distance_icons/relaxed.gif b/selfdrive/frogpilot/assets/active_theme/distance_icons/relaxed.gif deleted file mode 100644 index 7882afbbbe2352..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/distance_icons/relaxed.gif and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/distance_icons/standard.gif b/selfdrive/frogpilot/assets/active_theme/distance_icons/standard.gif deleted file mode 100644 index c97c85cd94d85b..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/distance_icons/standard.gif and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/distance_icons/traffic.gif b/selfdrive/frogpilot/assets/active_theme/distance_icons/traffic.gif deleted file mode 100644 index 3e3f898c349a92..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/distance_icons/traffic.gif and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/icons/button_flag.png b/selfdrive/frogpilot/assets/active_theme/icons/button_flag.png deleted file mode 100644 index 2658aa9b7a859d..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/icons/button_flag.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/icons/button_home.gif b/selfdrive/frogpilot/assets/active_theme/icons/button_home.gif deleted file mode 100644 index 133d8d34ed1c00..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/icons/button_home.gif and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/icons/button_settings.png b/selfdrive/frogpilot/assets/active_theme/icons/button_settings.png deleted file mode 100644 index 55927599758a9b..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/icons/button_settings.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/signals/traditional_100 b/selfdrive/frogpilot/assets/active_theme/signals/traditional_100 deleted file mode 100644 index e69de29bb2d1d6..00000000000000 diff --git a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_1.png b/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_1.png deleted file mode 100644 index 8c2f7a54ed26a3..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_1.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_2.png b/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_2.png deleted file mode 100644 index 3c7d2d9eb8a0ad..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_2.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_3.png b/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_3.png deleted file mode 100644 index b495eeae190cbc..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_3.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_4.png b/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_4.png deleted file mode 100644 index 024a0b03ea6c08..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_4.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_5.png b/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_5.png deleted file mode 100644 index 91eb05f12f4579..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_5.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_6.png b/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_6.png deleted file mode 100644 index 3c7d2d9eb8a0ad..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_6.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_blindspot.png b/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_blindspot.png deleted file mode 100644 index b721266fb2f1b1..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/signals/turn_signal_blindspot.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/sounds/disengage.wav b/selfdrive/frogpilot/assets/active_theme/sounds/disengage.wav deleted file mode 100644 index d46dd9eb6cab6b..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/sounds/disengage.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/sounds/engage.wav b/selfdrive/frogpilot/assets/active_theme/sounds/engage.wav deleted file mode 100644 index 795aa530e7ae76..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/sounds/engage.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/active_theme/steering_wheel/wheel.png b/selfdrive/frogpilot/assets/active_theme/steering_wheel/wheel.png deleted file mode 100644 index 712255984600c0..00000000000000 Binary files a/selfdrive/frogpilot/assets/active_theme/steering_wheel/wheel.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/aggressive.gif b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/aggressive.gif deleted file mode 100644 index 05cd8d01c4e7ec..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/aggressive.gif and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/relaxed.gif b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/relaxed.gif deleted file mode 100644 index 7882afbbbe2352..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/relaxed.gif and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/standard.gif b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/standard.gif deleted file mode 100644 index c97c85cd94d85b..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/standard.gif and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/steering_wheel/frog.png b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/steering_wheel/frog.png deleted file mode 100644 index 712255984600c0..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/steering_wheel/frog.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/traffic.gif b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/traffic.gif deleted file mode 100644 index 3e3f898c349a92..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/traffic.gif and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_1_red.png b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_1_red.png deleted file mode 100644 index 7c102456e6efb6..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_1_red.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/model_manager.py b/selfdrive/frogpilot/assets/model_manager.py index d7fa1162422bc5..999d35f167bd10 100644 --- a/selfdrive/frogpilot/assets/model_manager.py +++ b/selfdrive/frogpilot/assets/model_manager.py @@ -127,27 +127,29 @@ def update_model_params(self, model_info, repo_url): for model in model_info: available_models.append(model['id']) - params.put_nonblocking("AvailableModels", ','.join(available_models)) - params.put_nonblocking("AvailableModelsNames", ','.join([model['name'] for model in model_info])) - params.put_nonblocking("ClassicModels", ','.join([model['id'] for model in model_info if model.get("classic_model", False)])) - params.put_nonblocking("ExperimentalModels", ','.join([model['id'] for model in model_info if model.get("experimental", False)])) - params.put_nonblocking("NavigationModels", ','.join([model['id'] for model in model_info if "🗺️" in model['name']])) - params.put_nonblocking("RadarlessModels", ','.join([model['id'] for model in model_info if "📡" not in model['name']])) + params.put("AvailableModels", ','.join(available_models)) + params.put("AvailableModelsNames", ','.join([model['name'] for model in model_info])) + params.put("ClassicModels", ','.join([model['id'] for model in model_info if model.get("classic_model", False)])) + params.put("ExperimentalModels", ','.join([model['id'] for model in model_info if model.get("experimental", False)])) + params.put("NavigationModels", ','.join([model['id'] for model in model_info if "🗺️" in model['name']])) + params.put("RadarlessModels", ','.join([model['id'] for model in model_info if "📡" not in model['name']])) print("Models list updated successfully") if available_models: models_downloaded = self.are_all_models_downloaded(available_models, repo_url) - params.put_bool_nonblocking("ModelsDownloaded", models_downloaded) + params.put_bool("ModelsDownloaded", models_downloaded) def are_all_models_downloaded(self, available_models, repo_url): + model_sizes = self.fetch_all_model_sizes(repo_url) + if not model_sizes: + return + available_models = set(available_models) - {DEFAULT_MODEL, DEFAULT_CLASSIC_MODEL} - automatically_update_models = params.get_bool("AutomaticallyUpdateModels") all_models_downloaded = True + automatically_update_models = params.get_bool("AutomaticallyUpdateModels") - model_sizes = self.fetch_all_model_sizes(repo_url) download_queue = [] - for model in available_models: model_path = os.path.join(MODELS_PATH, f"{model}.thneed") expected_size = model_sizes.get(model) @@ -179,7 +181,7 @@ def validate_models(self): current_model_name = params.get("ModelName", encoding='utf-8') if "(Default)" in current_model_name and current_model_name != DEFAULT_CLASSIC_MODEL_NAME: - params.put_nonblocking("ModelName", current_model_name.replace(" (Default)", "")) + params.put("ModelName", current_model_name.replace(" (Default)", "")) available_models = params.get("AvailableModels", encoding='utf-8') if not available_models: @@ -194,8 +196,8 @@ def validate_models(self): model_name = model_file.replace(".thneed", "") if model_name not in available_models.split(','): if model_name == current_model: - params.put_nonblocking("Model", DEFAULT_CLASSIC_MODEL) - params.put_nonblocking("ModelName", DEFAULT_CLASSIC_MODEL_NAME) + params.put("Model", DEFAULT_CLASSIC_MODEL) + params.put("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") @@ -253,4 +255,4 @@ def download_all_models(self): params_memory.put(self.download_progress_param, "All models downloaded!") params_memory.remove("DownloadAllModels") - params.put_bool_nonblocking("ModelsDownloaded", True) + params.put_bool("ModelsDownloaded", True) diff --git a/selfdrive/frogpilot/assets/theme_manager.py b/selfdrive/frogpilot/assets/theme_manager.py index 2d25eacfa086ca..7102bee6d3f725 100644 --- a/selfdrive/frogpilot/assets/theme_manager.py +++ b/selfdrive/frogpilot/assets/theme_manager.py @@ -23,7 +23,7 @@ def update_theme_asset(asset_type, theme, holiday_theme): save_location = os.path.join(ACTIVE_THEME_PATH, asset_type) - if holiday_theme: + if holiday_theme is not None: asset_location = os.path.join(HOLIDAY_THEME_PATH, holiday_theme, asset_type) elif asset_type == "distance_icons": asset_location = os.path.join(THEME_SAVE_PATH, "distance_icons", theme) @@ -51,11 +51,14 @@ def update_theme_asset(asset_type, theme, holiday_theme): os.makedirs(os.path.dirname(save_location), exist_ok=True) + if os.path.islink(save_location): + os.unlink(save_location) + if os.path.exists(save_location): - if os.path.islink(save_location): - os.unlink(save_location) - elif os.path.isdir(save_location): + if os.path.isdir(save_location): shutil.rmtree(save_location) + else: + os.remove(save_location) os.symlink(asset_location, save_location, target_is_directory=True) print(f"Linked {save_location} to {asset_location}") @@ -63,7 +66,7 @@ def update_theme_asset(asset_type, theme, holiday_theme): def update_wheel_image(image, holiday_theme=None, random_event=True): wheel_save_location = os.path.join(ACTIVE_THEME_PATH, "steering_wheel") - if holiday_theme: + if holiday_theme is not None: wheel_location = os.path.join(HOLIDAY_THEME_PATH, holiday_theme, "steering_wheel") elif random_event: wheel_location = os.path.join(RANDOM_EVENTS_PATH, "icons") @@ -142,13 +145,13 @@ 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"): - params.put("CurrentHolidayTheme", holiday) + params_memory.put("CurrentHolidayTheme", holiday) update_frogpilot_toggles() self.previous_assets["holiday_theme"] = holiday return if "holiday_theme" in self.previous_assets: - params.remove("CurrentHolidayTheme") + params_memory.remove("CurrentHolidayTheme") update_frogpilot_toggles() self.previous_assets.pop("holiday_theme") @@ -426,6 +429,8 @@ def update_themes(self, frogpilot_toggles, boot_run=False): self.validate_themes(frogpilot_toggles) assets = self.fetch_assets(repo_url) + if not assets["themes"] and not assets["distance_icons"] and not assets["wheels"]: + return downloadable_colors = [] downloadable_icons = [] diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index dde238a451b926..85c945a5d92b2a 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -15,7 +15,9 @@ from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, MODEL_LENGTH, NON_DRIVING_GEARS, PLANNER_TIME, THRESHOLD class FrogPilotPlanner: - def __init__(self): + def __init__(self, error_log): + self.error_log = error_log + self.cem = ConditionalExperimentalMode(self) self.frogpilot_acceleration = FrogPilotAcceleration(self) self.frogpilot_events = FrogPilotEvents(self) @@ -34,7 +36,7 @@ def __init__(self): self.road_curvature = 1 self.v_cruise = 0 - def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarless_model, radarState, frogpilot_toggles): + def update(self, carControl, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarless_model, radarState, frogpilot_toggles): if radarless_model: model_leads = list(modelData.leadsV3) if len(model_leads) > 0: @@ -50,7 +52,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState v_ego = max(carState.vEgo, 0) v_lead = self.lead_one.vLead - self.frogpilot_acceleration.update(controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles) + self.frogpilot_acceleration.update(controlsState, frogpilotCarState, v_ego, frogpilot_toggles) run_cem = frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.force_stops or frogpilot_toggles.green_light_alert or frogpilot_toggles.show_stopping_point if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateralActive) and carState.gearShifter not in NON_DRIVING_GEARS: @@ -80,7 +82,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState self.road_curvature = calculate_road_curvature(modelData, v_ego) if not carState.standstill else 1 self.tracking_lead = self.set_lead_status(frogpilotCarState, v_ego, frogpilot_toggles) - self.v_cruise = self.frogpilot_vcruise.update(carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles) + self.v_cruise = self.frogpilot_vcruise.update(carControl, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, v_cruise, v_ego, frogpilot_toggles) def set_lead_status(self, frogpilotCarState, v_ego, frogpilot_toggles): distance_offset = frogpilot_toggles.increased_stopped_distance if not frogpilotCarState.trafficModeActive else 0 diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py b/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py index e4f04953da99f7..776e00e6425747 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py @@ -39,7 +39,7 @@ def __init__(self, FrogPilotPlanner): self.max_accel = 0 self.min_accel = 0 - def update(self, controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles): + def update(self, controlsState, frogpilotCarState, v_ego, frogpilot_toggles): eco_gear = frogpilotCarState.ecoGear sport_gear = frogpilotCarState.sportGear diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_events.py b/selfdrive/frogpilot/controls/lib/frogpilot_events.py index 5838aed82dc4ad..ce0d33531e03d2 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_events.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_events.py @@ -1,8 +1,6 @@ import os import random -import openpilot.system.sentry as sentry - from openpilot.common.conversions import Conversions as CV from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.controlsd import Desire @@ -11,11 +9,14 @@ from openpilot.selfdrive.frogpilot.assets.theme_manager import update_wheel_image from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, params, params_memory +RANDOM_EVENTS_CHANCE = 0.01 * DT_MDL + class FrogPilotEvents: def __init__(self, FrogPilotPlanner): - self.events = Events() self.frogpilot_planner = FrogPilotPlanner + self.events = Events() + self.accel30_played = False self.accel35_played = False self.accel40_played = False @@ -76,7 +77,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState else: self.tracking_lead_distance = 0 - if not self.openpilot_crashed_played and os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')): + if not self.openpilot_crashed_played and os.path.isfile(self.frogpilot_planner.error_log): if frogpilot_toggles.random_events: self.events.add(EventName.openpilotCrashedRandomEvent) else: @@ -135,7 +136,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState if not self.this_is_fine_played: event_choices.append("thisIsFineSteerSaturated") - if self.frame % 100 == 0 and event_choices: + if random.random() < RANDOM_EVENTS_CHANCE and event_choices: event_choice = random.choice(event_choices) if event_choice == "firefoxSteerSaturated": self.events.add(EventName.firefoxSteerSaturated) @@ -161,17 +162,17 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState if not self.fcw_played and frogpilotCarControl.fcwEventTriggered: event_choices = ["toBeContinued", "yourFrogTriedToKillMe"] - event_choice = random.choice(event_choices) - if random.random() < 0.1: + if random.random() < RANDOM_EVENTS_CHANCE: + event_choice = random.choice(event_choices) if event_choice == "toBeContinued": self.events.add(EventName.toBeContinued) elif event_choice == "yourFrogTriedToKillMe": self.events.add(EventName.yourFrogTriedToKillMe) - self.fcw_played = True - self.random_event_played = True + self.fcw_played = True + self.random_event_played = True if not self.youveGotMail_played and frogpilotCarControl.alwaysOnLateralActive and not self.always_on_lateral_active_previously: - if random.random() < 0.01 and not carState.standstill: + if random.random() < RANDOM_EVENTS_CHANCE and not carState.standstill: self.events.add(EventName.youveGotMail) self.youveGotMail_played = True self.random_event_played = True @@ -180,7 +181,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 params.get("NNFFModelName", encoding='utf-8') is not None: + if 5 > 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: @@ -195,4 +196,4 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState elif modelData.meta.turnDirection == Desire.turnRight: self.events.add(EventName.turningRight) - self.frame = round(self.frame + DT_MDL, 2) + self.frame += DT_MDL diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_following.py b/selfdrive/frogpilot/controls/lib/frogpilot_following.py index a29c4577700dbf..1605eeb97a0848 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_following.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_following.py @@ -32,7 +32,7 @@ def update(self, aEgo, controlsState, frogpilotCarState, lead_distance, v_ego, v self.base_speed_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_speed_decrease) self.base_danger_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_danger) - self.t_follow = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_t_follow) + self.t_follow = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_follow) else: if aEgo >= 0: self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor( @@ -76,17 +76,17 @@ def update_follow_values(self, lead_distance, v_ego, v_lead, frogpilot_toggles): # Offset by FrogAi for FrogPilot for a more natural approach to a faster lead if frogpilot_toggles.human_following and v_lead > v_ego: distance_factor = max(lead_distance - (v_ego * self.t_follow), 1) - standstill_offset = max(STOP_DISTANCE - v_ego, 0) * max(v_lead - v_ego, 1) - acceleration_offset = clip((v_lead - v_ego) + standstill_offset - COMFORT_BRAKE, 1, distance_factor) - self.acceleration_jerk /= acceleration_offset - self.speed_jerk /= acceleration_offset + standstill_offset = max((v_lead - v_ego) * (STOP_DISTANCE - v_ego), 1) + acceleration_offset = clip((v_lead - v_ego) * standstill_offset - COMFORT_BRAKE, 1, distance_factor) + self.acceleration_jerk /= standstill_offset + self.speed_jerk /= standstill_offset self.t_follow /= acceleration_offset # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead if (frogpilot_toggles.conditional_slower_lead or frogpilot_toggles.human_following) and v_lead < v_ego > CRUISING_SPEED: distance_factor = max(lead_distance - (v_lead * self.t_follow), 1) - far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - STOP_DISTANCE + (v_lead - CITY_SPEED_LIMIT), 0) - braking_offset = clip((v_ego - v_lead) + far_lead_offset - COMFORT_BRAKE, 1, distance_factor) + far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - STOP_DISTANCE + (v_lead - CITY_SPEED_LIMIT), 1) + braking_offset = clip((v_ego - v_lead) * far_lead_offset - COMFORT_BRAKE, 1, distance_factor) if frogpilot_toggles.human_following: self.t_follow /= braking_offset self.slower_lead = braking_offset - far_lead_offset > 1 diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py b/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py index fc4a918abf28b0..8b3da09a35e886 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py @@ -33,7 +33,7 @@ def __init__(self, FrogPilotPlanner): self.tracked_model_length = 0 self.vtsc_target = 0 - def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles): + def update(self, carControl, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, v_cruise, v_ego, frogpilot_toggles): force_stop = frogpilot_toggles.force_stops and self.frogpilot_planner.cem.stop_light_detected and controlsState.enabled force_stop &= self.frogpilot_planner.model_length < 100 force_stop &= self.override_force_stop_timer <= 0 @@ -52,11 +52,14 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState elif self.override_force_stop_timer > 0: self.override_force_stop_timer -= DT_MDL + v_cruise_cluster = max(controlsState.vCruiseCluster * CV.KPH_TO_MS, v_cruise) + v_cruise_diff = v_cruise_cluster - v_cruise + v_ego_cluster = max(carState.vEgoCluster, v_ego) v_ego_diff = v_ego_cluster - v_ego # Pfeiferj's Map Turn Speed Controller - if frogpilot_toggles.map_turn_speed_controller and v_ego > CRUISING_SPEED and controlsState.enabled: + if frogpilot_toggles.map_turn_speed_controller and v_ego > CRUISING_SPEED and carControl.longActive: mtsc_active = self.mtsc_target < v_cruise self.mtsc_target = clip(self.mtsc.target_speed(v_ego, carState.aEgo, frogpilot_toggles), CRUISING_SPEED, v_cruise) @@ -77,21 +80,13 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState unconfirmed_slc_target = self.slc.desired_speed_limit 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 + self.speed_limit_changed |= unconfirmed_slc_target != self.previous_speed_limit and abs(self.slc_target - 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 = 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 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 >= 30 - - if speed_limit_accepted or speed_limit_denied: - self.previous_speed_limit = unconfirmed_slc_target - params_memory.put_bool("SLCConfirmed", False) - params_memory.put_bool("SLCConfirmedPressed", False) + speed_limit_accepted = frogpilotCarControl.resumePressed and carControl.longActive or params_memory.get_bool("SLCConfirmed") + speed_limit_denied = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents) and carControl.longActive or self.speed_limit_timer >= 30 if speed_limit_decreased: speed_limit_confirmed = not frogpilot_toggles.speed_limit_confirmation_lower or speed_limit_accepted @@ -100,15 +95,18 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState else: speed_limit_confirmed = False + if speed_limit_confirmed: + self.slc_target = unconfirmed_slc_target + + if speed_limit_denied or self.slc_target == unconfirmed_slc_target: + self.speed_limit_changed = False + if self.speed_limit_changed: self.speed_limit_timer += DT_MDL else: self.speed_limit_timer = 0 - if speed_limit_confirmed: - self.slc_target = unconfirmed_slc_target - elif self.slc_target == unconfirmed_slc_target: - self.speed_limit_changed = False + self.previous_speed_limit = unconfirmed_slc_target else: self.slc_target = unconfirmed_slc_target @@ -122,14 +120,14 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState self.overridden_speed = v_ego_cluster self.overridden_speed = clip(self.overridden_speed, self.slc_target, v_cruise) elif frogpilot_toggles.speed_limit_controller_override_set_speed: - self.overridden_speed = v_cruise + self.overridden_speed = v_cruise_cluster else: self.overridden_speed = 0 else: self.slc_target = 0 # Pfeiferj's Vision Turn Controller - if frogpilot_toggles.vision_turn_controller and v_ego > CRUISING_SPEED and controlsState.enabled: + if frogpilot_toggles.vision_turn_controller and v_ego > CRUISING_SPEED and carControl.longActive: self.vtsc_target = ((TARGET_LAT_A * frogpilot_toggles.turn_aggressiveness) / (self.frogpilot_planner.road_curvature * frogpilot_toggles.curve_sensitivity))**0.5 self.vtsc_target = clip(self.vtsc_target, CRUISING_SPEED, v_cruise) else: @@ -155,4 +153,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff, self.vtsc_target] v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets])) + self.mtsc_target += v_cruise_diff + self.vtsc_target += v_cruise_diff + return v_cruise diff --git a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py index 9315ad9f3f776b..33298bcef2713f 100644 --- a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py +++ b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py @@ -85,8 +85,12 @@ def get_speed_limit(self, dashboard_speed_limit, max_speed_limit, navigation_spe self.source = min(filtered_limits, key=filtered_limits.get) return filtered_limits[self.source] - for priority in [frogpilot_toggles.speed_limit_priority1, frogpilot_toggles.speed_limit_priority2, frogpilot_toggles.speed_limit_priority3]: - if priority in filtered_limits: + for priority in [ + frogpilot_toggles.speed_limit_priority1, + frogpilot_toggles.speed_limit_priority2, + frogpilot_toggles.speed_limit_priority3 + ]: + if priority is not None and priority in filtered_limits: self.source = priority return filtered_limits[priority] diff --git a/selfdrive/frogpilot/frogpilot_functions.py b/selfdrive/frogpilot/frogpilot_functions.py index a02bf219aa2cf1..bb8fa9205a228a 100644 --- a/selfdrive/frogpilot/frogpilot_functions.py +++ b/selfdrive/frogpilot/frogpilot_functions.py @@ -8,15 +8,16 @@ import time from openpilot.common.basedir import BASEDIR -from openpilot.common.params_pyx import ParamKeyType, UnknownKeyName +from openpilot.common.params_pyx import Params, ParamKeyType, UnknownKeyName from openpilot.common.time import system_time_valid from openpilot.system.hardware import HARDWARE +from openpilot.selfdrive.frogpilot.assets.theme_manager import HOLIDAY_THEME_PATH, update_theme_asset, update_wheel_image from openpilot.selfdrive.frogpilot.frogpilot_utilities import copy_if_exists, run_cmd -from openpilot.selfdrive.frogpilot.frogpilot_variables import ACTIVE_THEME_PATH, MODELS_PATH, THEME_SAVE_PATH, FrogPilotVariables +from openpilot.selfdrive.frogpilot.frogpilot_variables import MODELS_PATH, THEME_SAVE_PATH, FrogPilotVariables, params -def backup_directory(backup, destination, success_message, fail_message, minimum_backup_size=0, params=None, compressed=False): +def backup_directory(backup, destination, success_message, fail_message, minimum_backup_size=0, compressed=False): if not compressed: if os.path.exists(destination): print("Backup already exists. Aborting") @@ -53,29 +54,31 @@ def backup_directory(backup, destination, success_message, fail_message, minimum params.put_int("MinimumBackupSize", compressed_backup_size) -def cleanup_backups(directory, limit, minimum_backup_size=0, compressed=False): +def cleanup_backups(directory, limit, compressed=False): os.makedirs(directory, exist_ok=True) backups = sorted(glob.glob(os.path.join(directory, "*_auto*")), key=os.path.getmtime, reverse=True) for backup in backups[:]: - if backup.endswith("_in_progress"): - run_cmd(["sudo", "rm", "-rf", backup], f"Deleted in-progress backup: {os.path.basename(backup)}", f"Failed to delete in-progress backup: {os.path.basename(backup)}") + if backup.endswith("_in_progress") or backup.endswith("_in_progress.tar.gz"): + if os.path.isdir(backup): + shutil.rmtree(backup) + elif os.path.isfile(backup): + os.remove(backup) + backups.remove(backup) - if compressed: - for backup in backups[:]: - if os.path.getsize(backup) < minimum_backup_size: - run_cmd(["sudo", "rm", "-rf", backup], f"Deleted incomplete backup: {os.path.basename(backup)}", f"Failed to delete incomplete backup: {os.path.basename(backup)}") + for oldest_backup in backups[limit:]: + if os.path.isdir(oldest_backup): + shutil.rmtree(oldest_backup) + elif os.path.isfile(oldest_backup): + os.remove(oldest_backup) - for old_backup in backups[limit - 1:]: - run_cmd(["sudo", "rm", "-rf", old_backup], f"Deleted oldest backup: {os.path.basename(old_backup)}", f"Failed to delete backup: {os.path.basename(old_backup)}") - -def backup_frogpilot(build_metadata, params): +def backup_frogpilot(build_metadata): backup_path = os.path.join("/data", "backups") maximum_backups = 5 minimum_backup_size = params.get_int("MinimumBackupSize") - cleanup_backups(backup_path, maximum_backups, minimum_backup_size, compressed=True) + cleanup_backups(backup_path, maximum_backups) _, _, free = shutil.disk_usage(backup_path) required_free_space = minimum_backup_size * maximum_backups @@ -84,10 +87,10 @@ def backup_frogpilot(build_metadata, params): branch = build_metadata.channel commit = build_metadata.openpilot.git_commit_date[12:-16] backup_dir = os.path.join(backup_path, f"{branch}_{commit}_auto") - backup_directory(BASEDIR, backup_dir, f"Successfully backed up FrogPilot to {backup_dir}.", f"Failed to backup FrogPilot to {backup_dir}.", minimum_backup_size, params, True) + backup_directory(BASEDIR, backup_dir, f"Successfully backed up FrogPilot to {backup_dir}.", f"Failed to backup FrogPilot to {backup_dir}.", minimum_backup_size, True) -def backup_toggles(params, params_storage): +def backup_toggles(params_storage): for key in params.all_keys(): if params.get_key_type(key) & ParamKeyType.FROGPILOT_STORAGE: value = params.get(key) @@ -103,7 +106,7 @@ def backup_toggles(params, params_storage): backup_directory(os.path.join("/data", "params", "d"), backup_dir, f"Successfully backed up toggles to {backup_dir}.", f"Failed to backup toggles to {backup_dir}.") -def convert_params(params, params_storage): +def convert_params(params_storage): print("Starting to convert params") required_type = str @@ -159,7 +162,10 @@ def decrease_param(key): print("Param conversion completed") -def frogpilot_boot_functions(build_metadata, params, params_storage): +def frogpilot_boot_functions(build_metadata, params_storage): + if params.get_bool("HasAcceptedTerms"): + params_storage.clear_all() + old_screenrecordings = os.path.join("/data", "media", "0", "videos") new_screenrecordings = os.path.join("/data", "media", "screen_recordings") @@ -171,15 +177,17 @@ def frogpilot_boot_functions(build_metadata, params, params_storage): print("Waiting for system time to become valid...") time.sleep(1) - backup_frogpilot(build_metadata, params) - backup_toggles(params, params_storage) + if params.get("UpdaterAvailableBranches") is None: + os.system("pkill -SIGUSR1 -f system.updated.updated") + + backup_frogpilot(build_metadata) + backup_toggles(params_storage) -def setup_frogpilot(build_metadata, params): +def setup_frogpilot(build_metadata): FrogPilotVariables().update(started=False) - remount_persist = ["sudo", "mount", "-o", "remount,rw", "/persist"] - run_cmd(remount_persist, "Successfully remounted /persist as read-write.", "Failed to remount /persist.") + run_cmd(["sudo", "mount", "-o", "remount,rw", "/persist"], "Successfully remounted /persist as read-write.", "Failed to remount /persist.") os.makedirs("/persist/params", exist_ok=True) os.makedirs(MODELS_PATH, exist_ok=True) @@ -197,38 +205,43 @@ def setup_frogpilot(build_metadata, params): shutil.rmtree(frog_theme_path) params.put_bool("ResetFrogTheme", not (os.path.exists(animated_frog_theme_path) or os.path.exists(frog_distance_theme_path) or os.path.exists(frog_theme_path))) - frog_color_source = os.path.join(ACTIVE_THEME_PATH, "colors") + frog_color_source = os.path.join(HOLIDAY_THEME_PATH, "world_frog_day", "colors") frog_color_destination = os.path.join(THEME_SAVE_PATH, "theme_packs/frog/colors") if not os.path.exists(frog_color_destination): copy_if_exists(frog_color_source, frog_color_destination) + update_theme_asset("colors", "world_frog_day", "world_frog_day") - frog_distance_icon_source = os.path.join(ACTIVE_THEME_PATH, "distance_icons") + frog_distance_icon_source = os.path.join(HOLIDAY_THEME_PATH, "world_frog_day", "distance_icons") frog_distance_icon_destination = os.path.join(THEME_SAVE_PATH, "distance_icons/frog-animated") if not os.path.exists(frog_distance_icon_destination): copy_if_exists(frog_distance_icon_source, frog_distance_icon_destination) + update_theme_asset("distance_icons", "world_frog_day", "world_frog_day") - frog_icon_source = os.path.join(ACTIVE_THEME_PATH, "icons") + frog_icon_source = os.path.join(HOLIDAY_THEME_PATH, "world_frog_day", "icons") frog_icon_destination = os.path.join(THEME_SAVE_PATH, "theme_packs/frog-animated/icons") if not os.path.exists(frog_icon_destination): copy_if_exists(frog_icon_source, frog_icon_destination) + update_theme_asset("icons", "world_frog_day", "world_frog_day") - frog_signal_source = os.path.join(ACTIVE_THEME_PATH, "signals") + frog_signal_source = os.path.join(HOLIDAY_THEME_PATH, "world_frog_day", "signals") frog_signal_destination = os.path.join(THEME_SAVE_PATH, "theme_packs/frog/signals") if not os.path.exists(frog_signal_destination): copy_if_exists(frog_signal_source, frog_signal_destination) + update_theme_asset("signals", "world_frog_day", "world_frog_day") - frog_sound_source = os.path.join(ACTIVE_THEME_PATH, "sounds") + frog_sound_source = os.path.join(HOLIDAY_THEME_PATH, "world_frog_day", "sounds") frog_sound_destination = os.path.join(THEME_SAVE_PATH, "theme_packs/frog/sounds") if not os.path.exists(frog_sound_destination): copy_if_exists(frog_sound_source, frog_sound_destination) + update_theme_asset("sounds", "world_frog_day", "world_frog_day") - frog_steering_wheel_source = os.path.join(ACTIVE_THEME_PATH, "steering_wheel") + frog_steering_wheel_source = os.path.join(HOLIDAY_THEME_PATH, "world_frog_day", "steering_wheel") frog_steering_wheel_destination = os.path.join(THEME_SAVE_PATH, "steering_wheels") if not os.path.exists(frog_steering_wheel_destination): copy_if_exists(frog_steering_wheel_source, frog_steering_wheel_destination, single_file_name="frog.png") + update_wheel_image("world_frog_day", "world_frog_day", random_event=False) - remount_root = ["sudo", "mount", "-o", "remount,rw", "/"] - run_cmd(remount_root, "File system remounted as read-write.", "Failed to remount file system.") + run_cmd(["sudo", "mount", "-o", "remount,rw", "/"], "File system remounted as read-write.", "Failed to remount file system.") boot_logo_location = "/usr/comma/bg.jpg" boot_logo_save_location = os.path.join(BASEDIR, "selfdrive", "frogpilot", "assets", "other_images", "original_bg.jpg") @@ -246,7 +259,6 @@ def uninstall_frogpilot(): boot_logo_location = "/usr/comma/bg.jpg" boot_logo_restore_location = os.path.join(BASEDIR, "selfdrive", "frogpilot", "assets", "other_images", "original_bg.jpg") - copy_cmd = ["sudo", "cp", boot_logo_restore_location, boot_logo_location] - run_cmd(copy_cmd, "Successfully restored the original boot logo.", "Failed to restore the original boot logo.") + run_cmd(["sudo", "cp", boot_logo_restore_location, boot_logo_location], "Successfully restored the original boot logo.", "Failed to restore the original boot logo.") HARDWARE.uninstall() diff --git a/selfdrive/frogpilot/frogpilot_process.py b/selfdrive/frogpilot/frogpilot_process.py index 570183b14f3e22..03372b9de56cd1 100644 --- a/selfdrive/frogpilot/frogpilot_process.py +++ b/selfdrive/frogpilot/frogpilot_process.py @@ -18,6 +18,7 @@ from openpilot.selfdrive.frogpilot.frogpilot_functions import backup_toggles from openpilot.selfdrive.frogpilot.frogpilot_utilities import is_url_pingable from openpilot.selfdrive.frogpilot.frogpilot_variables import FrogPilotVariables, get_frogpilot_toggles, params, params_memory +from openpilot.selfdrive.frogpilot.navigation.mapd import update_mapd locks = { "backup_toggles": threading.Lock(), @@ -26,13 +27,13 @@ "download_theme": threading.Lock(), "update_active_theme": threading.Lock(), "update_checks": threading.Lock(), + "update_mapd": threading.Lock(), "update_models": threading.Lock(), "update_themes": threading.Lock() } running_threads = {} - def run_thread_with_lock(name, target, args=()): if not running_threads.get(name, threading.Thread()).is_alive(): with locks[name]: @@ -40,27 +41,25 @@ def run_thread_with_lock(name, target, args=()): thread.start() running_threads[name] = thread +def automatic_update_check(): + if params_memory.get_bool("ManualUpdateInitiated"): + return + + os.system("pkill -SIGUSR1 -f system.updated.updated") + while params.get("UpdaterState", encoding="utf8") != "idle": + time.sleep(60) -def automatic_update_check(frogs_go_moo, started): - update_available = params.get_bool("UpdaterFetchAvailable") - update_ready = params.get_bool("UpdateAvailable") - update_state_idle = params.get("UpdaterState", encoding='utf8') == "idle" + if not params.get_bool("UpdaterFetchAvailable"): + return - if update_ready and not started: - if not frogs_go_moo: - os.system("pkill -SIGUSR1 -f system.updated.updated") - time.sleep(30) - if params.get_bool("UpdaterFetchAvailable"): - os.system("pkill -SIGHUP -f system.updated.updated") - time.sleep(300) - HARDWARE.reboot() - elif update_available: - os.system("pkill -SIGUSR1 -f system.updated.updated") - time.sleep(30) - os.system("pkill -SIGHUP -f system.updated.updated") - elif update_state_idle: - os.system("pkill -SIGUSR1 -f system.updated.updated") + os.system("pkill -SIGHUP -f system.updated.updated") + while not params.get_bool("UpdateAvailable"): + time.sleep(60) + while params.get_bool("IsOnroad"): + time.sleep(60) + + HARDWARE.reboot() def check_assets(model_manager, theme_manager, frogpilot_toggles): if params_memory.get_bool("DownloadAllModels"): @@ -84,19 +83,19 @@ def check_assets(model_manager, theme_manager, frogpilot_toggles): if asset_to_download is not None: run_thread_with_lock("download_theme", theme_manager.download_theme, (asset_type, asset_to_download, param)) - -def update_checks(automatic_updates, frogs_go_moo, model_manager, now, screen_off, started, theme_manager, time_validated): +def update_checks(model_manager, now, theme_manager, time_validated, frogpilot_toggles): if not (is_url_pingable("https://github.com") or is_url_pingable("https://gitlab.com")): return - if automatic_updates and (screen_off or frogs_go_moo): - automatic_update_check(frogs_go_moo, started) + if frogpilot_toggles.automatic_updates: + automatic_update_check() if time_validated: update_maps(now) + run_thread_with_lock("update_mapd", update_mapd()) run_thread_with_lock("update_models", model_manager.update_models) - + run_thread_with_lock("update_themes", theme_manager.update_themes(frogpilot_toggles)) def update_maps(now): maps_selected = params.get("MapsSelected", encoding='utf8') @@ -120,15 +119,18 @@ def update_maps(now): if params.get("OSMDownloadProgress", encoding='utf-8') is None: params_memory.put("OSMDownloadLocations", maps_selected) - params.put_nonblocking("LastMapsUpdate", todays_date) - + params.put("LastMapsUpdate", todays_date) def frogpilot_thread(): config_realtime_process(5, Priority.CTRL_LOW) + error_log = os.path.join(sentry.CRASHES_DIR, 'error.txt') + if os.path.isfile(error_log): + os.remove(error_log) + params_storage = Params("/persist/params") - frogpilot_planner = FrogPilotPlanner() + frogpilot_planner = FrogPilotPlanner(error_log) frogpilot_tracking = FrogPilotTracking() frogpilot_variables = FrogPilotVariables() model_manager = ModelManager() @@ -145,35 +147,37 @@ def frogpilot_thread(): toggles_last_updated = datetime.datetime.now() - error_log = os.path.join(sentry.CRASHES_DIR, 'error.txt') - if os.path.isfile(error_log): - os.remove(error_log) - pm = messaging.PubMaster(['frogpilotPlan']) - sm = messaging.SubMaster(['carState', 'controlsState', 'deviceState', 'modelV2', 'radarState', + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'deviceState', 'modelV2', 'radarState', 'frogpilotCarControl', 'frogpilotCarState', 'frogpilotNavigation'], poll='modelV2', ignore_avg_freq=['radarState']) + run_thread_with_lock("update_active_theme", theme_manager.update_active_theme, (frogpilot_toggles,)) + while True: sm.update() now = datetime.datetime.now() - deviceState = sm['deviceState'] - screen_off = deviceState.screenBrightnessPercent == 0 - started = deviceState.started + + started = sm['deviceState'].started if params_memory.get_bool("FrogPilotTogglesUpdated"): frogpilot_variables.update(started) frogpilot_toggles = get_frogpilot_toggles() + if time_validated: - run_thread_with_lock("backup_toggles", backup_toggles, (params, params_storage)) + run_thread_with_lock("backup_toggles", backup_toggles, (params_storage,)) + run_thread_with_lock("update_active_theme", theme_manager.update_active_theme, (frogpilot_toggles,)) + toggles_last_updated = now toggles_updated = (now - toggles_last_updated).total_seconds() <= 1 if not started and started_previously: - frogpilot_planner = FrogPilotPlanner() + frogpilot_planner = FrogPilotPlanner(error_log) frogpilot_tracking = FrogPilotTracking() + + run_update_checks = True elif started and not started_previously: radarless_model = frogpilot_toggles.radarless_model @@ -181,7 +185,7 @@ def frogpilot_thread(): os.remove(error_log) if started and sm.updated['modelV2']: - frogpilot_planner.update(sm['carState'], sm['controlsState'], sm['frogpilotCarControl'], sm['frogpilotCarState'], + frogpilot_planner.update(sm['carControl'], sm['carState'], sm['controlsState'], sm['frogpilotCarControl'], sm['frogpilotCarState'], sm['frogpilotNavigation'], sm['modelV2'], radarless_model, sm['radarState'], frogpilot_toggles) frogpilot_planner.publish(sm, pm, frogpilot_toggles, toggles_updated) @@ -193,25 +197,24 @@ def frogpilot_thread(): started_previously = started - check_assets(model_manager, theme_manager, frogpilot_toggles) + if now.second % 2 == 0: + check_assets(model_manager, theme_manager, frogpilot_toggles) + + run_update_checks |= params_memory.get_bool("ManualUpdateInitiated") + run_update_checks |= now.second == 0 and (now.minute % 60 == 0 or frogpilot_toggles.frogs_go_moo) - if params_memory.get_bool("ManualUpdateInitiated"): - run_thread_with_lock("update_checks", update_checks, (False, frogpilot_toggles.frogs_go_moo, model_manager, now, screen_off, started, theme_manager, time_validated)) - run_thread_with_lock("update_themes", theme_manager.update_themes(frogpilot_toggles)) - elif now.second == 0: - run_update_checks = not screen_off and not started or now.minute % 15 == 0 or frogpilot_toggles.frogs_go_moo - elif run_update_checks: - run_thread_with_lock("update_checks", update_checks, (frogpilot_toggles.automatic_updates, frogpilot_toggles.frogs_go_moo, model_manager, now, screen_off, started, theme_manager, time_validated)) + if run_update_checks: + run_thread_with_lock("update_checks", update_checks, (model_manager, now, theme_manager, time_validated, frogpilot_toggles)) if time_validated: theme_manager.update_holiday() run_update_checks = False - elif not time_validated: time_validated = system_time_valid() if not time_validated: continue + theme_manager.update_holiday() run_thread_with_lock("update_models", model_manager.update_models, (True,)) run_thread_with_lock("update_themes", theme_manager.update_themes, (frogpilot_toggles, True,)) @@ -219,6 +222,5 @@ def frogpilot_thread(): def main(): frogpilot_thread() - if __name__ == "__main__": main() diff --git a/selfdrive/frogpilot/frogpilot_variables.py b/selfdrive/frogpilot/frogpilot_variables.py index a41ba95513e0cd..4cfa049843e512 100644 --- a/selfdrive/frogpilot/frogpilot_variables.py +++ b/selfdrive/frogpilot/frogpilot_variables.py @@ -2,7 +2,6 @@ import math import os import random -import time from types import SimpleNamespace @@ -18,6 +17,7 @@ from panda import ALTERNATIVE_EXPERIENCE params = Params() +params_default = Params("/data/params_default") params_memory = Params("/dev/shm/params") GearShifter = car.CarState.GearShifter @@ -35,22 +35,14 @@ RANDOM_EVENTS_PATH = os.path.join(BASEDIR, "selfdrive", "frogpilot", "assets", "random_events") THEME_SAVE_PATH = os.path.join("/data", "themes") -DEFAULT_MODEL = "alabama" -DEFAULT_MODEL_NAME = "Alabama" +DEFAULT_MODEL = "playstation" +DEFAULT_MODEL_NAME = "PlayStation®" DEFAULT_CLASSIC_MODEL = "north-dakota" DEFAULT_CLASSIC_MODEL_NAME = "North Dakota (Default)" def get_frogpilot_toggles(): - while True: - toggles = params.get("FrogPilotToggles") - if toggles is not None: - try: - return SimpleNamespace(**json.loads(toggles)) - except Exception as e: - print(f"Unexpected error while retrieving toggles: {e}, value: {toggles}") - time.sleep(0.1) - time.sleep(0.1) + return SimpleNamespace(**json.loads(params_memory.get("FrogPilotToggles", block=True))) def has_prime(): return params.get_int("PrimeType") > 0 @@ -58,308 +50,318 @@ def has_prime(): def update_frogpilot_toggles(): params_memory.put_bool("FrogPilotTogglesUpdated", True) -frogpilot_default_params: list[tuple[str, bool | bytes | int | float | str]] = [ - ("AccelerationPath", 1), - ("AccelerationProfile", 2), - ("AdjacentLeadsUI", 1), - ("AdjacentPath", 0), - ("AdjacentPathMetrics", 0), - ("AdvancedCustomUI", 0), - ("AdvancedLateralTune", 0), - ("AggressiveFollow", 1.25), - ("AggressiveJerkAcceleration", 50), - ("AggressiveJerkDanger", 100), - ("AggressiveJerkDeceleration", 50), - ("AggressiveJerkSpeed", 50), - ("AggressiveJerkSpeedDecrease", 50), - ("AggressivePersonalityProfile", 1), - ("AlertVolumeControl", 0), - ("AlwaysOnLateral", 1), - ("AlwaysOnLateralLKAS", 0), - ("AlwaysOnLateralMain", 1), - ("AMapKey1", ""), - ("AMapKey2", ""), - ("AutomaticallyUpdateModels", 1), - ("AutomaticUpdates", 1), - ("AvailableModels", ""), - ("AvailableModelsNames", ""), - ("BigMap", 0), - ("BlacklistedModels", ""), - ("BlindSpotMetrics", 1), - ("BlindSpotPath", 1), - ("BorderMetrics", 1), - ("CameraView", 3), - ("CarMake", ""), - ("CarModel", ""), - ("CarModelName", ""), - ("CECurves", 1), - ("CECurvesLead", 0), - ("CELead", 0), - ("CEModelStopTime", 8), - ("CENavigation", 1), - ("CENavigationIntersections", 0), - ("CENavigationLead", 0), - ("CENavigationTurns", 1), - ("CertifiedHerbalistDrives", 0), - ("CertifiedHerbalistScore", 0), - ("CESignalSpeed", 55), - ("CESignalLaneDetection", 1), - ("CESlowerLead", 0), - ("CESpeed", 0), - ("CESpeedLead", 0), - ("CEStoppedLead", 0), - ("ClassicModels", ""), - ("ClusterOffset", 1.015), - ("Compass", 0), - ("ConditionalExperimental", 1), - ("CrosstrekTorque", 1), - ("CurveSensitivity", 100), - ("CurveSpeedControl", 0), - ("CustomAlerts", 1), - ("CustomColors", "frog"), - ("CustomCruise", 1), - ("CustomCruiseLong", 5), - ("CustomDistanceIcons", "stock"), - ("CustomIcons", "frog-animated"), - ("CustomizationLevel", 0), - ("CustomizationLevelConfirmed", 0), - ("CustomPersonalities", 0), - ("CustomSignals", "frog"), - ("CustomSounds", "frog"), - ("CustomUI", 1), - ("DecelerationProfile", 1), - ("DefaultModelName", DEFAULT_CLASSIC_MODEL_NAME), - ("DeveloperUI", 0), - ("DeviceManagement", 1), - ("DeviceShutdown", 9), - ("DisableOnroadUploads", 0), - ("DisableOpenpilotLongitudinal", 0), - ("DisengageVolume", 101), - ("DissolvedOxygenDrives", 0), - ("DissolvedOxygenScore", 0), - ("DriverCamera", 0), - ("DuckAmigoDrives", 0), - ("DuckAmigoScore", 0), - ("DynamicPathWidth", 0), - ("DynamicPedalsOnUI", 1), - ("EngageVolume", 101), - ("ExperimentalGMTune", 0), - ("ExperimentalModeActivation", 1), - ("ExperimentalModels", ""), - ("ExperimentalModeViaDistance", 1), - ("ExperimentalModeViaLKAS", 1), - ("ExperimentalModeViaTap", 0), - ("Fahrenheit", 0), - ("ForceAutoTune", 0), - ("ForceAutoTuneOff", 0), - ("ForceFingerprint", 0), - ("ForceMPHDashboard", 0), - ("ForceStandstill", 0), - ("ForceStops", 0), - ("FPSCounter", 1), - ("FrogPilotToggles", ""), - ("FrogsGoMoosTweak", 1), - ("FullMap", 0), - ("GasRegenCmd", 1), - ("GMapKey", ""), - ("GoatScream", 0), - ("GreenLightAlert", 0), - ("HideAlerts", 0), - ("HideAOLStatusBar", 0), - ("HideCEMStatusBar", 0), - ("HideCSCUI", 0), - ("HideLeadMarker", 0), - ("HideMapIcon", 0), - ("HideMaxSpeed", 0), - ("HideSpeed", 0), - ("HideSpeedLimit", 0), - ("HolidayThemes", 1), - ("HumanAcceleration", 1), - ("HumanFollowing", 1), - ("IncreasedStoppedDistance", 3), - ("IncreaseThermalLimits", 0), - ("JerkInfo", 1), - ("LaneChangeCustomizations", 0), - ("LaneChangeTime", 1.0), - ("LaneDetectionWidth", 9), - ("LaneLinesWidth", 4), - ("LateralMetrics", 1), - ("LateralTune", 1), - ("LeadDepartingAlert", 0), - ("LeadDetectionThreshold", 35), - ("LeadInfo", 1), - ("LockDoors", 1), - ("LongitudinalMetrics", 1), - ("LongitudinalTune", 1), - ("LongPitch", 1), - ("LosAngelesDrives", 0), - ("LosAngelesScore", 0), - ("LoudBlindspotAlert", 0), - ("LowVoltageShutdown", str(VBATT_PAUSE_CHARGING)), - ("MapAcceleration", 0), - ("MapDeceleration", 0), - ("MapGears", 0), - ("MapboxPublicKey", ""), - ("MapboxSecretKey", ""), - ("MapsSelected", ""), - ("MapStyle", 0), - ("MaxDesiredAcceleration", 4.0), - ("MinimumLaneChangeSpeed", str(LANE_CHANGE_SPEED_MIN / CV.MPH_TO_MS)), - ("Model", DEFAULT_CLASSIC_MODEL), - ("ModelName", DEFAULT_CLASSIC_MODEL_NAME), - ("ModelRandomizer", 0), - ("ModelUI", 1), - ("MTSCCurvatureCheck", 1), - ("MTSCEnabled", 1), - ("NavigationModels", ""), - ("NavigationUI", 1), - ("NewLongAPI", 0), - ("NewLongAPIGM", 1), - ("NNFF", 1), - ("NNFFLite", 1), - ("NoLogging", 0), - ("NorthDakotaDrives", 0), - ("NorthDakotaScore", 0), - ("NotreDameDrives", 0), - ("NotreDameScore", 0), - ("NoUploads", 0), - ("NudgelessLaneChange", 0), - ("NumericalTemp", 1), - ("OfflineMode", 1), - ("Offset1", 5), - ("Offset2", 5), - ("Offset3", 5), - ("Offset4", 10), - ("OneLaneChange", 1), - ("OnroadDistanceButton", 0), - ("openpilotMinutes", 0), - ("PathEdgeWidth", 20), - ("PathWidth", 6.1), - ("PauseAOLOnBrake", 0), - ("PauseLateralOnSignal", 0), - ("PauseLateralSpeed", 0), - ("PedalsOnUI", 0), - ("PersonalizeOpenpilot", 1), - ("PreferredSchedule", 2), - ("PromptDistractedVolume", 101), - ("PromptVolume", 101), - ("QOLLateral", 1), - ("QOLLongitudinal", 1), - ("QOLVisuals", 1), - ("RadarlessModels", ""), - ("RadicalTurtleDrives", 0), - ("RadicalTurtleScore", 0), - ("RainbowPath", 0), - ("RandomEvents", 0), - ("RecertifiedHerbalistDrives", 0), - ("RecertifiedHerbalistScore", 0), - ("RefuseVolume", 101), - ("RelaxedFollow", 1.75), - ("RelaxedJerkAcceleration", 100), - ("RelaxedJerkDanger", 100), - ("RelaxedJerkDeceleration", 100), - ("RelaxedJerkSpeed", 100), - ("RelaxedJerkSpeedDecrease", 100), - ("RelaxedPersonalityProfile", 1), - ("ResetFrogTheme", 0), - ("ReverseCruise", 0), - ("RoadEdgesWidth", 2), - ("RoadNameUI", 1), - ("RotatingWheel", 1), - ("ScreenBrightness", 101), - ("ScreenBrightnessOnroad", 101), - ("ScreenManagement", 1), - ("ScreenRecorder", 1), - ("ScreenTimeout", 30), - ("ScreenTimeoutOnroad", 30), - ("SearchInput", 0), - ("SecretGoodOpenpilotDrives", 0), - ("SecretGoodOpenpilotScore", 0), - ("SetSpeedLimit", 0), - ("SetSpeedOffset", 0), - ("ShowCPU", 1), - ("ShowGPU", 0), - ("ShowIP", 0), - ("ShowMemoryUsage", 1), - ("ShowSLCOffset", 1), - ("ShowSteering", 1), - ("ShowStoppingPoint", 1), - ("ShowStoppingPointMetrics", 0), - ("ShowStorageLeft", 0), - ("ShowStorageUsed", 0), - ("Sidebar", 0), - ("SidebarMetrics", 1), - ("SignalMetrics", 0), - ("SLCConfirmation", 0), - ("SLCConfirmationHigher", 0), - ("SLCConfirmationLower", 0), - ("SLCFallback", 2), - ("SLCLookaheadHigher", 5), - ("SLCLookaheadLower", 5), - ("SLCOverride", 1), - ("SLCPriority1", "Dashboard"), - ("SLCPriority2", "Navigation"), - ("SLCPriority3", "Map Data"), - ("SNGHack", 1), - ("SpeedLimitChangedAlert", 1), - ("SpeedLimitController", 1), - ("SpeedLimitSources", 0), - ("StartupMessageBottom", "so I do what I want 🐸"), - ("StartupMessageTop", "Hippity hoppity this is my property"), - ("StandardFollow", 1.45), - ("StandardJerkAcceleration", 100), - ("StandardJerkDanger", 100), - ("StandardJerkDeceleration", 100), - ("StandardJerkSpeed", 100), - ("StandardJerkSpeedDecrease", 100), - ("StandardPersonalityProfile", 1), - ("StandbyMode", 0), - ("StaticPedalsOnUI", 0), - ("SteerFriction", 0.1), - ("SteerFrictionStock", 0.1), - ("SteerLatAccel", 2.5), - ("SteerLatAccelStock", 2.5), - ("SteerKP", 1), - ("SteerKPStock", 1), - ("SteerRatio", 15), - ("SteerRatioStock", 15), - ("StoppedTimer", 0), - ("TacoTune", 0), - ("ToyotaDoors", 1), - ("TrafficFollow", 0.5), - ("TrafficJerkAcceleration", 50), - ("TrafficJerkDanger", 100), - ("TrafficJerkDeceleration", 50), - ("TrafficJerkSpeed", 50), - ("TrafficJerkSpeedDecrease", 50), - ("TrafficPersonalityProfile", 1), - ("TuningInfo", 1), - ("TurnAggressiveness", 100), - ("TurnDesires", 0), - ("UnlimitedLength", 1), - ("UnlockDoors", 1), - ("UseSI", 1), - ("UseVienna", 0), - ("VisionTurnControl", 1), - ("VoltSNG", 0), - ("WarningImmediateVolume", 101), - ("WarningSoftVolume", 101), - ("WD40Drives", 0), - ("WD40Score", 0), - ("WheelIcon", "frog"), - ("WheelSpeed", 0) +frogpilot_default_params: list[tuple[str, str | bytes, int]] = [ + ("AccelerationPath", "1", 2), + ("AccelerationProfile", "2", 0), + ("AdjacentLeadsUI", "0", 3), + ("AdjacentPath", "0", 3), + ("AdjacentPathMetrics", "0", 3), + ("AdvancedCustomUI", "0", 2), + ("AdvancedLateralTune", "0", 3), + ("AggressiveFollow", "1.25", 2), + ("AggressiveJerkAcceleration", "50", 3), + ("AggressiveJerkDanger", "100", 3), + ("AggressiveJerkDeceleration", "50", 3), + ("AggressiveJerkSpeed", "50", 3), + ("AggressiveJerkSpeedDecrease", "50", 3), + ("AggressivePersonalityProfile", "1", 2), + ("AlertVolumeControl", "0", 2), + ("AlwaysOnLateral", "1", 0), + ("AlwaysOnLateralLKAS", "0", 0), + ("AlwaysOnLateralMain", "1", 0), + ("AMapKey1", "", 0), + ("AMapKey2", "", 0), + ("AutomaticallyUpdateModels", "1", 2), + ("AutomaticUpdates", "1", 0), + ("AvailableModels", "", 2), + ("AvailableModelsNames", "", 2), + ("BigMap", "0", 2), + ("BlacklistedModels", "", 2), + ("BlindSpotMetrics", "1", 2), + ("BlindSpotPath", "1", 0), + ("BorderMetrics", "0", 2), + ("CameraView", "3", 2), + ("CarMake", "", 0), + ("CarModel", "", 0), + ("CarModelName", "", 0), + ("CECurves", "1", 1), + ("CECurvesLead", "0", 1), + ("CELead", "0", 1), + ("CEModelStopTime", "8", 1), + ("CENavigation", "1", 2), + ("CENavigationIntersections", "0", 2), + ("CENavigationLead", "1", 2), + ("CENavigationTurns", "1", 2), + ("CertifiedHerbalistDrives", "0", 2), + ("CertifiedHerbalistScore", "0", 2), + ("CESignalSpeed", "55", 1), + ("CESignalLaneDetection", "1", 1), + ("CESlowerLead", "0", 1), + ("CESpeed", "0", 2), + ("CESpeedLead", "0", 2), + ("CEStoppedLead", "0", 1), + ("ClassicModels", "", 2), + ("ClusterOffset", "1.015", 2), + ("Compass", "0", 1), + ("ConditionalExperimental", "1", 1), + ("CrosstrekTorque", "1", 3), + ("CurveSensitivity", "100", 2), + ("CurveSpeedControl", "0", 1), + ("CustomAlerts", "1", 0), + ("CustomColors", "frog", 0), + ("CustomCruise", "1", 2), + ("CustomCruiseLong", "5", 2), + ("CustomDistanceIcons", "stock", 0), + ("CustomIcons", "frog-animated", 0), + ("CustomPersonalities", "0", 2), + ("CustomSignals", "frog", 0), + ("CustomSounds", "frog", 0), + ("CustomUI", "1", 0), + ("DecelerationProfile", "1", 2), + ("DefaultModelName", DEFAULT_CLASSIC_MODEL_NAME, 2), + ("DeveloperUI", "0", 2), + ("DeviceManagement", "1", 1), + ("DeviceShutdown", "9", 1), + ("DisableOnroadUploads", "0", 2), + ("DisableOpenpilotLongitudinal", "0", 2), + ("DisengageVolume", "101", 2), + ("DissolvedOxygenDrives", "0", 2), + ("DissolvedOxygenScore", "0", 2), + ("DriverCamera", "0", 1), + ("DuckAmigoDrives", "0", 2), + ("DuckAmigoScore", "0", 2), + ("DynamicPathWidth", "0", 2), + ("DynamicPedalsOnUI", "1", 2), + ("EngageVolume", "101", 2), + ("ExperimentalGMTune", "0", 2), + ("ExperimentalModeActivation", "1", 1), + ("ExperimentalModels", "", 2), + ("ExperimentalModeViaDistance", "1", 1), + ("ExperimentalModeViaLKAS", "1", 1), + ("ExperimentalModeViaTap", "0", 1), + ("Fahrenheit", "0", 2), + ("ForceAutoTune", "0", 2), + ("ForceAutoTuneOff", "0", 2), + ("ForceFingerprint", "0", 2), + ("ForceMPHDashboard", "0", 2), + ("ForceStandstill", "0", 2), + ("ForceStops", "0", 2), + ("FPSCounter", "1", 3), + ("FrogsGoMoosTweak", "1", 2), + ("FullMap", "0", 2), + ("GasRegenCmd", "1", 2), + ("GMapKey", "", 0), + ("GoatScream", "0", 2), + ("GreenLightAlert", "0", 0), + ("HideAlerts", "0", 2), + ("HideAOLStatusBar", "0", 2), + ("HideCEMStatusBar", "0", 2), + ("HideCSCUI", "0", 2), + ("HideLeadMarker", "0", 2), + ("HideMapIcon", "0", 2), + ("HideMaxSpeed", "0", 2), + ("HideSpeed", "0", 2), + ("HideSpeedLimit", "0", 2), + ("HolidayThemes", "1", 0), + ("HumanAcceleration", "1", 2), + ("HumanFollowing", "1", 2), + ("IncreasedStoppedDistance", "0", 2), + ("IncreaseThermalLimits", "0", 3), + ("JerkInfo", "0", 3), + ("LaneChangeCustomizations", "0", 1), + ("LaneChangeTime", "2.0", 1), + ("LaneDetectionWidth", "9", 2), + ("LaneLinesWidth", "4", 2), + ("LateralMetrics", "1", 3), + ("LateralTune", "1", 2), + ("LeadDepartingAlert", "0", 0), + ("LeadDetectionThreshold", "35", 3), + ("LeadInfo", "1", 3), + ("LockDoors", "1", 0), + ("LongitudinalMetrics", "1", 3), + ("LongitudinalTune", "1", 0), + ("LongPitch", "1", 2), + ("LosAngelesDrives", "0", 2), + ("LosAngelesScore", "0", 2), + ("LoudBlindspotAlert", "0", 0), + ("LowVoltageShutdown", str(VBATT_PAUSE_CHARGING), 3), + ("MapAcceleration", "0", 2), + ("MapDeceleration", "0", 2), + ("MapGears", "0", 2), + ("MapboxPublicKey", "", 0), + ("MapboxSecretKey", "", 0), + ("MapsSelected", "", 0), + ("MapStyle", "0", 2), + ("MaxDesiredAcceleration", "4.0", 3), + ("MinimumLaneChangeSpeed", str(LANE_CHANGE_SPEED_MIN / CV.MPH_TO_MS), 2), + ("Model", DEFAULT_CLASSIC_MODEL, 2), + ("ModelName", DEFAULT_CLASSIC_MODEL_NAME, 2), + ("ModelRandomizer", "0", 2), + ("ModelUI", "1", 2), + ("MTSCCurvatureCheck", "1", 2), + ("MTSCEnabled", "1", 1), + ("NavigationModels", "", 2), + ("NavigationUI", "1", 2), + ("NewLongAPI", "0", 2), + ("NewLongAPIGM", "1", 2), + ("NNFF", "1", 2), + ("NNFFLite", "1", 2), + ("NoLogging", "0", 2), + ("NorthDakotaDrives", "0", 2), + ("NorthDakotaScore", "0", 2), + ("NotreDameDrives", "0", 2), + ("NotreDameScore", "0", 2), + ("NoUploads", "0", 2), + ("NudgelessLaneChange", "0", 1), + ("NumericalTemp", "1", 2), + ("OfflineMode", "0", 2), + ("Offset1", "5", 0), + ("Offset2", "5", 0), + ("Offset3", "5", 0), + ("Offset4", "10", 0), + ("OneLaneChange", "1", 2), + ("OnroadDistanceButton", "0", 0), + ("openpilotMinutes", "0", 0), + ("PathEdgeWidth", "20", 2), + ("PathWidth", "6.1", 2), + ("PauseAOLOnBrake", "0", 2), + ("PauseLateralOnSignal", "0", 2), + ("PauseLateralSpeed", "0", 2), + ("PedalsOnUI", "0", 2), + ("PersonalizeOpenpilot", "1", 0), + ("PreferredSchedule", "2", 0), + ("PromptDistractedVolume", "101", 2), + ("PromptVolume", "101", 2), + ("QOLLateral", "1", 2), + ("QOLLongitudinal", "1", 2), + ("QOLVisuals", "1", 0), + ("RadarlessModels", "", 2), + ("RadicalTurtleDrives", "0", 2), + ("RadicalTurtleScore", "0", 2), + ("RainbowPath", "0", 1), + ("RandomEvents", "0", 1), + ("RecertifiedHerbalistDrives", "0", 2), + ("RecertifiedHerbalistScore", "0", 2), + ("RefuseVolume", "101", 2), + ("RelaxedFollow", "1.75", 2), + ("RelaxedJerkAcceleration", "100", 3), + ("RelaxedJerkDanger", "100", 3), + ("RelaxedJerkDeceleration", "100", 3), + ("RelaxedJerkSpeed", "100", 3), + ("RelaxedJerkSpeedDecrease", "100", 3), + ("RelaxedPersonalityProfile", "1", 2), + ("ResetFrogTheme", "0", 0), + ("ReverseCruise", "0", 2), + ("RoadEdgesWidth", "2", 2), + ("RoadNameUI", "1", 2), + ("RotatingWheel", "1", 1), + ("ScreenBrightness", "101", 2), + ("ScreenBrightnessOnroad", "101", 2), + ("ScreenManagement", "1", 2), + ("ScreenRecorder", "1", 2), + ("ScreenTimeout", "30", 2), + ("ScreenTimeoutOnroad", "30", 2), + ("SearchInput", "0", 0), + ("SecretGoodOpenpilotDrives", "0", 2), + ("SecretGoodOpenpilotScore", "0", 2), + ("SetSpeedLimit", "0", 2), + ("SetSpeedOffset", "0", 2), + ("ShowCPU", "1", 3), + ("ShowGPU", "0", 3), + ("ShowIP", "0", 3), + ("ShowMemoryUsage", "1", 3), + ("ShowSLCOffset", "1", 2), + ("ShowSteering", "0", 2), + ("ShowStoppingPoint", "0", 3), + ("ShowStoppingPointMetrics", "0", 3), + ("ShowStorageLeft", "0", 3), + ("ShowStorageUsed", "0", 3), + ("Sidebar", "0", 0), + ("SidebarMetrics", "1", 3), + ("SignalMetrics", "0", 2), + ("SLCConfirmation", "0", 1), + ("SLCConfirmationHigher", "0", 1), + ("SLCConfirmationLower", "0", 1), + ("SLCFallback", "2", 2), + ("SLCLookaheadHigher", "5", 2), + ("SLCLookaheadLower", "5", 2), + ("SLCOverride", "1", 2), + ("SLCPriority1", "Navigation", 2), + ("SLCPriority2", "Map Data", 2), + ("SLCPriority3", "Dashboard", 2), + ("SNGHack", "1", 2), + ("SpeedLimitChangedAlert", "1", 0), + ("SpeedLimitController", "1", 0), + ("SpeedLimitSources", "0", 3), + ("StartupMessageBottom", "so I do what I want 🐸", 0), + ("StartupMessageTop", "Hippity hoppity this is my property", 0), + ("StandardFollow", "1.45", 2), + ("StandardJerkAcceleration", "100", 3), + ("StandardJerkDanger", "100", 3), + ("StandardJerkDeceleration", "100", 3), + ("StandardJerkSpeed", "100", 3), + ("StandardJerkSpeedDecrease", "100", 3), + ("StandardPersonalityProfile", "1", 2), + ("StandbyMode", "0", 2), + ("StaticPedalsOnUI", "0", 2), + ("SteerFriction", "0", 3), + ("SteerFrictionStock", "0", 3), + ("SteerKP", "0", 3), + ("SteerKPStock", "0", 3), + ("SteerLatAccel", "0", 3), + ("SteerLatAccelStock", "0", 3), + ("SteerRatio", "0", 3), + ("SteerRatioStock", "0", 3), + ("StoppedTimer", "0", 1), + ("TacoTune", "0", 2), + ("ToyotaDoors", "1", 0), + ("TrafficFollow", "0.5", 2), + ("TrafficJerkAcceleration", "50", 3), + ("TrafficJerkDanger", "100", 3), + ("TrafficJerkDeceleration", "50", 3), + ("TrafficJerkSpeed", "50", 3), + ("TrafficJerkSpeedDecrease", "50", 3), + ("TrafficPersonalityProfile", "1", 2), + ("TuningInfo", "0", 3), + ("TuningLevel", "0", 0), + ("TuningLevelConfirmed", "0", 0), + ("TurnAggressiveness", "100", 2), + ("TurnDesires", "0", 2), + ("UnlimitedLength", "1", 2), + ("UnlockDoors", "1", 0), + ("UseSI", "1", 3), + ("UseVienna", "0", 2), + ("VisionTurnControl", "1", 1), + ("VoltSNG", "0", 2), + ("WarningImmediateVolume", "101", 2), + ("WarningSoftVolume", "101", 2), + ("WD40Drives", "0", 2), + ("WD40Score", "0", 2), + ("WheelIcon", "frog", 0), + ("WheelSpeed", "0", 2) +] + +misc_tuning_levels: list[tuple[str, str | bytes, int]] = [ + ("SLCPriority", "", 2), + ("SLCQOL", "", 2), + ("StartupAlert", "", 0) ] class FrogPilotVariables: def __init__(self): - self.default_frogpilot_toggles = SimpleNamespace(**dict(frogpilot_default_params)) self.frogpilot_toggles = SimpleNamespace() + self.tuning_levels = {key: lvl for key, _, lvl in frogpilot_default_params + misc_tuning_levels} self.development_branch = get_build_metadata().channel == "FrogPilot-Development" self.frogpilot_toggles.frogs_go_moo = os.path.isfile("/persist/frogsgomoo.py") self.frogpilot_toggles.block_user = self.development_branch and not self.frogpilot_toggles.frogs_go_moo + for k, v, _ in frogpilot_default_params: + params_default.put(k, v) + + params_memory.put("FrogPilotTuningLevels", json.dumps(self.tuning_levels)) + def update(self, started): openpilot_installed = params.get_bool("HasAcceptedTerms") @@ -368,14 +370,14 @@ def update(self, started): if msg_bytes: with car.CarParams.from_bytes(msg_bytes) as CP: - always_on_lateral_set = key == "CarParams" and CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL + always_on_lateral_set = CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL car_make = CP.carName car_model = CP.carFingerprint - has_auto_tune = (car_model == "hyundai" or car_model == "toyota") and CP.lateralTuning.which == "torque" + has_auto_tune = car_model in {"hyundai", "toyota"} and CP.lateralTuning.which == "torque" has_bsm = CP.enableBsm has_radar = not CP.radarUnavailable is_pid_car = CP.lateralTuning.which == "pid" - max_acceleration_enabled = key == "CarParams" and CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX + max_acceleration_enabled = CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX openpilot_longitudinal = CP.openpilotLongitudinalControl pcm_cruise = CP.pcmCruise else: @@ -390,6 +392,10 @@ def update(self, started): openpilot_longitudinal = False pcm_cruise = False + tuning_level = params.get_int("TuningLevel") if params.get_bool("TuningLevelConfirmed") else 3 + + default = params_default + level = self.tuning_levels toggle = self.frogpilot_toggles toggle.is_metric = params.get_bool("IsMetric") @@ -397,215 +403,215 @@ def update(self, started): small_distance_conversion = 1 if toggle.is_metric else CV.INCH_TO_CM speed_conversion = CV.KPH_TO_MS if toggle.is_metric else CV.MPH_TO_MS - toggle.advanced_custom_onroad_ui = params.get_bool("AdvancedCustomUI") - toggle.hide_alerts = toggle.advanced_custom_onroad_ui and params.get_bool("HideAlerts") - toggle.hide_lead_marker = toggle.advanced_custom_onroad_ui and params.get_bool("HideLeadMarker") - toggle.hide_map_icon = toggle.advanced_custom_onroad_ui and params.get_bool("HideMapIcon") - toggle.hide_max_speed = toggle.advanced_custom_onroad_ui and params.get_bool("HideMaxSpeed") - toggle.hide_speed = toggle.advanced_custom_onroad_ui and params.get_bool("HideSpeed") - toggle.hide_speed_limit = toggle.advanced_custom_onroad_ui and params.get_bool("HideSpeedLimit") - toggle.use_wheel_speed = toggle.advanced_custom_onroad_ui and params.get_bool("WheelSpeed") - - toggle.advanced_lateral_tuning = params.get_bool("AdvancedLateralTune") + advanced_custom_ui = params.get_bool("AdvancedCustomUI") if tuning_level >= level["AdvancedCustomUI"] else default.get_bool("AdvancedCustomUI") + toggle.hide_alerts = advanced_custom_ui and (params.get_bool("HideAlerts") if tuning_level >= level["HideAlerts"] else default.get_bool("HideAlerts")) + toggle.hide_lead_marker = advanced_custom_ui and (params.get_bool("HideLeadMarker") if tuning_level >= level["HideLeadMarker"] else default.get_bool("HideLeadMarker")) + toggle.hide_map_icon = advanced_custom_ui and (params.get_bool("HideMapIcon") if tuning_level >= level["HideMapIcon"] else default.get_bool("HideMapIcon")) + toggle.hide_max_speed = advanced_custom_ui and (params.get_bool("HideMaxSpeed") if tuning_level >= level["HideMaxSpeed"] else default.get_bool("HideMaxSpeed")) + toggle.hide_speed = advanced_custom_ui and (params.get_bool("HideSpeed") if tuning_level >= level["HideSpeed"] else default.get_bool("HideSpeed")) + toggle.hide_speed_limit = advanced_custom_ui and (params.get_bool("HideSpeedLimit") if tuning_level >= level["HideSpeedLimit"] else default.get_bool("HideSpeedLimit")) + toggle.use_wheel_speed = advanced_custom_ui and (params.get_bool("WheelSpeed") if tuning_level >= level["WheelSpeed"] else default.get_bool("WheelSpeed")) + + advanced_lateral_tuning = params.get_bool("AdvancedLateralTune") if tuning_level >= level["AdvancedLateralTune"] else default.get_bool("AdvancedLateralTune") + toggle.force_auto_tune = advanced_lateral_tuning and not has_auto_tune and not is_pid_car and (params.get_bool("ForceAutoTune") if tuning_level >= level["ForceAutoTune"] else default.get_bool("ForceAutoTune")) + toggle.force_auto_tune_off = advanced_lateral_tuning and has_auto_tune and not is_pid_car and (params.get_bool("ForceAutoTuneOff") if tuning_level >= level["ForceAutoTuneOff"] else default.get_bool("ForceAutoTuneOff")) stock_steer_friction = params.get_float("SteerFrictionStock") - toggle.steer_friction = params.get_float("SteerFriction") if toggle.advanced_lateral_tuning else stock_steer_friction - toggle.use_custom_steer_friction = toggle.steer_friction != stock_steer_friction and not is_pid_car + toggle.steer_friction = params.get_float("SteerFriction") if advanced_lateral_tuning and tuning_level >= level["SteerFriction"] else stock_steer_friction + toggle.use_custom_steer_friction = toggle.steer_friction != stock_steer_friction and not is_pid_car and not toggle.force_auto_tune or toggle.force_auto_tune_off stock_steer_kp = params.get_float("SteerKPStock") - toggle.steer_kp = params.get_float("SteerKP") if toggle.advanced_lateral_tuning else stock_steer_kp + toggle.steer_kp = params.get_float("SteerKP") if advanced_lateral_tuning and tuning_level >= level["SteerKP"] else stock_steer_kp toggle.use_custom_kp = toggle.steer_kp != stock_steer_kp and not is_pid_car stock_steer_lat_accel_factor = params.get_float("SteerLatAccelStock") - toggle.steer_lat_accel_factor = params.get_float("SteerLatAccel") if toggle.advanced_lateral_tuning else stock_steer_lat_accel_factor - toggle.use_custom_lat_accel_factor = toggle.steer_lat_accel_factor != stock_steer_lat_accel_factor and not is_pid_car + toggle.steer_lat_accel_factor = params.get_float("SteerLatAccel") if advanced_lateral_tuning and tuning_level >= level["SteerLatAccel"] else stock_steer_lat_accel_factor + toggle.use_custom_lat_accel_factor = toggle.steer_lat_accel_factor != stock_steer_lat_accel_factor and not is_pid_car and not toggle.force_auto_tune or toggle.force_auto_tune_off stock_steer_ratio = params.get_float("SteerRatioStock") - toggle.steer_ratio = params.get_float("SteerRatio") if toggle.advanced_lateral_tuning else stock_steer_ratio - toggle.use_custom_steer_ratio = toggle.steer_ratio != stock_steer_ratio - toggle.force_auto_tune = toggle.advanced_lateral_tuning and not has_auto_tune and not is_pid_car and params.get_bool("ForceAutoTune") - toggle.force_auto_tune_off = toggle.advanced_lateral_tuning and has_auto_tune and not is_pid_car and params.get_bool("ForceAutoTuneOff") - - toggle.alert_volume_control = params.get_bool("AlertVolumeControl") - toggle.disengage_volume = params.get_int("DisengageVolume") if toggle.alert_volume_control else 101 - toggle.engage_volume = params.get_int("EngageVolume") if toggle.alert_volume_control else 101 - toggle.prompt_volume = params.get_int("PromptVolume") if toggle.alert_volume_control else 101 - toggle.promptDistracted_volume = params.get_int("PromptDistractedVolume") if toggle.alert_volume_control else 101 - toggle.refuse_volume = params.get_int("RefuseVolume") if toggle.alert_volume_control else 101 - toggle.warningSoft_volume = params.get_int("WarningSoftVolume") if toggle.alert_volume_control else 101 - toggle.warningImmediate_volume = max(params.get_int("WarningImmediateVolume"), 25) if toggle.alert_volume_control else 101 - - toggle.always_on_lateral = params.get_bool("AlwaysOnLateral") + toggle.steer_ratio = params.get_float("SteerRatio") if advanced_lateral_tuning and tuning_level >= level["SteerRatio"] else stock_steer_ratio + toggle.use_custom_steer_ratio = toggle.steer_ratio != stock_steer_ratio and not toggle.force_auto_tune or toggle.force_auto_tune_off + + toggle.alert_volume_control = params.get_bool("AlertVolumeControl") if tuning_level >= level["AlertVolumeControl"] else default.get_bool("AlertVolumeControl") + toggle.disengage_volume = params.get_int("DisengageVolume") if toggle.alert_volume_control and tuning_level >= level["DisengageVolume"] else default.get_int("DisengageVolume") + toggle.engage_volume = params.get_int("EngageVolume") if toggle.alert_volume_control and tuning_level >= level["EngageVolume"] else default.get_int("EngageVolume") + toggle.prompt_volume = params.get_int("PromptVolume") if toggle.alert_volume_control and tuning_level >= level["PromptVolume"] else default.get_int("PromptVolume") + toggle.promptDistracted_volume = params.get_int("PromptDistractedVolume") if toggle.alert_volume_control and tuning_level >= level["PromptDistractedVolume"] else default.get_int("PromptDistractedVolume") + toggle.refuse_volume = params.get_int("RefuseVolume") if toggle.alert_volume_control and tuning_level >= level["RefuseVolume"] else default.get_int("RefuseVolume") + toggle.warningSoft_volume = params.get_int("WarningSoftVolume") if toggle.alert_volume_control and tuning_level >= level["WarningSoftVolume"] else default.get_int("WarningSoftVolume") + toggle.warningImmediate_volume = max(params.get_int("WarningImmediateVolume"), 25) if toggle.alert_volume_control and tuning_level >= level["WarningImmediateVolume"] else default.get_int("WarningImmediateVolume") + + toggle.always_on_lateral = params.get_bool("AlwaysOnLateral") if tuning_level >= level["AlwaysOnLateral"] else default.get_bool("AlwaysOnLateral") toggle.always_on_lateral_set = toggle.always_on_lateral and always_on_lateral_set - toggle.always_on_lateral_lkas = toggle.always_on_lateral_set and car_make != "subaru" and params.get_bool("AlwaysOnLateralLKAS") - toggle.always_on_lateral_main = toggle.always_on_lateral_set and params.get_bool("AlwaysOnLateralMain") - toggle.always_on_lateral_pause_speed = params.get_int("PauseAOLOnBrake") if toggle.always_on_lateral_set else 0 - toggle.always_on_lateral_status_bar = toggle.always_on_lateral_set and not params.get_bool("HideAOLStatusBar") - - toggle.automatic_updates = params.get_bool("AutomaticUpdates") - - toggle.cluster_offset = params.get_float("ClusterOffset") if car_make == "toyota" else 1 - - toggle.conditional_experimental_mode = openpilot_longitudinal and params.get_bool("ConditionalExperimental") - toggle.conditional_curves = toggle.conditional_experimental_mode and params.get_bool("CECurves") - toggle.conditional_curves_lead = toggle.conditional_curves and params.get_bool("CECurvesLead") - toggle.conditional_lead = toggle.conditional_experimental_mode and params.get_bool("CELead") - toggle.conditional_slower_lead = toggle.conditional_lead and params.get_bool("CESlowerLead") - toggle.conditional_stopped_lead = toggle.conditional_lead and params.get_bool("CEStoppedLead") - toggle.conditional_limit = params.get_int("CESpeed") * speed_conversion if toggle.conditional_experimental_mode else 0 - toggle.conditional_limit_lead = params.get_int("CESpeedLead") * speed_conversion if toggle.conditional_experimental_mode else 0 - toggle.conditional_navigation = toggle.conditional_experimental_mode and params.get_bool("CENavigation") - toggle.conditional_navigation_intersections = toggle.conditional_navigation and params.get_bool("CENavigationIntersections") - toggle.conditional_navigation_lead = toggle.conditional_navigation and params.get_bool("CENavigationLead") - toggle.conditional_navigation_turns = toggle.conditional_navigation and params.get_bool("CENavigationTurns") - toggle.conditional_model_stop_time = params.get_int("CEModelStopTime") if toggle.conditional_experimental_mode else 0 - toggle.conditional_signal = params.get_int("CESignalSpeed") if toggle.conditional_experimental_mode else 0 - toggle.conditional_signal_lane_detection = toggle.conditional_signal != 0 and params.get_bool("CESignalLaneDetection") - toggle.conditional_status_bar = toggle.conditional_experimental_mode and not params.get_bool("HideCEMStatusBar") + toggle.always_on_lateral_lkas = toggle.always_on_lateral_set and car_make != "subaru" and (params.get_bool("AlwaysOnLateralLKAS") if tuning_level >= level["AlwaysOnLateralLKAS"] else default.get_bool("AlwaysOnLateralLKAS")) + toggle.always_on_lateral_main = toggle.always_on_lateral_set and (params.get_bool("AlwaysOnLateralMain") if tuning_level >= level["AlwaysOnLateralMain"] else default.get_bool("AlwaysOnLateralMain")) + toggle.always_on_lateral_pause_speed = params.get_int("PauseAOLOnBrake") if toggle.always_on_lateral_set and tuning_level >= level["PauseAOLOnBrake"] else default.get_int("PauseAOLOnBrake") + toggle.always_on_lateral_status_bar = toggle.always_on_lateral_set and not (params.get_bool("HideAOLStatusBar") if tuning_level >= level["HideAOLStatusBar"] else default.get_bool("HideAOLStatusBar")) + + toggle.automatic_updates = params.get_bool("AutomaticUpdates") if tuning_level >= level["AutomaticUpdates"] else default.get_bool("AutomaticUpdates") + + toggle.cluster_offset = params.get_float("ClusterOffset") if car_make == "toyota" and tuning_level >= level["ClusterOffset"] else default.get_float("ClusterOffset") + + toggle.conditional_experimental_mode = openpilot_longitudinal and (params.get_bool("ConditionalExperimental") if tuning_level >= level["ConditionalExperimental"] else default.get_bool("ConditionalExperimental")) + toggle.conditional_curves = toggle.conditional_experimental_mode and (params.get_bool("CECurves") if tuning_level >= level["CECurves"] else default.get_bool("CECurves")) + toggle.conditional_curves_lead = toggle.conditional_curves and (params.get_bool("CECurvesLead") if tuning_level >= level["CECurvesLead"] else default.get_bool("CECurvesLead")) + toggle.conditional_lead = toggle.conditional_experimental_mode and (params.get_bool("CELead") if tuning_level >= level["CELead"] else default.get_bool("CELead")) + toggle.conditional_slower_lead = toggle.conditional_lead and (params.get_bool("CESlowerLead") if tuning_level >= level["CESlowerLead"] else default.get_bool("CESlowerLead")) + toggle.conditional_stopped_lead = toggle.conditional_lead and (params.get_bool("CEStoppedLead") if tuning_level >= level["CEStoppedLead"] else default.get_bool("CEStoppedLead")) + toggle.conditional_limit = params.get_int("CESpeed") * speed_conversion if toggle.conditional_experimental_mode and tuning_level >= level["CESpeed"] else default.get_int("CESpeed") * CV.MPH_TO_MS + toggle.conditional_limit_lead = params.get_int("CESpeedLead") * speed_conversion if toggle.conditional_experimental_mode and tuning_level >= level["CESpeedLead"] else default.get_int("CESpeedLead") * CV.MPH_TO_MS + toggle.conditional_navigation = toggle.conditional_experimental_mode and (params.get_bool("CENavigation") if tuning_level >= level["CENavigation"] else default.get_bool("CENavigation")) + toggle.conditional_navigation_intersections = toggle.conditional_navigation and (params.get_bool("CENavigationIntersections") if tuning_level >= level["CENavigationIntersections"] else default.get_bool("CENavigationIntersections")) + toggle.conditional_navigation_lead = toggle.conditional_navigation and (params.get_bool("CENavigationLead") if tuning_level >= level["CENavigationLead"] else default.get_bool("CENavigationLead")) + toggle.conditional_navigation_turns = toggle.conditional_navigation and (params.get_bool("CENavigationTurns") if tuning_level >= level["CENavigationTurns"] else default.get_bool("CENavigationTurns")) + toggle.conditional_model_stop_time = params.get_int("CEModelStopTime") if toggle.conditional_experimental_mode and tuning_level >= level["CEModelStopTime"] else default.get_int("CEModelStopTime") + toggle.conditional_signal = params.get_int("CESignalSpeed") if toggle.conditional_experimental_mode and tuning_level >= level["CESignalSpeed"] else default.get_int("CESignalSpeed") + toggle.conditional_signal_lane_detection = toggle.conditional_signal != 0 and (params.get_bool("CESignalLaneDetection") if tuning_level >= level["CESignalLaneDetection"] else default.get_bool("CESignalLaneDetection")) + toggle.conditional_status_bar = toggle.conditional_experimental_mode and not (params.get_bool("HideCEMStatusBar") if tuning_level >= level["HideCEMStatusBar"] else default.get_bool("HideCEMStatusBar")) if toggle.conditional_experimental_mode: params.put_bool("ExperimentalMode", True) - toggle.crosstrek_torque = car_model == "SUBARU_IMPREZA" and params.get_bool("CrosstrekTorque") - - toggle.curve_speed_controller = openpilot_longitudinal and params.get_bool("CurveSpeedControl") - toggle.curve_sensitivity = params.get_int("CurveSensitivity") / 100 if toggle.curve_speed_controller else 1 - toggle.hide_csc_ui = toggle.curve_speed_controller and params.get_bool("HideCSCUI") - toggle.turn_aggressiveness = params.get_int("TurnAggressiveness") / 100 if toggle.curve_speed_controller else 1 - toggle.map_turn_speed_controller = toggle.curve_speed_controller and params.get_bool("MTSCEnabled") - toggle.mtsc_curvature_check = toggle.map_turn_speed_controller and params.get_bool("MTSCCurvatureCheck") - toggle.vision_turn_controller = toggle.curve_speed_controller and params.get_bool("VisionTurnControl") - - toggle.custom_alerts = params.get_bool("CustomAlerts") - toggle.goat_scream_alert = toggle.custom_alerts and params.get_bool("GoatScream") - toggle.green_light_alert = toggle.custom_alerts and params.get_bool("GreenLightAlert") - toggle.lead_departing_alert = toggle.custom_alerts and params.get_bool("LeadDepartingAlert") - toggle.loud_blindspot_alert = has_bsm and toggle.custom_alerts and params.get_bool("LoudBlindspotAlert") - toggle.speed_limit_changed_alert = toggle.custom_alerts and params.get_bool("SpeedLimitChangedAlert") - - toggle.custom_personalities = openpilot_longitudinal and params.get_bool("CustomPersonalities") - toggle.aggressive_profile = toggle.custom_personalities and params.get_bool("AggressivePersonalityProfile") - toggle.aggressive_jerk_acceleration = clip(params.get_int("AggressiveJerkAcceleration") / 100, 0.01, 5) if toggle.aggressive_profile else 0.5 - toggle.aggressive_jerk_deceleration = clip(params.get_int("AggressiveJerkDeceleration") / 100, 0.01, 5) if toggle.aggressive_profile else 0.5 - toggle.aggressive_jerk_danger = clip(params.get_int("AggressiveJerkDanger") / 100, 0.01, 5) if toggle.aggressive_profile else 0.5 - toggle.aggressive_jerk_speed = clip(params.get_int("AggressiveJerkSpeed") / 100, 0.01, 5) if toggle.aggressive_profile else 0.5 - toggle.aggressive_jerk_speed_decrease = clip(params.get_int("AggressiveJerkSpeedDecrease") / 100, 0.01, 5) if toggle.aggressive_profile else 0.5 - toggle.aggressive_follow = clip(params.get_float("AggressiveFollow"), 1, 5) if toggle.aggressive_profile else 1.25 - toggle.standard_profile = toggle.custom_personalities and params.get_bool("StandardPersonalityProfile") - toggle.standard_jerk_acceleration = clip(params.get_int("StandardJerkAcceleration") / 100, 0.01, 5) if toggle.standard_profile else 1.0 - toggle.standard_jerk_deceleration = clip(params.get_int("StandardJerkDeceleration") / 100, 0.01, 5) if toggle.standard_profile else 1.0 - toggle.standard_jerk_danger = clip(params.get_int("StandardJerkDanger") / 100, 0.01, 5) if toggle.standard_profile else 0.5 - toggle.standard_jerk_speed = clip(params.get_int("StandardJerkSpeed") / 100, 0.01, 5) if toggle.standard_profile else 1.0 - toggle.standard_jerk_speed_decrease = clip(params.get_int("StandardJerkSpeedDecrease") / 100, 0.01, 5) if toggle.standard_profile else 1.0 - toggle.standard_follow = clip(params.get_float("StandardFollow"), 1, 5) if toggle.standard_profile else 1.45 - toggle.relaxed_profile = toggle.custom_personalities and params.get_bool("RelaxedPersonalityProfile") - toggle.relaxed_jerk_acceleration = clip(params.get_int("RelaxedJerkAcceleration") / 100, 0.01, 5) if toggle.relaxed_profile else 1.0 - toggle.relaxed_jerk_deceleration = clip(params.get_int("RelaxedJerkDeceleration") / 100, 0.01, 5) if toggle.relaxed_profile else 1.0 - toggle.relaxed_jerk_danger = clip(params.get_int("RelaxedJerkDanger") / 100, 0.01, 5) if toggle.relaxed_profile else 0.5 - toggle.relaxed_jerk_speed = clip(params.get_int("RelaxedJerkSpeed") / 100, 0.01, 5) if toggle.relaxed_profile else 1.0 - toggle.relaxed_jerk_speed_decrease = clip(params.get_int("RelaxedJerkSpeedDecrease") / 100, 0.01, 5) if toggle.relaxed_profile else 1.0 - toggle.relaxed_follow = clip(params.get_float("RelaxedFollow"), 1, 5) if toggle.relaxed_profile else 1.75 - toggle.traffic_profile = toggle.custom_personalities and params.get_bool("TrafficPersonalityProfile") - toggle.traffic_mode_jerk_acceleration = [clip(params.get_int("TrafficJerkAcceleration") / 100, 0.01, 5), toggle.aggressive_jerk_acceleration] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_jerk_deceleration = [clip(params.get_int("TrafficJerkDeceleration") / 100, 0.01, 5), toggle.aggressive_jerk_deceleration] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_jerk_danger = [clip(params.get_int("TrafficJerkDanger") / 100, 0.01, 5), toggle.aggressive_jerk_danger] if toggle.traffic_profile else [1.0, 1.0] - toggle.traffic_mode_jerk_speed = [clip(params.get_int("TrafficJerkSpeed") / 100, 0.01, 5), toggle.aggressive_jerk_speed] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_jerk_speed_decrease = [clip(params.get_int("TrafficJerkSpeedDecrease") / 100, 0.01, 5), toggle.aggressive_jerk_speed_decrease] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_t_follow = [clip(params.get_float("TrafficFollow"), 0.5, 5), toggle.aggressive_follow] if toggle.traffic_profile else [0.5, 1.0] - - toggle.custom_ui = params.get_bool("CustomUI") - toggle.acceleration_path = toggle.custom_ui and params.get_bool("AccelerationPath") - toggle.adjacent_paths = toggle.custom_ui and params.get_bool("AdjacentPath") - toggle.blind_spot_path = has_bsm and toggle.custom_ui and params.get_bool("BlindSpotPath") - toggle.compass = toggle.custom_ui and params.get_bool("Compass") - toggle.pedals_on_ui = toggle.custom_ui and params.get_bool("PedalsOnUI") - toggle.dynamic_pedals_on_ui = toggle.pedals_on_ui and params.get_bool("DynamicPedalsOnUI") - toggle.static_pedals_on_ui = toggle.pedals_on_ui and params.get_bool("StaticPedalsOnUI") - toggle.rotating_wheel = toggle.custom_ui and params.get_bool("RotatingWheel") - - toggle.developer_ui = params.get_bool("DeveloperUI") - toggle.border_metrics = toggle.developer_ui and params.get_bool("BorderMetrics") - toggle.blind_spot_metrics = has_bsm and toggle.border_metrics and params.get_bool("BlindSpotMetrics") - toggle.signal_metrics = toggle.border_metrics and params.get_bool("SignalMetrics") - toggle.steering_metrics = toggle.border_metrics and params.get_bool("ShowSteering") - toggle.show_fps = toggle.developer_ui and params.get_bool("FPSCounter") - toggle.lateral_metrics = toggle.developer_ui and params.get_bool("LateralMetrics") - toggle.adjacent_path_metrics = toggle.lateral_metrics and params.get_bool("AdjacentPathMetrics") - toggle.lateral_tuning_metrics = toggle.lateral_metrics and params.get_bool("TuningInfo") - toggle.longitudinal_metrics = toggle.developer_ui and params.get_bool("LongitudinalMetrics") - toggle.adjacent_lead_tracking = has_radar and toggle.longitudinal_metrics and params.get_bool("AdjacentLeadsUI") - toggle.lead_metrics = toggle.longitudinal_metrics and params.get_bool("LeadInfo") - toggle.jerk_metrics = toggle.longitudinal_metrics and params.get_bool("JerkInfo") - toggle.numerical_temp = toggle.developer_ui and params.get_bool("NumericalTemp") - toggle.fahrenheit = toggle.numerical_temp and params.get_bool("Fahrenheit") - toggle.sidebar_metrics = toggle.developer_ui and params.get_bool("SidebarMetrics") - toggle.cpu_metrics = toggle.sidebar_metrics and params.get_bool("ShowCPU") - toggle.gpu_metrics = toggle.sidebar_metrics and params.get_bool("ShowGPU") - toggle.ip_metrics = toggle.sidebar_metrics and params.get_bool("ShowIP") - toggle.memory_metrics = toggle.sidebar_metrics and params.get_bool("ShowMemoryUsage") - toggle.storage_left_metrics = toggle.sidebar_metrics and params.get_bool("ShowStorageLeft") - toggle.storage_used_metrics = toggle.sidebar_metrics and params.get_bool("ShowStorageUsed") - toggle.use_si_metrics = toggle.developer_ui and params.get_bool("UseSI") - - toggle.device_management = params.get_bool("DeviceManagement") - device_shutdown_setting = params.get_int("DeviceShutdown") if toggle.device_management else 33 + toggle.crosstrek_torque = car_model == "SUBARU_IMPREZA" and params.get_bool("CrosstrekTorque") if tuning_level >= level["CrosstrekTorque"] else default.get_bool("CrosstrekTorque") + + toggle.curve_speed_controller = openpilot_longitudinal and (params.get_bool("CurveSpeedControl") if tuning_level >= level["CurveSpeedControl"] else default.get_bool("CurveSpeedControl")) + toggle.curve_sensitivity = params.get_int("CurveSensitivity") / 100 if toggle.curve_speed_controller and tuning_level >= level["CurveSensitivity"] else default.get_int("CurveSensitivity") + toggle.hide_csc_ui = toggle.curve_speed_controller and (params.get_bool("HideCSCUI") if tuning_level >= level["HideCSCUI"] else default.get_bool("HideCSCUI")) + toggle.turn_aggressiveness = params.get_int("TurnAggressiveness") / 100 if toggle.curve_speed_controller and tuning_level >= level["TurnAggressiveness"] else default.get_int("TurnAggressiveness") + toggle.map_turn_speed_controller = toggle.curve_speed_controller and (params.get_bool("MTSCEnabled") if tuning_level >= level["MTSCEnabled"] else default.get_bool("MTSCEnabled")) + toggle.mtsc_curvature_check = toggle.map_turn_speed_controller and (params.get_bool("MTSCCurvatureCheck") if tuning_level >= level["MTSCCurvatureCheck"] else default.get_bool("MTSCCurvatureCheck")) + toggle.vision_turn_controller = toggle.curve_speed_controller and (params.get_bool("VisionTurnControl") if tuning_level >= level["VisionTurnControl"] else default.get_bool("VisionTurnControl")) + + toggle.custom_alerts = params.get_bool("CustomAlerts") if tuning_level >= level["CustomAlerts"] else default.get_bool("CustomAlerts") + toggle.goat_scream_alert = toggle.custom_alerts and (params.get_bool("GoatScream") if tuning_level >= level["GoatScream"] else default.get_bool("GoatScream")) + toggle.green_light_alert = toggle.custom_alerts and (params.get_bool("GreenLightAlert") if tuning_level >= level["GreenLightAlert"] else default.get_bool("GreenLightAlert")) + toggle.lead_departing_alert = toggle.custom_alerts and (params.get_bool("LeadDepartingAlert") if tuning_level >= level["LeadDepartingAlert"] else default.get_bool("LeadDepartingAlert")) + toggle.loud_blindspot_alert = has_bsm and toggle.custom_alerts and (params.get_bool("LoudBlindspotAlert") if tuning_level >= level["LoudBlindspotAlert"] else default.get_bool("LoudBlindspotAlert")) + toggle.speed_limit_changed_alert = toggle.custom_alerts and (params.get_bool("SpeedLimitChangedAlert") if tuning_level >= level["SpeedLimitChangedAlert"] else default.get_bool("SpeedLimitChangedAlert")) + + toggle.custom_personalities = openpilot_longitudinal and params.get_bool("CustomPersonalities") if tuning_level >= level["CustomPersonalities"] else default.get_bool("CustomPersonalities") + aggressive_profile = toggle.custom_personalities and (params.get_bool("AggressivePersonalityProfile") if tuning_level >= level["AggressivePersonalityProfile"] else default.get_bool("AggressivePersonalityProfile")) + toggle.aggressive_jerk_acceleration = clip(params.get_int("AggressiveJerkAcceleration") / 100, 0.01, 5) if aggressive_profile and tuning_level >= level["AggressiveJerkAcceleration"] else default.get_int("AggressiveJerkAcceleration") / 100 + toggle.aggressive_jerk_deceleration = clip(params.get_int("AggressiveJerkDeceleration") / 100, 0.01, 5) if aggressive_profile and tuning_level >= level["AggressiveJerkDeceleration"] else default.get_int("AggressiveJerkDeceleration") / 100 + toggle.aggressive_jerk_danger = clip(params.get_int("AggressiveJerkDanger") / 100, 0.01, 5) if aggressive_profile and tuning_level >= level["AggressiveJerkDanger"] else default.get_int("AggressiveJerkDanger") / 100 + toggle.aggressive_jerk_speed = clip(params.get_int("AggressiveJerkSpeed") / 100, 0.01, 5) if aggressive_profile and tuning_level >= level["AggressiveJerkSpeed"] else default.get_int("AggressiveJerkSpeed") / 100 + toggle.aggressive_jerk_speed_decrease = clip(params.get_int("AggressiveJerkSpeedDecrease") / 100, 0.01, 5) if aggressive_profile and tuning_level >= level["AggressiveJerkSpeedDecrease"] else default.get_int("AggressiveJerkSpeedDecrease") / 100 + toggle.aggressive_follow = clip(params.get_float("AggressiveFollow"), 1, 5) if aggressive_profile and tuning_level >= level["AggressiveFollow"] else default.get_float("AggressiveFollow") / 100 + standard_profile = toggle.custom_personalities and (params.get_bool("StandardPersonalityProfile") if tuning_level >= level["StandardPersonalityProfile"] else default.get_bool("StandardPersonalityProfile")) + toggle.standard_jerk_acceleration = clip(params.get_int("StandardJerkAcceleration") / 100, 0.01, 5) if standard_profile and tuning_level >= level["StandardJerkAcceleration"] else default.get_int("StandardJerkAcceleration") / 100 + toggle.standard_jerk_deceleration = clip(params.get_int("StandardJerkDeceleration") / 100, 0.01, 5) if standard_profile and tuning_level >= level["StandardJerkDeceleration"] else default.get_int("StandardJerkDeceleration") / 100 + toggle.standard_jerk_danger = clip(params.get_int("StandardJerkDanger") / 100, 0.01, 5) if standard_profile and tuning_level >= level["StandardJerkDanger"] else default.get_int("StandardJerkDanger") / 100 + toggle.standard_jerk_speed = clip(params.get_int("StandardJerkSpeed") / 100, 0.01, 5) if standard_profile and tuning_level >= level["StandardJerkSpeed"] else default.get_int("StandardJerkSpeed") / 100 + toggle.standard_jerk_speed_decrease = clip(params.get_int("StandardJerkSpeedDecrease") / 100, 0.01, 5) if standard_profile and tuning_level >= level["StandardJerkSpeedDecrease"] else default.get_int("StandardJerkSpeedDecrease") / 100 + toggle.standard_follow = clip(params.get_float("StandardFollow"), 1, 5) if standard_profile and tuning_level >= level["StandardFollow"] else default.get_float("StandardFollow") / 100 + relaxed_profile = toggle.custom_personalities and (params.get_bool("RelaxedPersonalityProfile") if tuning_level >= level["RelaxedPersonalityProfile"] else default.get_bool("RelaxedPersonalityProfile")) + toggle.relaxed_jerk_acceleration = clip(params.get_int("RelaxedJerkAcceleration") / 100, 0.01, 5) if relaxed_profile and tuning_level >= level["RelaxedJerkAcceleration"] else default.get_int("RelaxedJerkAcceleration") / 100 + toggle.relaxed_jerk_deceleration = clip(params.get_int("RelaxedJerkDeceleration") / 100, 0.01, 5) if relaxed_profile and tuning_level >= level["RelaxedJerkDeceleration"] else default.get_int("RelaxedJerkDeceleration") / 100 + toggle.relaxed_jerk_danger = clip(params.get_int("RelaxedJerkDanger") / 100, 0.01, 5) if relaxed_profile and tuning_level >= level["RelaxedJerkDanger"] else default.get_int("RelaxedJerkDanger") / 100 + toggle.relaxed_jerk_speed = clip(params.get_int("RelaxedJerkSpeed") / 100, 0.01, 5) if relaxed_profile and tuning_level >= level["RelaxedJerkSpeed"] else default.get_int("RelaxedJerkSpeed") / 100 + toggle.relaxed_jerk_speed_decrease = clip(params.get_int("RelaxedJerkSpeedDecrease") / 100, 0.01, 5) if relaxed_profile and tuning_level >= level["RelaxedJerkSpeedDecrease"] else default.get_int("RelaxedJerkSpeedDecrease") / 100 + toggle.relaxed_follow = clip(params.get_float("RelaxedFollow"), 1, 5) if relaxed_profile and tuning_level >= level["RelaxedFollow"] else default.get_float("RelaxedFollow") / 100 + traffic_profile = toggle.custom_personalities and (params.get_bool("TrafficPersonalityProfile") if tuning_level >= level["TrafficPersonalityProfile"] else default.get_bool("TrafficPersonalityProfile")) + toggle.traffic_mode_jerk_acceleration = [clip(params.get_int("TrafficJerkAcceleration") / 100, 0.01, 5) if traffic_profile and tuning_level >= level["TrafficJerkAcceleration"] else default.get_int("TrafficJerkAcceleration") / 100, toggle.aggressive_jerk_acceleration] + toggle.traffic_mode_jerk_deceleration = [clip(params.get_int("TrafficJerkDeceleration") / 100, 0.01, 5) if traffic_profile and tuning_level >= level["TrafficJerkDeceleration"] else default.get_int("TrafficJerkDeceleration") / 100, toggle.aggressive_jerk_deceleration] + toggle.traffic_mode_jerk_danger = [clip(params.get_int("TrafficJerkDanger") / 100, 0.01, 5) if traffic_profile and tuning_level >= level["TrafficJerkDanger"] else default.get_int("TrafficJerkDanger") / 100, toggle.aggressive_jerk_danger] + toggle.traffic_mode_jerk_speed = [clip(params.get_int("TrafficJerkSpeed") / 100, 0.01, 5) if traffic_profile and tuning_level >= level["TrafficJerkSpeed"] else default.get_int("TrafficJerkSpeed") / 100, toggle.aggressive_jerk_speed] + toggle.traffic_mode_jerk_speed_decrease = [clip(params.get_int("TrafficJerkSpeedDecrease") / 100, 0.01, 5) if traffic_profile and tuning_level >= level["TrafficJerkSpeedDecrease"] else default.get_int("TrafficJerkSpeedDecrease") / 100, toggle.aggressive_jerk_speed_decrease] + toggle.traffic_mode_follow = [clip(params.get_float("TrafficFollow"), 0.5, 5) if traffic_profile and tuning_level >= level["TrafficFollow"] else default.get_float("TrafficFollow") / 100, toggle.aggressive_follow] + + custom_ui = params.get_bool("CustomUI") if tuning_level >= level["CustomUI"] else default.get_bool("CustomUI") + toggle.acceleration_path = custom_ui and (params.get_bool("AccelerationPath") if tuning_level >= level["AccelerationPath"] else default.get_bool("AccelerationPath")) + toggle.adjacent_paths = custom_ui and (params.get_bool("AdjacentPath") if tuning_level >= level["AdjacentPath"] else default.get_bool("AdjacentPath")) + toggle.blind_spot_path = has_bsm and custom_ui and (params.get_bool("BlindSpotPath") if tuning_level >= level["BlindSpotPath"] else default.get_bool("BlindSpotPath")) + toggle.compass = custom_ui and (params.get_bool("Compass") if tuning_level >= level["Compass"] else default.get_bool("Compass")) + toggle.pedals_on_ui = custom_ui and (params.get_bool("PedalsOnUI") if tuning_level >= level["PedalsOnUI"] else default.get_bool("PedalsOnUI")) + toggle.dynamic_pedals_on_ui = toggle.pedals_on_ui and (params.get_bool("DynamicPedalsOnUI") if tuning_level >= level["DynamicPedalsOnUI"] else default.get_bool("DynamicPedalsOnUI")) + toggle.static_pedals_on_ui = toggle.pedals_on_ui and (params.get_bool("StaticPedalsOnUI") if tuning_level >= level["StaticPedalsOnUI"] else default.get_bool("StaticPedalsOnUI")) + toggle.rotating_wheel = custom_ui and (params.get_bool("RotatingWheel") if tuning_level >= level["RotatingWheel"] else default.get_bool("RotatingWheel")) + + toggle.developer_ui = params.get_bool("DeveloperUI") if tuning_level >= level["DeveloperUI"] else default.get_bool("DeveloperUI") + border_metrics = toggle.developer_ui and (params.get_bool("BorderMetrics") if tuning_level >= level["BorderMetrics"] else default.get_bool("BorderMetrics")) + toggle.blind_spot_metrics = has_bsm and border_metrics and (params.get_bool("BlindSpotMetrics") if tuning_level >= level["BlindSpotMetrics"] else default.get_bool("BlindSpotMetrics")) + toggle.signal_metrics = border_metrics and (params.get_bool("SignalMetrics") if tuning_level >= level["SignalMetrics"] else default.get_bool("SignalMetrics")) + toggle.steering_metrics = border_metrics and (params.get_bool("ShowSteering") if tuning_level >= level["ShowSteering"] else default.get_bool("ShowSteering")) + toggle.show_fps = toggle.developer_ui and (params.get_bool("FPSCounter") if tuning_level >= level["FPSCounter"] else default.get_bool("FPSCounter")) + lateral_metrics = toggle.developer_ui and (params.get_bool("LateralMetrics") if tuning_level >= level["LateralMetrics"] else default.get_bool("LateralMetrics")) + toggle.adjacent_path_metrics = lateral_metrics and (params.get_bool("AdjacentPathMetrics") if tuning_level >= level["AdjacentPathMetrics"] else default.get_bool("AdjacentPathMetrics")) + lateral_tuning_metrics = lateral_metrics and (params.get_bool("TuningInfo") if tuning_level >= level["TuningInfo"] else default.get_bool("TuningInfo")) + longitudinal_metrics = toggle.developer_ui and (params.get_bool("LongitudinalMetrics") if tuning_level >= level["LongitudinalMetrics"] else default.get_bool("LongitudinalMetrics")) + toggle.adjacent_lead_tracking = has_radar and longitudinal_metrics and (params.get_bool("AdjacentLeadsUI") if tuning_level >= level["AdjacentLeadsUI"] else default.get_bool("AdjacentLeadsUI")) + toggle.lead_metrics = longitudinal_metrics and (params.get_bool("LeadInfo") if tuning_level >= level["LeadInfo"] else default.get_bool("LeadInfo")) + toggle.jerk_metrics = longitudinal_metrics and (params.get_bool("JerkInfo") if tuning_level >= level["JerkInfo"] else default.get_bool("JerkInfo")) + toggle.numerical_temp = toggle.developer_ui and (params.get_bool("NumericalTemp") if tuning_level >= level["NumericalTemp"] else default.get_bool("NumericalTemp")) + toggle.fahrenheit = toggle.numerical_temp and (params.get_bool("Fahrenheit") if tuning_level >= level["Fahrenheit"] else default.get_bool("Fahrenheit")) + toggle.sidebar_metrics = toggle.developer_ui and (params.get_bool("SidebarMetrics") if tuning_level >= level["SidebarMetrics"] else default.get_bool("SidebarMetrics")) + toggle.cpu_metrics = toggle.sidebar_metrics and (params.get_bool("ShowCPU") if tuning_level >= level["ShowCPU"] else default.get_bool("ShowCPU")) + toggle.gpu_metrics = toggle.sidebar_metrics and (params.get_bool("ShowGPU") if tuning_level >= level["ShowGPU"] else default.get_bool("ShowGPU")) + toggle.ip_metrics = toggle.sidebar_metrics and (params.get_bool("ShowIP") if tuning_level >= level["ShowIP"] else default.get_bool("ShowIP")) + toggle.memory_metrics = toggle.sidebar_metrics and (params.get_bool("ShowMemoryUsage") if tuning_level >= level["ShowMemoryUsage"] else default.get_bool("ShowMemoryUsage")) + toggle.storage_left_metrics = toggle.sidebar_metrics and (params.get_bool("ShowStorageLeft") if tuning_level >= level["ShowStorageLeft"] else default.get_bool("ShowStorageLeft")) + toggle.storage_used_metrics = toggle.sidebar_metrics and (params.get_bool("ShowStorageUsed") if tuning_level >= level["ShowStorageUsed"] else default.get_bool("ShowStorageUsed")) + toggle.use_si_metrics = toggle.developer_ui and (params.get_bool("UseSI") if tuning_level >= level["UseSI"] else default.get_bool("UseSI")) + + device_management = params.get_bool("DeviceManagement") if tuning_level >= level["DeviceManagement"] else default.get_bool("DeviceManagement") + device_shutdown_setting = params.get_int("DeviceShutdown") if device_management and tuning_level >= level["DeviceShutdown"] else default.get_int("DeviceShutdown") toggle.device_shutdown_time = (device_shutdown_setting - 3) * 3600 if device_shutdown_setting >= 4 else device_shutdown_setting * (60 * 15) - toggle.increase_thermal_limits = toggle.device_management and params.get_bool("IncreaseThermalLimits") - toggle.low_voltage_shutdown = clip(params.get_float("LowVoltageShutdown"), VBATT_PAUSE_CHARGING, 12.5) if toggle.device_management else VBATT_PAUSE_CHARGING - toggle.no_logging = toggle.device_management and params.get_bool("NoLogging") or self.development_branch - toggle.no_uploads = toggle.device_management and params.get_bool("NoUploads") or self.development_branch - toggle.no_onroad_uploads = toggle.no_uploads and params.get_bool("DisableOnroadUploads") - toggle.offline_mode = toggle.device_management and params.get_bool("OfflineMode") + toggle.increase_thermal_limits = device_management and (params.get_bool("IncreaseThermalLimits") if tuning_level >= level["IncreaseThermalLimits"] else default.get_bool("IncreaseThermalLimits")) + toggle.low_voltage_shutdown = clip(params.get_float("LowVoltageShutdown"), VBATT_PAUSE_CHARGING, 12.5) if device_management and tuning_level >= level["LowVoltageShutdown"] else default.get_float("LowVoltageShutdown") + toggle.no_logging = device_management and (params.get_bool("NoLogging") if tuning_level >= level["NoLogging"] else default.get_bool("NoLogging")) + toggle.no_uploads = device_management and (params.get_bool("NoUploads") if tuning_level >= level["NoUploads"] else default.get_bool("NoUploads")) + toggle.no_onroad_uploads = toggle.no_uploads and (params.get_bool("DisableOnroadUploads") if tuning_level >= level["DisableOnroadUploads"] else default.get_bool("DisableOnroadUploads")) + toggle.offline_mode = device_management and (params.get_bool("OfflineMode") if tuning_level >= level["OfflineMode"] else default.get_bool("OfflineMode")) - toggle.experimental_gm_tune = openpilot_longitudinal and car_make == "gm" and params.get_bool("ExperimentalGMTune") + toggle.experimental_gm_tune = openpilot_longitudinal and car_make == "gm" and (params.get_bool("ExperimentalGMTune") if tuning_level >= level["ExperimentalGMTune"] else default.get_bool("ExperimentalGMTune")) - toggle.experimental_mode_via_press = openpilot_longitudinal and params.get_bool("ExperimentalModeActivation") - toggle.experimental_mode_via_distance = toggle.experimental_mode_via_press and params.get_bool("ExperimentalModeViaDistance") - toggle.experimental_mode_via_lkas = not toggle.always_on_lateral_lkas and toggle.experimental_mode_via_press and car_make != "subaru" and params.get_bool("ExperimentalModeViaLKAS") - toggle.experimental_mode_via_tap = toggle.experimental_mode_via_press and params.get_bool("ExperimentalModeViaTap") + toggle.experimental_mode_via_press = openpilot_longitudinal and (params.get_bool("ExperimentalModeActivation") if tuning_level >= level["ExperimentalModeActivation"] else default.get_bool("ExperimentalModeActivation")) + toggle.experimental_mode_via_distance = toggle.experimental_mode_via_press and (params.get_bool("ExperimentalModeViaDistance") if tuning_level >= level["ExperimentalModeViaDistance"] else default.get_bool("ExperimentalModeViaDistance")) + toggle.experimental_mode_via_lkas = not toggle.always_on_lateral_lkas and toggle.experimental_mode_via_press and car_make != "subaru" and (params.get_bool("ExperimentalModeViaLKAS") if tuning_level >= level["ExperimentalModeViaLKAS"] else default.get_bool("ExperimentalModeViaLKAS")) + toggle.experimental_mode_via_tap = toggle.experimental_mode_via_press and (params.get_bool("ExperimentalModeViaTap") if tuning_level >= level["ExperimentalModeViaTap"] else default.get_bool("ExperimentalModeViaTap")) - toggle.frogsgomoo_tweak = openpilot_longitudinal and car_make == "toyota" and params.get_bool("FrogsGoMoosTweak") + toggle.frogsgomoo_tweak = openpilot_longitudinal and car_make == "toyota" and (params.get_bool("FrogsGoMoosTweak") if tuning_level >= level["FrogsGoMoosTweak"] else default.get_bool("FrogsGoMoosTweak")) - toggle.holiday_themes = params.get_bool("HolidayThemes") - toggle.current_holiday_theme = params.get("CurrentHolidayTheme", encoding='utf-8') if params.get_bool("HolidayThemes") else None + holiday_themes = params.get_bool("HolidayThemes") if tuning_level >= level["HolidayThemes"] else default.get_bool("HolidayThemes") + toggle.current_holiday_theme = params_memory.get("CurrentHolidayTheme", encoding="utf-8") if holiday_themes else None - toggle.lane_change_customizations = params.get_bool("LaneChangeCustomizations") - toggle.lane_change_delay = params.get_float("LaneChangeTime") if toggle.lane_change_customizations else 0 - toggle.lane_detection_width = params.get_float("LaneDetectionWidth") * distance_conversion if toggle.lane_change_customizations else 0 + lane_change_customizations = params.get_bool("LaneChangeCustomizations") if tuning_level >= level["LaneChangeCustomizations"] else default.get_bool("LaneChangeCustomizations") + toggle.lane_change_delay = params.get_float("LaneChangeTime") if lane_change_customizations and tuning_level >= level["LaneChangeTime"] else default.get_float("LaneChangeTime") + toggle.lane_detection_width = params.get_float("LaneDetectionWidth") * distance_conversion if lane_change_customizations and tuning_level >= level["LaneDetectionWidth"] else default.get_float("LaneDetectionWidth") * CV.FOOT_TO_METER toggle.lane_detection = toggle.lane_detection_width != 0 - toggle.minimum_lane_change_speed = params.get_float("MinimumLaneChangeSpeed") * speed_conversion if toggle.lane_change_customizations else LANE_CHANGE_SPEED_MIN - toggle.nudgeless = toggle.lane_change_customizations and params.get_bool("NudgelessLaneChange") - toggle.one_lane_change = toggle.lane_change_customizations and params.get_bool("OneLaneChange") + toggle.minimum_lane_change_speed = params.get_float("MinimumLaneChangeSpeed") * speed_conversion if lane_change_customizations and tuning_level >= level["MinimumLaneChangeSpeed"] else default.get_float("MinimumLaneChangeSpeed") * CV.MPH_TO_MS + toggle.nudgeless = lane_change_customizations and (params.get_bool("NudgelessLaneChange") if tuning_level >= level["NudgelessLaneChange"] else default.get_bool("NudgelessLaneChange")) + toggle.one_lane_change = lane_change_customizations and (params.get_bool("OneLaneChange") if tuning_level >= level["OneLaneChange"] else default.get_bool("OneLaneChange")) - toggle.lateral_tuning = params.get_bool("LateralTune") - toggle.nnff = toggle.lateral_tuning and params.get_bool("NNFF") - toggle.nnff_lite = toggle.lateral_tuning and params.get_bool("NNFFLite") - toggle.use_turn_desires = toggle.lateral_tuning and params.get_bool("TurnDesires") + lateral_tuning = params.get_bool("LateralTune") if tuning_level >= level["LateralTune"] else default.get_bool("LateralTune") + toggle.nnff = lateral_tuning and (params.get_bool("NNFF") if tuning_level >= level["NNFF"] else default.get_bool("NNFF")) + toggle.nnff_lite = lateral_tuning and (params.get_bool("NNFFLite") if tuning_level >= level["NNFFLite"] else default.get_bool("NNFFLite")) + toggle.use_turn_desires = lateral_tuning and (params.get_bool("TurnDesires") if tuning_level >= level["TurnDesires"] else default.get_bool("TurnDesires")) - toggle.long_pitch = openpilot_longitudinal and car_make == "gm" and params.get_bool("LongPitch") + toggle.long_pitch = openpilot_longitudinal and car_make == "gm" and (params.get_bool("LongPitch") if tuning_level >= level["LongPitch"] else default.get_bool("LongPitch")) - toggle.longitudinal_tuning = openpilot_longitudinal and params.get_bool("LongitudinalTune") - toggle.acceleration_profile = params.get_int("AccelerationProfile") if toggle.longitudinal_tuning else 0 + longitudinal_tuning = openpilot_longitudinal and (params.get_bool("LongitudinalTune") if tuning_level >= level["LongitudinalTune"] else default.get_bool("LongitudinalTune")) + toggle.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tuning and tuning_level >= level["AccelerationProfile"] else default.get_int("AccelerationProfile") toggle.sport_plus = max_acceleration_enabled and toggle.acceleration_profile == 3 - toggle.deceleration_profile = params.get_int("DecelerationProfile") if toggle.longitudinal_tuning else 0 - toggle.human_acceleration = toggle.longitudinal_tuning and params.get_bool("HumanAcceleration") - toggle.human_following = toggle.longitudinal_tuning and params.get_bool("HumanFollowing") - toggle.lead_detection_probability = clip(params.get_int("LeadDetectionThreshold") / 100, 0.01, 0.99) if toggle.longitudinal_tuning else 0.5 - toggle.max_desired_acceleration = clip(params.get_float("MaxDesiredAcceleration"), 0.1, 4.0) if toggle.longitudinal_tuning else 4.0 - toggle.taco_tune = toggle.longitudinal_tuning and params.get_bool("TacoTune") + toggle.deceleration_profile = params.get_int("DecelerationProfile") if longitudinal_tuning and tuning_level >= level["DecelerationProfile"] else default.get_int("DecelerationProfile") + toggle.human_acceleration = longitudinal_tuning and (params.get_bool("HumanAcceleration") if tuning_level >= level["HumanAcceleration"] else default.get_bool("HumanAcceleration")) + toggle.human_following = longitudinal_tuning and (params.get_bool("HumanFollowing") if tuning_level >= level["HumanFollowing"] else default.get_bool("HumanFollowing")) + toggle.lead_detection_probability = clip(params.get_int("LeadDetectionThreshold") / 100, 0.01, 0.99) if longitudinal_tuning and tuning_level >= level["LeadDetectionThreshold"] else clip(default.get_int("LeadDetectionThreshold") / 100, 0.01, 0.99) + toggle.max_desired_acceleration = clip(params.get_float("MaxDesiredAcceleration"), 0.1, 4.0) if longitudinal_tuning and tuning_level >= level["MaxDesiredAcceleration"] else default.get_float("MaxDesiredAcceleration") + toggle.taco_tune = longitudinal_tuning and (params.get_bool("TacoTune") if tuning_level >= level["TacoTune"] else default.get_bool("TacoTune")) available_models = params.get("AvailableModels", encoding='utf-8') or "" available_model_names = params.get("AvailableModelsNames", encoding='utf-8') or "" - toggle.model_randomizer = params.get_bool("ModelRandomizer") + toggle.model_randomizer = params.get_bool("ModelRandomizer") if tuning_level >= level["ModelRandomizer"] else default.get_bool("ModelRandomizer") if available_models: if toggle.model_randomizer: blacklisted_models = (params.get("BlacklistedModels", encoding='utf-8') or "").split(',') existing_models = [model for model in available_models.split(',') if model not in blacklisted_models and os.path.exists(os.path.join(MODELS_PATH, f"{model}.thneed"))] - toggle.model = random.choice(existing_models) if existing_models else DEFAULT_CLASSIC_MODEL + toggle.model = random.choice(existing_models) if existing_models else default.Model else: - toggle.model = params.get("Model", encoding='utf-8') + toggle.model = params.get("Model", encoding='utf-8') if tuning_level >= level["Model"] else default.get("Model", encoding='utf-8') else: - toggle.model = DEFAULT_CLASSIC_MODEL + toggle.model = default.Model if toggle.model in available_models.split(',') and os.path.exists(os.path.join(MODELS_PATH, f"{toggle.model}.thneed")): current_model_name = available_model_names.split(',')[available_models.split(',').index(toggle.model)] params_memory.put("CurrentModelName", current_model_name) else: - toggle.model = DEFAULT_CLASSIC_MODEL + toggle.model = default.Model classic_models = params.get("ClassicModels", encoding='utf-8') or "" toggle.classic_model = classic_models and toggle.model in classic_models.split(',') navigation_models = params.get("NavigationModels", encoding='utf-8') or "" @@ -613,28 +619,28 @@ def update(self, started): radarless_models = params.get("RadarlessModels", encoding='utf-8') or "" toggle.radarless_model = radarless_models and toggle.model in radarless_models.split(',') - toggle.model_ui = params.get_bool("ModelUI") - toggle.dynamic_path_width = toggle.model_ui and params.get_bool("DynamicPathWidth") - toggle.lane_line_width = params.get_int("LaneLinesWidth") * small_distance_conversion / 200 if toggle.model_ui else 0.025 - toggle.path_edge_width = params.get_int("PathEdgeWidth") if toggle.model_ui else 20 - toggle.path_width = params.get_float("PathWidth") * distance_conversion / 2 if toggle.model_ui else 0.9 - toggle.road_edge_width = params.get_int("RoadEdgesWidth") * small_distance_conversion / 200 if toggle.model_ui else 0.025 - toggle.show_stopping_point = toggle.model_ui and params.get_bool("ShowStoppingPoint") - toggle.show_stopping_point_metrics = toggle.show_stopping_point and params.get_bool("ShowStoppingPointMetrics") - toggle.unlimited_road_ui_length = toggle.model_ui and params.get_bool("UnlimitedLength") - - toggle.navigation_ui = params.get_bool("NavigationUI") - toggle.big_map = toggle.navigation_ui and params.get_bool("BigMap") - toggle.full_map = toggle.big_map and params.get_bool("FullMap") - toggle.map_style = params.get_int("MapStyle") if toggle.navigation_ui else 0 - toggle.road_name_ui = toggle.navigation_ui and params.get_bool("RoadNameUI") - toggle.show_speed_limit_offset = toggle.navigation_ui and params.get_bool("ShowSLCOffset") - toggle.speed_limit_vienna = toggle.navigation_ui and params.get_bool("UseVienna") - - toggle.old_long_api = openpilot_longitudinal and car_make == "gm" and not params.get_bool("NewLongAPIGM") - toggle.old_long_api |= openpilot_longitudinal and car_make == "hyundai" and not params.get_bool("NewLongAPI") - - toggle.personalize_openpilot = params.get_bool("PersonalizeOpenpilot") + toggle.model_ui = params.get_bool("ModelUI") if tuning_level >= level["ModelUI"] else default.get_bool("ModelUI") + toggle.dynamic_path_width = toggle.model_ui and (params.get_bool("DynamicPathWidth") if tuning_level >= level["DynamicPathWidth"] else default.get_bool("DynamicPathWidth")) + toggle.lane_line_width = params.get_int("LaneLinesWidth") * small_distance_conversion / 200 if toggle.model_ui and tuning_level >= level["LaneLinesWidth"] else default.get_int("LaneLinesWidth") * CV.INCH_TO_CM / 200 + toggle.path_edge_width = params.get_int("PathEdgeWidth") if toggle.model_ui and tuning_level >= level["PathEdgeWidth"] else default.get_int("PathEdgeWidth") + toggle.path_width = params.get_float("PathWidth") * distance_conversion / 2 if toggle.model_ui and tuning_level >= level["PathWidth"] else default.get_float("PathWidth") * CV.FOOT_TO_METER / 2 + toggle.road_edge_width = params.get_int("RoadEdgesWidth") * small_distance_conversion / 200 if toggle.model_ui and tuning_level >= level["RoadEdgesWidth"] else default.get_int("RoadEdgesWidth") * CV.INCH_TO_CM / 200 + toggle.show_stopping_point = toggle.model_ui and (params.get_bool("ShowStoppingPoint") if tuning_level >= level["ShowStoppingPoint"] else default.get_bool("ShowStoppingPoint")) + toggle.show_stopping_point_metrics = toggle.show_stopping_point and (params.get_bool("ShowStoppingPointMetrics") if tuning_level >= level["ShowStoppingPointMetrics"] else default.get_bool("ShowStoppingPointMetrics")) + toggle.unlimited_road_ui_length = toggle.model_ui and (params.get_bool("UnlimitedLength") if tuning_level >= level["UnlimitedLength"] else default.get_bool("UnlimitedLength")) + + toggle.navigation_ui = params.get_bool("NavigationUI") if tuning_level >= level["NavigationUI"] else default.get_bool("NavigationUI") + toggle.big_map = toggle.navigation_ui and (params.get_bool("BigMap") if tuning_level >= level["BigMap"] else default.get_bool("BigMap")) + toggle.full_map = toggle.big_map and (params.get_bool("FullMap") if tuning_level >= level["FullMap"] else default.get_bool("FullMap")) + toggle.map_style = params.get_int("MapStyle") if toggle.navigation_ui and tuning_level >= level["MapStyle"] else default.get_int("MapStyle") + toggle.road_name_ui = toggle.navigation_ui and (params.get_bool("RoadNameUI") if tuning_level >= level["RoadNameUI"] else default.get_bool("RoadNameUI")) + toggle.show_speed_limit_offset = toggle.navigation_ui and (params.get_bool("ShowSLCOffset") if tuning_level >= level["ShowSLCOffset"] else default.get_bool("ShowSLCOffset")) + toggle.speed_limit_vienna = toggle.navigation_ui and (params.get_bool("UseVienna") if tuning_level >= level["UseVienna"] else default.get_bool("UseVienna")) + + toggle.old_long_api = openpilot_longitudinal and car_make == "gm" and not (params.get_bool("NewLongAPIGM") if tuning_level >= level["NewLongAPIGM"] else default.get_bool("NewLongAPIGM")) + toggle.old_long_api |= openpilot_longitudinal and car_make == "hyundai" and not (params.get_bool("NewLongAPI") if tuning_level >= level["NewLongAPI"] else default.get_bool("NewLongAPI")) + + toggle.personalize_openpilot = params.get_bool("PersonalizeOpenpilot") if tuning_level >= level["PersonalizeOpenpilot"] else default.get_bool("PersonalizeOpenpilot") toggle.color_scheme = params.get("CustomColors", encoding='utf-8') if toggle.personalize_openpilot else "stock" toggle.distance_icons = params.get("CustomDistanceIcons", encoding='utf-8') if toggle.personalize_openpilot else "stock" toggle.icon_pack = params.get("CustomIcons", encoding='utf-8') if toggle.personalize_openpilot else "stock" @@ -642,524 +648,78 @@ def update(self, started): toggle.sound_pack = params.get("CustomSounds", encoding='utf-8') if toggle.personalize_openpilot else "stock" toggle.wheel_image = params.get("WheelIcon", encoding='utf-8') if toggle.personalize_openpilot else "stock" - toggle.quality_of_life_lateral = params.get_bool("QOLLateral") - toggle.pause_lateral_below_speed = params.get_int("PauseLateralSpeed") * speed_conversion if toggle.quality_of_life_lateral else 0 - toggle.pause_lateral_below_signal = toggle.pause_lateral_below_speed != 0 and params.get_bool("PauseLateralOnSignal") - - toggle.quality_of_life_longitudinal = params.get_bool("QOLLongitudinal") - toggle.custom_cruise_increase = params.get_int("CustomCruise") if toggle.quality_of_life_longitudinal and not pcm_cruise else 1 - toggle.custom_cruise_increase_long = params.get_int("CustomCruiseLong") if toggle.quality_of_life_longitudinal and not pcm_cruise else 5 - toggle.force_standstill = toggle.quality_of_life_longitudinal and params.get_bool("ForceStandstill") - toggle.force_stops = toggle.quality_of_life_longitudinal and params.get_bool("ForceStops") - toggle.increased_stopped_distance = params.get_int("IncreasedStoppedDistance") * distance_conversion if toggle.quality_of_life_longitudinal else 0 - toggle.map_gears = toggle.quality_of_life_longitudinal and params.get_bool("MapGears") - toggle.map_acceleration = toggle.map_gears and params.get_bool("MapAcceleration") - toggle.map_deceleration = toggle.map_gears and params.get_bool("MapDeceleration") - toggle.reverse_cruise_increase = toggle.quality_of_life_longitudinal and pcm_cruise and params.get_bool("ReverseCruise") - toggle.set_speed_offset = params.get_int("SetSpeedOffset") * (1 if toggle.is_metric else CV.MPH_TO_KPH) if toggle.quality_of_life_longitudinal and not pcm_cruise else 0 - - toggle.quality_of_life_visuals = params.get_bool("QOLVisuals") - toggle.camera_view = params.get_int("CameraView") if toggle.quality_of_life_visuals else 0 - toggle.driver_camera_in_reverse = toggle.quality_of_life_visuals and params.get_bool("DriverCamera") - toggle.onroad_distance_button = toggle.quality_of_life_visuals and params.get_bool("OnroadDistanceButton") - toggle.standby_mode = toggle.quality_of_life_visuals and params.get_bool("StandbyMode") - toggle.stopped_timer = toggle.quality_of_life_visuals and params.get_bool("StoppedTimer") - - toggle.rainbow_path = params.get_bool("RainbowPath") - - toggle.random_events = params.get_bool("RandomEvents") - - toggle.screen_management = params.get_bool("ScreenManagement") - toggle.screen_brightness = params.get_int("ScreenBrightness") if toggle.screen_management else 101 - toggle.screen_brightness_onroad = params.get_int("ScreenBrightnessOnroad") if toggle.screen_management else 101 - toggle.screen_recorder = toggle.screen_management and params.get_bool("ScreenRecorder") - toggle.screen_timeout = params.get_int("ScreenTimeout") if toggle.screen_management else 30 - toggle.screen_timeout_onroad = params.get_int("ScreenTimeoutOnroad") if toggle.screen_management else 10 - - toggle.sng_hack = openpilot_longitudinal and car_make == "toyota" and params.get_bool("SNGHack") - - toggle.speed_limit_controller = openpilot_longitudinal and params.get_bool("SpeedLimitController") - toggle.force_mph_dashboard = toggle.speed_limit_controller and params.get_bool("ForceMPHDashboard") - toggle.map_speed_lookahead_higher = params.get_int("SLCLookaheadHigher") if toggle.speed_limit_controller else 0 - toggle.map_speed_lookahead_lower = params.get_int("SLCLookaheadLower") if toggle.speed_limit_controller else 0 - toggle.set_speed_limit = toggle.speed_limit_controller and params.get_bool("SetSpeedLimit") - slc_fallback_method = params.get_int("SLCFallback") if toggle.speed_limit_controller else 0 - toggle.slc_fallback_experimental_mode = toggle.speed_limit_controller and slc_fallback_method == 1 - toggle.slc_fallback_previous_speed_limit = toggle.speed_limit_controller and slc_fallback_method == 2 - toggle.slc_fallback_set_speed = toggle.speed_limit_controller and slc_fallback_method == 0 - toggle.speed_limit_confirmation = toggle.speed_limit_controller and params.get_bool("SLCConfirmation") - toggle.speed_limit_confirmation_higher = toggle.speed_limit_confirmation and params.get_bool("SLCConfirmationHigher") - toggle.speed_limit_confirmation_lower = toggle.speed_limit_confirmation and params.get_bool("SLCConfirmationLower") - toggle.speed_limit_controller_override = params.get_int("SLCOverride") if toggle.speed_limit_controller else 0 - toggle.speed_limit_controller_override_manual = toggle.speed_limit_controller_override == 1 - toggle.speed_limit_controller_override_set_speed = toggle.speed_limit_controller_override == 2 - toggle.speed_limit_offset1 = params.get_int("Offset1") * speed_conversion if toggle.speed_limit_controller else 0 - toggle.speed_limit_offset2 = params.get_int("Offset2") * speed_conversion if toggle.speed_limit_controller else 0 - toggle.speed_limit_offset3 = params.get_int("Offset3") * speed_conversion if toggle.speed_limit_controller else 0 - toggle.speed_limit_offset4 = params.get_int("Offset4") * speed_conversion if toggle.speed_limit_controller else 0 - toggle.speed_limit_priority1 = params.get("SLCPriority1", encoding='utf-8') if toggle.speed_limit_controller else None - toggle.speed_limit_priority2 = params.get("SLCPriority2", encoding='utf-8') if toggle.speed_limit_controller else None - toggle.speed_limit_priority3 = params.get("SLCPriority3", encoding='utf-8') if toggle.speed_limit_controller else None + quality_of_life_lateral = params.get_bool("QOLLateral") if tuning_level >= level["QOLLateral"] else default.get_bool("QOLLateral") + toggle.pause_lateral_below_speed = params.get_int("PauseLateralSpeed") * speed_conversion if quality_of_life_lateral and tuning_level >= level["PauseLateralSpeed"] else default.get_int("PauseLateralSpeed") * CV.MPH_TO_MS + toggle.pause_lateral_below_signal = toggle.pause_lateral_below_speed != 0 and (params.get_bool("PauseLateralOnSignal") if tuning_level >= level["PauseLateralOnSignal"] else default.get_bool("PauseLateralOnSignal")) + + quality_of_life_longitudinal = params.get_bool("QOLLongitudinal") if tuning_level >= level["QOLLongitudinal"] else default.get_bool("QOLLongitudinal") + toggle.custom_cruise_increase = params.get_int("CustomCruise") if quality_of_life_longitudinal and not pcm_cruise and tuning_level >= level["CustomCruise"] else default.get_int("CustomCruise") + toggle.custom_cruise_increase_long = params.get_int("CustomCruiseLong") if quality_of_life_longitudinal and not pcm_cruise and tuning_level >= level["CustomCruiseLong"] else default.get_int("CustomCruiseLong") + toggle.force_standstill = quality_of_life_longitudinal and (params.get_bool("ForceStandstill") if tuning_level >= level["ForceStandstill"] else default.get_bool("ForceStandstill")) + toggle.force_stops = quality_of_life_longitudinal and (params.get_bool("ForceStops") if tuning_level >= level["ForceStops"] else default.get_bool("ForceStops")) + toggle.increased_stopped_distance = params.get_int("IncreasedStoppedDistance") * distance_conversion if quality_of_life_longitudinal and tuning_level >= level["IncreasedStoppedDistance"] else default.get_int("IncreasedStoppedDistance") * CV.FOOT_TO_METER + map_gears = quality_of_life_longitudinal and (params.get_bool("MapGears") if tuning_level >= level["MapGears"] else default.get_bool("MapGears")) + toggle.map_acceleration = map_gears and (params.get_bool("MapAcceleration") if tuning_level >= level["MapAcceleration"] else default.get_bool("MapAcceleration")) + toggle.map_deceleration = map_gears and (params.get_bool("MapDeceleration") if tuning_level >= level["MapDeceleration"] else default.get_bool("MapDeceleration")) + toggle.reverse_cruise_increase = quality_of_life_longitudinal and pcm_cruise and (params.get_bool("ReverseCruise") if tuning_level >= level["ReverseCruise"] else default.get_bool("ReverseCruise")) + toggle.set_speed_offset = params.get_int("SetSpeedOffset") * (1 if toggle.is_metric else CV.MPH_TO_KPH) if quality_of_life_longitudinal and not pcm_cruise and tuning_level >= level["SetSpeedOffset"] else default.get_int("SetSpeedOffset") * CV.MPH_TO_KPH + + quality_of_life_visuals = params.get_bool("QOLVisuals") if tuning_level >= level["QOLVisuals"] else default.get_bool("QOLVisuals") + toggle.camera_view = params.get_int("CameraView") if quality_of_life_visuals and tuning_level >= level["CameraView"] else default.get_int("CameraView") + toggle.driver_camera_in_reverse = quality_of_life_visuals and (params.get_bool("DriverCamera") if tuning_level >= level["DriverCamera"] else default.get_bool("DriverCamera")) + toggle.onroad_distance_button = openpilot_longitudinal and quality_of_life_visuals and (params.get_bool("OnroadDistanceButton") if tuning_level >= level["OnroadDistanceButton"] else default.get_bool("OnroadDistanceButton")) + toggle.standby_mode = quality_of_life_visuals and (params.get_bool("StandbyMode") if tuning_level >= level["StandbyMode"] else default.get_bool("StandbyMode")) + toggle.stopped_timer = quality_of_life_visuals and (params.get_bool("StoppedTimer") if tuning_level >= level["StoppedTimer"] else default.get_bool("StoppedTimer")) + + toggle.rainbow_path = params.get_bool("RainbowPath") if tuning_level >= level["RainbowPath"] else default.get_bool("RainbowPath") + + toggle.random_events = params.get_bool("RandomEvents") if tuning_level >= level["RandomEvents"] else default.get_bool("RandomEvents") + + screen_management = params.get_bool("ScreenManagement") if tuning_level >= level["ScreenManagement"] else default.get_bool("ScreenManagement") + toggle.screen_brightness = params.get_int("ScreenBrightness") if screen_management and tuning_level >= level["ScreenBrightness"] else default.get_int("ScreenBrightness") + toggle.screen_brightness_onroad = params.get_int("ScreenBrightnessOnroad") if screen_management and tuning_level >= level["ScreenBrightnessOnroad"] else default.get_int("ScreenBrightnessOnroad") + toggle.screen_recorder = screen_management and (params.get_bool("ScreenRecorder") if tuning_level >= level["ScreenRecorder"] else default.get_bool("ScreenRecorder")) + toggle.screen_timeout = params.get_int("ScreenTimeout") if screen_management and tuning_level >= level["ScreenTimeout"] else default.get_int("ScreenTimeout") + toggle.screen_timeout_onroad = params.get_int("ScreenTimeoutOnroad") if screen_management and tuning_level >= level["ScreenTimeoutOnroad"] else default.get_int("ScreenTimeoutOnroad") + + toggle.sng_hack = openpilot_longitudinal and car_make == "toyota" and (params.get_bool("SNGHack") if tuning_level >= level["SNGHack"] else default.get_bool("SNGHack")) + + toggle.speed_limit_controller = openpilot_longitudinal and (params.get_bool("SpeedLimitController") if tuning_level >= level["SpeedLimitController"] else default.get_bool("SpeedLimitController")) + toggle.force_mph_dashboard = toggle.speed_limit_controller and (params.get_bool("ForceMPHDashboard") if tuning_level >= level["ForceMPHDashboard"] else default.get_bool("ForceMPHDashboard")) + toggle.map_speed_lookahead_higher = params.get_int("SLCLookaheadHigher") if toggle.speed_limit_controller and tuning_level >= level["SLCLookaheadHigher"] else default.get_int("SLCLookaheadHigher") + toggle.map_speed_lookahead_lower = params.get_int("SLCLookaheadLower") if toggle.speed_limit_controller and tuning_level >= level["SLCLookaheadLower"] else default.get_int("SLCLookaheadLower") + toggle.set_speed_limit = toggle.speed_limit_controller and (params.get_bool("SetSpeedLimit") if tuning_level >= level["SetSpeedLimit"] else default.get_bool("SetSpeedLimit")) + slc_fallback_method = params.get_int("SLCFallback") if toggle.speed_limit_controller and tuning_level >= level["SLCFallback"] else default.get_int("SLCFallback") + toggle.slc_fallback_experimental_mode = slc_fallback_method == 1 + toggle.slc_fallback_previous_speed_limit = slc_fallback_method == 2 + toggle.slc_fallback_set_speed = slc_fallback_method == 0 + toggle.speed_limit_confirmation = toggle.speed_limit_controller and (params.get_bool("SLCConfirmation") if tuning_level >= level["SLCConfirmation"] else default.get_bool("SLCConfirmation")) + toggle.speed_limit_confirmation_higher = toggle.speed_limit_confirmation and (params.get_bool("SLCConfirmationHigher") if tuning_level >= level["SLCConfirmationHigher"] else default.get_bool("SLCConfirmationHigher")) + toggle.speed_limit_confirmation_lower = toggle.speed_limit_confirmation and (params.get_bool("SLCConfirmationLower") if tuning_level >= level["SLCConfirmationLower"] else default.get_bool("SLCConfirmationLower")) + slc_override_method = params.get_int("SLCOverride") if toggle.speed_limit_controller and tuning_level >= level["SLCOverride"] else default.get_int("SLCOverride") + toggle.speed_limit_controller_override_manual = slc_override_method == 1 + toggle.speed_limit_controller_override_set_speed = slc_override_method == 2 + toggle.speed_limit_offset1 = params.get_int("Offset1") * speed_conversion if toggle.speed_limit_controller and tuning_level >= level["Offset1"] else default.get_int("Offset1") * CV.MPH_TO_MS + toggle.speed_limit_offset2 = params.get_int("Offset2") * speed_conversion if toggle.speed_limit_controller and tuning_level >= level["Offset2"] else default.get_int("Offset2") * CV.MPH_TO_MS + toggle.speed_limit_offset3 = params.get_int("Offset3") * speed_conversion if toggle.speed_limit_controller and tuning_level >= level["Offset3"] else default.get_int("Offset3") * CV.MPH_TO_MS + toggle.speed_limit_offset4 = params.get_int("Offset4") * speed_conversion if toggle.speed_limit_controller and tuning_level >= level["Offset4"] else default.get_int("Offset4") * CV.MPH_TO_MS + toggle.speed_limit_priority1 = params.get("SLCPriority1", encoding='utf-8') if toggle.speed_limit_controller and tuning_level >= level["SLCPriority1"] else default.get("SLCPriority1", encoding='utf-8') + toggle.speed_limit_priority2 = params.get("SLCPriority2", encoding='utf-8') if toggle.speed_limit_controller and tuning_level >= level["SLCPriority2"] else default.get("SLCPriority2", encoding='utf-8') + toggle.speed_limit_priority3 = params.get("SLCPriority3", encoding='utf-8') if toggle.speed_limit_controller and tuning_level >= level["SLCPriority3"] else default.get("SLCPriority3", encoding='utf-8') toggle.speed_limit_priority_highest = toggle.speed_limit_priority1 == "Highest" toggle.speed_limit_priority_lowest = toggle.speed_limit_priority1 == "Lowest" - toggle.speed_limit_sources = toggle.speed_limit_controller and params.get_bool("SpeedLimitSources") + toggle.speed_limit_sources = toggle.speed_limit_controller and (params.get_bool("SpeedLimitSources") if tuning_level >= level["SpeedLimitSources"] else default.get_bool("SpeedLimitSources")) - toggle.startup_alert_top = params.get("StartupMessageTop", encoding='utf-8') or "" - toggle.startup_alert_bottom = params.get("StartupMessageBottom", encoding='utf-8') or "" + toggle.startup_alert_top = params.get("StartupMessageTop", encoding='utf-8') if tuning_level >= level["StartupMessageTop"] else default.get("StartupMessageTop", encoding='utf-8') + toggle.startup_alert_bottom = params.get("StartupMessageBottom", encoding='utf-8') if tuning_level >= level["StartupMessageBottom"] else default.get("StartupMessageBottom", encoding='utf-8') toggle.tethering_config = params.get_int("TetheringEnabled") - toggle.toyota_doors = car_make == "toyota" and params.get_bool("ToyotaDoors") - toggle.lock_doors = toggle.toyota_doors and params.get_bool("LockDoors") - toggle.unlock_doors = toggle.toyota_doors and params.get_bool("UnlockDoors") - - toggle.volt_sng = car_model == "CHEVROLET_VOLT" and params.get_bool("VoltSNG") - - customization_level = params.get_int("CustomizationLevel") if params.get_bool("CustomizationLevelConfirmed") else 2 - - if customization_level == 0: - toggle.advanced_custom_onroad_ui = bool(self.default_frogpilot_toggles.AdvancedCustomUI) - toggle.hide_alerts = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideAlerts) - toggle.hide_lead_marker = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideLeadMarker) - toggle.hide_map_icon = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideMapIcon) - toggle.hide_max_speed = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideMaxSpeed) - toggle.hide_speed = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideSpeed) - toggle.hide_speed_limit = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideSpeedLimit) - toggle.use_wheel_speed = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.WheelSpeed) - - toggle.advanced_lateral_tuning = bool(self.default_frogpilot_toggles.AdvancedLateralTune) - toggle.use_custom_steer_friction = False - toggle.use_custom_kp = False - toggle.use_custom_lat_accel_factor = False - toggle.use_custom_steer_ratio = False - toggle.force_auto_tune = bool(toggle.advanced_lateral_tuning and not has_auto_tune and not is_pid_car and self.default_frogpilot_toggles.ForceAutoTune) - toggle.force_auto_tune_off = bool(toggle.advanced_lateral_tuning and has_auto_tune and not is_pid_car and self.default_frogpilot_toggles.ForceAutoTuneOff) - - toggle.alert_volume_control = bool(self.default_frogpilot_toggles.AlertVolumeControl) - toggle.disengage_volume = int(self.default_frogpilot_toggles.DisengageVolume if toggle.alert_volume_control else 101) - toggle.engage_volume = int(self.default_frogpilot_toggles.EngageVolume if toggle.alert_volume_control else 101) - toggle.prompt_volume = int(self.default_frogpilot_toggles.PromptVolume if toggle.alert_volume_control else 101) - toggle.promptDistracted_volume = int(self.default_frogpilot_toggles.PromptDistractedVolume if toggle.alert_volume_control else 101) - toggle.refuse_volume = int(self.default_frogpilot_toggles.RefuseVolume if toggle.alert_volume_control else 101) - toggle.warningSoft_volume = int(self.default_frogpilot_toggles.WarningSoftVolume if toggle.alert_volume_control else 101) - toggle.warningImmediate_volume = int(self.default_frogpilot_toggles.WarningImmediateVolume if toggle.alert_volume_control else 101) - - toggle.always_on_lateral = bool(always_on_lateral_set and self.default_frogpilot_toggles.AlwaysOnLateral) - toggle.always_on_lateral_lkas = bool(toggle.always_on_lateral and car_make != "subaru" and self.default_frogpilot_toggles.AlwaysOnLateralLKAS) - toggle.always_on_lateral_main = bool(toggle.always_on_lateral and self.default_frogpilot_toggles.AlwaysOnLateralMain) - toggle.always_on_lateral_pause_speed = int(self.default_frogpilot_toggles.PauseAOLOnBrake if toggle.always_on_lateral else 0) - toggle.always_on_lateral_status_bar = bool(toggle.always_on_lateral and not self.default_frogpilot_toggles.HideAOLStatusBar) - - toggle.cluster_offset = float(self.default_frogpilot_toggles.ClusterOffset if car_make == "toyota" else 1) - - toggle.conditional_experimental_mode = bool(openpilot_longitudinal and self.default_frogpilot_toggles.ConditionalExperimental) - toggle.conditional_curves = bool(toggle.conditional_experimental_mode and self.default_frogpilot_toggles.CECurves) - toggle.conditional_curves_lead = bool(toggle.conditional_curves and self.default_frogpilot_toggles.CECurvesLead) - toggle.conditional_lead = bool(toggle.conditional_experimental_mode and self.default_frogpilot_toggles.CELead) - toggle.conditional_slower_lead = bool(toggle.conditional_lead and self.default_frogpilot_toggles.CESlowerLead) - toggle.conditional_stopped_lead = bool(toggle.conditional_lead and self.default_frogpilot_toggles.CEStoppedLead) - toggle.conditional_limit = int(self.default_frogpilot_toggles.CESpeed) * speed_conversion if toggle.conditional_experimental_mode else 0 - toggle.conditional_limit_lead = int(self.default_frogpilot_toggles.CESpeedLead) * speed_conversion if toggle.conditional_experimental_mode else 0 - toggle.conditional_navigation = bool(toggle.conditional_experimental_mode and self.default_frogpilot_toggles.CENavigation) - toggle.conditional_navigation_intersections = bool(toggle.conditional_navigation and self.default_frogpilot_toggles.CENavigationIntersections) - toggle.conditional_navigation_lead = bool(toggle.conditional_navigation and self.default_frogpilot_toggles.CENavigationLead) - toggle.conditional_navigation_turns = bool(toggle.conditional_navigation and self.default_frogpilot_toggles.CENavigationTurns) - toggle.conditional_model_stop_time = int(self.default_frogpilot_toggles.CEModelStopTime if toggle.conditional_experimental_mode else 0) - toggle.conditional_signal = int(self.default_frogpilot_toggles.CESignalSpeed if toggle.conditional_experimental_mode else 0) - toggle.conditional_signal_lane_detection = bool(toggle.conditional_signal != 0 and self.default_frogpilot_toggles.CESignalLaneDetection) - toggle.conditional_status_bar = bool(toggle.conditional_experimental_mode and not self.default_frogpilot_toggles.HideCEMStatusBar) - if toggle.conditional_experimental_mode: - params.put_bool("ExperimentalMode", True) - - toggle.crosstrek_torque = bool(car_model == "SUBARU_IMPREZA" and self.default_frogpilot_toggles.CrosstrekTorque) - - toggle.curve_speed_controller = bool(openpilot_longitudinal and self.default_frogpilot_toggles.CurveSpeedControl) - toggle.curve_sensitivity = int(self.default_frogpilot_toggles.CurveSensitivity) / 100 if toggle.curve_speed_controller else 1 - toggle.hide_csc_ui = bool(toggle.curve_speed_controller and self.default_frogpilot_toggles.HideCSCUI) - toggle.turn_aggressiveness = int(self.default_frogpilot_toggles.TurnAggressiveness) / 100 if toggle.curve_speed_controller else 1 - toggle.map_turn_speed_controller = bool(toggle.curve_speed_controller and self.default_frogpilot_toggles.MTSCEnabled) - toggle.mtsc_curvature_check = bool(toggle.map_turn_speed_controller and self.default_frogpilot_toggles.MTSCCurvatureCheck) - toggle.vision_turn_controller = bool(toggle.curve_speed_controller and self.default_frogpilot_toggles.VisionTurnControl) - - toggle.custom_personalities = bool(openpilot_longitudinal and self.default_frogpilot_toggles.CustomPersonalities) - toggle.aggressive_profile = bool(toggle.custom_personalities and self.default_frogpilot_toggles.AggressivePersonalityProfile) - toggle.aggressive_jerk_deceleration = int(self.default_frogpilot_toggles.AggressiveJerkDeceleration) / 100 if toggle.aggressive_profile else 0.5 - toggle.aggressive_jerk_danger = int(self.default_frogpilot_toggles.AggressiveJerkDanger) / 100 if toggle.aggressive_profile else 1.0 - toggle.aggressive_jerk_speed = int(self.default_frogpilot_toggles.AggressiveJerkSpeed) / 100 if toggle.aggressive_profile else 0.5 - toggle.aggressive_jerk_speed_decrease = int(self.default_frogpilot_toggles.AggressiveJerkSpeedDecrease) / 100 if toggle.aggressive_profile else 0.5 - toggle.aggressive_follow = float(self.default_frogpilot_toggles.AggressiveFollow) if toggle.aggressive_profile else 1.25 - toggle.standard_profile = bool(toggle.custom_personalities and self.default_frogpilot_toggles.StandardPersonalityProfile) - toggle.standard_jerk_acceleration = int(self.default_frogpilot_toggles.StandardJerkAcceleration) / 100 if toggle.standard_profile else 1.0 - toggle.standard_jerk_deceleration = int(self.default_frogpilot_toggles.StandardJerkDeceleration) / 100 if toggle.standard_profile else 1.0 - toggle.standard_jerk_danger = int(self.default_frogpilot_toggles.StandardJerkDanger) / 100 if toggle.standard_profile else 1.0 - toggle.standard_jerk_speed = int(self.default_frogpilot_toggles.StandardJerkSpeed) / 100 if toggle.standard_profile else 1.0 - toggle.standard_jerk_speed_decrease = int(self.default_frogpilot_toggles.StandardJerkSpeedDecrease) / 100 if toggle.standard_profile else 1.0 - toggle.standard_follow = float(self.default_frogpilot_toggles.StandardFollow) if toggle.standard_profile else 1.45 - toggle.relaxed_profile = bool(toggle.custom_personalities and self.default_frogpilot_toggles.RelaxedPersonalityProfile) - toggle.relaxed_jerk_acceleration = int(self.default_frogpilot_toggles.RelaxedJerkAcceleration) / 100 if toggle.relaxed_profile else 1.0 - toggle.relaxed_jerk_deceleration = int(self.default_frogpilot_toggles.RelaxedJerkDeceleration) / 100 if toggle.relaxed_profile else 1.0 - toggle.relaxed_jerk_danger = int(self.default_frogpilot_toggles.RelaxedJerkDanger) / 100 if toggle.relaxed_profile else 1.0 - toggle.relaxed_jerk_speed = int(self.default_frogpilot_toggles.RelaxedJerkSpeed) / 100 if toggle.relaxed_profile else 1.0 - toggle.relaxed_jerk_speed_decrease = int(self.default_frogpilot_toggles.RelaxedJerkSpeedDecrease) / 100 if toggle.relaxed_profile else 1.0 - toggle.relaxed_follow = float(self.default_frogpilot_toggles.RelaxedFollow) if toggle.relaxed_profile else 1.75 - toggle.traffic_profile = bool(toggle.custom_personalities and self.default_frogpilot_toggles.TrafficPersonalityProfile) - toggle.traffic_mode_jerk_acceleration = [int(self.default_frogpilot_toggles.TrafficJerkAcceleration) / 100, toggle.aggressive_jerk_acceleration] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_jerk_deceleration = [int(self.default_frogpilot_toggles.TrafficJerkDeceleration) / 100, toggle.aggressive_jerk_deceleration] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_jerk_danger = [int(self.default_frogpilot_toggles.TrafficJerkDanger) / 100, toggle.aggressive_jerk_danger] if toggle.traffic_profile else [1.0, 1.0] - toggle.traffic_mode_jerk_speed = [int(self.default_frogpilot_toggles.TrafficJerkSpeed) / 100, toggle.aggressive_jerk_speed] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_jerk_speed_decrease = [int(self.default_frogpilot_toggles.TrafficJerkSpeedDecrease) / 100, toggle.aggressive_jerk_speed_decrease] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_t_follow = [float(self.default_frogpilot_toggles.TrafficFollow), toggle.aggressive_follow] if toggle.traffic_profile else [0.5, 1.0] - - toggle.custom_ui = bool(self.default_frogpilot_toggles.CustomUI) - toggle.acceleration_path = bool(toggle.custom_ui and self.default_frogpilot_toggles.AccelerationPath) - toggle.adjacent_paths = bool(toggle.custom_ui and self.default_frogpilot_toggles.AdjacentPath) - toggle.blind_spot_path = bool(has_bsm and toggle.custom_ui and self.default_frogpilot_toggles.BlindSpotPath) - toggle.compass = bool(toggle.custom_ui and self.default_frogpilot_toggles.Compass) - toggle.pedals_on_ui = bool(toggle.custom_ui and self.default_frogpilot_toggles.PedalsOnUI) - toggle.dynamic_pedals_on_ui = bool(toggle.pedals_on_ui and self.default_frogpilot_toggles.DynamicPedalsOnUI) - toggle.static_pedals_on_ui = bool(toggle.pedals_on_ui and self.default_frogpilot_toggles.StaticPedalsOnUI) - toggle.rotating_wheel = bool(toggle.custom_ui and self.default_frogpilot_toggles.RotatingWheel) - - toggle.developer_ui = bool(self.default_frogpilot_toggles.DeveloperUI) - toggle.border_metrics = bool(toggle.developer_ui and self.default_frogpilot_toggles.BorderMetrics) - toggle.blind_spot_metrics = bool(has_bsm and toggle.border_metrics and self.default_frogpilot_toggles.BlindSpotMetrics) - toggle.signal_metrics = bool(toggle.border_metrics and self.default_frogpilot_toggles.SignalMetrics) - toggle.steering_metrics = bool(toggle.border_metrics and self.default_frogpilot_toggles.ShowSteering) - toggle.show_fps = bool(toggle.developer_ui and self.default_frogpilot_toggles.FPSCounter) - toggle.lateral_metrics = bool(toggle.developer_ui and self.default_frogpilot_toggles.LateralMetrics) - toggle.adjacent_path_metrics = bool(toggle.lateral_metrics and self.default_frogpilot_toggles.AdjacentPathMetrics) - toggle.lateral_tuning_metrics = bool(toggle.lateral_metrics and self.default_frogpilot_toggles.TuningInfo) - toggle.longitudinal_metrics = bool(toggle.developer_ui and self.default_frogpilot_toggles.LongitudinalMetrics) - toggle.adjacent_lead_tracking = bool(has_radar and toggle.longitudinal_metrics and self.default_frogpilot_toggles.AdjacentLeadsUI) - toggle.lead_metrics = bool(toggle.longitudinal_metrics and self.default_frogpilot_toggles.LeadInfo) - toggle.jerk_metrics = bool(toggle.longitudinal_metrics and self.default_frogpilot_toggles.JerkInfo) - toggle.numerical_temp = bool(toggle.developer_ui and self.default_frogpilot_toggles.NumericalTemp) - toggle.fahrenheit = bool(toggle.numerical_temp and self.default_frogpilot_toggles.Fahrenheit) - toggle.sidebar_metrics = bool(toggle.developer_ui and self.default_frogpilot_toggles.SidebarMetrics) - toggle.cpu_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowCPU) - toggle.gpu_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowGPU) - toggle.ip_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowIP) - toggle.memory_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowMemoryUsage) - toggle.storage_left_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowStorageLeft) - toggle.storage_used_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowStorageUsed) - toggle.use_si_metrics = bool(toggle.developer_ui and self.default_frogpilot_toggles.UseSI) - - toggle.device_management = bool(self.default_frogpilot_toggles.DeviceManagement) - device_shutdown_setting = int(self.default_frogpilot_toggles.DeviceShutdown) if toggle.device_management else 33 - toggle.device_shutdown_time = (device_shutdown_setting - 3) * 3600 if device_shutdown_setting >= 4 else device_shutdown_setting * (60 * 15) - toggle.increase_thermal_limits = bool(toggle.device_management and self.default_frogpilot_toggles.IncreaseThermalLimits) - toggle.low_voltage_shutdown = clip(float(self.default_frogpilot_toggles.LowVoltageShutdown), VBATT_PAUSE_CHARGING, 12.5) if toggle.device_management else VBATT_PAUSE_CHARGING - toggle.no_logging = bool(toggle.device_management and self.default_frogpilot_toggles.NoLogging or self.development_branch) - toggle.no_uploads = bool(toggle.device_management and self.default_frogpilot_toggles.NoUploads or self.development_branch) - toggle.no_onroad_uploads = bool(toggle.no_uploads and self.default_frogpilot_toggles.DisableOnroadUploads) - toggle.offline_mode = bool(toggle.device_management and self.default_frogpilot_toggles.OfflineMode) - - toggle.experimental_gm_tune = bool(openpilot_longitudinal and car_make == "gm" and self.default_frogpilot_toggles.ExperimentalGMTune) - - toggle.experimental_mode_via_press = bool(openpilot_longitudinal and self.default_frogpilot_toggles.ExperimentalModeActivation) - toggle.experimental_mode_via_distance = bool(toggle.experimental_mode_via_press and self.default_frogpilot_toggles.ExperimentalModeViaDistance) - toggle.experimental_mode_via_lkas = bool(not toggle.always_on_lateral_lkas and toggle.experimental_mode_via_press and car_make != "subaru" and self.default_frogpilot_toggles.ExperimentalModeViaLKAS) - toggle.experimental_mode_via_tap = bool(toggle.experimental_mode_via_press and self.default_frogpilot_toggles.ExperimentalModeViaTap) - - toggle.frogsgomoo_tweak = bool(openpilot_longitudinal and car_make == "toyota" and self.default_frogpilot_toggles.FrogsGoMoosTweak) - - toggle.lane_change_customizations = bool(self.default_frogpilot_toggles.LaneChangeCustomizations) - toggle.lane_change_delay = float(self.default_frogpilot_toggles.LaneChangeTime) if toggle.lane_change_customizations else 0 - toggle.lane_detection_width = float(self.default_frogpilot_toggles.LaneDetectionWidth) * distance_conversion if toggle.lane_change_customizations else 0 - toggle.lane_detection = bool(toggle.lane_detection_width != 0) - toggle.minimum_lane_change_speed = float(self.default_frogpilot_toggles.MinimumLaneChangeSpeed) * speed_conversion if toggle.lane_change_customizations else LANE_CHANGE_SPEED_MIN - toggle.nudgeless = bool(toggle.lane_change_customizations and self.default_frogpilot_toggles.NudgelessLaneChange) - toggle.one_lane_change = bool(toggle.lane_change_customizations and self.default_frogpilot_toggles.OneLaneChange) - - toggle.lateral_tuning = bool(self.default_frogpilot_toggles.LateralTune) - toggle.nnff = bool(toggle.lateral_tuning and self.default_frogpilot_toggles.NNFF) - toggle.nnff_lite = bool(toggle.lateral_tuning and self.default_frogpilot_toggles.NNFFLite) - toggle.use_turn_desires = bool(toggle.lateral_tuning and self.default_frogpilot_toggles.TurnDesires) - - toggle.long_pitch = bool(openpilot_longitudinal and car_make == "gm" and self.default_frogpilot_toggles.LongPitch) - - toggle.human_acceleration = bool(toggle.longitudinal_tuning and self.default_frogpilot_toggles.HumanAcceleration) - toggle.human_following = bool(toggle.longitudinal_tuning and self.default_frogpilot_toggles.HumanFollowing) - toggle.lead_detection_probability = clip(float(self.default_frogpilot_toggles.LeadDetectionThreshold) / 100, 0.01, 0.99) if toggle.longitudinal_tuning else 0.5 - toggle.max_desired_acceleration = clip(float(self.default_frogpilot_toggles.MaxDesiredAcceleration), 0.1, 4.0) if toggle.longitudinal_tuning else 4.0 - toggle.taco_tune = bool(toggle.longitudinal_tuning and self.default_frogpilot_toggles.TacoTune) - - toggle.model = DEFAULT_CLASSIC_MODEL - toggle.model_randomizer = self.default_frogpilot_toggles.ModelRandomizer - toggle.part_model_param = "" - toggle.classic_model = bool(classic_models and toggle.model in classic_models.split(',')) - toggle.navigationless_model = bool(navigation_models and toggle.model not in navigation_models.split(',')) - toggle.radarless_model = bool(radarless_models and toggle.model in radarless_models.split(',')) - - toggle.model_ui = bool(self.default_frogpilot_toggles.ModelUI) - toggle.dynamic_path_width = bool(toggle.model_ui and self.default_frogpilot_toggles.DynamicPathWidth) - toggle.lane_line_width = int(self.default_frogpilot_toggles.LaneLinesWidth) * small_distance_conversion / 200 if toggle.model_ui else 0.025 - toggle.path_edge_width = int(self.default_frogpilot_toggles.PathEdgeWidth) if toggle.model_ui else 20 - toggle.path_width = float(self.default_frogpilot_toggles.PathWidth) * distance_conversion / 2 if toggle.model_ui else 0.9 - toggle.road_edge_width = int(self.default_frogpilot_toggles.RoadEdgesWidth) * small_distance_conversion / 200 if toggle.model_ui else 0.025 - toggle.show_stopping_point = bool(toggle.model_ui and self.default_frogpilot_toggles.ShowStoppingPoint) - toggle.show_stopping_point_metrics = bool(toggle.show_stopping_point and self.default_frogpilot_toggles.ShowStoppingPointMetrics) - toggle.unlimited_road_ui_length = bool(toggle.model_ui and self.default_frogpilot_toggles.UnlimitedLength) - - toggle.navigation_ui = bool(self.default_frogpilot_toggles.NavigationUI) - toggle.big_map = bool(toggle.navigation_ui and params.get_bool("BigMap")) - toggle.full_map = bool(toggle.big_map and self.default_frogpilot_toggles.FullMap) - toggle.map_style = int(self.default_frogpilot_toggles.MapStyle) if toggle.navigation_ui else 0 - toggle.road_name_ui = bool(toggle.navigation_ui and self.default_frogpilot_toggles.RoadNameUI) - toggle.show_speed_limit_offset = bool(toggle.navigation_ui and self.default_frogpilot_toggles.ShowSLCOffset) - toggle.speed_limit_vienna = bool(toggle.navigation_ui and self.default_frogpilot_toggles.UseVienna) - - toggle.old_long_api = bool(openpilot_longitudinal and car_make == "gm" and not self.default_frogpilot_toggles.NewLongAPIGM) - toggle.old_long_api |= bool(openpilot_longitudinal and car_make == "hyundai" and not self.default_frogpilot_toggles.NewLongAPI) - - toggle.quality_of_life_lateral = bool(self.default_frogpilot_toggles.QOLLateral) - toggle.pause_lateral_below_speed = int(self.default_frogpilot_toggles.PauseLateralSpeed) * speed_conversion if toggle.quality_of_life_lateral else 0 - toggle.pause_lateral_below_signal = bool(toggle.pause_lateral_below_speed != 0 and self.default_frogpilot_toggles.PauseLateralOnSignal) - - toggle.quality_of_life_longitudinal = bool(self.default_frogpilot_toggles.QOLLongitudinal) - toggle.custom_cruise_increase = int(self.default_frogpilot_toggles.CustomCruise) if toggle.quality_of_life_longitudinal and not pcm_cruise else 1 - toggle.custom_cruise_increase_long = int(self.default_frogpilot_toggles.CustomCruiseLong) if toggle.quality_of_life_longitudinal and not pcm_cruise else 5 - toggle.force_standstill = bool(toggle.quality_of_life_longitudinal and self.default_frogpilot_toggles.ForceStandstill) - toggle.force_stops = bool(toggle.quality_of_life_longitudinal and self.default_frogpilot_toggles.ForceStops) - toggle.increased_stopped_distance = int(self.default_frogpilot_toggles.IncreasedStoppedDistance) * distance_conversion if toggle.quality_of_life_longitudinal else 0 - toggle.map_gears = bool(toggle.quality_of_life_longitudinal and self.default_frogpilot_toggles.MapGears) - toggle.map_acceleration = bool(toggle.map_gears and self.default_frogpilot_toggles.MapAcceleration) - toggle.map_deceleration = bool(toggle.map_gears and self.default_frogpilot_toggles.MapDeceleration) - toggle.reverse_cruise_increase = bool(toggle.quality_of_life_longitudinal and pcm_cruise and self.default_frogpilot_toggles.ReverseCruise) - toggle.set_speed_offset = int(self.default_frogpilot_toggles.SetSpeedOffset) * (1 if toggle.is_metric else CV.MPH_TO_KPH) if toggle.quality_of_life_longitudinal and not pcm_cruise else 0 - - toggle.camera_view = int(self.default_frogpilot_toggles.CameraView) if toggle.quality_of_life_visuals else 0 - toggle.driver_camera_in_reverse = bool(toggle.quality_of_life_visuals and self.default_frogpilot_toggles.DriverCamera) - toggle.standby_mode = bool(toggle.quality_of_life_visuals and self.default_frogpilot_toggles.StandbyMode) - toggle.stopped_timer = bool(toggle.quality_of_life_visuals and self.default_frogpilot_toggles.StoppedTimer) - - toggle.rainbow_path = bool(self.default_frogpilot_toggles.RainbowPath) - - toggle.random_events = bool(self.default_frogpilot_toggles.RandomEvents) - - toggle.screen_management = bool(self.default_frogpilot_toggles.ScreenManagement) - toggle.screen_brightness = int(self.default_frogpilot_toggles.ScreenBrightness) if toggle.screen_management else 101 - toggle.screen_brightness_onroad = int(self.default_frogpilot_toggles.ScreenBrightnessOnroad) if toggle.screen_management else 101 - toggle.screen_recorder = bool(toggle.screen_management and self.default_frogpilot_toggles.ScreenRecorder) - toggle.screen_timeout = int(self.default_frogpilot_toggles.ScreenTimeout) if toggle.screen_management else 30 - toggle.screen_timeout_onroad = int(self.default_frogpilot_toggles.ScreenTimeoutOnroad) if toggle.screen_management else 10 - - toggle.sng_hack = bool(openpilot_longitudinal and car_make == "toyota" and self.default_frogpilot_toggles.SNGHack) - - toggle.force_mph_dashboard = bool(toggle.speed_limit_controller and self.default_frogpilot_toggles.ForceMPHDashboard) - toggle.map_speed_lookahead_higher = int(self.default_frogpilot_toggles.SLCLookaheadHigher) if toggle.speed_limit_controller else 0 - toggle.map_speed_lookahead_lower = int(self.default_frogpilot_toggles.SLCLookaheadLower) if toggle.speed_limit_controller else 0 - toggle.set_speed_limit = bool(toggle.speed_limit_controller and self.default_frogpilot_toggles.SetSpeedLimit) - slc_fallback_method = int(self.default_frogpilot_toggles.SLCFallback) if toggle.speed_limit_controller else 0 - toggle.slc_fallback_experimental_mode = bool(toggle.speed_limit_controller and slc_fallback_method == 1) - toggle.slc_fallback_previous_speed_limit = bool(toggle.speed_limit_controller and slc_fallback_method == 2) - toggle.slc_fallback_set_speed = bool(toggle.speed_limit_controller and slc_fallback_method == 0) - toggle.speed_limit_controller_override = int(self.default_frogpilot_toggles.SLCOverride) if toggle.speed_limit_controller else 0 - toggle.speed_limit_controller_override_manual = bool(toggle.speed_limit_controller_override == 1) - toggle.speed_limit_controller_override_set_speed = bool(toggle.speed_limit_controller_override == 2) - toggle.speed_limit_priority1 = self.default_frogpilot_toggles.SLCPriority1 if toggle.speed_limit_controller else None - toggle.speed_limit_priority2 = self.default_frogpilot_toggles.SLCPriority2 if toggle.speed_limit_controller else None - toggle.speed_limit_priority3 = self.default_frogpilot_toggles.SLCPriority3 if toggle.speed_limit_controller else None - toggle.speed_limit_priority_highest = bool(toggle.speed_limit_priority1 == "Highest") - toggle.speed_limit_priority_lowest = bool(toggle.speed_limit_priority1 == "Lowest") - toggle.speed_limit_sources = bool(toggle.speed_limit_controller and self.default_frogpilot_toggles.SpeedLimitSources) - - toggle.startup_alert_top = str(self.default_frogpilot_toggles.StartupMessageTop) - toggle.startup_alert_bottom = str(self.default_frogpilot_toggles.StartupMessageBottom) - - toggle.volt_sng = bool(car_model == "CHEVROLET_VOLT" and self.default_frogpilot_toggles.VoltSNG) - - elif customization_level != 2: - toggle.advanced_custom_onroad_ui = bool(self.default_frogpilot_toggles.AdvancedCustomUI) - toggle.hide_alerts = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideAlerts) - toggle.hide_lead_marker = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideLeadMarker) - toggle.hide_map_icon = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideMapIcon) - toggle.hide_max_speed = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideMaxSpeed) - toggle.hide_speed = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideSpeed) - toggle.hide_speed_limit = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideSpeedLimit) - toggle.use_wheel_speed = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.WheelSpeed) - - toggle.advanced_lateral_tuning = bool(self.default_frogpilot_toggles.AdvancedLateralTune) - toggle.use_custom_steer_friction = False - toggle.use_custom_kp = False - toggle.use_custom_lat_accel_factor = False - toggle.use_custom_steer_ratio = False - toggle.force_auto_tune = bool(toggle.advanced_lateral_tuning and not has_auto_tune and not is_pid_car and self.default_frogpilot_toggles.ForceAutoTune) - toggle.force_auto_tune_off = bool(toggle.advanced_lateral_tuning and has_auto_tune and not is_pid_car and self.default_frogpilot_toggles.ForceAutoTuneOff) - - toggle.alert_volume_control = bool(self.default_frogpilot_toggles.AlertVolumeControl) - toggle.disengage_volume = int(self.default_frogpilot_toggles.DisengageVolume if toggle.alert_volume_control else 101) - toggle.engage_volume = int(self.default_frogpilot_toggles.EngageVolume if toggle.alert_volume_control else 101) - toggle.prompt_volume = int(self.default_frogpilot_toggles.PromptVolume if toggle.alert_volume_control else 101) - toggle.promptDistracted_volume = int(self.default_frogpilot_toggles.PromptDistractedVolume if toggle.alert_volume_control else 101) - toggle.refuse_volume = int(self.default_frogpilot_toggles.RefuseVolume if toggle.alert_volume_control else 101) - toggle.warningSoft_volume = int(self.default_frogpilot_toggles.WarningSoftVolume if toggle.alert_volume_control else 101) - toggle.warningImmediate_volume = int(self.default_frogpilot_toggles.WarningImmediateVolume if toggle.alert_volume_control else 101) - - toggle.always_on_lateral_status_bar = bool(toggle.always_on_lateral and not self.default_frogpilot_toggles.HideAOLStatusBar) - - toggle.cluster_offset = float(self.default_frogpilot_toggles.ClusterOffset if car_make == "toyota" else 1) - - toggle.conditional_navigation = bool(toggle.conditional_experimental_mode and self.default_frogpilot_toggles.CENavigation) - toggle.conditional_navigation_intersections = bool(toggle.conditional_navigation and self.default_frogpilot_toggles.CENavigationIntersections) - toggle.conditional_navigation_lead = bool(toggle.conditional_navigation and self.default_frogpilot_toggles.CENavigationLead) - toggle.conditional_navigation_turns = bool(toggle.conditional_navigation and self.default_frogpilot_toggles.CENavigationTurns) - toggle.conditional_signal = int(self.default_frogpilot_toggles.CESignalSpeed if toggle.conditional_experimental_mode else 0) - toggle.conditional_signal_lane_detection = bool(toggle.conditional_signal != 0 and self.default_frogpilot_toggles.CESignalLaneDetection) - toggle.conditional_status_bar = bool(toggle.conditional_experimental_mode and not self.default_frogpilot_toggles.HideCEMStatusBar) - if toggle.conditional_experimental_mode: - params.put_bool("ExperimentalMode", True) - - toggle.crosstrek_torque = bool(car_model == "SUBARU_IMPREZA" and self.default_frogpilot_toggles.CrosstrekTorque) - - toggle.curve_sensitivity = int(self.default_frogpilot_toggles.CurveSensitivity) / 100 if toggle.curve_speed_controller else 1 - toggle.hide_csc_ui = bool(toggle.curve_speed_controller and self.default_frogpilot_toggles.HideCSCUI) - toggle.turn_aggressiveness = int(self.default_frogpilot_toggles.TurnAggressiveness) / 100 if toggle.curve_speed_controller else 1 - toggle.mtsc_curvature_check = bool(toggle.map_turn_speed_controller and self.default_frogpilot_toggles.MTSCCurvatureCheck) - - toggle.custom_personalities = bool(openpilot_longitudinal and self.default_frogpilot_toggles.CustomPersonalities) - toggle.aggressive_profile = bool(toggle.custom_personalities and self.default_frogpilot_toggles.AggressivePersonalityProfile) - toggle.aggressive_jerk_deceleration = int(self.default_frogpilot_toggles.AggressiveJerkDeceleration) / 100 if toggle.aggressive_profile else 0.5 - toggle.aggressive_jerk_danger = int(self.default_frogpilot_toggles.AggressiveJerkDanger) / 100 if toggle.aggressive_profile else 1.0 - toggle.aggressive_jerk_speed = int(self.default_frogpilot_toggles.AggressiveJerkSpeed) / 100 if toggle.aggressive_profile else 0.5 - toggle.aggressive_jerk_speed_decrease = int(self.default_frogpilot_toggles.AggressiveJerkSpeedDecrease) / 100 if toggle.aggressive_profile else 0.5 - toggle.aggressive_follow = float(self.default_frogpilot_toggles.AggressiveFollow) if toggle.aggressive_profile else 1.25 - toggle.standard_profile = bool(toggle.custom_personalities and self.default_frogpilot_toggles.StandardPersonalityProfile) - toggle.standard_jerk_acceleration = int(self.default_frogpilot_toggles.StandardJerkAcceleration) / 100 if toggle.standard_profile else 1.0 - toggle.standard_jerk_deceleration = int(self.default_frogpilot_toggles.StandardJerkDeceleration) / 100 if toggle.standard_profile else 1.0 - toggle.standard_jerk_danger = int(self.default_frogpilot_toggles.StandardJerkDanger) / 100 if toggle.standard_profile else 1.0 - toggle.standard_jerk_speed = int(self.default_frogpilot_toggles.StandardJerkSpeed) / 100 if toggle.standard_profile else 1.0 - toggle.standard_jerk_speed_decrease = int(self.default_frogpilot_toggles.StandardJerkSpeedDecrease) / 100 if toggle.standard_profile else 1.0 - toggle.standard_follow = float(self.default_frogpilot_toggles.StandardFollow) if toggle.standard_profile else 1.45 - toggle.relaxed_profile = bool(toggle.custom_personalities and self.default_frogpilot_toggles.RelaxedPersonalityProfile) - toggle.relaxed_jerk_acceleration = int(self.default_frogpilot_toggles.RelaxedJerkAcceleration) / 100 if toggle.relaxed_profile else 1.0 - toggle.relaxed_jerk_deceleration = int(self.default_frogpilot_toggles.RelaxedJerkDeceleration) / 100 if toggle.relaxed_profile else 1.0 - toggle.relaxed_jerk_danger = int(self.default_frogpilot_toggles.RelaxedJerkDanger) / 100 if toggle.relaxed_profile else 1.0 - toggle.relaxed_jerk_speed = int(self.default_frogpilot_toggles.RelaxedJerkSpeed) / 100 if toggle.relaxed_profile else 1.0 - toggle.relaxed_jerk_speed_decrease = int(self.default_frogpilot_toggles.RelaxedJerkSpeedDecrease) / 100 if toggle.relaxed_profile else 1.0 - toggle.relaxed_follow = float(self.default_frogpilot_toggles.RelaxedFollow) if toggle.relaxed_profile else 1.75 - toggle.traffic_profile = bool(toggle.custom_personalities and self.default_frogpilot_toggles.TrafficPersonalityProfile) - toggle.traffic_mode_jerk_acceleration = [int(self.default_frogpilot_toggles.TrafficJerkAcceleration) / 100, toggle.aggressive_jerk_acceleration] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_jerk_deceleration = [int(self.default_frogpilot_toggles.TrafficJerkDeceleration) / 100, toggle.aggressive_jerk_deceleration] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_jerk_danger = [int(self.default_frogpilot_toggles.TrafficJerkDanger) / 100, toggle.aggressive_jerk_danger] if toggle.traffic_profile else [1.0, 1.0] - toggle.traffic_mode_jerk_speed = [int(self.default_frogpilot_toggles.TrafficJerkSpeed) / 100, toggle.aggressive_jerk_speed] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_jerk_speed_decrease = [int(self.default_frogpilot_toggles.TrafficJerkSpeedDecrease) / 100, toggle.aggressive_jerk_speed_decrease] if toggle.traffic_profile else [0.5, 0.5] - toggle.traffic_mode_t_follow = [float(self.default_frogpilot_toggles.TrafficFollow), toggle.aggressive_follow] if toggle.traffic_profile else [0.5, 1.0] - - toggle.adjacent_paths = bool(toggle.custom_ui and self.default_frogpilot_toggles.AdjacentPath) - - toggle.developer_ui = bool(self.default_frogpilot_toggles.DeveloperUI) - toggle.border_metrics = bool(toggle.developer_ui and self.default_frogpilot_toggles.BorderMetrics) - toggle.blind_spot_metrics = bool(has_bsm and toggle.border_metrics and self.default_frogpilot_toggles.BlindSpotMetrics) - toggle.signal_metrics = bool(toggle.border_metrics and self.default_frogpilot_toggles.SignalMetrics) - toggle.steering_metrics = bool(toggle.border_metrics and self.default_frogpilot_toggles.ShowSteering) - toggle.show_fps = bool(toggle.developer_ui and self.default_frogpilot_toggles.FPSCounter) - toggle.lateral_metrics = bool(toggle.developer_ui and self.default_frogpilot_toggles.LateralMetrics) - toggle.adjacent_path_metrics = bool(toggle.lateral_metrics and self.default_frogpilot_toggles.AdjacentPathMetrics) - toggle.lateral_tuning_metrics = bool(toggle.lateral_metrics and self.default_frogpilot_toggles.TuningInfo) - toggle.longitudinal_metrics = bool(toggle.developer_ui and self.default_frogpilot_toggles.LongitudinalMetrics) - toggle.adjacent_lead_tracking = bool(has_radar and toggle.longitudinal_metrics and self.default_frogpilot_toggles.AdjacentLeadsUI) - toggle.lead_metrics = bool(toggle.longitudinal_metrics and self.default_frogpilot_toggles.LeadInfo) - toggle.jerk_metrics = bool(toggle.longitudinal_metrics and self.default_frogpilot_toggles.JerkInfo) - toggle.numerical_temp = bool(toggle.developer_ui and self.default_frogpilot_toggles.NumericalTemp) - toggle.fahrenheit = bool(toggle.numerical_temp and self.default_frogpilot_toggles.Fahrenheit) - toggle.sidebar_metrics = bool(toggle.developer_ui and self.default_frogpilot_toggles.SidebarMetrics) - toggle.cpu_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowCPU) - toggle.gpu_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowGPU) - toggle.ip_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowIP) - toggle.memory_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowMemoryUsage) - toggle.storage_left_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowStorageLeft) - toggle.storage_used_metrics = bool(toggle.sidebar_metrics and self.default_frogpilot_toggles.ShowStorageUsed) - toggle.use_si_metrics = bool(toggle.developer_ui and self.default_frogpilot_toggles.UseSI) - - toggle.device_management = bool(self.default_frogpilot_toggles.DeviceManagement) - toggle.increase_thermal_limits = bool(toggle.device_management and self.default_frogpilot_toggles.IncreaseThermalLimits) - toggle.low_voltage_shutdown = clip(float(self.default_frogpilot_toggles.LowVoltageShutdown), VBATT_PAUSE_CHARGING, 12.5) if toggle.device_management else VBATT_PAUSE_CHARGING - toggle.no_logging = bool(toggle.device_management and self.default_frogpilot_toggles.NoLogging or self.development_branch) - toggle.no_uploads = bool(toggle.device_management and self.default_frogpilot_toggles.NoUploads or self.development_branch) - toggle.offline_mode = bool(toggle.device_management and self.default_frogpilot_toggles.OfflineMode) - - toggle.experimental_gm_tune = bool(openpilot_longitudinal and car_make == "gm" and self.default_frogpilot_toggles.ExperimentalGMTune) - - toggle.frogsgomoo_tweak = bool(openpilot_longitudinal and car_make == "toyota" and self.default_frogpilot_toggles.FrogsGoMoosTweak) - - toggle.lane_detection_width = float(self.default_frogpilot_toggles.LaneDetectionWidth) * distance_conversion if toggle.lane_change_customizations else 0 - toggle.lane_detection = bool(toggle.lane_detection_width != 0) - toggle.minimum_lane_change_speed = float(self.default_frogpilot_toggles.MinimumLaneChangeSpeed) * speed_conversion if toggle.lane_change_customizations else LANE_CHANGE_SPEED_MIN - - toggle.lateral_tuning = bool(self.default_frogpilot_toggles.LateralTune) - toggle.nnff_lite = bool(toggle.lateral_tuning and self.default_frogpilot_toggles.NNFFLite) - toggle.use_turn_desires = bool(toggle.lateral_tuning and self.default_frogpilot_toggles.TurnDesires) - - toggle.long_pitch = bool(openpilot_longitudinal and car_make == "gm" and self.default_frogpilot_toggles.LongPitch) - - toggle.lead_detection_probability = clip(float(self.default_frogpilot_toggles.LeadDetectionThreshold) / 100, 0.01, 0.99) if toggle.longitudinal_tuning else 0.5 - toggle.max_desired_acceleration = clip(float(self.default_frogpilot_toggles.MaxDesiredAcceleration), 0.1, 4.0) if toggle.longitudinal_tuning else 4.0 - toggle.taco_tune = bool(toggle.longitudinal_tuning and self.default_frogpilot_toggles.TacoTune) - - toggle.model = DEFAULT_CLASSIC_MODEL - toggle.model_randomizer = self.default_frogpilot_toggles.ModelRandomizer - toggle.part_model_param = "" - toggle.classic_model = bool(classic_models and toggle.model in classic_models.split(',')) - toggle.navigationless_model = bool(navigation_models and toggle.model not in navigation_models.split(',')) - toggle.radarless_model = bool(radarless_models and toggle.model in radarless_models.split(',')) - - toggle.model_ui = bool(self.default_frogpilot_toggles.ModelUI) - toggle.dynamic_path_width = bool(toggle.model_ui and self.default_frogpilot_toggles.DynamicPathWidth) - toggle.lane_line_width = int(self.default_frogpilot_toggles.LaneLinesWidth) * small_distance_conversion / 200 if toggle.model_ui else 0.025 - toggle.path_edge_width = int(self.default_frogpilot_toggles.PathEdgeWidth) if toggle.model_ui else 20 - toggle.path_width = float(self.default_frogpilot_toggles.PathWidth) * distance_conversion / 2 if toggle.model_ui else 0.9 - toggle.road_edge_width = int(self.default_frogpilot_toggles.RoadEdgesWidth) * small_distance_conversion / 200 if toggle.model_ui else 0.025 - toggle.show_stopping_point = bool(toggle.model_ui and self.default_frogpilot_toggles.ShowStoppingPoint) - toggle.show_stopping_point_metrics = bool(toggle.show_stopping_point and self.default_frogpilot_toggles.ShowStoppingPointMetrics) - toggle.unlimited_road_ui_length = bool(toggle.model_ui and self.default_frogpilot_toggles.UnlimitedLength) - - toggle.map_style = int(self.default_frogpilot_toggles.MapStyle) if toggle.navigation_ui else 0 - toggle.road_name_ui = bool(toggle.navigation_ui and self.default_frogpilot_toggles.RoadNameUI) - toggle.show_speed_limit_offset = bool(toggle.navigation_ui and self.default_frogpilot_toggles.ShowSLCOffset) - toggle.speed_limit_vienna = bool(toggle.navigation_ui and self.default_frogpilot_toggles.UseVienna) - - toggle.old_long_api = bool(openpilot_longitudinal and car_make == "gm" and not self.default_frogpilot_toggles.NewLongAPIGM) - toggle.old_long_api |= bool(openpilot_longitudinal and car_make == "hyundai" and not self.default_frogpilot_toggles.NewLongAPI) - - toggle.quality_of_life_lateral = bool(self.default_frogpilot_toggles.QOLLateral) - toggle.pause_lateral_below_speed = int(self.default_frogpilot_toggles.PauseLateralSpeed) * speed_conversion if toggle.quality_of_life_lateral else 0 - toggle.pause_lateral_below_signal = bool(toggle.pause_lateral_below_speed != 0 and self.default_frogpilot_toggles.PauseLateralOnSignal) - - toggle.custom_cruise_increase = int(self.default_frogpilot_toggles.CustomCruise) if toggle.quality_of_life_longitudinal and not pcm_cruise else 1 - toggle.custom_cruise_increase_long = int(self.default_frogpilot_toggles.CustomCruiseLong) if toggle.quality_of_life_longitudinal and not pcm_cruise else 5 - toggle.force_standstill = bool(toggle.quality_of_life_longitudinal and self.default_frogpilot_toggles.ForceStandstill) - toggle.force_stops = bool(toggle.quality_of_life_longitudinal and self.default_frogpilot_toggles.ForceStops) - toggle.reverse_cruise_increase = bool(toggle.quality_of_life_longitudinal and pcm_cruise and self.default_frogpilot_toggles.ReverseCruise) - toggle.set_speed_offset = int(self.default_frogpilot_toggles.SetSpeedOffset) * (1 if toggle.is_metric else CV.MPH_TO_KPH) if toggle.quality_of_life_longitudinal and not pcm_cruise else 0 - - toggle.camera_view = int(self.default_frogpilot_toggles.CameraView) if toggle.quality_of_life_visuals else 0 - toggle.standby_mode = bool(toggle.quality_of_life_visuals and self.default_frogpilot_toggles.StandbyMode) - - toggle.screen_management = bool(self.default_frogpilot_toggles.ScreenManagement) - toggle.screen_brightness = int(self.default_frogpilot_toggles.ScreenBrightness) if toggle.screen_management else 101 - toggle.screen_brightness_onroad = int(self.default_frogpilot_toggles.ScreenBrightnessOnroad) if toggle.screen_management else 101 - toggle.screen_recorder = bool(toggle.screen_management and self.default_frogpilot_toggles.ScreenRecorder) - toggle.screen_timeout = int(self.default_frogpilot_toggles.ScreenTimeout) if toggle.screen_management else 30 - toggle.screen_timeout_onroad = int(self.default_frogpilot_toggles.ScreenTimeoutOnroad) if toggle.screen_management else 10 - - toggle.sng_hack = bool(openpilot_longitudinal and car_make == "toyota" and self.default_frogpilot_toggles.SNGHack) - - toggle.force_mph_dashboard = bool(toggle.speed_limit_controller and self.default_frogpilot_toggles.ForceMPHDashboard) - toggle.map_speed_lookahead_higher = int(self.default_frogpilot_toggles.SLCLookaheadHigher) if toggle.speed_limit_controller else 0 - toggle.map_speed_lookahead_lower = int(self.default_frogpilot_toggles.SLCLookaheadLower) if toggle.speed_limit_controller else 0 - toggle.set_speed_limit = bool(toggle.speed_limit_controller and self.default_frogpilot_toggles.SetSpeedLimit) - toggle.speed_limit_priority1 = self.default_frogpilot_toggles.SLCPriority1 if toggle.speed_limit_controller else None - toggle.speed_limit_priority2 = self.default_frogpilot_toggles.SLCPriority2 if toggle.speed_limit_controller else None - toggle.speed_limit_priority3 = self.default_frogpilot_toggles.SLCPriority3 if toggle.speed_limit_controller else None - toggle.speed_limit_priority_highest = bool(toggle.speed_limit_priority1 == "Highest") - toggle.speed_limit_priority_lowest = bool(toggle.speed_limit_priority1 == "Lowest") - toggle.speed_limit_sources = bool(toggle.speed_limit_controller and self.default_frogpilot_toggles.SpeedLimitSources) - - toggle.startup_alert_top = str(self.default_frogpilot_toggles.StartupMessageTop) - toggle.startup_alert_bottom = str(self.default_frogpilot_toggles.StartupMessageBottom) - - toggle.volt_sng = bool(car_model == "CHEVROLET_VOLT" and self.default_frogpilot_toggles.VoltSNG) - - params.put("FrogPilotToggles", json.dumps(toggle.__dict__)) + toyota_doors = car_make == "toyota" and (params.get_bool("ToyotaDoors") if tuning_level >= level["ToyotaDoors"] else default.get_bool("ToyotaDoors")) + toggle.lock_doors = toyota_doors and (params.get_bool("LockDoors") if tuning_level >= level["LockDoors"] else default.get_bool("LockDoors")) + toggle.unlock_doors = toyota_doors and (params.get_bool("UnlockDoors") if tuning_level >= level["UnlockDoors"] else default.get_bool("UnlockDoors")) + + toggle.volt_sng = car_model == "CHEVROLET_VOLT" and (params.get_bool("VoltSNG") if tuning_level >= level["VoltSNG"] else default.get_bool("VoltSNG")) + + params_memory.put("FrogPilotToggles", json.dumps(toggle.__dict__)) params_memory.remove("FrogPilotTogglesUpdated") diff --git a/selfdrive/frogpilot/navigation/mapd.py b/selfdrive/frogpilot/navigation/mapd.py index 4bb08eb147b871..e6fd26df07586b 100644 --- a/selfdrive/frogpilot/navigation/mapd.py +++ b/selfdrive/frogpilot/navigation/mapd.py @@ -6,10 +6,6 @@ import time import urllib.request -from openpilot.common.realtime import Ratekeeper - -from openpilot.selfdrive.frogpilot.frogpilot_utilities import is_url_pingable - VERSION = 'v1' GITHUB_VERSION_URL = f"https://github.com/FrogAi/FrogPilot-Resources/raw/Versions/mapd_version_{VERSION}.json" @@ -18,16 +14,6 @@ MAPD_PATH = '/data/media/0/osm/mapd' VERSION_PATH = '/data/media/0/osm/mapd_version' -def get_latest_version(): - for url in [GITHUB_VERSION_URL, GITLAB_VERSION_URL]: - try: - with urllib.request.urlopen(url, timeout=5) as response: - return json.loads(response.read().decode('utf-8'))['version'] - except Exception as e: - print(f"Error fetching version from {url}: {e}") - print("Failed to get the latest version from both sources.") - return None - def download(current_version): urls = [ f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{current_version}/mapd", @@ -45,49 +31,65 @@ def download(current_version): with open(VERSION_PATH, 'w') as version_file: version_file.write(current_version) - print(f"Successfully downloaded mapd from {url}") return True except Exception as e: - print(f"Failed to download from {url}: {e}") + print(f"Failed to download mapd from {url}: {e}") - print(f"Failed to download mapd for version {current_version} from both sources.") + print(f"Failed to download mapd for version {current_version}") return False -def ensure_mapd_is_running(): - while True: - if os.path.exists(MAPD_PATH): - try: - subprocess.run([MAPD_PATH], check=True) - except Exception as e: - print(f"Error running mapd process: {e}") - else: - print(f"Error: {MAPD_PATH} does not exist.") - time.sleep(1) +def get_installed_version(): + try: + with open(VERSION_PATH, 'r') as version_file: + return version_file.read().strip() + except FileNotFoundError: + return None + except Exception as e: + print(f"Error reading installed version: {e}") + return None -def mapd_thread(): - rk = Ratekeeper(0.05) +def get_latest_version(): + for url in [GITHUB_VERSION_URL, GITLAB_VERSION_URL]: + try: + with urllib.request.urlopen(url, timeout=5) as response: + return json.loads(response.read().decode('utf-8'))['version'] + except Exception as e: + print(f"Error fetching mapd version from {url}: {e}") + print("Failed to get the latest mapd version") + return None - while True: +def update_mapd(): + installed_version = get_installed_version() + latest_version = get_latest_version() + + if latest_version is None: + print("Could not get the latest mapd version") + return + + if installed_version != latest_version: + print("New mapd version available, stopping the mapd process for update") try: - if is_url_pingable("https://github.com"): - current_version = get_latest_version() - if current_version: - if not os.path.exists(MAPD_PATH) or not os.path.exists(VERSION_PATH) or open(VERSION_PATH).read() != current_version: - if download(current_version): - continue - ensure_mapd_is_running() + subprocess.run(["pkill", "-f", MAPD_PATH], stdout=subprocess.PIPE, stderr=subprocess.PIPE) except Exception as e: - print(f"Exception in mapd_thread: {e}") - time.sleep(1) + print(f"Error stopping mapd process: {e}") + if download(latest_version): + print(f"Updated mapd to version {latest_version}") + else: + print("Failed to update mapd") + else: + print("Mapd is up to date") - rk.keep_time() +def ensure_mapd_is_running(): + while True: + try: + subprocess.run([MAPD_PATH], check=True) + except Exception as e: + print(f"Error running mapd process: {e}") + time.sleep(60) def main(): - try: - mapd_thread() - except Exception as e: - print(f"Unhandled exception in main: {e}") + ensure_mapd_is_running() if __name__ == "__main__": main() diff --git a/selfdrive/frogpilot/navigation/ui/maps_settings.cc b/selfdrive/frogpilot/navigation/ui/maps_settings.cc index 7bc573c1f42832..7f5c53d209faa5 100644 --- a/selfdrive/frogpilot/navigation/ui/maps_settings.cc +++ b/selfdrive/frogpilot/navigation/ui/maps_settings.cc @@ -162,7 +162,7 @@ void FrogPilotMapsPanel::downloadMaps() { return; } - paramsMemory.put("OSMDownloadLocations", params.get("MapsSelected")); + params_memory.put("OSMDownloadLocations", params.get("MapsSelected")); downloadActive = true; diff --git a/selfdrive/frogpilot/navigation/ui/maps_settings.h b/selfdrive/frogpilot/navigation/ui/maps_settings.h index 2373c1ed198fcc..ce0b524946745f 100644 --- a/selfdrive/frogpilot/navigation/ui/maps_settings.h +++ b/selfdrive/frogpilot/navigation/ui/maps_settings.h @@ -65,7 +65,7 @@ class FrogPilotMapsPanel : public FrogPilotListWidget { MapSelectionControl *westMaps; Params params; - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; bool countriesOpen; bool downloadActive; diff --git a/selfdrive/frogpilot/screenrecorder/omx_encoder.cc b/selfdrive/frogpilot/screenrecorder/omx_encoder.cc index 0700cd7cf0defb..69e3f5c6ec874a 100644 --- a/selfdrive/frogpilot/screenrecorder/omx_encoder.cc +++ b/selfdrive/frogpilot/screenrecorder/omx_encoder.cc @@ -209,10 +209,18 @@ OmxEncoder::OmxEncoder(const char* path, int width, int height, int fps, int bit this->height = height; this->fps = fps; + OMX_ERRORTYPE err = OMX_Init(); + if (err != OMX_ErrorNone) { + LOGE("OMX_Init failed: %x", err); + return; + } + OMX_STRING component = (OMX_STRING)("OMX.qcom.video.encoder.avc"); - int err = OMX_GetHandle(&handle, component, this, &omx_callbacks); + err = OMX_GetHandle(&handle, component, this, &omx_callbacks); if (err != OMX_ErrorNone) { - LOGE("error getting codec: %x", err); + LOGE("Error getting codec: %x", err); + OMX_Deinit(); + return; } // setup input port @@ -543,6 +551,11 @@ OmxEncoder::~OmxEncoder() { OMX_CHECK(OMX_FreeHandle(handle)); + OMX_ERRORTYPE err = OMX_Deinit(); + if (err != OMX_ErrorNone) { + LOGE("OMX_Deinit failed: %x", err); + } + OMX_BUFFERHEADERTYPE *out_buf; while (free_in.try_pop(out_buf)); while (done_out.try_pop(out_buf)); diff --git a/selfdrive/frogpilot/screenrecorder/screenrecorder.cc b/selfdrive/frogpilot/screenrecorder/screenrecorder.cc index 906063bfc57f71..be6307d7a1cc0a 100644 --- a/selfdrive/frogpilot/screenrecorder/screenrecorder.cc +++ b/selfdrive/frogpilot/screenrecorder/screenrecorder.cc @@ -5,30 +5,48 @@ #include "selfdrive/frogpilot/screenrecorder/screenrecorder.h" namespace { - inline long long milliseconds() { + long long currentMilliseconds() { return std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); } + + uint64_t nanosSinceBoot() { + return std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + } } -ScreenRecorder::ScreenRecorder(QWidget *parent) : QPushButton(parent), recording(false) { +ScreenRecorder::ScreenRecorder(QWidget *parent) : QPushButton(parent) { setFixedSize(btn_size, btn_size); - encoder = std::make_unique("/data/media/screen_recordings", screenWidth, screenHeight, UI_FREQ, 8 * 1024 * 1024); + std::thread encoderInitThread([this]() { + try { + encoder = std::make_unique("/data/media/screen_recordings", screenWidth, screenHeight, UI_FREQ, 8 * 1024 * 1024); + encoderReady = true; + } catch (const std::exception &e) { + std::cerr << "Error initializing OmxEncoder: " << e.what() << std::endl; + } + }); + encoderInitThread.detach(); + rgbScaleBuffer = std::make_unique(screenWidth * screenHeight * 4); QObject::connect(this, &QPushButton::clicked, this, &ScreenRecorder::toggleRecording); } ScreenRecorder::~ScreenRecorder() { - stop(); + stopRecording(); } void ScreenRecorder::toggleRecording() { - recording ? stop() : start(); + if (recording) { + stopRecording(); + } else { + startRecording(); + } } -void ScreenRecorder::start() { - if (recording) { +void ScreenRecorder::startRecording() { + if (recording || !encoderReady) { + std::cerr << "Recording already in progress or encoder not ready." << std::endl; return; } @@ -48,10 +66,10 @@ void ScreenRecorder::start() { recording = false; } - started_time = milliseconds(); + startedTime = currentMilliseconds(); } -void ScreenRecorder::stop() { +void ScreenRecorder::stopRecording() { if (!recording) { return; } @@ -69,7 +87,7 @@ void ScreenRecorder::stop() { } imageQueue.clear(); - rgbScaleBuffer.reset(new uint8_t[screenWidth * screenHeight * 4]); + rgbScaleBuffer = std::make_unique(screenWidth * screenHeight * 4); } void ScreenRecorder::openEncoder(const std::string &filename) { @@ -85,20 +103,29 @@ void ScreenRecorder::closeEncoder() { } void ScreenRecorder::encodingThreadFunction() { - uint64_t start_time = nanos_since_boot(); + uint64_t startTime = nanosSinceBoot(); while (recording) { - if (!encoder) { - break; - } - - QImage popImage; - if (imageQueue.pop_wait_for(popImage, std::chrono::milliseconds(10))) { - QImage image = popImage.convertToFormat(QImage::Format_RGBA8888); - libyuv::ARGBScale(image.bits(), image.width() * 4, image.width(), image.height(), - rgbScaleBuffer.get(), screenWidth * 4, screenWidth, screenHeight, - libyuv::kFilterBilinear); - encoder->encode_frame_rgba(rgbScaleBuffer.get(), screenWidth, screenHeight, nanos_since_boot() - start_time); + QImage image; + if (imageQueue.pop_wait_for(image, std::chrono::milliseconds(10))) { + QImage convertedImage = image.convertToFormat(QImage::Format_RGBA8888); + libyuv::ARGBScale( + convertedImage.bits(), + convertedImage.width() * 4, + convertedImage.width(), + convertedImage.height(), + rgbScaleBuffer.get(), + screenWidth * 4, + screenWidth, + screenHeight, + libyuv::kFilterBilinear + ); + encoder->encode_frame_rgba( + rgbScaleBuffer.get(), + screenWidth, + screenHeight, + nanosSinceBoot() - startTime + ); } } } @@ -109,13 +136,14 @@ void ScreenRecorder::updateScreen(double fps, bool started) { } if (!started) { - stop(); + stopRecording(); return; } - if (milliseconds() - started_time > 1000 * 60 * 3) { - stop(); - start(); + static long long recordingDurationLimit = 1000 * 60 * 3; + if (currentMilliseconds() - startedTime > recordingDurationLimit) { + stopRecording(); + startRecording(); return; } @@ -157,7 +185,7 @@ void ScreenRecorder::paintEvent(QPaintEvent *event) { painter.setPen(QPen(whiteColor(), 6)); painter.drawText(textRect, Qt::AlignLeft | Qt::AlignVCenter, tr("RECORD")); - if (recording && ((milliseconds() - started_time) / 1000) % 2 == 0) { + if (recording && ((currentMilliseconds() - startedTime) / 1000) % 2 == 0) { painter.setPen(Qt::NoPen); painter.drawEllipse(QPoint(buttonRect.right() - btn_size / 10 - centeringOffset, buttonRect.center().y()), btn_size / 10, btn_size / 10); } diff --git a/selfdrive/frogpilot/screenrecorder/screenrecorder.h b/selfdrive/frogpilot/screenrecorder/screenrecorder.h index a3f4d536e0608a..4747d6269c5759 100644 --- a/selfdrive/frogpilot/screenrecorder/screenrecorder.h +++ b/selfdrive/frogpilot/screenrecorder/screenrecorder.h @@ -9,39 +9,43 @@ class ScreenRecorder : public QPushButton { Q_OBJECT public: - explicit ScreenRecorder(QWidget *parent = 0); + explicit ScreenRecorder(QWidget *parent = nullptr); ~ScreenRecorder() override; void updateScreen(double fps, bool started); +protected: + void paintEvent(QPaintEvent *event) override; + +private slots: + void toggleRecording(); + private: void closeEncoder(); void encodingThreadFunction(); void openEncoder(const std::string &filename); - void paintEvent(QPaintEvent *event) override; - void start(); - void stop(); - void toggleRecording(); + void startRecording(); + void stopRecording(); BlockingQueue imageQueue{UI_FREQ}; - BlockingQueue encodeQueue{UI_FREQ}; inline QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); } inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); } - QWidget *rootWidget; - - bool recording; + QWidget *rootWidget = nullptr; int screenHeight = 1080; int screenWidth = 2160; - long long started_time; + long long startedTime = 0; - std::thread encodingThread; + std::atomic encoderReady{false}; + std::atomic recording{false}; std::unique_ptr encoder; std::unique_ptr rgbScaleBuffer; + + std::thread encodingThread; }; diff --git a/selfdrive/frogpilot/ui/qt/offroad/device_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/device_settings.cc index 5269561684cfe5..5234a3bc6d18d9 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/device_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/device_settings.cc @@ -24,17 +24,7 @@ FrogPilotDevicePanel::FrogPilotDevicePanel(FrogPilotSettingsWindow *parent) : Fr if (param == "DeviceManagement") { FrogPilotParamManageControl *deviceManagementToggle = new FrogPilotParamManageControl(param, title, desc, icon); QObject::connect(deviceManagementToggle, &FrogPilotParamManageControl::manageButtonClicked, [this]() { - std::set modifiedDeviceManagementKeys = deviceManagementKeys; - - if (customizationLevel != 2) { - modifiedDeviceManagementKeys.erase("IncreaseThermalLimits"); - modifiedDeviceManagementKeys.erase("LowVoltageShutdown"); - modifiedDeviceManagementKeys.erase("NoLogging"); - modifiedDeviceManagementKeys.erase("NoUploads"); - modifiedDeviceManagementKeys.erase("OfflineMode"); - } - - showToggles(modifiedDeviceManagementKeys); + showToggles(deviceManagementKeys); }); deviceToggle = deviceManagementToggle; } else if (param == "DeviceShutdown") { @@ -110,9 +100,10 @@ FrogPilotDevicePanel::FrogPilotDevicePanel(FrogPilotSettingsWindow *parent) : Fr } void FrogPilotDevicePanel::showEvent(QShowEvent *event) { - customizationLevel = parent->customizationLevel; + frogpilot_toggle_levels = parent->frogpilot_toggle_levels; + tuningLevel = parent->tuningLevel; - toggles["ScreenManagement"]->setVisible(customizationLevel == 2); + hideToggles(); } void FrogPilotDevicePanel::updateState(const UIState &s) { @@ -127,7 +118,7 @@ void FrogPilotDevicePanel::showToggles(const std::set &keys) { setUpdatesEnabled(false); for (auto &[key, toggle] : toggles) { - toggle->setVisible(keys.find(key) != keys.end()); + toggle->setVisible(keys.find(key) != keys.end() && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } setUpdatesEnabled(true); @@ -140,11 +131,9 @@ void FrogPilotDevicePanel::hideToggles() { for (auto &[key, toggle] : toggles) { bool subToggles = deviceManagementKeys.find(key) != deviceManagementKeys.end() || screenKeys.find(key) != screenKeys.end(); - toggle->setVisible(!subToggles); + toggle->setVisible(!subToggles && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } - toggles["ScreenManagement"]->setVisible(customizationLevel == 2); - setUpdatesEnabled(true); update(); } diff --git a/selfdrive/frogpilot/ui/qt/offroad/device_settings.h b/selfdrive/frogpilot/ui/qt/offroad/device_settings.h index a1385d0590b542..4011d5923a8e8a 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/device_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/device_settings.h @@ -21,9 +21,11 @@ class FrogPilotDevicePanel : public FrogPilotListWidget { FrogPilotSettingsWindow *parent; + QJsonObject frogpilot_toggle_levels; + bool started; - int customizationLevel; + int tuningLevel; std::map toggles; diff --git a/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.cc index 211fdc20ca89c5..756eb943be9262 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.cc @@ -44,15 +44,39 @@ FrogPilotSettingsWindow::FrogPilotSettingsWindow(SettingsWindow *parent) : QFram FrogPilotListWidget *list = new FrogPilotListWidget(frogpilotSettingsWidget); - std::vector toggle_presets{tr("Basic"), tr("Standard"), tr("Advanced")}; - ButtonParamControl *toggle_preset = new ButtonParamControl("CustomizationLevel", tr("Customization Level"), - tr("Choose your preferred customization level. 'Standard' is recommended for most users, offering a balanced experience and automatically managing more 'Advanced' features," - " while 'Basic' is designed for those new to customization or seeking simplicity."), - "../frogpilot/assets/toggle_icons/icon_customization.png", - toggle_presets); - QObject::connect(toggle_preset, &ButtonParamControl::buttonClicked, this, &FrogPilotSettingsWindow::updatePanelVisibility); - QObject::connect(toggle_preset, &ButtonParamControl::buttonClicked, this, &updateFrogPilotToggles); - list->addItem(toggle_preset); + std::vector togglePresets{tr("Minimal"), tr("Standard"), tr("Advanced"), tr("Developer")}; + ButtonParamControl *togglePreset = new ButtonParamControl("TuningLevel", tr("Tuning Level"), + tr("Select the tuning level that best suits your needs. 'Basic' is ideal for those who prefer simplicity and ease of use, " + "'Standard' is recommended for most users, offering a balanced experience, " + "'Advanced' provides more control for experienced users, " + "while 'Developer' unlocks highly customizable settings designed for seasoned enthusiasts."), + "../frogpilot/assets/toggle_icons/icon_customization.png", + togglePresets); + int timeTo100FPHours = 100 - (paramsTracking.getInt("FrogPilotMinutes") / 60); + int timeTo250OPHours = 250 - (params.getInt("openpilotMinutes") / 60); + togglePreset->setEnabledButtons(3, timeTo100FPHours <= 0 || timeTo250OPHours <= 0); + QObject::connect(togglePreset, &ButtonParamControl::buttonClicked, [=](int id) { + tuningLevel = id; + + if (id == 3) { + FrogPilotConfirmationDialog::toggleAlert( + tr("WARNING: This unlocks some potentially dangerous settings that can DRASTICALLY alter your driving experience!"), + tr("I understand the risks."), this + ); + } + + updateFrogPilotToggles(); + updatePanelVisibility(); + }); + QObject::connect(togglePreset, &ButtonParamControl::disabledButtonClicked, [=](int id) { + if (id == 3) { + FrogPilotConfirmationDialog::toggleAlert( + tr("The 'Developer' preset is only available for users with either over 100 hours on FrogPilot, or 250 hours with openpilot."), + tr("Okay"), this + ); + } + }); + list->addItem(togglePreset); FrogPilotDevicePanel *frogpilotDevicePanel = new FrogPilotDevicePanel(this); QObject::connect(frogpilotDevicePanel, &FrogPilotDevicePanel::openParentToggle, this, &FrogPilotSettingsWindow::openParentToggle); @@ -136,12 +160,15 @@ FrogPilotSettingsWindow::FrogPilotSettingsWindow(SettingsWindow *parent) : QFram QObject::connect(parent, &SettingsWindow::closeParentToggle, this, &FrogPilotSettingsWindow::closeParentToggle); QObject::connect(parent, &SettingsWindow::closeSubParentToggle, this, &FrogPilotSettingsWindow::closeSubParentToggle); QObject::connect(parent, &SettingsWindow::updateMetric, this, &FrogPilotSettingsWindow::updateMetric); - QObject::connect(uiState(), &UIState::offroadTransition, this, &FrogPilotSettingsWindow::updateCarVariables); + + frogpilot_toggle_levels = QJsonDocument::fromJson(QString::fromStdString(params_memory.get("FrogPilotTuningLevels", true)).toUtf8()).object(); + tuningLevel = params.getInt("TuningLevel"); closeParentToggle(); } void FrogPilotSettingsWindow::showEvent(QShowEvent *event) { + updateCarVariables(); updatePanelVisibility(); } @@ -154,33 +181,9 @@ void FrogPilotSettingsWindow::closePanel() { uiState()->scene.keep_screen_on = false; } -void FrogPilotSettingsWindow::updatePanelVisibility() { - customizationLevel = params.getInt("CustomizationLevel"); - disableOpenpilotLongitudinal = params.getBool("DisableOpenpilotLongitudinal"); - - if ((hasOpenpilotLongitudinal && !disableOpenpilotLongitudinal) || customizationLevel != 0) { - drivingButton->setVisible(true); - drivingButton->setVisibleButton(0, customizationLevel == 2); - drivingButton->setVisibleButton(1, hasOpenpilotLongitudinal && !disableOpenpilotLongitudinal); - drivingButton->setVisibleButton(2, customizationLevel != 0); - update(); - } else { - drivingButton->setVisible(false); - update(); - } - navigationButton->setVisibleButton(1, !uiState()->hasPrime()); - systemButton->setVisibleButton(1, customizationLevel != 0); - - mainLayout->setCurrentWidget(frogpilotSettingsWidget); -} - void FrogPilotSettingsWindow::updateCarVariables() { - std::thread([this] { - std::string carParams = params.get("CarParamsPersistent"); - if (carParams.empty()) { - carParams = params.get("CarParams", true); - } - + std::string carParams = params.get("CarParamsPersistent"); + if (!carParams.empty()) { AlignedBuffer aligned_buf; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(carParams.data(), carParams.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); @@ -200,7 +203,6 @@ void FrogPilotSettingsWindow::updateCarVariables() { hasSNG = CP.getMinEnableSpeed() <= 0; isBolt = carFingerprint == "CHEVROLET_BOLT_CC" || carFingerprint == "CHEVROLET_BOLT_EUV"; isGM = carModel == "gm"; - isGMPCMCruise = CP.getCarName() == "gm" && CP.getPcmCruise(); isHKGCanFd = carModel == "hyundai" && safetyModel == cereal::CarParams::SafetyModel::HYUNDAI_CANFD; isImpreza = carFingerprint == "SUBARU_IMPREZA"; isPIDCar = CP.getLateralTuning().which() == cereal::CarParams::LateralTuning::PID; @@ -219,31 +221,31 @@ void FrogPilotSettingsWindow::updateCarVariables() { float currentRatioStock = params.getFloat("SteerRatioStock"); if (currentFrictionStock != steerFrictionStock && steerFrictionStock != 0) { - if (params.getFloat("SteerFriction") == currentFrictionStock) { - params.putFloatNonBlocking("SteerFriction", steerFrictionStock); + if (params.getFloat("SteerFriction") == currentFrictionStock || currentFrictionStock == 0) { + params.putFloat("SteerFriction", steerFrictionStock); } - params.putFloatNonBlocking("SteerFrictionStock", steerFrictionStock); + params.putFloat("SteerFrictionStock", steerFrictionStock); } if (currentKPStock != steerKPStock && currentKPStock != 0) { - if (params.getFloat("SteerKP") == currentKPStock) { - params.putFloatNonBlocking("SteerKP", steerKPStock); + if (params.getFloat("SteerKP") == currentKPStock || currentKPStock == 0) { + params.putFloat("SteerKP", steerKPStock); } - params.putFloatNonBlocking("SteerKPStock", steerKPStock); + params.putFloat("SteerKPStock", steerKPStock); } if (currentLatAccelStock != steerLatAccelStock && steerLatAccelStock != 0) { - if (params.getFloat("SteerLatAccel") == steerLatAccelStock) { - params.putFloatNonBlocking("SteerLatAccel", steerLatAccelStock); + if (params.getFloat("SteerLatAccel") == steerLatAccelStock || steerLatAccelStock == 0) { + params.putFloat("SteerLatAccel", steerLatAccelStock); } - params.putFloatNonBlocking("SteerLatAccelStock", steerLatAccelStock); + params.putFloat("SteerLatAccelStock", steerLatAccelStock); } if (currentRatioStock != steerRatioStock && steerRatioStock != 0) { - if (params.getFloat("SteerRatio") == steerRatioStock) { - params.putFloatNonBlocking("SteerRatio", steerRatioStock); + if (params.getFloat("SteerRatio") == steerRatioStock || steerRatioStock == 0) { + params.putFloat("SteerRatio", steerRatioStock); } - params.putFloatNonBlocking("SteerRatioStock", steerRatioStock); + params.putFloat("SteerRatioStock", steerRatioStock); } uiState()->scene.has_auto_tune = hasAutoTune || forcingAutoTune; @@ -263,9 +265,25 @@ void FrogPilotSettingsWindow::updateCarVariables() { } else { liveValid = false; } + } +} - emit updateCarToggles(); - }).detach(); +void FrogPilotSettingsWindow::updatePanelVisibility() { + disableOpenpilotLongitudinal = params.getBool("DisableOpenpilotLongitudinal"); + + if ((hasOpenpilotLongitudinal && !disableOpenpilotLongitudinal) || tuningLevel != 0) { + drivingButton->setVisible(true); + drivingButton->setVisibleButton(0, tuningLevel >= frogpilot_toggle_levels.value("Model").toDouble()); + drivingButton->setVisibleButton(1, hasOpenpilotLongitudinal && !disableOpenpilotLongitudinal); + update(); + } else { + drivingButton->setVisible(false); + update(); + } + navigationButton->setVisibleButton(1, !uiState()->hasPrime()); + systemButton->setVisibleButton(1, tuningLevel >= frogpilot_toggle_levels.value("DeviceManagement").toDouble() || tuningLevel >= frogpilot_toggle_levels.value("ScreenManagement").toDouble()); + + mainLayout->setCurrentWidget(frogpilotSettingsWidget); } void FrogPilotSettingsWindow::addPanelControl(FrogPilotListWidget *list, QString &title, QString &desc, std::vector &button_labels, QString &icon, std::vector &panels, QString ¤tPanel) { diff --git a/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.h b/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.h index a8c27615fb4e44..0d80cc1bac4b20 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.h @@ -8,6 +8,8 @@ class FrogPilotSettingsWindow : public QFrame { public: explicit FrogPilotSettingsWindow(SettingsWindow *parent); + QJsonObject frogpilot_toggle_levels; + bool disableOpenpilotLongitudinal = false; bool forcingAutoTune = false; bool hasAutoTune = true; @@ -16,16 +18,15 @@ class FrogPilotSettingsWindow : public QFrame { bool hasExperimentalOpenpilotLongitudinal = false; bool hasNNFFLog = true; bool hasOpenpilotLongitudinal = true; - bool hasPCMCruise = true; + bool hasPCMCruise = false; bool hasRadar = true; bool hasSNG = false; bool isBolt = false; bool isGM = true; - bool isGMPCMCruise = false; bool isHKGCanFd = true; bool isImpreza = true; bool isPIDCar = false; - bool isSubaru = true; + bool isSubaru = false; bool isToyota = true; bool isVolt = true; bool liveValid = false; @@ -35,7 +36,7 @@ class FrogPilotSettingsWindow : public QFrame { float steerLatAccelStock; float steerRatioStock; - int customizationLevel; + int tuningLevel; signals: void closeMapBoxInstructions(); @@ -47,7 +48,6 @@ class FrogPilotSettingsWindow : public QFrame { void openPanel(); void openParentToggle(); void openSubParentToggle(); - void updateCarToggles(); void updateMetric(); private: @@ -62,6 +62,8 @@ class FrogPilotSettingsWindow : public QFrame { FrogPilotButtonsControl *systemButton; Params params; + Params params_memory{"/dev/shm/params"}; + Params paramsTracking{"/persist/tracking"}; QStackedLayout *mainLayout; diff --git a/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.cc index dcaa426e298993..91c08d7651fbc7 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.cc @@ -40,23 +40,27 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : QObject::connect(advancedLateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, [this]() { std::set modifiedAdvancedLateralTuneKeys = advancedLateralTuneKeys; - bool usingNNFF = hasNNFFLog && params.getBool("LateralTune") && params.getBool("NNFF"); - if (usingNNFF) { + bool forcingAutoTune = params.getBool("ForceAutoTune"); + bool forcingAutoTuneOff = params.getBool("ForceAutoTuneOff"); + if (!hasAutoTune && forcingAutoTune || hasAutoTune && !forcingAutoTuneOff) { + modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); + modifiedAdvancedLateralTuneKeys.erase("SteerKP"); + modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); + modifiedAdvancedLateralTuneKeys.erase("SteerRatio"); + } + + if (hasAutoTune) { modifiedAdvancedLateralTuneKeys.erase("ForceAutoTune"); + } else if (isPIDCar) { modifiedAdvancedLateralTuneKeys.erase("ForceAutoTuneOff"); + modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); + modifiedAdvancedLateralTuneKeys.erase("SteerKP"); + modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); } else { - if (hasAutoTune) { - modifiedAdvancedLateralTuneKeys.erase("ForceAutoTune"); - } else if (isPIDCar) { - modifiedAdvancedLateralTuneKeys.erase("ForceAutoTuneOff"); - modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); - modifiedAdvancedLateralTuneKeys.erase("SteerKP"); - modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); - } else { - modifiedAdvancedLateralTuneKeys.erase("ForceAutoTuneOff"); - } + modifiedAdvancedLateralTuneKeys.erase("ForceAutoTuneOff"); } + bool usingNNFF = hasNNFFLog && params.getBool("LateralTune") && params.getBool("NNFF"); if (!liveValid || usingNNFF) { modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); @@ -87,11 +91,6 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : modifiedAOLKeys.erase("AlwaysOnLateralLKAS"); } - if (customizationLevel != 2) { - modifiedAOLKeys.erase("AlwaysOnLateralMain"); - modifiedAOLKeys.erase("HideAOLStatusBar"); - } - showToggles(modifiedAOLKeys); }); lateralToggle = aolToggle; @@ -101,14 +100,7 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : } else if (param == "LaneChangeCustomizations") { FrogPilotParamManageControl *laneChangeToggle = new FrogPilotParamManageControl(param, title, desc, icon); QObject::connect(laneChangeToggle, &FrogPilotParamManageControl::manageButtonClicked, [this]() { - std::set modifiedLaneChangeKeys = laneChangeKeys; - - if (customizationLevel != 2) { - modifiedLaneChangeKeys.erase("LaneDetectionWidth"); - modifiedLaneChangeKeys.erase("MinimumLaneChangeSpeed"); - } - - showToggles(modifiedLaneChangeKeys); + showToggles(laneChangeKeys); }); lateralToggle = laneChangeToggle; } else if (param == "LaneChangeTime") { @@ -130,10 +122,6 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : modifiedLateralTuneKeys.erase("NNFFLite"); } - if (customizationLevel != 2) { - modifiedLateralTuneKeys.erase("TurnDesires"); - } - showToggles(modifiedLateralTuneKeys); }); lateralToggle = lateralTuneToggle; @@ -175,6 +163,36 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : } }); + QObject::connect(static_cast(toggles["ForceAutoTune"]), &ToggleControl::toggleFlipped, [this](bool state) { + std::set modifiedAdvancedLateralTuneKeys = advancedLateralTuneKeys; + + modifiedAdvancedLateralTuneKeys.erase("ForceAutoTuneOff"); + + if (!hasAutoTune && state) { + modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); + modifiedAdvancedLateralTuneKeys.erase("SteerKP"); + modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); + modifiedAdvancedLateralTuneKeys.erase("SteerRatio"); + } + + showToggles(modifiedAdvancedLateralTuneKeys); + }); + + QObject::connect(static_cast(toggles["ForceAutoTuneOff"]), &ToggleControl::toggleFlipped, [this](bool state) { + std::set modifiedAdvancedLateralTuneKeys = advancedLateralTuneKeys; + + modifiedAdvancedLateralTuneKeys.erase("ForceAutoTune"); + + if (hasAutoTune && !state) { + modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); + modifiedAdvancedLateralTuneKeys.erase("SteerKP"); + modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); + modifiedAdvancedLateralTuneKeys.erase("SteerRatio"); + } + + showToggles(modifiedAdvancedLateralTuneKeys); + }); + QObject::connect(static_cast(toggles["NNFF"]), &ToggleControl::toggleFlipped, [this](bool state) { std::set modifiedLateralTuneKeys = lateralTuneKeys; @@ -234,7 +252,6 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : }); QObject::connect(parent, &FrogPilotSettingsWindow::closeParentToggle, this, &FrogPilotLateralPanel::hideToggles); - QObject::connect(parent, &FrogPilotSettingsWindow::updateCarToggles, this, &FrogPilotLateralPanel::updateCarToggles); QObject::connect(parent, &FrogPilotSettingsWindow::updateMetric, this, &FrogPilotLateralPanel::updateMetric); QObject::connect(uiState(), &UIState::uiUpdate, this, &FrogPilotLateralPanel::updateState); @@ -242,12 +259,7 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : } void FrogPilotLateralPanel::showEvent(QShowEvent *event) { - customizationLevel = parent->customizationLevel; - - toggles["AdvancedLateralTune"]->setVisible(customizationLevel == 2); -} - -void FrogPilotLateralPanel::updateCarToggles() { + frogpilot_toggle_levels = parent->frogpilot_toggle_levels; hasAutoTune = parent->hasAutoTune; hasNNFFLog = parent->hasNNFFLog; isPIDCar = parent->isPIDCar; @@ -257,6 +269,7 @@ void FrogPilotLateralPanel::updateCarToggles() { steerKPStock = parent->steerKPStock; steerLatAccelStock = parent->steerLatAccelStock; steerRatioStock = parent->steerRatioStock; + tuningLevel = parent->tuningLevel; steerFrictionToggle->setTitle(QString(tr("Friction (Default: %1)")).arg(QString::number(steerFrictionStock, 'f', 2))); steerKPToggle->setTitle(QString(tr("Kp Factor (Default: %1)")).arg(QString::number(steerKPStock, 'f', 2))); @@ -315,7 +328,7 @@ void FrogPilotLateralPanel::showToggles(const std::set &keys) { setUpdatesEnabled(false); for (auto &[key, toggle] : toggles) { - toggle->setVisible(keys.find(key) != keys.end()); + toggle->setVisible(keys.find(key) != keys.end() && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } setUpdatesEnabled(true); @@ -332,11 +345,9 @@ void FrogPilotLateralPanel::hideToggles() { lateralTuneKeys.find(key) != lateralTuneKeys.end() || qolKeys.find(key) != qolKeys.end(); - toggle->setVisible(!subToggles); + toggle->setVisible(!subToggles && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } - toggles["AdvancedLateralTune"]->setVisible(customizationLevel == 2); - setUpdatesEnabled(true); update(); } diff --git a/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.h b/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.h index 190c969212f765..ea6afc479f6018 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.h @@ -18,7 +18,6 @@ class FrogPilotLateralPanel : public FrogPilotListWidget { void showEvent(QShowEvent *event) override; void showToggles(const std::set &keys); void updateMetric(); - void updateCarToggles(); void updateState(const UIState &s); FrogPilotParamValueButtonControl *steerFrictionToggle; @@ -28,6 +27,8 @@ class FrogPilotLateralPanel : public FrogPilotListWidget { FrogPilotSettingsWindow *parent; + QJsonObject frogpilot_toggle_levels; + Params params; bool hasAutoTune; @@ -43,7 +44,7 @@ class FrogPilotLateralPanel : public FrogPilotListWidget { float steerKPStock; float steerRatioStock; - int customizationLevel; + int tuningLevel; std::map toggles; diff --git a/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.cc index c4220fdb7d8e23..639a98de1acc8c 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.cc @@ -70,8 +70,8 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * {"TacoTune", tr("'Taco Bell Run' Turn Speed Hack"), tr("Uses comma's speed hack they used to help handle left and right turns more precisely during their 2022 'Taco Bell' drive by reducing the maximum allowed speed and acceleration while turning."), ""}, {"QOLLongitudinal", tr("Quality of Life Improvements"), tr("Miscellaneous longitudinal focused features to improve your overall openpilot experience."), "../frogpilot/assets/toggle_icons/quality_of_life.png"}, - {"CustomCruise", tr("Cruise Increase Interval"), tr("Controls the interval used when increasing the cruise control speed."), ""}, - {"CustomCruiseLong", tr("Custom Cruise Interval (Long Press)"), tr("Controls the interval used when increasing the cruise control speed while holding down the button for 0.5+ seconds."), ""}, + {"CustomCruise", tr("Cruise Increase"), tr("Controls the interval used when increasing the cruise control speed."), ""}, + {"CustomCruiseLong", tr("Cruise Increase (Long Press)"), tr("Controls the interval used when increasing the cruise control speed while holding down the button for 0.5+ seconds."), ""}, {"ForceStandstill", tr("Force Keep openpilot in the Standstill State"), tr("Keeps openpilot in the 'standstill' state until the gas pedal or 'resume' button is pressed."), ""}, {"ForceStops", tr("Force Stop for 'Detected' Stop Lights/Signs"), tr("Forces a stop whenever openpilot 'detects' a potential red light/stop sign to prevent it from running the red light/stop sign."), ""}, {"IncreasedStoppedDistance", tr("Increase Stopped Distance"), tr("Increases the distance to stop behind vehicles."), ""}, @@ -158,15 +158,7 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * } else if (param == "ConditionalExperimental") { FrogPilotParamManageControl *conditionalExperimentalToggle = new FrogPilotParamManageControl(param, title, desc, icon); QObject::connect(conditionalExperimentalToggle, &FrogPilotParamManageControl::manageButtonClicked, [this]() { - std::set modifiedConditionalExperimentalKeys = conditionalExperimentalKeys; - - if (customizationLevel != 2) { - modifiedConditionalExperimentalKeys.erase("CENavigation"); - modifiedConditionalExperimentalKeys.erase("CESignalSpeed"); - modifiedConditionalExperimentalKeys.erase("HideCEMStatusBar"); - } - - showToggles(modifiedConditionalExperimentalKeys); + showToggles(conditionalExperimentalKeys); }); longitudinalToggle = conditionalExperimentalToggle; } else if (param == "CESpeed") { @@ -206,12 +198,6 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * modifiedCurveSpeedKeys.erase("MTSCCurvatureCheck"); } - if (customizationLevel != 2) { - modifiedCurveSpeedKeys.erase("CurveSensitivity"); - modifiedCurveSpeedKeys.erase("MTSCCurvatureCheck"); - modifiedCurveSpeedKeys.erase("TurnAggressiveness"); - } - showToggles(modifiedCurveSpeedKeys); }); longitudinalToggle = curveControlToggle; @@ -247,6 +233,14 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * curveDetectionBtn->setCheckedButton(1, !vtscEnabled); } }); + QObject::connect(curveDetectionBtn, &FrogPilotButtonsControl::disabledButtonClicked, [=](int id) { + if (id == 0) { + FrogPilotConfirmationDialog::toggleAlert( + tr("The 'Map Based' option is only available when some 'Map Data' has been downloaded!"), + tr("Okay"), this + ); + } + }); longitudinalToggle = curveDetectionBtn; } else if (param == "CurveSensitivity" || param == "TurnAggressiveness") { longitudinalToggle = new FrogPilotParamValueControl(param, title, desc, icon, 1, 200, "%"); @@ -267,21 +261,7 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * } else if (param == "LongitudinalTune") { FrogPilotParamManageControl *longitudinalTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon); QObject::connect(longitudinalTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, [this]() { - std::set modifiedLongitudinalTuneKeys = longitudinalTuneKeys; - - if (customizationLevel == 0) { - modifiedLongitudinalTuneKeys.erase("HumanAcceleration"); - modifiedLongitudinalTuneKeys.erase("HumanFollowing"); - modifiedLongitudinalTuneKeys.erase("LeadDetectionThreshold"); - modifiedLongitudinalTuneKeys.erase("MaxDesiredAcceleration"); - } else if (customizationLevel == 1) { - modifiedLongitudinalTuneKeys.erase("HumanAcceleration"); - modifiedLongitudinalTuneKeys.erase("HumanFollowing"); - modifiedLongitudinalTuneKeys.erase("LeadDetectionThreshold"); - modifiedLongitudinalTuneKeys.erase("MaxDesiredAcceleration"); - } - - showToggles(modifiedLongitudinalTuneKeys); + showToggles(longitudinalTuneKeys); }); longitudinalToggle = longitudinalTuneToggle; } else if (param == "AccelerationProfile") { @@ -314,13 +294,6 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * modifiedQolKeys.erase("MapGears"); } - if (customizationLevel != 2) { - modifiedQolKeys.erase("ForceStandstill"); - modifiedQolKeys.erase("ForceStops"); - modifiedQolKeys.erase("ReverseCruise"); - modifiedQolKeys.erase("SetSpeedOffset"); - } - showToggles(modifiedQolKeys); }); longitudinalToggle = qolLongitudinalToggle; @@ -340,22 +313,9 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * } else if (param == "SpeedLimitController") { FrogPilotParamManageControl *speedLimitControllerToggle = new FrogPilotParamManageControl(param, title, desc, icon); QObject::connect(speedLimitControllerToggle, &FrogPilotParamManageControl::manageButtonClicked, [this]() { - std::set modifiedSpeedLimitControllerKeys = speedLimitControllerKeys; - - if (customizationLevel == 0) { - modifiedSpeedLimitControllerKeys.erase("SLCFallback"); - modifiedSpeedLimitControllerKeys.erase("SLCOverride"); - modifiedSpeedLimitControllerKeys.erase("SLCPriority"); - modifiedSpeedLimitControllerKeys.erase("SLCQOL"); - modifiedSpeedLimitControllerKeys.erase("SpeedLimitSources"); - } else if (customizationLevel != 2) { - modifiedSpeedLimitControllerKeys.erase("SLCPriority"); - modifiedSpeedLimitControllerKeys.erase("SLCQOL"); - } - - showToggles(modifiedSpeedLimitControllerKeys); - slcOpen = true; + + showToggles(speedLimitControllerKeys); }); longitudinalToggle = speedLimitControllerToggle; } else if (param == "SLCConfirmation") { @@ -372,43 +332,35 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * longitudinalToggle = overrideSelection; } else if (param == "SLCPriority") { ButtonControl *slcPriorityButton = new ButtonControl(title, tr("SELECT"), desc); - QStringList primaryPriorities = {tr("None"), tr("Dashboard"), tr("Map Data"), tr("Navigation"), tr("Highest"), tr("Lowest")}; - QStringList secondaryTertiaryPriorities = {tr("None"), tr("Dashboard"), tr("Map Data"), tr("Navigation")}; + QStringList primaryPriorities = {tr("Dashboard"), tr("Map Data"), tr("Navigation"), tr("Highest"), tr("Lowest")}; + QStringList otherPriorities = {tr("None"), tr("Dashboard"), tr("Map Data"), tr("Navigation")}; QStringList priorityPrompts = {tr("Select your primary priority"), tr("Select your secondary priority"), tr("Select your tertiary priority")}; QObject::connect(slcPriorityButton, &ButtonControl::clicked, [=]() { QStringList selectedPriorities; for (int i = 1; i <= 3; ++i) { - QStringList currentPriorities = (i == 1) ? primaryPriorities : secondaryTertiaryPriorities; - QStringList prioritiesToDisplay = currentPriorities; - - for (QString selectedPriority : qAsConst(selectedPriorities)) { - prioritiesToDisplay.removeAll(selectedPriority); - } + QStringList availablePriorities = (i == 1) ? primaryPriorities : otherPriorities; + availablePriorities = availablePriorities.toSet().subtract(selectedPriorities.toSet()).toList(); if (!hasDashSpeedLimits) { - prioritiesToDisplay.removeAll(tr("Dashboard")); + availablePriorities.removeAll(tr("Dashboard")); } - - if (prioritiesToDisplay.size() == 1 && prioritiesToDisplay.contains(tr("None"))) { + if (availablePriorities.size() == 1 && availablePriorities.contains(tr("None"))) { break; } - QString priorityKey = QString("SLCPriority%1").arg(i); - QString selection = MultiOptionDialog::getSelection(priorityPrompts[i - 1], prioritiesToDisplay, "", this); - + QString selection = MultiOptionDialog::getSelection(priorityPrompts[i - 1], availablePriorities, "", this); if (selection.isEmpty()) { break; } - params.putNonBlocking(priorityKey.toStdString(), selection.toStdString()); + params.put(QString("SLCPriority%1").arg(i).toStdString(), selection.toStdString()); selectedPriorities.append(selection); if (selection == tr("None")) { for (int j = i + 1; j <= 3; ++j) { - QString priorityKeyNext = QString("SLCPriority%1").arg(j); - params.putNonBlocking(priorityKeyNext.toStdString(), "None"); + params.put(QString("SLCPriority%1").arg(j).toStdString(), tr("None").toStdString()); } break; } @@ -426,10 +378,8 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * QStringList initialPriorities; for (int i = 1; i <= 3; ++i) { - QString priorityKey = QString("SLCPriority%1").arg(i); - QString priority = QString::fromStdString(params.get(priorityKey.toStdString())); - - if (!priority.isEmpty() && primaryPriorities.contains(priority) && priority != tr("None")) { + QString priority = QString::fromStdString(params.get(QString("SLCPriority%1").arg(i).toStdString())); + if (!priority.isEmpty() && priority != tr("None") && primaryPriorities.contains(priority)) { initialPriorities.append(priority); } } @@ -590,29 +540,20 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * QObject::connect(parent, &FrogPilotSettingsWindow::closeParentToggle, this, &FrogPilotLongitudinalPanel::hideToggles); QObject::connect(parent, &FrogPilotSettingsWindow::closeSubParentToggle, this, &FrogPilotLongitudinalPanel::hideSubToggles); - QObject::connect(parent, &FrogPilotSettingsWindow::updateCarToggles, this, &FrogPilotLongitudinalPanel::updateCarToggles); QObject::connect(parent, &FrogPilotSettingsWindow::updateMetric, this, &FrogPilotLongitudinalPanel::updateMetric); updateMetric(); } void FrogPilotLongitudinalPanel::showEvent(QShowEvent *event) { - customizationLevel = parent->customizationLevel; - - toggles["ConditionalExperimental"]->setVisible(customizationLevel != 0); - toggles["CurveSpeedControl"]->setVisible(customizationLevel != 0); - toggles["CustomPersonalities"]->setVisible(customizationLevel == 2); - toggles["ExperimentalModeActivation"]->setVisible(customizationLevel != 0); - toggles["QOLLongitudinal"]->setVisible(customizationLevel != 0); -} - -void FrogPilotLongitudinalPanel::updateCarToggles() { + frogpilot_toggle_levels = parent->frogpilot_toggle_levels; hasDashSpeedLimits = parent->hasDashSpeedLimits; hasPCMCruise = parent->hasPCMCruise; isGM = parent->isGM; isHKGCanFd = parent->isHKGCanFd; isSubaru = parent->isSubaru; isToyota = parent->isToyota; + tuningLevel = parent->tuningLevel; hideToggles(); } @@ -701,7 +642,7 @@ void FrogPilotLongitudinalPanel::showToggles(const std::set &keys) { setUpdatesEnabled(false); for (auto &[key, toggle] : toggles) { - toggle->setVisible(keys.find(key) != keys.end()); + toggle->setVisible(keys.find(key) != keys.end() && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } setUpdatesEnabled(true); @@ -729,15 +670,9 @@ void FrogPilotLongitudinalPanel::hideToggles() { standardPersonalityKeys.find(key) != standardPersonalityKeys.end() || trafficPersonalityKeys.find(key) != trafficPersonalityKeys.end(); - toggle->setVisible(!subToggles); + toggle->setVisible(!subToggles && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } - toggles["ConditionalExperimental"]->setVisible(customizationLevel != 0); - toggles["CurveSpeedControl"]->setVisible(customizationLevel != 0); - toggles["CustomPersonalities"]->setVisible(customizationLevel == 2); - toggles["ExperimentalModeActivation"]->setVisible(customizationLevel != 0); - toggles["QOLLongitudinal"]->setVisible(customizationLevel != 0); - setUpdatesEnabled(true); update(); } @@ -747,16 +682,6 @@ void FrogPilotLongitudinalPanel::hideSubToggles() { customPersonalityOpen = false; showToggles(customDrivingPersonalityKeys); } else if (slcOpen) { - std::set modifiedSpeedLimitControllerKeys = speedLimitControllerKeys; - - if (customizationLevel == 0) { - modifiedSpeedLimitControllerKeys.erase("SLCFallback"); - modifiedSpeedLimitControllerKeys.erase("SLCOverride"); - modifiedSpeedLimitControllerKeys.erase("SLCPriority"); - modifiedSpeedLimitControllerKeys.erase("SLCQOL"); - modifiedSpeedLimitControllerKeys.erase("SpeedLimitSources"); - } - - showToggles(modifiedSpeedLimitControllerKeys); + showToggles(speedLimitControllerKeys); } } diff --git a/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.h b/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.h index bbb023bfd1f54c..8c88d66a555e0f 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.h @@ -19,13 +19,14 @@ class FrogPilotLongitudinalPanel : public FrogPilotListWidget { void hideToggles(); void showEvent(QShowEvent *event) override; void showToggles(const std::set &keys); - void updateCarToggles(); void updateMetric(); FrogPilotSettingsWindow *parent; FrogPilotButtonsControl *curveDetectionBtn; + QJsonObject frogpilot_toggle_levels; + Params params; bool customPersonalityOpen; @@ -39,7 +40,7 @@ class FrogPilotLongitudinalPanel : public FrogPilotListWidget { bool isToyota; bool slcOpen; - int customizationLevel; + int tuningLevel; std::map toggles; diff --git a/selfdrive/frogpilot/ui/qt/offroad/model_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/model_settings.cc index cf23ec66d0ac3a..d229c1d209f73d 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/model_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/model_settings.cc @@ -144,8 +144,8 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog downloadModelBtn = new ButtonControl(title, tr("DOWNLOAD"), desc); QObject::connect(downloadModelBtn, &ButtonControl::clicked, [this]() { if (downloadModelBtn->text() == tr("CANCEL")) { - paramsMemory.remove("ModelToDownload"); - paramsMemory.putBool("CancelModelDownload", true); + params_memory.remove("ModelToDownload"); + params_memory.putBool("CancelModelDownload", true); cancellingDownload = true; } else { QMap labelToModelMap; @@ -163,8 +163,8 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog QString modelToDownload = MultiOptionDialog::getSelection(tr("Select a driving model to download"), downloadableModels, "", this); if (!modelToDownload.isEmpty()) { modelDownloading = true; - paramsMemory.put("ModelToDownload", labelToModelMap.value(modelToDownload).toStdString()); - paramsMemory.put("ModelDownloadProgress", "0%"); + params_memory.put("ModelToDownload", labelToModelMap.value(modelToDownload).toStdString()); + params_memory.put("ModelDownloadProgress", "0%"); downloadModelBtn->setValue(tr("Downloading %1...").arg(modelToDownload.remove(QRegularExpression("[🗺️👀📡]")).trimmed())); @@ -172,7 +172,7 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog progressTimer->setInterval(100); QObject::connect(progressTimer, &QTimer::timeout, this, [=]() { - QString progress = QString::fromStdString(paramsMemory.get("ModelDownloadProgress")); + QString progress = QString::fromStdString(params_memory.get("ModelDownloadProgress")); bool downloadComplete = progress.contains(QRegularExpression("downloaded", QRegularExpression::CaseInsensitiveOption)); bool downloadFailed = progress.contains(QRegularExpression("cancelled|exists|failed|offline", QRegularExpression::CaseInsensitiveOption)); @@ -199,8 +199,8 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog downloadModelBtn->setValue(progress); - paramsMemory.remove("CancelModelDownload"); - paramsMemory.remove("ModelDownloadProgress"); + params_memory.remove("CancelModelDownload"); + params_memory.remove("ModelDownloadProgress"); progressTimer->stop(); progressTimer->deleteLater(); @@ -229,8 +229,8 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog downloadAllModelsBtn = new ButtonControl(title, tr("DOWNLOAD"), desc); QObject::connect(downloadAllModelsBtn, &ButtonControl::clicked, [this]() { if (downloadAllModelsBtn->text() == tr("CANCEL")) { - paramsMemory.remove("DownloadAllModels"); - paramsMemory.putBool("CancelModelDownload", true); + params_memory.remove("DownloadAllModels"); + params_memory.putBool("CancelModelDownload", true); cancellingDownload = true; } else { startDownloadAllModels(); @@ -340,7 +340,7 @@ void FrogPilotModelPanel::startDownloadAllModels() { allModelsDownloading = true; modelDownloading = true; - paramsMemory.putBool("DownloadAllModels", true); + params_memory.putBool("DownloadAllModels", true); downloadAllModelsBtn->setValue(tr("Downloading models...")); @@ -348,7 +348,7 @@ void FrogPilotModelPanel::startDownloadAllModels() { progressTimer->setInterval(100); QObject::connect(progressTimer, &QTimer::timeout, this, [=]() { - QString progress = QString::fromStdString(paramsMemory.get("ModelDownloadProgress")); + QString progress = QString::fromStdString(params_memory.get("ModelDownloadProgress")); bool downloadComplete = progress.contains(QRegularExpression("All models downloaded!", QRegularExpression::CaseInsensitiveOption)); bool downloadFailed = progress.contains(QRegularExpression("cancelled|exists|failed|offline", QRegularExpression::CaseInsensitiveOption)); @@ -364,8 +364,8 @@ void FrogPilotModelPanel::startDownloadAllModels() { downloadAllModelsBtn->setValue(progress); - paramsMemory.remove("CancelModelDownload"); - paramsMemory.remove("ModelDownloadProgress"); + params_memory.remove("CancelModelDownload"); + params_memory.remove("ModelDownloadProgress"); progressTimer->stop(); progressTimer->deleteLater(); @@ -374,7 +374,7 @@ void FrogPilotModelPanel::startDownloadAllModels() { cancellingDownload = false; modelDownloading = false; - paramsMemory.remove("DownloadAllModels"); + params_memory.remove("DownloadAllModels"); downloadAllModelsBtn->setValue(""); }); diff --git a/selfdrive/frogpilot/ui/qt/offroad/model_settings.h b/selfdrive/frogpilot/ui/qt/offroad/model_settings.h index cc3229e6c7d048..379c192c82acef 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/model_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/model_settings.h @@ -33,7 +33,7 @@ class FrogPilotModelPanel : public FrogPilotListWidget { ButtonControl *selectModelBtn; Params params; - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; Params paramsStorage{"/persist/params"}; QDir modelDir{"/data/models/"}; diff --git a/selfdrive/frogpilot/ui/qt/offroad/sounds_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/sounds_settings.cc index 432a133560fa50..c63ce6afe95c86 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/sounds_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/sounds_settings.cc @@ -124,18 +124,13 @@ FrogPilotSoundsPanel::FrogPilotSoundsPanel(FrogPilotSettingsWindow *parent) : Fr } QObject::connect(parent, &FrogPilotSettingsWindow::closeParentToggle, this, &FrogPilotSoundsPanel::hideToggles); - QObject::connect(parent, &FrogPilotSettingsWindow::updateCarToggles, this, &FrogPilotSoundsPanel::updateCarToggles); } void FrogPilotSoundsPanel::showEvent(QShowEvent *event) { - customizationLevel = parent->customizationLevel; - - toggles["AlertVolumeControl"]->setVisible(customizationLevel == 2); -} - -void FrogPilotSoundsPanel::updateCarToggles() { + frogpilot_toggle_levels = parent->frogpilot_toggle_levels; hasBSM = parent->hasBSM; hasOpenpilotLongitudinal = parent->hasOpenpilotLongitudinal; + tuningLevel = parent->tuningLevel; hideToggles(); } @@ -144,7 +139,7 @@ void FrogPilotSoundsPanel::showToggles(const std::set &keys) { setUpdatesEnabled(false); for (auto &[key, toggle] : toggles) { - toggle->setVisible(keys.find(key) != keys.end()); + toggle->setVisible(keys.find(key) != keys.end() && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } setUpdatesEnabled(true); @@ -157,10 +152,9 @@ void FrogPilotSoundsPanel::hideToggles() { for (auto &[key, toggle] : toggles) { bool subToggles = alertVolumeControlKeys.find(key) != alertVolumeControlKeys.end() || customAlertsKeys.find(key) != customAlertsKeys.end(); - toggle->setVisible(!subToggles); - } - toggles["AlertVolumeControl"]->setVisible(customizationLevel == 2); + toggle->setVisible(!subToggles && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); + } setUpdatesEnabled(true); update(); diff --git a/selfdrive/frogpilot/ui/qt/offroad/sounds_settings.h b/selfdrive/frogpilot/ui/qt/offroad/sounds_settings.h index 1a59f40dd9030e..5f63f83d729996 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/sounds_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/sounds_settings.h @@ -17,16 +17,17 @@ class FrogPilotSoundsPanel : public FrogPilotListWidget { void hideToggles(); void showEvent(QShowEvent *event) override; void showToggles(const std::set &keys); - void updateCarToggles(); FrogPilotSettingsWindow *parent; + QJsonObject frogpilot_toggle_levels; + Params params; bool hasBSM; bool hasOpenpilotLongitudinal; - int customizationLevel; + int tuningLevel; std::map toggles; diff --git a/selfdrive/frogpilot/ui/qt/offroad/theme_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/theme_settings.cc index 6fe782ed9a80d1..24dbd9eda10446 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/theme_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/theme_settings.cc @@ -113,11 +113,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (colorDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; colorDownloading = false; themeDownloading = false; @@ -128,9 +128,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!colorSchemeToDownload.isEmpty()) { QString convertedColorScheme = formatColorNameForStorage(colorSchemeToDownload); - paramsMemory.put("ColorToDownload", convertedColorScheme.toStdString()); + params_memory.put("ColorToDownload", convertedColorScheme.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); colorDownloading = true; themeDownloading = true; @@ -241,11 +241,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (distanceIconDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; distanceIconDownloading = false; themeDownloading = false; @@ -256,9 +256,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!iconPackToDownload.isEmpty()) { QString convertedIconPack = formatIconNameForStorage(iconPackToDownload); - paramsMemory.put("DistanceIconToDownload", convertedIconPack.toStdString()); + params_memory.put("DistanceIconToDownload", convertedIconPack.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); distanceIconDownloading = true; themeDownloading = true; @@ -370,11 +370,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (iconDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; iconDownloading = false; themeDownloading = false; @@ -385,9 +385,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!iconPackToDownload.isEmpty()) { QString convertedIconPack = formatIconNameForStorage(iconPackToDownload); - paramsMemory.put("IconToDownload", convertedIconPack.toStdString()); + params_memory.put("IconToDownload", convertedIconPack.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); iconDownloading = true; themeDownloading = true; @@ -465,12 +465,12 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr availableSignals << formatSignalName(dirInfo.fileName()); } } - availableSignals.append("Stock"); + availableSignals.append("None"); std::sort(availableSignals.begin(), availableSignals.end()); if (id == 0) { QStringList customSignalsList = availableSignals; - customSignalsList.removeAll("Stock"); + customSignalsList.removeAll("None"); customSignalsList.removeAll(currentSignal); QString signalPackToDelete = MultiOptionDialog::getSelection(tr("Select a signal pack to delete"), customSignalsList, "", this); @@ -499,11 +499,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (signalDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; signalDownloading = false; themeDownloading = false; @@ -514,9 +514,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!signalPackToDownload.isEmpty()) { QString convertedSignalPack = formatSignalNameForStorage(signalPackToDownload); - paramsMemory.put("SignalToDownload", convertedSignalPack.toStdString()); + params_memory.put("SignalToDownload", convertedSignalPack.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); signalDownloading = true; themeDownloading = true; @@ -628,11 +628,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (soundDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; soundDownloading = false; themeDownloading = false; @@ -643,9 +643,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!soundSchemeToDownload.isEmpty()) { QString convertedSoundScheme = formatSoundNameForStorage(soundSchemeToDownload); - paramsMemory.put("SoundToDownload", convertedSoundScheme.toStdString()); + params_memory.put("SoundToDownload", convertedSoundScheme.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); soundDownloading = true; themeDownloading = true; @@ -757,11 +757,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (wheelDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; wheelDownloading = false; themeDownloading = false; @@ -772,9 +772,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!wheelToDownload.isEmpty()) { QString convertedImage = formatWheelNameForStorage(wheelToDownload); - paramsMemory.put("WheelToDownload", convertedImage.toStdString()); + params_memory.put("WheelToDownload", convertedImage.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); themeDownloading = true; wheelDownloading = true; @@ -831,10 +831,10 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } else if (id == 2) { QString newTop = InputDialog::getText(tr("Enter the text for the top half"), this, tr("Characters: 0/%1").arg(maxLengthTop), false, -1, currentTop, maxLengthTop).trimmed(); if (newTop.length() > 0) { - params.putNonBlocking("StartupMessageTop", newTop.toStdString()); + params.put("StartupMessageTop", newTop.toStdString()); QString newBottom = InputDialog::getText(tr("Enter the text for the bottom half"), this, tr("Characters: 0/%1").arg(maxLengthBottom), false, -1, currentBottom, maxLengthBottom).trimmed(); if (newBottom.length() > 0) { - params.putNonBlocking("StartupMessageBottom", newBottom.toStdString()); + params.put("StartupMessageBottom", newBottom.toStdString()); } } } else if (id == 3) { @@ -876,10 +876,10 @@ void FrogPilotThemesPanel::showEvent(QShowEvent *event) { soundsDownloaded = params.get("DownloadableSounds").empty(); wheelsDownloaded = params.get("DownloadableWheels").empty(); - customizationLevel = parent->customizationLevel; + frogpilot_toggle_levels = parent->frogpilot_toggle_levels; + tuningLevel = parent->tuningLevel; - toggles["RandomEvents"]->setVisible(customizationLevel != 0); - toggles["StartupAlert"]->setVisible(customizationLevel == 2); + hideToggles(); } void FrogPilotThemesPanel::updateState(const UIState &s) { @@ -891,7 +891,7 @@ void FrogPilotThemesPanel::updateState(const UIState &s) { if (personalizeOpenpilotOpen) { if (themeDownloading) { - QString progress = QString::fromStdString(paramsMemory.get("ThemeDownloadProgress")); + QString progress = QString::fromStdString(params_memory.get("ThemeDownloadProgress")); bool downloadFailed = progress.contains(QRegularExpression("cancelled|exists|Failed|offline", QRegularExpression::CaseInsensitiveOption)); if (progress != "Downloading...") { @@ -904,7 +904,7 @@ void FrogPilotThemesPanel::updateState(const UIState &s) { downloadStatusLabel->setText("Idle"); } }); - paramsMemory.remove("ThemeDownloadProgress"); + params_memory.remove("ThemeDownloadProgress"); colorDownloading = false; distanceIconDownloading = false; iconDownloading = false; @@ -958,7 +958,7 @@ void FrogPilotThemesPanel::showToggles(const std::set &keys) { setUpdatesEnabled(false); for (auto &[key, toggle] : toggles) { - toggle->setVisible(keys.find(key) != keys.end()); + toggle->setVisible(keys.find(key) != keys.end() && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } setUpdatesEnabled(true); @@ -973,12 +973,9 @@ void FrogPilotThemesPanel::hideToggles() { for (auto &[key, toggle] : toggles) { bool subToggles = customThemeKeys.find(key) != customThemeKeys.end(); - toggle->setVisible(!subToggles); + toggle->setVisible(!subToggles && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } - toggles["RandomEvents"]->setVisible(customizationLevel != 0); - toggles["StartupAlert"]->setVisible(customizationLevel == 2); - setUpdatesEnabled(true); update(); } diff --git a/selfdrive/frogpilot/ui/qt/offroad/theme_settings.h b/selfdrive/frogpilot/ui/qt/offroad/theme_settings.h index 9ddfe7b6aa3914..b16e1c5f702a8d 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/theme_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/theme_settings.h @@ -34,8 +34,10 @@ class FrogPilotThemesPanel : public FrogPilotListWidget { LabelControl *downloadStatusLabel; + QJsonObject frogpilot_toggle_levels; + Params params; - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; bool cancellingDownload; bool colorDownloading; @@ -54,7 +56,7 @@ class FrogPilotThemesPanel : public FrogPilotListWidget { bool wheelDownloading; bool wheelsDownloaded; - int customizationLevel; + int tuningLevel; std::map toggles; }; diff --git a/selfdrive/frogpilot/ui/qt/offroad/utilities.cc b/selfdrive/frogpilot/ui/qt/offroad/utilities.cc index eb6a5c10868922..e9f5aa5ea8a63c 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/utilities.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/utilities.cc @@ -29,43 +29,36 @@ UtilitiesPanel::UtilitiesPanel(FrogPilotSettingsWindow *parent) : FrogPilotListW forceStartedBtn = new FrogPilotButtonsControl(tr("Force Started State"), tr("Forces openpilot either offroad or onroad."), {tr("OFFROAD"), tr("ONROAD"), tr("OFF")}, true); QObject::connect(forceStartedBtn, &FrogPilotButtonsControl::buttonClicked, [=](int id) { if (id == 0) { - paramsMemory.putBool("ForceOffroad", true); - paramsMemory.putBool("ForceOnroad", false); + params_memory.putBool("ForceOffroad", true); + params_memory.putBool("ForceOnroad", false); } else if (id == 1) { - paramsMemory.putBool("ForceOffroad", false); - paramsMemory.putBool("ForceOnroad", true); + params_memory.putBool("ForceOffroad", false); + params_memory.putBool("ForceOnroad", true); } else if (id == 2) { - paramsMemory.putBool("ForceOffroad", false); - paramsMemory.putBool("ForceOnroad", false); + params_memory.putBool("ForceOffroad", false); + params_memory.putBool("ForceOnroad", false); } forceStartedBtn->setCheckedButton(id); }); forceStartedBtn->setCheckedButton(2); addItem(forceStartedBtn); - ButtonControl *resetTogglesBtn = new ButtonControl(tr("Reset Toggles to Default"), tr("RESET"), tr("Resets your toggle settings back to their default settings.")); - QObject::connect(resetTogglesBtn, &ButtonControl::clicked, [=]() mutable { - QDir toggleDir("/data/params/d"); - + ButtonControl *resetTogglesBtn = new ButtonControl(tr("Reset Toggles to Default"), tr("RESET"), tr("Reset your toggle settings back to their default settings.")); + QObject::connect(resetTogglesBtn, &ButtonControl::clicked, [=]() { if (ConfirmationDialog::confirm(tr("Are you sure you want to completely reset all of your toggle settings?"), tr("Reset"), this)) { - std::thread([=]() mutable { + std::thread([=] { resetTogglesBtn->setEnabled(false); resetTogglesBtn->setValue(tr("Resetting...")); - if (toggleDir.removeRecursively()) { - toggleDir.mkpath("."); - params.putBool("DoToggleReset", true); + params.putBool("DoToggleReset", true); - resetTogglesBtn->setValue(tr("Reset!")); + resetTogglesBtn->setValue(tr("Reset!")); - util::sleep_for(2000); - resetTogglesBtn->setValue(tr("Rebooting...")); - util::sleep_for(2000); + util::sleep_for(2000); + resetTogglesBtn->setValue(tr("Rebooting...")); + util::sleep_for(2000); - Hardware::reboot(); - } else { - resetTogglesBtn->setValue(tr("Failed...")); - } + Hardware::reboot(); }).detach(); } }); diff --git a/selfdrive/frogpilot/ui/qt/offroad/utilities.h b/selfdrive/frogpilot/ui/qt/offroad/utilities.h index 93f5481c9b1e85..4be541371c7685 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/utilities.h +++ b/selfdrive/frogpilot/ui/qt/offroad/utilities.h @@ -12,5 +12,5 @@ class UtilitiesPanel : public FrogPilotListWidget { FrogPilotButtonsControl *forceStartedBtn; Params params; - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; }; diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc index 7f60fac6bccda6..7d2ae94a012bb9 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc @@ -211,25 +211,19 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(FrogPilotSettingsWindow *parent) setModels(); } - QObject::connect(parent, &FrogPilotSettingsWindow::updateCarToggles, this, &FrogPilotVehiclesPanel::updateCarToggles); QObject::connect(uiState(), &UIState::uiUpdate, this, &FrogPilotVehiclesPanel::updateState); } void FrogPilotVehiclesPanel::showEvent(QShowEvent *event) { - customizationLevel = parent->customizationLevel; - - updateToggles(); -} - -void FrogPilotVehiclesPanel::updateCarToggles() { disableOpenpilotLongitudinal = parent->disableOpenpilotLongitudinal; + frogpilot_toggle_levels = parent->frogpilot_toggle_levels; hasExperimentalOpenpilotLongitudinal = parent->hasExperimentalOpenpilotLongitudinal; hasOpenpilotLongitudinal = parent->hasOpenpilotLongitudinal; hasSNG = parent->hasSNG; isBolt = parent->isBolt; - isGMPCMCruise = parent->isGMPCMCruise; isImpreza = parent->isImpreza; isVolt = parent->isVolt; + tuningLevel = parent->tuningLevel; updateToggles(); } @@ -250,8 +244,9 @@ void FrogPilotVehiclesPanel::setModels() { void FrogPilotVehiclesPanel::updateToggles() { setUpdatesEnabled(false); - disableOpenpilotLong->setVisible((hasOpenpilotLongitudinal && !hasExperimentalOpenpilotLongitudinal && !isGMPCMCruise && customizationLevel == 2) || disableOpenpilotLongitudinal); - forceFingerprint ->setVisible(customizationLevel == 2 || isBolt); + disableOpenpilotLong->setVisible(hasOpenpilotLongitudinal && !hasExperimentalOpenpilotLongitudinal && tuningLevel >= frogpilot_toggle_levels["DisableOpenpilotLongitudinal"].toDouble()); + disableOpenpilotLong->setVisible(disableOpenpilotLong->isVisible() || disableOpenpilotLongitudinal); + forceFingerprint ->setVisible(tuningLevel >= frogpilot_toggle_levels["ForceFingerprint"].toDouble() || isBolt); selectMakeButton->setValue(carMake); selectModelButton->setValue(carModel); @@ -297,16 +292,9 @@ void FrogPilotVehiclesPanel::updateToggles() { } } - toggle->setVisible(setVisible); + toggle->setVisible(setVisible && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } - toggles["ClusterOffset"]->setVisible(toggles["ClusterOffset"]->isVisible() && customizationLevel == 2); - toggles["CrosstrekTorque"]->setVisible(toggles["CrosstrekTorque"]->isVisible() && customizationLevel == 2); - toggles["ExperimentalGMTune"]->setVisible(toggles["ExperimentalGMTune"]->isVisible() && customizationLevel == 2); - toggles["FrogsGoMoosTweak"]->setVisible(toggles["FrogsGoMoosTweak"]->isVisible() && customizationLevel == 2); - toggles["LongPitch"]->setVisible(toggles["LongPitch"]->isVisible() && customizationLevel == 2); - toggles["NewLongAPI"]->setVisible(toggles["NewLongAPI"]->isVisible() && customizationLevel == 2); - setUpdatesEnabled(true); update(); } diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h index faa15611645d25..528ed35043b72c 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h @@ -13,7 +13,6 @@ class FrogPilotVehiclesPanel : public FrogPilotListWidget { private: void setModels(); void showEvent(QShowEvent *event) override; - void updateCarToggles(); void updateState(const UIState &s); void updateToggles(); @@ -22,6 +21,8 @@ class FrogPilotVehiclesPanel : public FrogPilotListWidget { FrogPilotSettingsWindow *parent; + QJsonObject frogpilot_toggle_levels; + QMap carModels; QString carMake; @@ -40,12 +41,11 @@ class FrogPilotVehiclesPanel : public FrogPilotListWidget { bool hasOpenpilotLongitudinal; bool hasSNG; bool isBolt; - bool isGMPCMCruise; bool isImpreza; bool isVolt; bool started; - int customizationLevel; + int tuningLevel; std::map toggles; diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc index 43aa8ab0c63dfb..71297ac81ba01b 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc @@ -64,16 +64,6 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(FrogPilotSettingsWindow *parent) : modifiedAccessibilityKeys.erase("OnroadDistanceButton"); } - if (customizationLevel == 0) { - modifiedAccessibilityKeys.erase("CameraView"); - modifiedAccessibilityKeys.erase("DriverCamera"); - modifiedAccessibilityKeys.erase("StandbyMode"); - modifiedAccessibilityKeys.erase("StoppedTimer"); - } else if (customizationLevel != 2) { - modifiedAccessibilityKeys.erase("CameraView"); - modifiedAccessibilityKeys.erase("StandbyMode"); - } - showToggles(modifiedAccessibilityKeys); }); visualToggle = qolToggle; @@ -185,13 +175,6 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(FrogPilotSettingsWindow *parent) : modifiedNavigationUIKeys.erase("ShowSLCOffset"); } - if (customizationLevel != 2) { - modifiedNavigationUIKeys.erase("MapStyle"); - modifiedNavigationUIKeys.erase("RoadNameUI"); - modifiedNavigationUIKeys.erase("ShowSLCOffset"); - modifiedNavigationUIKeys.erase("UseVienna"); - } - showToggles(modifiedNavigationUIKeys); }); visualToggle = customUIToggle; @@ -243,10 +226,6 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(FrogPilotSettingsWindow *parent) : modifiedCustomOnroadUIKeys.erase("PedalsOnUI"); } - if (customizationLevel != 2) { - modifiedCustomOnroadUIKeys.erase("AdjacentPath"); - } - showToggles(modifiedCustomOnroadUIKeys); }); visualToggle = customUIToggle; @@ -282,27 +261,17 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(FrogPilotSettingsWindow *parent) : } QObject::connect(parent, &FrogPilotSettingsWindow::closeParentToggle, this, &FrogPilotVisualsPanel::hideToggles); - QObject::connect(parent, &FrogPilotSettingsWindow::updateCarToggles, this, &FrogPilotVisualsPanel::updateCarToggles); QObject::connect(parent, &FrogPilotSettingsWindow::updateMetric, this, &FrogPilotVisualsPanel::updateMetric); } void FrogPilotVisualsPanel::showEvent(QShowEvent *event) { - customizationLevel = parent->customizationLevel; - - toggles["AdvancedCustomUI"]->setVisible(customizationLevel == 2); - toggles["CustomUI"]->setVisible(customizationLevel != 0); - toggles["DeveloperUI"]->setVisible(customizationLevel == 2); - toggles["ModelUI"]->setVisible(customizationLevel == 2); - toggles["NavigationUI"]->setVisible(customizationLevel != 0); - toggles["QOLVisuals"]->setVisible(customizationLevel != 0 || !disableOpenpilotLongitudinal && hasOpenpilotLongitudinal); -} - -void FrogPilotVisualsPanel::updateCarToggles() { disableOpenpilotLongitudinal = parent->disableOpenpilotLongitudinal; + frogpilot_toggle_levels = parent->frogpilot_toggle_levels; hasAutoTune = parent->hasAutoTune; hasBSM = parent->hasBSM; hasOpenpilotLongitudinal = parent->hasOpenpilotLongitudinal; hasRadar = parent->hasRadar; + tuningLevel = parent->tuningLevel; hideToggles(); } @@ -348,7 +317,7 @@ void FrogPilotVisualsPanel::showToggles(const std::set &keys) { setUpdatesEnabled(false); for (auto &[key, toggle] : toggles) { - toggle->setVisible(keys.find(key) != keys.end()); + toggle->setVisible(keys.find(key) != keys.end() && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } setUpdatesEnabled(true); @@ -366,15 +335,10 @@ void FrogPilotVisualsPanel::hideToggles() { modelUIKeys.find(key) != modelUIKeys.end() || navigationUIKeys.find(key) != navigationUIKeys.end(); - toggle->setVisible(!subToggles); + toggle->setVisible(!subToggles && tuningLevel >= frogpilot_toggle_levels[key].toDouble()); } - toggles["AdvancedCustomUI"]->setVisible(customizationLevel == 2); - toggles["CustomUI"]->setVisible(customizationLevel != 0); - toggles["DeveloperUI"]->setVisible(customizationLevel == 2); - toggles["ModelUI"]->setVisible(customizationLevel == 2); - toggles["NavigationUI"]->setVisible(customizationLevel != 0); - toggles["QOLVisuals"]->setVisible(customizationLevel != 0 || !disableOpenpilotLongitudinal && hasOpenpilotLongitudinal); + toggles["QOLVisuals"]->setVisible(toggles["QOLVisuals"]->isVisible() || !disableOpenpilotLongitudinal && hasOpenpilotLongitudinal); setUpdatesEnabled(true); update(); diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h index 92eed7d6e9bb62..3e77d8d26e5067 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h @@ -17,7 +17,6 @@ class FrogPilotVisualsPanel : public FrogPilotListWidget { void hideToggles(); void showEvent(QShowEvent *event) override; void showToggles(const std::set &keys); - void updateCarToggles(); void updateMetric(); FrogPilotButtonToggleControl *borderMetricsBtn; @@ -28,6 +27,8 @@ class FrogPilotVisualsPanel : public FrogPilotListWidget { Params params; + QJsonObject frogpilot_toggle_levels; + bool disableOpenpilotLongitudinal; bool hasAutoTune; bool hasBSM; @@ -35,7 +36,7 @@ class FrogPilotVisualsPanel : public FrogPilotListWidget { bool hasRadar; bool isMetric = params.getBool("IsMetric"); - int customizationLevel; + int tuningLevel; std::map toggles; diff --git a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc index e44bce01e52dd0..bc82cbda65c918 100644 --- a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc +++ b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc @@ -5,8 +5,8 @@ #include "selfdrive/ui/ui.h" void updateFrogPilotToggles() { - static Params paramsMemory{"/dev/shm/params"}; - paramsMemory.putBool("FrogPilotTogglesUpdated", true); + static Params params_memory{"/dev/shm/params"}; + params_memory.putBool("FrogPilotTogglesUpdated", true); } QColor loadThemeColors(const QString &colorKey, bool clearCache) { diff --git a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h index b4c78b959b91fb..27c02af39599f9 100644 --- a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h +++ b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h @@ -112,6 +112,7 @@ class FrogPilotButtonsControl : public AbstractControl { button->setMinimumWidth(minimumButtonWidth); hlayout->addWidget(button); buttonGroup->addButton(button, i); + button->installEventFilter(this); } QObject::connect(buttonGroup, QOverload::of(&QButtonGroup::buttonClicked), [=](int id) { @@ -151,6 +152,18 @@ class FrogPilotButtonsControl : public AbstractControl { signals: void buttonClicked(int id); + void disabledButtonClicked(int id); + +protected: + bool eventFilter(QObject *obj, QEvent *event) override { + if (event->type() == QEvent::MouseButtonPress) { + QPushButton *button = qobject_cast(obj); + if (button && !button->isEnabled()) { + emit disabledButtonClicked(buttonGroup->id(button)); + } + } + return AbstractControl::eventFilter(obj, event); + } private: QButtonGroup *buttonGroup; @@ -203,12 +216,6 @@ class FrogPilotButtonToggleControl : public ParamControl { } } - void setEnabledButtons(int id, bool enable) { - if (QAbstractButton *button = buttonGroup->button(id)) { - button->setEnabled(enable); - } - } - void setVisibleButton(int id, bool visible) { if (QAbstractButton *button = buttonGroup->button(id)) { button->setVisible(visible); @@ -343,7 +350,7 @@ private slots: void onButtonReleased() { if (instantUpdate) { - params.putFloatNonBlocking(key, value); + params.putFloat(key, value); } float lastValue = value; diff --git a/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.cc b/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.cc index 5bc2d310c55300..cc34f895e822e2 100644 --- a/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.cc +++ b/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.cc @@ -110,7 +110,7 @@ QPushButton *ModelReview::createButton(const QString &text, const QString &type, } void ModelReview::showEvent(QShowEvent *event) { - currentModel = QString::fromStdString(paramsMemory.get("CurrentModelName")); + currentModel = QString::fromStdString(params_memory.get("CurrentModelName")); currentModelFiltered = processModelName(currentModel); mainLayout->setCurrentIndex(modelRated ? 1 : 0); diff --git a/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.h b/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.h index 4f5af5c0164dc2..f3593d2c00da94 100644 --- a/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.h +++ b/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.h @@ -43,7 +43,7 @@ private slots: QPushButton *blacklistButton; Params params; - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; QString currentModel; QString currentModelFiltered; diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index 3253c560229f94..f7cf65323234ee 100644 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -254,7 +254,7 @@ void OffroadHome::hideEvent(QHideEvent *event) { void OffroadHome::refresh() { QString model = QString::fromStdString(params.get("ModelName")).remove(QRegularExpression("[🗺️👀📡]")).remove("(Default)").trimmed(); - if (params.getBool("CustomizationLevelConfirmed") && params.getInt("CustomizationLevel") != 2) { + if (params.getBool("TuningLevelConfirmed") && params.getInt("TuningLevel") != 2) { model = QString::fromStdString(params.get("DefaultModelName")).trimmed(); } diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 1801b4eeeda9e7..49a8d60c0d0a16 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -233,6 +233,8 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { if (ConfirmationDialog::confirm(tr("Are you sure you want to reset calibration?"), tr("Reset"), this)) { params.remove("CalibrationParams"); params.remove("LiveTorqueParameters"); + paramsStorage.remove("CalibrationParams"); + paramsStorage.remove("LiveTorqueParameters"); } }); addItem(resetCalibBtn); @@ -488,38 +490,38 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { QObject::connect(btn, &QPushButton::clicked, [=, w = panel_frame]() { if (w->widget() == frogpilotSettingsWindow) { - bool customizationLevelConfirmed = params.getBool("CustomizationLevelConfirmed"); + bool tuningLevelConfirmed = params.getBool("TuningLevelConfirmed"); - if (!customizationLevelConfirmed) { + if (!tuningLevelConfirmed) { int frogpilotHours = paramsTracking.getInt("FrogPilotMinutes") / 60; int openpilotHours = params.getInt("openpilotMinutes") / 60; if (frogpilotHours < 1 && openpilotHours < 100) { - if (FrogPilotConfirmationDialog::toggleAlert(tr("Welcome to FrogPilot! Since you're new to FrogPilot, the 'Basic' toggle preset has been applied, but you can change this at any time via the 'Customization Level' button!"), tr("Sounds good!"), this, true)) { - params.putBool("CustomizationLevelConfirmed", true); - params.putInt("CustomizationLevel", 0); + if (FrogPilotConfirmationDialog::toggleAlert(tr("Welcome to FrogPilot! Since you're new to FrogPilot, the 'Minimal' toggle preset has been applied, but you can change this at any time via the 'Tuning Level' button!"), tr("Sounds good!"), this, true)) { + params.putBool("TuningLevelConfirmed", true); + params.putInt("TuningLevel", 0); } } else if (frogpilotHours < 50 && openpilotHours < 100) { - if (FrogPilotConfirmationDialog::toggleAlert(tr("Since you're fairly new to FrogPilot, the 'Basic' toggle preset has been applied, but you can change this at any time via the 'Customization Level' button!"), tr("Sounds good!"), this, true)) { - params.putBool("CustomizationLevelConfirmed", true); - params.putInt("CustomizationLevel", 0); + if (FrogPilotConfirmationDialog::toggleAlert(tr("Since you're fairly new to FrogPilot, the 'Minimal' toggle preset has been applied, but you can change this at any time via the 'Tuning Level' button!"), tr("Sounds good!"), this, true)) { + params.putBool("TuningLevelConfirmed", true); + params.putInt("TuningLevel", 0); } } else if (frogpilotHours < 100) { - if (openpilotHours >= 100 && frogpilotHours < 100) { - if (FrogPilotConfirmationDialog::toggleAlert(tr("Since you're experienced with openpilot, the 'Standard' toggle preset has been applied, but you can change this at any time via the 'Customization Level' button!"), tr("Sounds good!"), this, true)) { - params.putBool("CustomizationLevelConfirmed", true); - params.putInt("CustomizationLevel", 1); + if (openpilotHours >= 100) { + if (FrogPilotConfirmationDialog::toggleAlert(tr("Since you're experienced with openpilot, the 'Standard' toggle preset has been applied, but you can change this at any time via the 'Tuning Level' button!"), tr("Sounds good!"), this, true)) { + params.putBool("TuningLevelConfirmed", true); + params.putInt("TuningLevel", 1); } } else { - if (FrogPilotConfirmationDialog::toggleAlert(tr("Since you're experienced with FrogPilot, the 'Standard' toggle preset has been applied, but you can change this at any time via the 'Customization Level' button!"), tr("Sounds good!"), this, true)) { - params.putBool("CustomizationLevelConfirmed", true); - params.putInt("CustomizationLevel", 1); + if (FrogPilotConfirmationDialog::toggleAlert(tr("Since you're experienced with FrogPilot, the 'Standard' toggle preset has been applied, but you can change this at any time via the 'Tuning Level' button!"), tr("Sounds good!"), this, true)) { + params.putBool("TuningLevelConfirmed", true); + params.putInt("TuningLevel", 1); } } } else if (frogpilotHours >= 100) { - if (FrogPilotConfirmationDialog::toggleAlert(tr("Since you're very experienced with FrogPilot, the 'Advanced' toggle preset has been applied, but you can change this at any time via the 'Customization Level' button!"), tr("Sounds good!"), this, true)) { - params.putBool("CustomizationLevelConfirmed", true); - params.putInt("CustomizationLevel", 2); + if (FrogPilotConfirmationDialog::toggleAlert(tr("Since you're very experienced with FrogPilot, the 'Advanced' toggle preset has been applied, but you can change this at any time via the 'Tuning Level' button!"), tr("Sounds good!"), this, true)) { + params.putBool("TuningLevelConfirmed", true); + params.putInt("TuningLevel", 2); } } } diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index e2ea3ac5a04624..a862106c85cc3e 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -77,6 +77,9 @@ private slots: private: Params params; ButtonControl *pair_device; + + // FrogPilot variables + Params paramsStorage{"/persist/params"}; }; class TogglesPanel : public ListWidget { @@ -125,5 +128,5 @@ class SoftwarePanel : public ListWidget { ParamWatcher *fs_watch; // FrogPilot variables - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; }; diff --git a/selfdrive/ui/qt/offroad/software_settings.cc b/selfdrive/ui/qt/offroad/software_settings.cc index 081e2ea5e3cd1d..8c6992d076fb43 100644 --- a/selfdrive/ui/qt/offroad/software_settings.cc +++ b/selfdrive/ui/qt/offroad/software_settings.cc @@ -44,7 +44,7 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { } else { std::system("pkill -SIGHUP -f system.updated.updated"); } - paramsMemory.putBool("ManualUpdateInitiated", true); + params_memory.putBool("ManualUpdateInitiated", true); }); addItem(downloadBtn); @@ -89,10 +89,9 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { auto uninstallBtn = new ButtonControl(tr("Uninstall %1").arg(getBrand()), tr("UNINSTALL")); connect(uninstallBtn, &ButtonControl::clicked, [&]() { if (ConfirmationDialog::confirm(tr("Are you sure you want to uninstall?"), tr("Uninstall"), this)) { - if (FrogPilotConfirmationDialog::yesorno(tr("Do you want to delete deep storage FrogPilot assets? This is 100% unrecoverable and includes FrogPilot stats and toggle settings for quick reinstalls."), this)) { - if (FrogPilotConfirmationDialog::yesorno(tr("Are you sure? This is 100% unrecoverable and includes FrogPilot stats and toggle settings for quick reinstalls."), this)) { + if (FrogPilotConfirmationDialog::yesorno(tr("Do you want to delete deep storage FrogPilot assets? This includes your toggle settings for quick reinstalls."), this)) { + if (FrogPilotConfirmationDialog::yesorno(tr("Are you sure? This is 100% unrecoverable and if you reinstall FrogPilot you'll lose all your previous settings!"), this)) { std::system("rm -rf /persist/params"); - std::system("rm -rf /persist/tracking"); } } params.putBool("DoUninstall", true); @@ -143,10 +142,10 @@ void SoftwarePanel::updateLabels() { } // updater only runs offroad or when parked - bool parked = scene.parked; + bool parked = scene.parked || scene.frogs_go_moo; - onroadLbl->setVisible(is_onroad && !parked && !scene.frogs_go_moo); - downloadBtn->setVisible(!is_onroad || parked || scene.frogs_go_moo); + onroadLbl->setVisible(is_onroad && !parked); + downloadBtn->setVisible(!is_onroad || parked); // download update QString updater_state = QString::fromStdString(params.get("UpdaterState")); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 8a817bc47a5dca..8c6d9929345223 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -209,29 +209,24 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.drawText(rect.adjusted(20, 0, 0, 0), Qt::AlignVCenter | Qt::AlignLeft, speedStr); }; - static float previousSetSpeed = 0; - if (setSpeed == previousSetSpeed) { - QRect curveSpeedRect(QPoint(set_speed_rect.right() + 25, set_speed_rect.top()), QSize(default_size.width() * 1.25, default_size.width() * 1.25)); - QPixmap scaledCurveSpeedIcon = (leftCurve ? curveSpeedLeftIcon : curveSpeedRightIcon).scaled(curveSpeedRect.size(), Qt::KeepAspectRatio, Qt::SmoothTransformation); + QRect curveSpeedRect(QPoint(set_speed_rect.right() + 25, set_speed_rect.top()), QSize(default_size.width() * 1.25, default_size.width() * 1.25)); + QPixmap scaledCurveSpeedIcon = (leftCurve ? curveSpeedLeftIcon : curveSpeedRightIcon).scaled(curveSpeedRect.size(), Qt::KeepAspectRatio, Qt::SmoothTransformation); - p.setOpacity(1.0); - p.setRenderHint(QPainter::Antialiasing); - p.drawPixmap(curveSpeedRect, scaledCurveSpeedIcon); + p.setOpacity(1.0); + p.setRenderHint(QPainter::Antialiasing); + p.drawPixmap(curveSpeedRect, scaledCurveSpeedIcon); - if (mtscEnabled) { - QRect mtscRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), vtscControllingCurve ? 50 : 100)); + if (mtscEnabled) { + QRect mtscRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), vtscControllingCurve ? 50 : 100)); drawCurveSpeedControl(mtscRect, mtscSpeedStr, true); - if (vtscEnabled) { - QRect vtscRect(mtscRect.topLeft() + QPoint(0, mtscRect.height() + 20), QSize(mtscRect.width(), vtscControllingCurve ? 100 : 50)); - drawCurveSpeedControl(vtscRect, vtscSpeedStr, false); - } - } else if (vtscEnabled) { - QRect vtscRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), 150)); + if (vtscEnabled) { + QRect vtscRect(mtscRect.topLeft() + QPoint(0, mtscRect.height() + 20), QSize(mtscRect.width(), vtscControllingCurve ? 100 : 50)); drawCurveSpeedControl(vtscRect, vtscSpeedStr, false); } - } else { - previousSetSpeed = setSpeed; + } else if (vtscEnabled) { + QRect vtscRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), 150)); + drawCurveSpeedControl(vtscRect, vtscSpeedStr, false); } } @@ -560,7 +555,7 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s, f if (scene.show_stopping_point_metrics) { QFont font = InterFont(35, QFont::DemiBold); - QString text = QString::number(modelLength * distanceConversion) + leadDistanceUnit; + QString text = QString::number(std::nearbyint(modelLength * distanceConversion)) + leadDistanceUnit; int text_width = QFontMetrics(font).horizontalAdvance(text); QPointF text_position = last_point - QPointF(text_width / 2, stopSignImg.height() + 35); @@ -741,10 +736,10 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState // chevron QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}}; - if (useStockColors) { + if (useStockColors && !adjacent) { painter.setBrush(redColor(fillAlpha)); } else { - painter.setBrush(lead_marker_color); + painter.setBrush(QColor(lead_marker_color.red(), lead_marker_color.green(), lead_marker_color.blue(), fillAlpha)); } painter.drawPolygon(chevron, std::size(chevron)); @@ -864,21 +859,25 @@ void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) { auto lead_right = radar_state.getLeadRight(); auto lead_left_far = radar_state.getLeadLeftFar(); auto lead_right_far = radar_state.getLeadRightFar(); + auto leads_lead = radar_state.getLeadsLead(); if (lead_left.getStatus()) { drawLead(painter, lead_left, s->scene.lead_vertices[2], v_ego, blueColor(), true); } if (lead_right.getStatus()) { - drawLead(painter, lead_right, s->scene.lead_vertices[3], v_ego, redColor(), true); + drawLead(painter, lead_right, s->scene.lead_vertices[3], v_ego, orangeColor(), true); } if (lead_left_far.getStatus()) { - drawLead(painter, lead_left_far, s->scene.lead_vertices[4], v_ego, greenColor(), true); + drawLead(painter, lead_left_far, s->scene.lead_vertices[4], v_ego, purpleColor(), true); } if (lead_right_far.getStatus()) { - drawLead(painter, lead_right_far, s->scene.lead_vertices[5], v_ego, whiteColor(), true); + drawLead(painter, lead_right_far, s->scene.lead_vertices[5], v_ego, yellowColor(), true); } if (lead_two.getStatus()) { drawLead(painter, lead_two, s->scene.lead_vertices[1], v_ego, s->scene.lead_marker_color); } else if (lead_one.getStatus()) { + if (leads_lead.getStatus()) { + drawLead(painter, leads_lead, s->scene.lead_vertices[6], v_ego, s->scene.lead_marker_color); + } drawLead(painter, lead_one, s->scene.lead_vertices[0], v_ego, s->scene.lead_marker_color); } else { lead_x = 0; @@ -921,26 +920,6 @@ void AnnotatedCameraWidget::showEvent(QShowEvent *event) { prev_draw_t = millis_since_boot(); // FrogPilot variables - if (is_metric || useSI) { - accelerationUnit = tr("m/s²"); - leadDistanceUnit = tr(mapOpen ? "m" : "meters"); - leadSpeedUnit = useSI ? tr("m/s") : tr("kph"); - - accelerationConversion = 1.0f; - distanceConversion = 1.0f; - speedConversion = is_metric ? MS_TO_KPH : MS_TO_MPH; - speedConversionMetrics = useSI ? 1.0f : MS_TO_KPH; - } else { - accelerationUnit = tr("ft/s²"); - leadDistanceUnit = tr(mapOpen ? "ft" : "feet"); - leadSpeedUnit = tr("mph"); - - accelerationConversion = METER_TO_FOOT; - distanceConversion = METER_TO_FOOT; - speedConversion = MS_TO_MPH; - speedConversionMetrics = MS_TO_MPH; - } - distance_btn->updateIcon(); experimental_btn->updateIcon(); updateSignals(); @@ -1038,6 +1017,26 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { } void AnnotatedCameraWidget::updateFrogPilotVariables(int alert_height, const UIScene &scene) { + if (is_metric || useSI) { + accelerationUnit = tr("m/s²"); + leadDistanceUnit = tr(mapOpen ? "m" : "meters"); + leadSpeedUnit = useSI ? tr("m/s") : tr("kph"); + + accelerationConversion = 1.0f; + distanceConversion = 1.0f; + speedConversion = is_metric ? MS_TO_KPH : MS_TO_MPH; + speedConversionMetrics = useSI ? 1.0f : MS_TO_KPH; + } else { + accelerationUnit = tr("ft/s²"); + leadDistanceUnit = tr(mapOpen ? "ft" : "feet"); + leadSpeedUnit = tr("mph"); + + accelerationConversion = METER_TO_FOOT; + distanceConversion = METER_TO_FOOT; + speedConversion = MS_TO_MPH; + speedConversionMetrics = MS_TO_MPH; + } + alertHeight = alert_height; alwaysOnLateralActive = scene.always_on_lateral_active; @@ -1093,7 +1092,7 @@ void AnnotatedCameraWidget::updateFrogPilotVariables(int alert_height, const UIS modelLength = scene.model_length; mtscEnabled = scene.mtsc_enabled; - mtscSpeed = scene.mtsc_speed * speedConversion + vCruiseDiff; + mtscSpeed = mtscEnabled ? scene.mtsc_speed * speedConversion : setSpeed; onroadDistanceButton = scene.onroad_distance_button; bool enableDistanceButton = onroadDistanceButton && !hideBottomIcons; @@ -1151,11 +1150,9 @@ void AnnotatedCameraWidget::updateFrogPilotVariables(int alert_height, const UIS useStockColors = scene.use_stock_colors; - vCruiseDiff = scene.v_cruise_diff; - vtscControllingCurve = scene.vtsc_controlling_curve; vtscEnabled = scene.vtsc_enabled; - vtscSpeed = scene.vtsc_speed * speedConversion + vCruiseDiff; + vtscSpeed = vtscEnabled ? scene.vtsc_speed * speedConversion : setSpeed; } void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter) { diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 56ab666315f343..7ba29548ea6135 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -179,7 +179,6 @@ class AnnotatedCameraWidget : public CameraWidget { float speedConversionMetrics; float unconfirmedSpeedLimit; float upcomingSpeedLimit; - float vCruiseDiff; float vtscSpeed; int alertHeight; @@ -203,8 +202,11 @@ class AnnotatedCameraWidget : public CameraWidget { std::string speedLimitSource; - inline QColor blueColor(int alpha = 255) { return QColor(0, 150, 255, alpha); } + inline QColor blueColor(int alpha = 255) { return QColor(0, 0, 255, alpha); } inline QColor greenColor(int alpha = 242) { return QColor(23, 134, 68, alpha); } + inline QColor orangeColor(int alpha = 255) { return QColor(255, 165, 0, alpha); } + inline QColor purpleColor(int alpha = 255) { return QColor(128, 0, 128, alpha); } + inline QColor yellowColor(int alpha = 255) { return QColor(255, 255, 0, alpha); } protected: void paintGL() override; diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index 2f0722ccb58747..e524a0dbc9a088 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -39,6 +39,7 @@ ExperimentalButton::~ExperimentalButton() { if (gif != nullptr) { gif->stop(); delete gif; + gif = nullptr; gif_label->hide(); } } @@ -126,12 +127,19 @@ void ExperimentalButton::updateIcon() { if (gif != nullptr) { gif->stop(); delete gif; + gif = nullptr; gif_label->hide(); } if (QFile::exists(wheel_gif_path)) { gif = new QMovie(wheel_gif_path); + if (!gif->isValid()) { + delete gif; + gif = nullptr; + return; + } + gif_label->setMovie(gif); gif_label->resize(img_size, img_size); gif_label->move((btn_size - img_size) / 2, (btn_size - img_size) / 2 + y_offset); diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 0749dd2cc708d0..d4deac8fc3557d 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -125,7 +125,6 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { if (scene.speed_limit_changed && nvg->newSpeedLimitRect.contains(pos)) { paramsMemory.putBool("SLCConfirmed", true); - paramsMemory.putBool("SLCConfirmedPressed", true); return; } diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index abb864eb3676a4..bd2a843f6598ab 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -63,24 +63,28 @@ void Sidebar::updateIcon(QLabel *&label, QMovie *&gif, const QString &gifPath, c gif->stop(); delete gif; gif = nullptr; - label->hide(); + if (label) { + label->hide(); + } } if (QFile::exists(selectedGifPath)) { gif = new QMovie(selectedGifPath); - if (!gif->fileName().isEmpty()) { + if (gif->isValid()) { gif->setScaledSize(btnRect.size()); - label->setGeometry(btnRect); - label->setMovie(gif); - label->show(); + if (label) { + label->setGeometry(btnRect); + label->setMovie(gif); + label->show(); + } gif->start(); - isGif = true; } else { delete gif; + gif = nullptr; isGif = false; } } else { diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h index 1a5b2c38be70a7..cd6b1e82799dba 100644 --- a/selfdrive/ui/qt/widgets/controls.h +++ b/selfdrive/ui/qt/widgets/controls.h @@ -225,11 +225,12 @@ class ButtonParamControl : public AbstractControl { button->setMinimumWidth(minimum_button_width); hlayout->addWidget(button); button_group->addButton(button, i); + button->installEventFilter(this); } QObject::connect(button_group, QOverload::of(&QButtonGroup::buttonClicked), [=](int id) { params.put(key, std::to_string(id)); - emit buttonClicked(); + emit buttonClicked(id); }); } @@ -239,6 +240,12 @@ class ButtonParamControl : public AbstractControl { } } + void setEnabledButtons(int id, bool enable) { + if (QAbstractButton *button = button_group->button(id)) { + button->setEnabled(enable); + } + } + void setCheckedButton(int id) { button_group->button(id)->setChecked(true); } @@ -253,7 +260,19 @@ class ButtonParamControl : public AbstractControl { } signals: - void buttonClicked(); + void buttonClicked(int id); + void disabledButtonClicked(int id); + +protected: + bool eventFilter(QObject *obj, QEvent *event) override { + if (event->type() == QEvent::MouseButtonPress) { + QPushButton *button = qobject_cast(obj); + if (button && !button->isEnabled()) { + emit disabledButtonClicked(button_group->id(button)); + } + } + return AbstractControl::eventFilter(obj, event); + } private: std::string key; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 6e52e9139d1a1c..7c472e163a735f 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -48,16 +48,17 @@ void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, con SubMaster &sm = *(s->sm); float path_offset_z = sm["liveCalibration"].getLiveCalibration().getHeight()[0]; - cereal::RadarState::LeadData::Reader (cereal::RadarState::Reader::*get_lead_data[6])() const = { + cereal::RadarState::LeadData::Reader (cereal::RadarState::Reader::*get_lead_data[7])() const = { &cereal::RadarState::Reader::getLeadOne, &cereal::RadarState::Reader::getLeadTwo, &cereal::RadarState::Reader::getLeadLeft, &cereal::RadarState::Reader::getLeadRight, &cereal::RadarState::Reader::getLeadLeftFar, - &cereal::RadarState::Reader::getLeadRightFar + &cereal::RadarState::Reader::getLeadRightFar, + &cereal::RadarState::Reader::getLeadsLead }; - for (int i = 0; i < 6; ++i) { + for (int i = 0; i < 7; ++i) { auto lead_data = (radar_state.*get_lead_data[i])(); if (lead_data.getStatus()) { float z = line.getZ()[get_path_length_idx(line, lead_data.getDRel())]; @@ -271,7 +272,6 @@ static void update_state(UIState *s) { auto controlsState = sm["controlsState"].getControlsState(); scene.enabled = controlsState.getEnabled(); scene.experimental_mode = scene.enabled && controlsState.getExperimentalMode(); - scene.v_cruise_diff = controlsState.getVCruiseCluster() - controlsState.getVCruise(); } if (sm.updated("deviceState")) { auto deviceState = sm["deviceState"].getDeviceState(); @@ -317,7 +317,7 @@ static void update_state(UIState *s) { scene.vtsc_controlling_curve = frogpilotPlan.getVtscControllingCurve(); scene.vtsc_speed = frogpilotPlan.getVtscSpeed(); if (frogpilotPlan.getTogglesUpdated()) { - scene.frogpilot_toggles = QJsonDocument::fromJson(QString::fromStdString(params.get("FrogPilotToggles")).toUtf8()).object(); + scene.frogpilot_toggles = QJsonDocument::fromJson(QString::fromStdString(s->params_memory.get("FrogPilotToggles", true)).toUtf8()).object(); ui_update_params(s); } } @@ -490,7 +490,7 @@ void UIState::updateStatus() { } scene.started |= scene.force_onroad; - scene.started &= !paramsMemory.getBool("ForceOffroad"); + scene.started &= !params_memory.getBool("ForceOffroad"); // Handle onroad/offroad transition if (scene.started != started_prev || sm->frame == 1) { @@ -533,7 +533,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { // FrogPilot variables wifi = new WifiManager(this); - scene.frogpilot_toggles = QJsonDocument::fromJson(QString::fromStdString(params.get("FrogPilotToggles")).toUtf8()).object(); + scene.frogpilot_toggles = QJsonDocument::fromJson(QString::fromStdString(params_memory.get("FrogPilotToggles", true)).toUtf8()).object(); ui_update_params(this); } @@ -548,9 +548,9 @@ void UIState::update() { emit uiUpdate(*this); // FrogPilot variables - scene.conditional_status = scene.conditional_experimental && scene.enabled ? paramsMemory.getInt("CEStatus") : 0; + scene.conditional_status = scene.conditional_experimental && scene.enabled ? params_memory.getInt("CEStatus") : 0; scene.driver_camera_timer = scene.driver_camera_in_reverse && scene.reverse ? scene.driver_camera_timer + 1 : 0; - scene.force_onroad = paramsMemory.getBool("ForceOnroad"); + scene.force_onroad = params_memory.getBool("ForceOnroad"); scene.started_timer = scene.started || started_prev ? scene.started_timer + 1 : 0; if (scene.keep_screen_on) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 9cccb54f61d090..e92b2de1776a90 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -104,7 +104,7 @@ typedef struct UIScene { QPolygonF road_edge_vertices[2]; // lead - QPointF lead_vertices[6]; + QPointF lead_vertices[7]; // DMoji state float driver_pose_vals[3]; @@ -257,7 +257,6 @@ typedef struct UIScene { float steer; float unconfirmed_speed_limit; float upcoming_speed_limit; - float v_cruise_diff; float vtsc_speed; int bearing_deg; @@ -311,6 +310,8 @@ class UIState : public QObject { QTransform car_space_transform; // FrogPilot variables + Params params_memory{"/dev/shm/params"}; + WifiManager *wifi = nullptr; signals: @@ -330,9 +331,6 @@ private slots: QTimer *timer; bool started_prev = false; PrimeType prime_type = PrimeType::UNKNOWN; - - // FrogPilot variables - Params paramsMemory{"/dev/shm/params"}; }; UIState *uiState(); diff --git a/system/manager/manager.py b/system/manager/manager.py index 29dcb0efe072f3..b3e88a98eb20e2 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -29,20 +29,20 @@ def manager_init() -> None: build_metadata = get_build_metadata() params = Params() - setup_frogpilot(build_metadata, params) - params_storage = Params("/persist/params") params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) if build_metadata.release_channel: params.clear_all(ParamKeyType.DEVELOPMENT_ONLY) - convert_params(params, params_storage) - threading.Thread(target=frogpilot_boot_functions, args=(build_metadata, params, params_storage,)).start() + # FrogPilot variables + setup_frogpilot(build_metadata) + params_storage = Params("/persist/params") + convert_params(params_storage) + threading.Thread(target=frogpilot_boot_functions, args=(build_metadata, params_storage,)).start() default_params: list[tuple[str, str | bytes]] = [ ("AlwaysOnDM", "0"), - ("CalibrationParams", ""), ("CarParamsPersistent", ""), ("CompletedTrainingVersion", "0"), ("DisengageOnAccelerator", "0"), @@ -58,13 +58,13 @@ def manager_init() -> None: ("IsLdwEnabled", "0"), ("IsMetric", "0"), ("LanguageSetting", "main_en"), - ("LiveTorqueParameters", ""), ("NavSettingLeftSide", "0"), ("NavSettingTime24h", "0"), ("OpenpilotEnabledToggle", "1"), ("RecordFront", "0"), ("SshEnabled", "0"), ("TetheringEnabled", "0"), + ("UpdaterAvailableBranches", ""), ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), ] if not PC: @@ -75,10 +75,10 @@ def manager_init() -> None: # set unset params reset_toggles = params.get_bool("DoToggleReset") - for k, v in default_params + frogpilot_default_params: + for k, v in default_params + [(k, v) for k, v, _ in frogpilot_default_params]: if params.get(k) is None or reset_toggles: if params_storage.get(k) is None or reset_toggles: - params.put(k, v if isinstance(v, bytes) else str(v).encode('utf-8')) + params.put(k, v) else: params.put(k, params_storage.get(k)) else: diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 39d418c33e61f9..115b951e7556a7 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -72,7 +72,7 @@ def run_new_modeld(started, params, CP: car.CarParams, classic_model, frogpilot_ NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], only_onroad), PythonProcess("navmodeld", "selfdrive.modeld.navmodeld", only_onroad), NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC), - NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(30 if not PC else None)), + NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)), PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad), NativeProcess("locationd", "selfdrive/locationd", ["./locationd"], only_onroad), NativeProcess("pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), diff --git a/system/sentry.py b/system/sentry.py index 35bf570e6f1612..01f6a375e279da 100644 --- a/system/sentry.py +++ b/system/sentry.py @@ -82,21 +82,21 @@ def capture_fingerprint(candidate, params, blocked=False): for label, key_values in matched_params.items(): if label == "FrogPilot Tracking": - matched_params[label] = {k: f"{v:,}" for k, v in key_values.items()} + matched_params[label] = {key: f"{value:,}" for key, value in key_values.items()} else: - matched_params[label] = {k: int(v) if isinstance(v, float) and v.is_integer() else v for k, v in sorted(key_values.items())} + matched_params[label] = {key: int(value) if isinstance(value, float) and value.is_integer() else value for key, value in sorted(key_values.items())} with sentry_sdk.configure_scope() as scope: scope.fingerprint = [params.get("DongleId", encoding='utf-8')] for label, key_values in matched_params.items(): - scope.set_extra(label, "\n".join(f"{k}: {v}" for k, v in key_values.items())) + scope.set_extra(label, "\n".join(f"{key}: {value}" for key, value in key_values.items())) if blocked: sentry_sdk.capture_message("Blocked user from using the development branch", level='error') else: sentry_sdk.capture_message(f"Fingerprinted {candidate}", level='info') - params.put_bool_nonblocking("FingerprintLogged", True) + params.put_bool("FingerprintLogged", True) sentry_sdk.flush() diff --git a/system/updated/updated.py b/system/updated/updated.py index 180a4a54e08ec6..40fd8b898e8f0a 100755 --- a/system/updated/updated.py +++ b/system/updated/updated.py @@ -411,7 +411,7 @@ def fetch_update(self) -> None: cloudlog.info("finalize success!") # Format "Updated" to Phoenix time zone - self.params.put_nonblocking("Updated", datetime.datetime.now().astimezone(ZoneInfo('America/Phoenix')).strftime("%B %d, %Y - %I:%M%p").encode('utf8')) + self.params.put("Updated", datetime.datetime.now().astimezone(ZoneInfo('America/Phoenix')).strftime("%B %d, %Y - %I:%M%p").encode('utf8')) def main() -> None: params = Params() @@ -468,7 +468,7 @@ def main() -> None: # Format "InstallDate" to Phoenix time zone if not install_date_set: - params.put_nonblocking("InstallDate", datetime.datetime.now().astimezone(ZoneInfo('America/Phoenix')).strftime("%B %d, %Y - %I:%M%p").encode('utf8')) + params.put("InstallDate", datetime.datetime.now().astimezone(ZoneInfo('America/Phoenix')).strftime("%B %d, %Y - %I:%M%p").encode('utf8')) install_date_set = True if not (frogpilot_toggles.automatic_updates or params_memory.get_bool("ManualUpdateInitiated")):