diff --git a/README.md b/README.md index 21928a8611ae77..2ba14f988c5652 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ FrogPilot is a fully open-sourced fork of openpilot, featuring clear and concise ------ FrogPilot was last updated on: -**October 21st, 2024** +**November 24th, 2024** Features ------ diff --git a/SConstruct b/SConstruct index 747cd09dd8a5fd..944d650d742d99 100644 --- a/SConstruct +++ b/SConstruct @@ -277,7 +277,6 @@ Export('envCython') # Qt build environment qt_env = env.Clone() - qt_modules = ["Widgets", "Gui", "Core", "Network", "Concurrent", "Multimedia", "Quick", "Qml", "QuickWidgets", "Location", "Positioning", "DBus", "Xml"] qt_libs = [] diff --git a/cereal/car.capnp b/cereal/car.capnp index ec40d92c040f10..62559a402b7817 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -84,6 +84,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { startup @75; startupNoCar @76; startupNoControl @77; + startupNoSecOcKey @149; startupMaster @78; startupNoFw @104; fcw @79; @@ -137,14 +138,15 @@ struct CarEvent @0x9b1657f34caf3ad3 { openpilotCrashedRandomEvent @137; pedalInterceptorNoBrake @138; speedLimitChanged @139; - torqueNNLoad @140; - trafficModeActive @141; - trafficModeInactive @142; - turningLeft @143; - turningRight @144; - vCruise69 @145; - yourFrogTriedToKillMe @146; - youveGotMail @147; + thisIsFineSteerSaturated @140; + torqueNNLoad @141; + trafficModeActive @142; + trafficModeInactive @143; + turningLeft @144; + turningRight @145; + vCruise69 @146; + yourFrogTriedToKillMe @147; + youveGotMail @148; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; @@ -453,7 +455,8 @@ struct CarControl { mail @16; nessie @17; noice @18; - uwu @19; + thisIsFine @19; + uwu @20; } } @@ -546,6 +549,9 @@ struct CarParams { wheelSpeedFactor @63 :Float32; # Multiplier on wheels speeds to computer actual speeds + secOcRequired @74 :Bool; # Car requires SecOC message authentication to operate + secOcKeyAvailable @75 :Bool; # Stored SecOC key loaded from params + struct SafetyConfig { safetyModel @0 :SafetyModel; safetyParam @3 :UInt16; diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 6b5554fe7b7c09..172b802f3b7bed 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -51,30 +51,32 @@ struct FrogPilotPlan @0x80ae746ee2596b11 { accelerationJerkStock @1 :Float32; adjustedCruise @2 :Float32; dangerJerk @3 :Float32; - desiredFollowDistance @4 :Float32; + desiredFollowDistance @4 :Int64; experimentalMode @5 :Bool; forcingStop @6 :Bool; - frogpilotEvents @7 :List(Car.CarEvent); - lateralCheck @8 :Bool; - laneWidthLeft @9 :Float32; - laneWidthRight @10 :Float32; - maxAcceleration @11 :Float32; - minAcceleration @12 :Float32; - redLight @13 :Bool; - safeObstacleDistance @14 :Int32; - safeObstacleDistanceStock @15 :Int32; - slcOverridden @16 :Bool; - slcOverriddenSpeed @17 :Float32; - slcSpeedLimit @18 :Float32; - slcSpeedLimitOffset @19 :Float32; - speedJerk @20 :Float32; - speedJerkStock @21 :Float32; - speedLimitChanged @22 :Bool; - stoppedEquivalenceFactor @23 :Int32; - tFollow @24 :Float32; - unconfirmedSlcSpeedLimit @25 :Float32; - vCruise @26 :Float32; - vtscControllingCurve @27 :Bool; + forcingStopLength @7 :Float32; + frogpilotEvents @8 :List(Car.CarEvent); + lateralCheck @9 :Bool; + laneWidthLeft @10 :Float32; + laneWidthRight @11 :Float32; + maxAcceleration @12 :Float32; + minAcceleration @13 :Float32; + redLight @14 :Bool; + safeObstacleDistance @15 :Int64; + safeObstacleDistanceStock @16 :Int64; + slcOverridden @17 :Bool; + slcOverriddenSpeed @18 :Float32; + slcSpeedLimit @19 :Float32; + slcSpeedLimitOffset @20 :Float32; + speedJerk @21 :Float32; + speedJerkStock @22 :Float32; + speedLimitChanged @23 :Bool; + stoppedEquivalenceFactor @24 :Int64; + tFollow @25 :Float32; + togglesUpdated @26 :Bool; + unconfirmedSlcSpeedLimit @27 :Float32; + vCruise @28 :Float32; + vtscControllingCurve @29 :Bool; } struct CustomReserved5 @0xa5cd762cd951a455 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 3b1df446a51ff5..9a944eac3429f6 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -618,6 +618,10 @@ struct RadarState @0x9a185389d6fdd05f { leadOne @3 :LeadData; leadTwo @4 :LeadData; + leadLeft @13 :LeadData; + leadRight @14 :LeadData; + leadLeftFar @15 :LeadData; + leadRightFar @16 :LeadData; cumLagMs @5 :Float32; struct LeadData { diff --git a/cereal/services.py b/cereal/services.py index 1ac0ae4760138c..6a15c9672c27ad 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -74,13 +74,6 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] "userFlag": (True, 0., 1), "microphone": (True, 10., 10), - # FrogPilot - "frogpilotCarControl": (True, 100., 10), - "frogpilotCarState": (True, 100., 10), - "frogpilotDeviceState": (True, 2., 1), - "frogpilotNavigation": (True, 1., 10), - "frogpilotPlan": (True, 20., 5), - # debug "uiDebug": (True, 0., 1), "testJoystick": (True, 0.), @@ -97,6 +90,13 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] "customReservedRawData0": (True, 0.), "customReservedRawData1": (True, 0.), "customReservedRawData2": (True, 0.), + + # FrogPilot + "frogpilotCarControl": (True, 100., 10), + "frogpilotCarState": (True, 100., 10), + "frogpilotDeviceState": (True, 2., 1), + "frogpilotNavigation": (True, 1., 10), + "frogpilotPlan": (True, 20., 5), } SERVICE_LIST = {name: Service(*vals) for idx, (name, vals) in enumerate(_services.items())} diff --git a/common/conversions.py b/common/conversions.py index 8554646ed0bb72..62c8debdb56e0e 100644 --- a/common/conversions.py +++ b/common/conversions.py @@ -12,8 +12,6 @@ class Conversions: KNOTS_TO_MS = 1. / MS_TO_KNOTS # Distance - KM_TO_MILES = KPH_TO_MPH - MILES_TO_KM = MPH_TO_KPH METER_TO_FOOT = 3.28084 FOOT_TO_METER = 1. / METER_TO_FOOT CM_TO_INCH = 1. / 2.54 diff --git a/common/params.cc b/common/params.cc index 918f714f3e3cf8..eba32fab4fceb2 100644 --- a/common/params.cc +++ b/common/params.cc @@ -112,7 +112,7 @@ std::unordered_map keys = { {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"DisablePowerDown", PERSISTENT}, - {"DisableUpdates", PERSISTENT | FROGPILOT_STORAGE}, + {"DisableUpdates", PERSISTENT}, {"DisengageOnAccelerator", PERSISTENT | FROGPILOT_STORAGE}, {"DmModelInitialized", CLEAR_ON_ONROAD_TRANSITION}, {"DongleId", PERSISTENT}, @@ -189,6 +189,7 @@ std::unordered_map keys = { {"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}, {"TermsVersion", PERSISTENT}, @@ -211,12 +212,11 @@ std::unordered_map keys = { // FrogPilot parameters {"AccelerationPath", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"AccelerationProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AdjacentLeadsUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"AdjacentPath", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"AdjacentPathMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"AdvancedCustomUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"AdvancedLateralTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"AdvancedLongitudinalTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"AdvancedQOLDriving", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"AggressiveFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"AggressiveJerkAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"AggressiveJerkDanger", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -240,7 +240,6 @@ std::unordered_map keys = { {"BlindSpotMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"BlindSpotPath", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"BorderMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"CameraFPS", PERSISTENT}, {"CameraView", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"CancelModelDownload", PERSISTENT}, {"CancelThemeDownload", PERSISTENT}, @@ -255,9 +254,7 @@ std::unordered_map keys = { {"CENavigationIntersections", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CENavigationLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CENavigationTurns", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"CertifiedHerbalistCalibrationParams", PERSISTENT}, {"CertifiedHerbalistDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"CertifiedHerbalistLiveTorqueParameters", PERSISTENT}, {"CertifiedHerbalistScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CESignalSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CESignalLaneDetection", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -266,6 +263,7 @@ std::unordered_map keys = { {"CESpeedLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CEStatus", CLEAR_ON_OFFROAD_TRANSITION}, {"CEStoppedLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ClassicModels", PERSISTENT}, {"ClusterOffset", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"ColorToDownload", PERSISTENT}, {"Compass", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -281,18 +279,22 @@ std::unordered_map keys = { {"CustomCruiseLong", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CustomDistanceIcons", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CustomIcons", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"CustomPaths", 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}, {"CustomUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"DecelerationProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DefaultModelName", CLEAR_ON_MANAGER_START}, {"DeveloperUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"DeviceManagement", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DeviceShutdown", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DisableCurveSpeedSmoothing", 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}, {"DistanceIconToDownload", PERSISTENT}, {"DisengageVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"DoToggleReset", PERSISTENT}, @@ -303,13 +305,10 @@ std::unordered_map keys = { {"DownloadableSounds", PERSISTENT}, {"DownloadableWheels", PERSISTENT}, {"DownloadAllModels", PERSISTENT}, - {"DragonPilotTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, - {"DriveRated", CLEAR_ON_ONROAD_TRANSITION}, + {"DragonRiderDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DragonRiderScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DriverCamera", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"DrivingPersonalities", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"DuckAmigoCalibrationParams", PERSISTENT}, {"DuckAmigoDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"DuckAmigoLiveTorqueParameters", PERSISTENT}, {"DuckAmigoScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DynamicPathWidth", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"DynamicPedalsOnUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -334,14 +333,10 @@ std::unordered_map keys = { {"FrogPilotDrives", PERSISTENT | FROGPILOT_TRACKING}, {"FrogPilotKilometers", PERSISTENT | FROGPILOT_TRACKING}, {"FrogPilotMinutes", PERSISTENT | FROGPILOT_TRACKING}, + {"FrogPilotToggles", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, - {"FrogsGoMoo", PERSISTENT}, {"FrogsGoMoosTweak", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"FullMap", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"GameBoyCalibrationParams", PERSISTENT}, - {"GameBoyDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"GameBoyLiveTorqueParameters", PERSISTENT}, - {"GameBoyScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"GasRegenCmd", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"GMapKey", PERSISTENT}, {"GoatScream", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -353,8 +348,7 @@ std::unordered_map keys = { {"HideMapIcon", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"HideMaxSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"HideSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"HideSpeedUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"HideUIElements", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"HideSpeedLimit", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"HolidayThemes", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"HumanAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"HumanFollowing", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -376,9 +370,7 @@ std::unordered_map keys = { {"LongitudinalMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"LongitudinalTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"LongPitch", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, - {"LosAngelesCalibrationParams", PERSISTENT}, {"LosAngelesDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"LosAngelesLiveTorqueParameters", PERSISTENT}, {"LosAngelesScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"LoudBlindspotAlert", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"LowVoltageShutdown", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -388,7 +380,7 @@ std::unordered_map keys = { {"MapboxSecretKey", PERSISTENT}, {"MapDeceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"MapGears", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"MapsSelected", PERSISTENT | FROGPILOT_OTHER}, + {"MapsSelected", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, {"MapSpeedLimit", PERSISTENT}, {"MapStyle", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"MapTargetVelocities", PERSISTENT}, @@ -397,31 +389,25 @@ std::unordered_map keys = { {"MinimumLaneChangeSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"Model", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ModelDownloadProgress", PERSISTENT}, - {"ModelManagement", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ModelName", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ModelRandomizer", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ModelsDownloaded", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"ModelSelector", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ModelToDownload", PERSISTENT}, {"ModelUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"MTSCCurvatureCheck", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"MTSCEnabled", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NavigationModels", PERSISTENT}, + {"NavigationUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"NextMapSpeedLimit", PERSISTENT}, {"NewLongAPI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"NewLongAPIGM", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, - {"NewToyotaTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"NNFF", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NNFFLite", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NNFFModelName", CLEAR_ON_OFFROAD_TRANSITION}, {"NoLogging", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NorthDakotaCalibrationParams", PERSISTENT}, {"NorthDakotaDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NorthDakotaLiveTorqueParameters", PERSISTENT}, {"NorthDakotaScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NotreDameCalibrationParams", PERSISTENT}, {"NotreDameDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NotreDameLiveTorqueParameters", PERSISTENT}, {"NotreDameScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NoUploads", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NudgelessLaneChange", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -434,6 +420,7 @@ std::unordered_map keys = { {"OneLaneChange", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"OnroadDistanceButton", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"OnroadDistanceButtonPressed", PERSISTENT}, + {"openpilotMinutes", PERSISTENT}, {"OSMDownloadBounds", PERSISTENT}, {"OSMDownloadLocations", PERSISTENT}, {"OSMDownloadProgress", CLEAR_ON_MANAGER_START}, @@ -452,14 +439,10 @@ std::unordered_map keys = { {"QOLLongitudinal", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"QOLVisuals", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"RadarlessModels", PERSISTENT}, - {"RadicalTurtleCalibrationParams", PERSISTENT}, {"RadicalTurtleDrives", PERSISTENT | FROGPILOT_CONTROLS}, - {"RadicalTurtleLiveTorqueParameters", PERSISTENT}, {"RadicalTurtleScore", PERSISTENT | FROGPILOT_CONTROLS}, {"RandomEvents", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"RecertifiedHerbalistCalibrationParams", PERSISTENT}, {"RecertifiedHerbalistDrives", PERSISTENT | FROGPILOT_CONTROLS}, - {"RecertifiedHerbalistLiveTorqueParameters", PERSISTENT}, {"RecertifiedHerbalistScore", PERSISTENT | FROGPILOT_CONTROLS}, {"RefuseVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"RelaxedFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -482,9 +465,7 @@ std::unordered_map keys = { {"ScreenTimeout", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"ScreenTimeoutOnroad", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"SearchInput", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, - {"SecretGoodOpenpilotCalibrationParams", PERSISTENT}, {"SecretGoodOpenpilotDrives", PERSISTENT | FROGPILOT_CONTROLS}, - {"SecretGoodOpenpilotLiveTorqueParameters", PERSISTENT}, {"SecretGoodOpenpilotScore", PERSISTENT | FROGPILOT_CONTROLS}, {"SetSpeedLimit", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SetSpeedOffset", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -493,7 +474,6 @@ std::unordered_map keys = { {"ShowIP", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"ShowMemoryUsage", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"ShowSLCOffset", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"ShowSLCOffsetUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ShowSteering", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"ShowStoppingPoint", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"ShowStoppingPointMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -503,6 +483,7 @@ std::unordered_map keys = { {"SidebarMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"SignalMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"SignalToDownload", PERSISTENT}, + {"SLCConfirmation", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SLCConfirmationHigher", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SLCConfirmationLower", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SLCConfirmed", PERSISTENT}, @@ -539,12 +520,9 @@ std::unordered_map keys = { {"SteerRatioStock", PERSISTENT}, {"StoppedTimer", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"TacoTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TestingSound", PERSISTENT}, {"TetheringEnabled", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, {"ThemeDownloadProgress", PERSISTENT}, - {"TombRaiderCalibrationParams", PERSISTENT}, - {"TombRaiderDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"TombRaiderLiveTorqueParameters", PERSISTENT}, - {"TombRaiderScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ToyotaDoors", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"TrafficFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"TrafficJerkAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -564,14 +542,11 @@ std::unordered_map keys = { {"UseSI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"UseStockColors", CLEAR_ON_MANAGER_START}, {"UseVienna", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"VelocityModels", PERSISTENT}, {"VisionTurnControl", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"VoltSNG", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"WarningImmediateVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"WarningSoftVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"WD40CalibrationParams", PERSISTENT}, {"WD40Drives", PERSISTENT | FROGPILOT_CONTROLS}, - {"WD40LiveTorqueParameters", PERSISTENT}, {"WD40Score", PERSISTENT | FROGPILOT_CONTROLS}, {"WheelIcon", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"WheelSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, diff --git a/common/params.h b/common/params.h index e7ad3e1c8040c7..461f522cf806bf 100644 --- a/common/params.h +++ b/common/params.h @@ -77,11 +77,9 @@ class Params { inline void putBoolNonBlocking(const std::string &key, bool val) { putNonBlocking(key, val ? "1" : "0"); } - void putIntNonBlocking(const std::string &key, const std::string &val); inline void putIntNonBlocking(const std::string &key, int val) { putNonBlocking(key, std::to_string(val)); } - void putFloatNonBlocking(const std::string &key, const std::string &val); inline void putFloatNonBlocking(const std::string &key, float val) { putNonBlocking(key, std::to_string(val)); } diff --git a/opendbc/toyota_new_mc_pt_generated.dbc b/opendbc/toyota_new_mc_pt_generated.dbc index 1a9a2a9a26c204..2b74b4651cbe87 100644 --- a/opendbc/toyota_new_mc_pt_generated.dbc +++ b/opendbc/toyota_new_mc_pt_generated.dbc @@ -197,7 +197,11 @@ BO_ 643 PRE_COLLISION: 7 DSU BO_ 705 GAS_PEDAL: 8 XXX SG_ GAS_RELEASED : 3|1@0+ (1,0) [0|1] "" XXX - SG_ GAS_PEDAL : 55|8@0+ (0.005,0) [0|1] "" XXX + SG_ ETQLVSC : 15|16@0- (0.03125,0) [0|0] "Nm" XXX + SG_ ETQREAL : 31|16@0- (0.03125,0) [0|0] "Nm" SCS + SG_ ETQISC : 47|8@0+ (1,-192) [0|0] "Nm" XXX + SG_ GAS_PEDAL : 55|8@0+ (0.5,0) [0|0] "%" DS1,FCM + SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" DS1,FCM BO_ 740 STEERING_LKA: 5 XXX SG_ LKA_STATE : 31|8@0+ (1,0) [0|255] "" XXX diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index 5f106bfc1797ce..ef60ba9319682e 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -197,7 +197,11 @@ BO_ 643 PRE_COLLISION: 7 DSU BO_ 705 GAS_PEDAL: 8 XXX SG_ GAS_RELEASED : 3|1@0+ (1,0) [0|1] "" XXX - SG_ GAS_PEDAL : 55|8@0+ (0.005,0) [0|1] "" XXX + SG_ ETQLVSC : 15|16@0- (0.03125,0) [0|0] "Nm" XXX + SG_ ETQREAL : 31|16@0- (0.03125,0) [0|0] "Nm" SCS + SG_ ETQISC : 47|8@0+ (1,-192) [0|0] "Nm" XXX + SG_ GAS_PEDAL : 55|8@0+ (0.5,0) [0|0] "%" DS1,FCM + SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" DS1,FCM BO_ 740 STEERING_LKA: 5 XXX SG_ LKA_STATE : 31|8@0+ (1,0) [0|255] "" XXX diff --git a/opendbc/toyota_rav4_prime_generated.dbc b/opendbc/toyota_rav4_prime_generated.dbc new file mode 100644 index 00000000000000..c2477c15182ddf --- /dev/null +++ b/opendbc/toyota_rav4_prime_generated.dbc @@ -0,0 +1,526 @@ +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; + + +CM_ "Imported file _community.dbc starts here"; +BO_ 359 STEERING_IPAS_COMMA: 8 IPAS + SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX + SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX + SG_ SET_ME_X10 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX + SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +CM_ "BO_ STEERING_IPAS_COMMA: Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; + +BO_ 512 GAS_COMMAND: 6 EON + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR + SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR + SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" INTERCEPTOR + SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" INTERCEPTOR + +BO_ 513 GAS_SENSOR: 6 INTERCEPTOR + SG_ INTERCEPTOR_GAS : 7|16@0+ (1,0) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (1,0) [0|1] "" EON + SG_ STATE : 39|4@0+ (1,0) [0|15] "" EON + SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" EON + SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" EON + +VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; + +BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX + SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX + +CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss"; + +BO_ 767 SDSU: 8 XXX + SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX + SG_ STATE : 23|4@0+ (1,0) [0|15] "" XXX + +CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS-P Toyotas. Learn more: https://github.com/RetroPilot/ocelot/tree/main/firmware/smart_dsu"; +CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu"; + +VAL_ 767 STATE 7 "STATE_AEB_CTRL" 6 "FAULT_INVALID" 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; + + +CM_ "toyota_rav4_prime.dbc starts here"; +VERSION "" + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: + +BU_: XXX DSU HCU EPS IPAS CGW BGM + +BO_ 15 SECOC_SYNCHRONIZATION: 8 XXX + SG_ TRIP_CNT : 7|16@0+ (1,0) [0|65535] "" XXX + SG_ RESET_CNT : 23|20@0+ (1,0) [0|65535] "" XXX + SG_ AUTHENTICATOR : 35|28@0+ (1,0) [0|268435455] "" XXX + +BO_ 37 STEER_ANGLE_SENSOR: 8 XXX + SG_ STEER_ANGLE : 3|12@0- (1.5,0) [-500|500] "deg" XXX + SG_ STEER_RATE : 35|12@0- (1,0) [-2000|2000] "deg/s" XXX + SG_ STEER_FRACTION : 39|4@0- (0.1,0) [-0.7|0.7] "deg" XXX + +BO_ 170 WHEEL_SPEEDS: 8 XXX + SG_ WHEEL_SPEED_FR : 7|16@0+ (0.01,-67.67) [0|250] "km/h" XXX + SG_ WHEEL_SPEED_FL : 23|16@0+ (0.01,-67.67) [0|250] "km/h" XXX + SG_ WHEEL_SPEED_RR : 39|16@0+ (0.01,-67.67) [0|250] "km/h" XXX + SG_ WHEEL_SPEED_RL : 55|16@0+ (0.01,-67.67) [0|250] "km/h" XXX + +BO_ 257 BRAKE_MODULE: 8 XXX + SG_ BRAKE_PRESSED : 3|1@0+ (1,0) [0|1] "" XXX + SG_ BRAKE_PRESSURE_1 : 31|8@0+ (1,0) [0|15] "" XXX + SG_ BRAKE_PRESSURE_2 : 61|6@0+ (1,0) [0|63] "" XXX + +BO_ 278 GAS_PEDAL: 8 XXX + SG_ GAS_PEDAL_ACC : 7|8@0+ (0.005,0) [0|255] "" XXX + SG_ GAS_PEDAL_USER : 15|8@0+ (0.005,0) [0|255] "" XXX + SG_ AUTHENTICATOR : 35|28@0+ (1,0) [0|268435455] "" XXX + SG_ RESET_FLAG : 37|2@0+ (1,0) [0|3] "" XXX + SG_ MSG_CNT_LOWER : 39|2@0+ (1,0) [0|3] "" XXX + +BO_ 295 GEAR_PACKET_HYBRID: 8 XXX + SG_ CAR_MOVEMENT : 25|10@0- (1,0) [0|255] "" XXX + SG_ GEAR : 47|4@0+ (1,0) [0|15] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 305 STEERING_LTA_2: 8 XXX + SG_ STEER_REQUEST_2 : 0|1@0+ (1,0) [0|1] "" XXX + SG_ STEER_REQUEST : 3|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 13|6@0+ (1,0) [0|63] "" XXX + SG_ STEER_ANGLE_CMD : 23|16@0- (0.0573,0) [0|65535] "" XXX + SG_ AUTHENTICATOR : 35|28@0+ (1,0) [0|268435455] "" XXX + SG_ RESET_FLAG : 37|2@0+ (1,0) [0|3] "" XXX + SG_ MSG_CNT_LOWER : 39|2@0+ (1,0) [0|3] "" XXX + +BO_ 353 DSU_SPEED: 7 XXX + SG_ FORWARD_SPEED : 15|16@0- (0.00390625,-30) [0|255] "km/h" XXX + +BO_ 374 PCM_CRUISE: 8 XXX + SG_ CRUISE_ACTIVE : 5|1@0+ (1,0) [0|1] "" XXX + SG_ CRUISE_STATE : 31|4@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 375 PCM_CRUISE_3: 8 XXX + SG_ NEW_SIGNAL_1 : 7|16@0- (1,0) [0|65535] "" XXX + SG_ NEW_SIGNAL_4 : 16|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_6 : 19|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_7 : 21|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_5 : 22|1@0+ (1,0) [0|1] "" XXX + SG_ GAS_RELEASED : 30|1@0+ (1,0) [0|1] "" XXX + SG_ NEW_SIGNAL_2 : 31|1@0+ (1,0) [0|1] "" XXX + SG_ AUTHENTICATOR : 35|28@0+ (1,0) [0|268435455] "" XXX + SG_ RESET_FLAG : 37|2@0+ (1,0) [0|3] "" XXX + SG_ MSG_CNT_LOWER : 39|2@0+ (1,0) [0|3] "" XXX + +BO_ 387 ACC_CONTROL_2: 8 XXX + SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s^2" HCU + SG_ AUTHENTICATOR : 35|28@0+ (1,0) [0|268435455] "" XXX + SG_ RESET_FLAG : 37|2@0+ (1,0) [0|3] "" XXX + SG_ MSG_CNT_LOWER : 39|2@0+ (1,0) [0|3] "" XXX + +BO_ 401 STEERING_LTA: 8 XXX + SG_ STEER_REQUEST : 0|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 6|6@0+ (1,0) [0|255] "" XXX + SG_ SETME_X1 : 7|1@0+ (1,0) [0|1] "" XXX + SG_ STEER_ANGLE_CMD : 15|16@0- (0.0573,0) [-540|540] "" XXX + SG_ STEER_REQUEST_2 : 25|1@0+ (1,0) [0|1] "" XXX + SG_ LKA_ACTIVE : 26|1@0+ (1,0) [0|1] "" XXX + SG_ SETME_X3 : 29|2@0+ (1,0) [0|3] "" XXX + SG_ CLEAR_HOLD_STEERING_ALERT : 30|1@0+ (1,0) [0|1] "" XXX + SG_ PERCENTAGE : 39|8@0+ (1,0) [0|255] "" XXX + SG_ TORQUE_WIND_DOWN : 47|8@0+ (1,0) [0|255] "" XXX + SG_ ANGLE : 55|8@0- (0.5,0) [0|255] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 418 CRUISE_RELATED: 8 XXX + SG_ CRUISE_ACTIVE : 7|1@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 467 PCM_CRUISE_2: 8 XXX + SG_ BRAKE_PRESSED : 3|1@0+ (1,0) [0|1] "" XXX + SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX + SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX + SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX + SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "km/h" XXX + SG_ ACC_FAULTED : 47|1@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 608 STEER_TORQUE_SENSOR: 8 XXX + SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX + SG_ STEER_ANGLE_INITIALIZING : 3|1@0+ (1,0) [0|1] "" XXX + SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX + SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX + SG_ STEER_TORQUE_EPS : 47|16@0- (1,0) [-32768|32767] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 610 EPS_STATUS: 8 EPS + SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX + SG_ LTA_STATE : 15|5@0+ (1,0) [0|31] "" XXX + SG_ TYPE : 24|1@0+ (1,0) [0|1] "" XXX + SG_ LKA_STATE : 31|7@0+ (1,0) [0|127] "" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 643 PRE_COLLISION: 8 DSU + SG_ AUTHENTICATOR : 35|28@0+ (1,0) [0|268435455] "" XXX + SG_ RESET_FLAG : 37|2@0+ (1,0) [0|3] "" XXX + SG_ MSG_CNT_LOWER : 39|2@0+ (1,0) [0|3] "" XXX + +BO_ 740 STEERING_LKA: 8 XXX + SG_ STEER_REQUEST : 0|1@0+ (1,0) [0|1] "" XXX + SG_ COUNTER : 6|6@0+ (1,0) [0|63] "" XXX + SG_ SET_ME_1 : 7|1@0+ (1,0) [0|1] "" XXX + SG_ STEER_TORQUE_CMD : 15|16@0- (1,0) [0|65535] "" XXX + SG_ LKA_STATE : 31|8@0+ (1,0) [0|255] "" XXX + SG_ AUTHENTICATOR : 35|28@0+ (1,0) [0|268435455] "" XXX + SG_ RESET_FLAG : 37|2@0+ (1,0) [0|3] "" XXX + SG_ MSG_CNT_LOWER : 39|2@0+ (1,0) [0|3] "" XXX + +BO_ 742 LEAD_INFO: 8 DSU + SG_ LEAD_LONG_DIST : 7|13@0+ (0.05,0) [0|300] "m" HCU + SG_ LEAD_REL_SPEED : 23|12@0- (0.025,0) [-100|100] "m/s" HCU + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" HCU + +BO_ 835 ACC_CONTROL: 8 DSU + SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s^2" HCU + SG_ ALLOW_LONG_PRESS : 17|2@0+ (1,0) [0|2] "" XXX + SG_ ACC_MALFUNCTION : 18|1@0+ (1,0) [0|0] "" XXX + SG_ RADAR_DIRTY : 19|1@0+ (1,0) [0|1] "" XXX + SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX + SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX + SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU + SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU + SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX + SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU + SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU + SG_ ITS_CONNECT_LEAD : 39|8@0+ (1,0) [0|1] "" Vector__XXX + SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX + SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX + +BO_ 836 PRE_COLLISION_2: 8 DSU + SG_ AUTHENTICATOR : 35|28@0+ (1,0) [0|268435455] "" XXX + SG_ RESET_FLAG : 37|2@0+ (1,0) [0|3] "" XXX + SG_ MSG_CNT_LOWER : 39|2@0+ (1,0) [0|3] "" XXX + +BO_ 881 LTA_RELATED: 8 FCM + SG_ GAS_PEDAL : 15|8@0+ (0.005,0) [0|1] "" XXX + SG_ STEER_ANGLE : 23|16@0- (0.0573,0) [-500|500] "" XXX + SG_ TURN_SIGNALS : 35|2@0+ (1,0) [0|3] "" XXX + SG_ UNKNOWN_2 : 58|1@0+ (1,0) [0|1] "" XXX + SG_ LDA_SA_TOGGLE : 59|1@0+ (1,0) [0|1] "" XXX + SG_ LTA_STEER_REQUEST : 60|1@0+ (1,0) [0|1] "" XXX + SG_ UNKNOWN : 61|1@0+ (1,0) [0|1] "" XXX + SG_ STEERING_PRESSED : 63|1@0+ (1,0) [0|1] "" XXX + +BO_ 921 PCM_CRUISE_SM: 8 XXX + SG_ MAIN_ON : 4|1@0+ (1,0) [0|1] "" XXX + SG_ CRUISE_CONTROL_STATE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ DISTANCE_LINES : 14|2@0+ (1,0) [0|3] "" XXX + SG_ TEMP_ACC_FAULTED : 15|1@0+ (1,0) [0|1] "" XXX + SG_ UI_SET_SPEED : 31|8@0+ (1,0) [0|255] "" XXX + +BO_ 951 ESP_CONTROL: 8 ESP + SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX + SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX + SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX + SG_ BRAKE_HOLD_ENABLED : 33|1@1+ (1,0) [0|1] "" XXX + SG_ BRAKE_HOLD_ACTIVE : 36|1@0+ (1,0) [0|1] "" XXX + +BO_ 1014 BSM: 8 XXX + SG_ L_ADJACENT : 0|1@0+ (1,0) [0|1] "" XXX + SG_ R_ADJACENT : 1|1@0+ (1,0) [0|1] "" XXX + SG_ ADJACENT_ENABLED : 7|1@0+ (1,0) [0|1] "" XXX + SG_ L_APPROACHING : 8|1@0+ (1,0) [0|1] "" XXX + SG_ R_APPROACHING : 10|1@0+ (1,0) [0|1] "" XXX + SG_ APPROACHING_ENABLED : 15|1@0+ (1,0) [0|1] "" XXX + +BO_ 1020 SOLAR_SENSOR: 8 XXX + SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX + +BO_ 1041 PCS_HUD: 8 DSU + SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX + SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX + SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX + SG_ PCS_DUST : 34|1@0+ (1,0) [0|0] "" XXX + SG_ PCS_TEMP : 35|1@0+ (1,0) [0|0] "" XXX + SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX + SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX + SG_ PCS_DUST2 : 41|1@0+ (1,0) [0|0] "" XXX + SG_ PCS_TEMP2 : 42|1@0+ (1,0) [0|0] "" XXX + SG_ FRD_ADJ : 53|3@0+ (1,0) [0|0] "" XXX + SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX + +BO_ 1042 LKAS_HUD: 8 DSU + SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX + SG_ RIGHT_LINE : 3|2@0+ (1,0) [0|3] "" XXX + SG_ LEFT_LINE : 5|2@0+ (1,0) [0|3] "" XXX + SG_ LKAS_STATUS : 7|2@0+ (1,0) [0|3] "" XXX + SG_ LDA_ALERT : 9|2@0+ (1,0) [0|3] "" XXX + SG_ LDW_EXIST : 10|1@0+ (1,0) [0|1] "" XXX + SG_ TWO_BEEPS : 12|1@0+ (1,0) [0|1] "" XXX + SG_ ADJUSTING_CAMERA : 13|1@0+ (1,0) [0|1] "" XXX + SG_ LDA_UNAVAILABLE_QUIET : 14|1@0+ (1,0) [0|1] "" XXX + SG_ LDA_MALFUNCTION : 15|1@0+ (1,0) [0|1] "" XXX + SG_ LDA_UNAVAILABLE : 16|1@0+ (1,0) [0|1] "" XXX + SG_ LDA_SENSITIVITY : 18|2@0+ (1,0) [0|3] "" XXX + SG_ LDA_SA_TOGGLE : 20|2@0+ (1,0) [0|3] "" XXX + SG_ LDA_MESSAGES : 23|3@0+ (1,0) [0|1] "" XXX + SG_ LDA_ON_MESSAGE : 31|2@0+ (1,0) [0|3] "" XXX + SG_ REPEATED_BEEPS : 32|1@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_X01 : 42|1@0+ (1,0) [0|1] "" XXX + SG_ LANE_SWAY_TOGGLE : 43|1@0+ (1,0) [0|1] "" XXX + SG_ LANE_SWAY_SENSITIVITY : 45|2@0+ (1,0) [0|3] "" XXX + SG_ TAKE_CONTROL : 46|1@0+ (1,0) [0|1] "" XXX + SG_ LDA_FRONT_CAMERA_BLOCKED : 47|1@0+ (1,0) [0|1] "" XXX + SG_ LANE_SWAY_BUZZER : 50|2@0+ (1,0) [0|0] "" XXX + SG_ LANE_SWAY_FLD : 53|3@0+ (1,0) [0|7] "" XXX + SG_ LANE_SWAY_WARNING : 55|2@0+ (1,0) [0|3] "" XXX + SG_ SET_ME_X02 : 63|8@0+ (1,0) [0|1] "" XXX + +BO_ 1044 AUTO_HIGH_BEAM: 8 FCM + SG_ AHB_DUTY : 47|8@0+ (0.5,0) [0|0] "%" Vector__XXX + SG_ C_AHB : 51|4@0+ (1,0) [0|0] "" Vector__XXX + SG_ F_AHB : 55|4@0+ (1,0) [0|0] "" Vector__XXX + +BO_ 1161 RSA1: 8 FCM + SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX + SG_ TSGNHLT1 : 9|2@0+ (1,0) [0|0] "" XXX + SG_ TSGNGRY1 : 12|3@0+ (1,0) [0|0] "" XXX + SG_ SPDVAL1 : 23|8@0+ (1,0) [0|0] "km/h" XXX + SG_ SPLSGN2 : 27|4@0+ (1,0) [0|0] "" XXX + SG_ SPLSGN1 : 31|4@0+ (1,0) [0|0] "" XXX + SG_ TSGN2 : 39|8@0+ (1,0) [0|0] "" XXX + SG_ TSGNHLT2 : 41|2@0+ (1,0) [0|0] "" XXX + SG_ TSGNGRY2 : 44|3@0+ (1,0) [0|0] "" XXX + SG_ SPDVAL2 : 55|8@0+ (1,0) [0|0] "" XXX + SG_ SYNCID1 : 59|4@0+ (1,0) [0|0] "" XXX + SG_ BZRRQ_A : 61|2@0+ (1,0) [0|0] "" XXX + SG_ BZRRQ_P : 63|2@0+ (1,0) [0|0] "" XXX + +BO_ 1162 RSA2: 8 FCM + SG_ TSGN3 : 7|8@0+ (1,0) [0|0] "" XXX + SG_ TSGNHLT3 : 9|2@0+ (1,0) [0|0] "" XXX + SG_ TSGNGRY3 : 12|3@0+ (1,0) [0|0] "" XXX + SG_ SPLSGN4 : 27|4@0+ (1,0) [0|0] "" XXX + SG_ SPLSGN3 : 31|4@0+ (1,0) [0|0] "" XXX + SG_ TSGN4 : 39|8@0+ (1,0) [0|0] "" XXX + SG_ TSGNHLT4 : 41|2@0+ (1,0) [0|0] "" XXX + SG_ TSGNGRY4 : 44|3@0+ (1,0) [0|0] "" XXX + SG_ SGNNUMA : 50|3@0+ (1,0) [0|0] "" XXX + SG_ SGNNUMP : 53|3@0+ (1,0) [0|0] "" XXX + SG_ DPSGNREQ : 54|1@0+ (1,0) [0|0] "" XXX + SG_ SYNCID2 : 59|4@0+ (1,0) [0|0] "" XXX + SG_ TSRWMSG : 61|2@0+ (1,0) [0|0] "" XXX + SG_ SPDUNT : 63|2@0+ (1,0) [0|0] "" XXX + +BO_ 1163 RSA3: 8 FCM + SG_ OVSPNTM : 1|2@0+ (1,0) [0|0] "" XXX + SG_ NTLVLSPD : 3|2@0+ (1,0) [0|0] "" XXX + SG_ OTSGNNTM : 5|2@0+ (1,0) [0|0] "" XXX + SG_ TSRMSW : 6|1@0+ (1,0) [0|0] "" XXX + SG_ TSREQPD : 7|1@0+ (1,0) [0|0] "" XXX + SG_ OVSPVALL : 11|4@0+ (1,-5) [0|0] "" XXX + SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX + SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX + SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX + +BO_ 1552 BODY_CONTROL_STATE_2: 8 XXX + SG_ UI_SPEED : 23|8@0+ (1,0) [0|255] "" XXX + SG_ METER_SLIDER_BRIGHTNESS_PCT : 30|7@0+ (1,0) [12|100] "%" XXX + SG_ METER_SLIDER_LOW_BRIGHTNESS : 37|1@0+ (1,0) [0|1] "" XXX + SG_ METER_SLIDER_DIMMED : 38|1@0+ (1,0) [0|1] "" XXX + SG_ UNITS : 63|3@0+ (1,0) [1|4] "" XXX + +BO_ 1553 UI_SETTING: 8 XXX + SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 39|32@0+ (1,0) [0|1048575] "" XXX + +BO_ 1556 BLINKERS_STATE: 8 XXX + SG_ HAZARD_LIGHT : 27|1@0+ (1,0) [0|1] "" XXX + SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX + +BO_ 1568 BODY_CONTROL_STATE: 8 XXX + SG_ METER_DIMMED : 38|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_RL : 42|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_RR : 43|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_FR : 44|1@0+ (1,0) [0|1] "" XXX + SG_ DOOR_OPEN_FL : 45|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_BRAKE : 60|1@0+ (1,0) [0|1] "" XXX + SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX + +BO_ 1570 LIGHT_STALK: 8 SCM + SG_ FRONT_FOG : 27|1@0+ (1,0) [0|1] "" XXX + SG_ PARKING_LIGHT : 28|1@0+ (1,0) [0|1] "" XXX + SG_ LOW_BEAM : 29|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAM : 30|1@0+ (1,0) [0|1] "" XXX + SG_ DAYTIME_RUNNING_LIGHT : 31|1@0+ (1,0) [0|1] "" XXX + SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX + +BO_ 1571 CERTIFICATION_ECU: 8 CGW + SG_ DOOR_LOCK_FEEDBACK_LIGHT : 15|1@0+ (1,0) [0|0] "" XXX + SG_ KEYFOB_LOCKING_FEEDBACK_LIGHT : 61|1@0+ (1,0) [0|0] "" XXX + SG_ KEYFOB_UNLOCKING_FEEDBACK_LIGHT : 62|1@0+ (1,0) [0|0] "" XXX + +BO_ 1592 DOOR_LOCKS: 8 XXX + SG_ LOCK_STATUS_CHANGED : 15|1@0+ (1,0) [0|1] "" XXX + SG_ LOCK_STATUS : 20|1@0+ (1,0) [0|1] "" XXX + SG_ LOCKED_VIA_KEYFOB : 23|1@0+ (1,0) [0|1] "" XXX + +CM_ SG_ 37 STEER_RATE "factor is tbd"; +CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; +CM_ SG_ 305 STEER_ANGLE_CMD "Used in place of STEERING_LTA.STEER_ANGLE_CMD on SecOC cars"; +CM_ SG_ 387 ACCEL_CMD "Used in place of ACC_CONTROL.ACCEL_CMD on SecOC cars"; +CM_ SG_ 401 STEER_REQUEST "enable bit for steering, 1 to steer, 0 to not"; +CM_ SG_ 401 SETME_X1 "usually 1, seen at 0 on some South American Corollas indicating lack of stock Lane Tracing Assist"; +CM_ SG_ 401 STEER_ANGLE_CMD "desired angle, OEM steers up to 95 degrees, no angle limit but torque will bottom out"; +CM_ SG_ 401 STEER_REQUEST_2 "enable bit for steering, 1 to steer, 0 to not"; +CM_ SG_ 401 LKA_ACTIVE "1 when using LTA for LKA"; +CM_ SG_ 401 SETME_X3 "almost completely correlates with Toyota Safety Sense version, but may instead describe max torque when using LTA. if TSS 2.5 or 2022 RAV4, this is always 1. if TSS 2.0 this is always 3 (or 0 on Alphard, Highlander, NX)"; +CM_ SG_ 401 CLEAR_HOLD_STEERING_ALERT "set to 1 when user clears LKAS_HUD->LDA_ALERT ('Hold Steering') by applying torque to steering wheel"; +CM_ SG_ 401 PERCENTAGE "driver override percentage (0-100), very close to steeringPressed in OP"; +CM_ SG_ 401 TORQUE_WIND_DOWN "used to wind down torque on user override"; +CM_ SG_ 401 ANGLE "angle of car relative to lane center on LTA camera"; +CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; +CM_ SG_ 467 SET_SPEED "43 km/h are shown as 28 mph, so conversion isn't perfect"; +CM_ SG_ 467 ACC_FAULTED "1 when ACC is faulted and the PCM disallows engagement"; +CM_ SG_ 608 STEER_OVERRIDE "set when driver torque exceeds a certain value"; +CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque"; +CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; +CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars"; +CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1"; +CM_ SG_ 835 RADAR_DIRTY "Display Clean Radar Sensor message on HUD"; +CM_ SG_ 835 DISTANCE "Cycle through ACC following distance from long, mid, short when set to 1"; +CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok"; +CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1"; +CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; +CM_ SG_ 835 ITS_CONNECT_LEAD "Displayed when lead car is capable of ITS Connect"; +CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; +CM_ SG_ 881 GAS_PEDAL "not set on all cars, only seen on TSS 2.5 Camry Hybrid so far"; +CM_ SG_ 881 STEER_ANGLE "matches STEER_TORQUE_SENSOR->STEER_ANGLE"; +CM_ SG_ 881 TURN_SIGNALS "flipped on some cars"; +CM_ SG_ 881 LDA_SA_TOGGLE "not applicable for all cars"; +CM_ SG_ 881 LTA_STEER_REQUEST "only applicable for TSS 2.5: matches STEERING_LTA->STEER_REQUEST"; +CM_ SG_ 881 UNKNOWN "related to steering wheel angle"; +CM_ SG_ 881 STEERING_PRESSED "only applicable for TSS 2.5: low sensitivity steering wheel pressed by driver signal"; +CM_ SG_ 921 TEMP_ACC_FAULTED "1 when the UI is displaying or playing fault-related alerts or sounds. Also 1 when pressing main on."; +CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit"; +CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; +CM_ SG_ 1014 L_ADJACENT "vehicle adjacent left side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED"; +CM_ SG_ 1014 R_ADJACENT "vehicle adjacent right side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED"; +CM_ SG_ 1014 ADJACENT_ENABLED "when BSM is enabled in settings, this is on along with APPROACHING_ENABLED. this controls bsm alert visibility"; +CM_ SG_ 1014 L_APPROACHING "vehicle approaching from left side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED"; +CM_ SG_ 1014 R_APPROACHING "vehicle approaching from right side of car. enabled above 10mph, regardless of ADJACENT_ENABLED or APPROACHING_ENABLED"; +CM_ SG_ 1014 APPROACHING_ENABLED "when BSM is enabled in settings, this is on along with ADJACENT_ENABLED. this controls bsm alert visibility"; +CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator"; +CM_ SG_ 1041 PCS_DUST "alert: Front Camera Low Visibility Unavailable See Owner's Manual"; +CM_ SG_ 1041 PCS_TEMP "alert: Front Camera Out of Temperature Range Unavailable Wait until Normal Temperature"; +CM_ SG_ 1041 PCS_DUST2 "alert: Pre-Collision System Radar Sensor Blocked Unavailable Clean Radar Sensor"; +CM_ SG_ 1041 PCS_TEMP2 "alert: Pre-Collision System Out of Temperature Range Unavailable See Owner's Manual"; +CM_ SG_ 1041 FRD_ADJ "alert: ERROR ADJUSTING FRONT RADAR BEAM"; +CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity"; +CM_ SG_ 1042 LDW_EXIST "Unclear what this is, it's usually set to 0"; +CM_ SG_ 1042 LDA_UNAVAILABLE_QUIET "LDA toggles and sensitivity settings are greyed out if set to 1"; +CM_ SG_ 1042 LDA_SENSITIVITY "LDA Sensitivity"; +CM_ SG_ 1042 LDA_SA_TOGGLE "LDA Steering Assist Toggle"; +CM_ SG_ 1042 LDA_MESSAGES "Various LDA Messages"; +CM_ SG_ 1042 LDA_ON_MESSAGE "Display LDA Turned ON message"; +CM_ SG_ 1042 REPEATED_BEEPS "LDA audible warning"; +CM_ SG_ 1042 SET_ME_X01 "empty bit on leaked dbc, always set to 1 during normal operations"; +CM_ SG_ 1042 LANE_SWAY_TOGGLE "Lane Sway Warning System SWS Switch"; +CM_ SG_ 1042 TAKE_CONTROL "Please Control Steering Wheel warning"; +CM_ SG_ 1042 LDA_FRONT_CAMERA_BLOCKED "originally LDAFCVB, LDA related settings are greyed out if set to 1"; +CM_ SG_ 1042 LANE_SWAY_BUZZER "Similar to TWO_BEEPS"; +CM_ SG_ 1042 LANE_SWAY_FLD "Unknown signal for Lane Sway Warning System, set to 7 on stock system when SWS is enabled, 0 when SWS is disabled"; +CM_ SG_ 1042 LANE_SWAY_WARNING "Lane Sway Warning System Triggered"; +CM_ SG_ 1042 SET_ME_X02 "empty bit on leaked dbc, always set to 2 during normal operations"; +CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit."; +CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"; +CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz"; +CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0"; +CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz"; +CM_ SG_ 1163 OVSPNTM "always 3"; +CM_ SG_ 1163 NTLVLSPD "always 3"; +CM_ SG_ 1163 OTSGNNTM "always 3"; +CM_ SG_ 1163 TSRMSW "always 1"; +CM_ SG_ 1163 TSREQPD "always 1"; +CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; +CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; +CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; +CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1552 UI_SPEED "Does not appear to match dash"; +CM_ SG_ 1552 METER_SLIDER_BRIGHTNESS_PCT "Combination display brightness setting, scales from 12 per cent to 100 per cent, reflects combination meter settings only, not linked with headlight state"; +CM_ SG_ 1552 METER_SLIDER_LOW_BRIGHTNESS "Combination display low brightness mode, also controls footwell lighting"; +CM_ SG_ 1552 METER_SLIDER_DIMMED "Combination display slider not at max, reflects combination meter settings only, not linked with headlight state"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; +CM_ SG_ 1592 LOCK_STATUS_CHANGED "1 on rising edge of lock/unlocking"; +CM_ SG_ 1592 LOCK_STATUS "The next 3 bits always seem to follow this signal."; +CM_ SG_ 1592 LOCKED_VIA_KEYFOB "1 for as long as car is locked with key fob or door handle touch"; +VAL_ 295 GEAR 0 "P" 1 "R" 2 "N" 3 "D" 4 "B"; +VAL_ 401 SETME_X3 3 "TSS 2.0" 1 "TSS 2.5 or 2022 RAV4" 0 "TSS 2.0 on Alphard, Highlander, NX"; +VAL_ 467 PCM_FOLLOW_DISTANCE 1 "far" 2 "medium" 3 "close"; +VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; +VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled"; +VAL_ 610 LTA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 3 "lta_missing_unavailable" 1 "standby"; +VAL_ 610 LKA_STATE 25 "temporary_fault" 17 "permanent_fault" 11 "lka_missing_unavailable2" 9 "temporary_fault2" 5 "active" 3 "lka_missing_unavailable" 1 "standby"; +VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press"; +VAL_ 835 ACC_MALFUNCTION 1 "faulted" 0 "ok"; +VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok"; +VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear"; +VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted"; +VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled"; +VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off"; +VAL_ 1042 BARRIERS 3 "left" 2 "right" 1 "both" 0 "none"; +VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none"; +VAL_ 1042 LEFT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none"; +VAL_ 1042 LKAS_STATUS 1 "on" 0 "off"; +VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none"; +VAL_ 1042 LDA_SENSITIVITY 2 "standard" 1 "high" 0 "undefined"; +VAL_ 1042 LDA_SA_TOGGLE 2 "steering assist off" 1 "steering assist on"; +VAL_ 1042 LDA_MESSAGES 4 "lda unavailable at this speed" 1 "lda unavailable below approx 50km/h" 0 "ok"; +VAL_ 1042 LDA_ON_MESSAGE 2 "Lane Departure Alert Turned ON, Steering Assist Inactive" 1 "Lane Departure Alert Turned ON, Steering Assist Active" 0 "clear"; +VAL_ 1042 TAKE_CONTROL 1 "take control" 0 "ok"; +VAL_ 1042 LDA_FRONT_CAMERA_BLOCKED 1 "lda unavailable" 0 "ok"; +VAL_ 1042 LANE_SWAY_BUZZER 3 "ok" 2 "beep twice" 1 "beep twice" 0 "ok"; +VAL_ 1042 LANE_SWAY_WARNING 3 "ok" 2 "orange please take a break" 1 "prompt would you like to take a break" 0 "ok"; +VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 81 "no right turn" 97 "stop" 105 "yield" 113 "stop" 114 "yield us" 129 "no entry" 138 "no entry tss2" 145 "do not enter"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1552 METER_SLIDER_LOW_BRIGHTNESS 1 "Low brightness mode, footwell lights off" 0 "Normal mode, footwell lights on"; +VAL_ 1552 METER_SLIDER_DIMMED 1 "Dimmed" 0 "Not Dimmed"; +VAL_ 1552 UNITS 1 "km (km/L)" 2 "km (L/100km)" 3 "miles (MPG US)" 4 "miles (MPG Imperial)"; +VAL_ 1553 UNITS 1 "km" 2 "miles"; +VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; +VAL_ 1592 LOCK_STATUS 0 "locked" 1 "unlocked"; diff --git a/opendbc/toyota_tnga_k_pt_generated.dbc b/opendbc/toyota_tnga_k_pt_generated.dbc index 14c1d7f40f9d7e..7f99d387326126 100644 --- a/opendbc/toyota_tnga_k_pt_generated.dbc +++ b/opendbc/toyota_tnga_k_pt_generated.dbc @@ -197,7 +197,11 @@ BO_ 643 PRE_COLLISION: 7 DSU BO_ 705 GAS_PEDAL: 8 XXX SG_ GAS_RELEASED : 3|1@0+ (1,0) [0|1] "" XXX - SG_ GAS_PEDAL : 55|8@0+ (0.005,0) [0|1] "" XXX + SG_ ETQLVSC : 15|16@0- (0.03125,0) [0|0] "Nm" XXX + SG_ ETQREAL : 31|16@0- (0.03125,0) [0|0] "Nm" SCS + SG_ ETQISC : 47|8@0+ (1,-192) [0|0] "Nm" XXX + SG_ GAS_PEDAL : 55|8@0+ (0.5,0) [0|0] "%" DS1,FCM + SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" DS1,FCM BO_ 740 STEERING_LKA: 5 XXX SG_ LKA_STATE : 31|8@0+ (1,0) [0|255] "" XXX diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index a849a87e1dc87d..faa86fba8babb9 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -17,7 +17,7 @@ .has_steer_req_tolerance = true, \ } -const SteeringLimits HYUNDAI_STEERING_LIMITS = HYUNDAI_LIMITS(384, 3, 7); +const SteeringLimits HYUNDAI_STEERING_LIMITS = HYUNDAI_LIMITS(500, 4, 7); #up from 384, 3, 7 const SteeringLimits HYUNDAI_STEERING_LIMITS_ALT = HYUNDAI_LIMITS(270, 2, 3); const LongitudinalLimits HYUNDAI_LONG_LIMITS = { diff --git a/panda/board/safety/safety_hyundai_canfd.h b/panda/board/safety/safety_hyundai_canfd.h index fb6ccf55a0db41..d9c799be24266b 100644 --- a/panda/board/safety/safety_hyundai_canfd.h +++ b/panda/board/safety/safety_hyundai_canfd.h @@ -1,7 +1,7 @@ #include "safety_hyundai_common.h" const SteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = { - .max_steer = 270, + .max_steer = 384, .max_rt_delta = 112, .max_rt_interval = 250000, .max_rate_up = 2, diff --git a/selfdrive/SConscript b/selfdrive/SConscript index 43296d6fdab16b..52898f758f2ae7 100644 --- a/selfdrive/SConscript +++ b/selfdrive/SConscript @@ -4,4 +4,4 @@ SConscript(['controls/lib/longitudinal_mpc_lib/SConscript']) SConscript(['locationd/SConscript']) SConscript(['navd/SConscript']) SConscript(['modeld/SConscript']) -SConscript(['ui/SConscript']) +SConscript(['ui/SConscript']) \ No newline at end of file diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 30acfd4a1c0a36..c5e265496d36df 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -7,7 +7,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.notCar = True ret.carName = "body" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index d3f9120e00cffd..c9f30f6e3de616 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -14,18 +14,14 @@ import cereal.messaging as messaging import openpilot.system.sentry as sentry from openpilot.selfdrive.car import gen_empty_fingerprint -from openpilot.system.version import get_build_metadata FRAME_FINGERPRINT = 100 # 1s EventName = car.CarEvent.EventName -def get_startup_event(car_recognized, controller_available, fw_seen, block_user, frogpilot_toggles): - if block_user: - return EventName.blockUser - else: - event = EventName.customStartupAlert +def get_startup_event(car_recognized, controller_available, fw_seen): + event = EventName.customStartupAlert if not car_recognized: if fw_seen: @@ -191,7 +187,7 @@ def get_car_interface(CP): return CarInterface(CP, CarController, CarState) -def get_car(logcan, sendcan, disable_openpilot_long, experimental_long_allowed, params, num_pandas=1): +def get_car(logcan, sendcan, disable_openpilot_long, experimental_long_allowed, params, num_pandas=1, frogpilot_toggles=None): car_model = params.get("CarModel", encoding='utf-8') force_fingerprint = params.get_bool("ForceFingerprint") @@ -207,7 +203,7 @@ def get_car(logcan, sendcan, disable_openpilot_long, experimental_long_allowed, params.put_nonblocking("CarMake", candidate.split('_')[0].title()) params.put_nonblocking("CarModel", candidate) - if get_build_metadata().channel == "FrogPilot-Development" and params.get("DongleId", encoding='utf-8') != "FrogsGoMoo": + if frogpilot_toggles.block_user: candidate = "MOCK" threading.Thread(target=sentry.capture_fingerprint, args=(candidate, params, True,)).start() elif not params.get_bool("FingerprintLogged"): diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 863c1c3996554f..6e38b918ff59b7 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -10,13 +10,14 @@ from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL +from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.pandad import can_list_to_can_capnp from openpilot.selfdrive.car.car_helpers import get_car, get_one_can from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.controls.lib.events import Events -from openpilot.selfdrive.frogpilot.frogpilot_variables import FrogPilotVariables +from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles, update_frogpilot_toggles REPLAY = "REPLAY" in os.environ @@ -28,7 +29,7 @@ class Car: def __init__(self, CI=None): self.can_sock = messaging.sub_sock('can', timeout=20) - self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents']) + self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents', 'frogpilotPlan']) self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'frogpilotCarState']) self.can_rcv_cum_timeout_counter = 0 @@ -48,11 +49,17 @@ def __init__(self, CI=None): num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) disable_openpilot_long = self.params.get_bool("DisableOpenpilotLongitudinal") - experimental_long_allowed = not disable_openpilot_long and self.params.get_bool("ExperimentalLongitudinalEnabled") - self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], disable_openpilot_long, experimental_long_allowed, self.params, num_pandas) + experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], disable_openpilot_long, experimental_long_allowed, self.params, num_pandas, get_frogpilot_toggles()) else: self.CI, self.CP = CI, CI.CP + # set alternative experiences from parameters + self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") + self.CP.alternativeExperience = 0 + if not self.disengage_on_accelerator: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly @@ -63,6 +70,18 @@ def __init__(self, CI=None): safety_config.safetyModel = car.CarParams.SafetyModel.noOutput self.CP.safetyConfigs = [safety_config] + if self.CP.secOcRequired and not self.params.get_bool("IsReleaseBranch"): + secoc_key = self.params.get("SecOCKey", encoding='utf8') + if secoc_key is not None: + saved_secoc_key = bytes.fromhex(secoc_key.strip()) + if len(saved_secoc_key) == 16: + self.CP.secOcKeyAvailable = True + self.CI.CS.secoc_key = saved_secoc_key + if controller_available: + self.CI.CC.secoc_key = saved_secoc_key + else: + cloudlog.warning("Saved SecOC key is invalid") + # Write previous route's CarParams prev_cp = self.params.get("CarParamsPersistent") if prev_cp is not None: @@ -74,21 +93,13 @@ def __init__(self, CI=None): self.rk = Ratekeeper(100, print_delay_threshold=None) # FrogPilot variables - self.frogpilot_toggles = FrogPilotVariables.toggles - FrogPilotVariables.update_frogpilot_params() - - self.update_toggles = False + self.frogpilot_toggles = get_frogpilot_toggles() - # set alternative experiences from parameters - self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") - self.CP.alternativeExperience = 0 - if not self.disengage_on_accelerator: - self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS - - if self.params.get_bool("AlwaysOnLateral"): + if self.frogpilot_toggles.always_on_lateral: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS - if self.params.get_int("AccelerationProfile") == 3: + if self.frogpilot_toggles.acceleration_profile == 3: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX # Write CarParams for controls and radard @@ -97,6 +108,8 @@ def __init__(self, CI=None): self.params.put_nonblocking("CarParamsCache", cp_bytes) self.params.put_nonblocking("CarParamsPersistent", cp_bytes) + update_frogpilot_toggles() + def state_update(self) -> car.CarState: """carState update loop, driven by can""" @@ -199,11 +212,8 @@ def card_thread(self): self.rk.monitor_time() # Update FrogPilot parameters - if FrogPilotVariables.toggles_updated: - self.update_toggles = True - elif self.update_toggles: - FrogPilotVariables.update_frogpilot_params() - self.update_toggles = False + if self.sm['frogpilotPlan'].togglesUpdated: + self.frogpilot_toggles = get_frogpilot_toggles() def main(): config_realtime_process(4, Priority.CTRL_HIGH) diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index 56d14ebe3529dc..b5e9956c090e70 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -1,5 +1,5 @@ from cereal import car -from openpilot.selfdrive.car.chrysler.values import ChryslerFlags, RAM_CARS +from openpilot.selfdrive.car.chrysler.values import RAM_CARS, ChryslerFlags GearShifter = car.CarState.GearShifter VisualAlert = car.CarControl.HUDControl.VisualAlert diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 27e6bf801c0b08..e3e76433aa7013 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -11,7 +11,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "chrysler" # radar parsing needs some work, see https://github.com/commaai/openpilot/issues/26842 diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 4103e5f4dbc7f8..fc0dd01257c072 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -97,9 +97,9 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0: # Both gas and accel are in m/s^2, accel is used solely for braking if frogpilot_toggles.sport_plus: - accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, min(frogpilot_toggles.max_desired_accel, get_max_allowed_accel(CS.out.vEgo))) + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, get_max_allowed_accel(CS.out.vEgo))) else: - accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, min(frogpilot_toggles.max_desired_accel, CarControllerParams.ACCEL_MAX)) + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, CarControllerParams.ACCEL_MAX)) gas = accel if not CC.longActive or gas < CarControllerParams.MIN_GAS: gas = CarControllerParams.INACTIVE_GAS diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 4a4e1782514900..15ecb4bbf0b0bf 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -14,7 +14,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "ford" ret.dashcamOnly = False diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 847240bbd9a0fd..337f268523c71c 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -12,6 +12,8 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel from openpilot.selfdrive.controls.lib.drive_helpers import get_friction +from openpilot.selfdrive.frogpilot.frogpilot_variables import params + ButtonType = car.CarState.ButtonEvent.Type FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -38,7 +40,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_toggles): + def get_pid_accel_limits(CP, current_speed, cruise_speed): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX # Determined by iteratively plotting and minimizing error for f(angle, speed) = steer. @@ -94,7 +96,7 @@ def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: return self.torque_from_lateral_accel_linear @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): use_new_api = params.get_bool("NewLongAPIGM") ret.carName = "gm" diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 153a6784871d5b..9dd120ec7dcc11 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -219,9 +219,9 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): if self.CP.carFingerprint in HONDA_BOSCH: if frogpilot_toggles.sport_plus: - self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, min(frogpilot_toggles.max_desired_accel, get_max_allowed_accel(CS.out.vEgo))) + self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, get_max_allowed_accel(CS.out.vEgo))) else: - self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, min(frogpilot_toggles.max_desired_accel, self.params.BOSCH_ACCEL_MAX)) + self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, self.params.BOSCH_ACCEL_MAX)) self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V) stopping = actuators.longControlState == LongCtrlState.stopping diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 9fb34a071049ef..253e44dc259d07 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -22,7 +22,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_toggles): + def get_pid_accel_limits(CP, current_speed, cruise_speed): if CP.carFingerprint in HONDA_BOSCH: return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX elif CP.enableGasInterceptor: @@ -35,7 +35,7 @@ def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_toggles): return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "honda" CAN = CanBus(ret, fingerprint) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 92c0f5e4905bbb..ff037c116e5c20 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -82,9 +82,9 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): # accel + longitudinal if frogpilot_toggles.sport_plus: - accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, min(frogpilot_toggles.max_desired_accel, get_max_allowed_accel(CS.out.vEgo))) + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, get_max_allowed_accel(CS.out.vEgo))) else: - accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, min(frogpilot_toggles.max_desired_accel, CarControllerParams.ACCEL_MAX)) + accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, CarControllerParams.ACCEL_MAX)) stopping = actuators.longControlState == LongCtrlState.stopping set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 0de21000304a72..94f5950fff97d6 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -64,11 +64,9 @@ def calculate_speed_limit(self, cp, cp_cam): speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam return speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"] else: - if self.CP.flags & HyundaiFlags.LKAS12: - speed_limit = cp_cam.vl["LKAS12"]["CF_Lkas_TsrSpeed_Display_Clu"] - else: - speed_limit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"] - return speed_limit if speed_limit not in (0, 255) else 0 + speed_limit_nav = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"] if self.CP.flags & HyundaiFlags.NAV_MSG else 0 + speed_limit_cam = cp_cam.vl["LKAS12"]["CF_Lkas_TsrSpeed_Display_Clu"] if self.CP.flags & HyundaiFlags.LKAS12 else 0 + return speed_limit_cam if speed_limit_cam not in (0, 255) else speed_limit_nav def update(self, cp, cp_cam, frogpilot_toggles): if self.CP.carFingerprint in CANFD_CAR: diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index ebd8ce39981137..7ae1679bfd0e34 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -9,6 +9,8 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.disable_ecu import disable_ecu +from openpilot.selfdrive.frogpilot.frogpilot_variables import params + Ecu = car.CarParams.Ecu ButtonType = car.CarState.ButtonEvent.Type FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type @@ -21,7 +23,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): use_new_api = params.get_bool("NewLongAPI") ret.carName = "hyundai" @@ -163,6 +165,10 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp ret.centerToFront = ret.wheelbase * 0.4 + # Detect smartMDPS + if 0x2AA in fingerprint[0]: + ret.minSteerSpeed = 0. + return ret @staticmethod diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index df4079449344b0..59ecb9986225e3 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -26,12 +26,12 @@ def __init__(self, CP): self.STEER_STEP = 1 # 100 Hz if CP.carFingerprint in CANFD_CAR: - self.STEER_MAX = 270 + self.STEER_MAX = 384 #up from 270 self.STEER_DRIVER_ALLOWANCE = 250 self.STEER_DRIVER_MULTIPLIER = 2 self.STEER_THRESHOLD = 250 - self.STEER_DELTA_UP = 2 - self.STEER_DELTA_DOWN = 3 + self.STEER_DELTA_UP = 4 # up from 2 + self.STEER_DELTA_DOWN = 7 #up from 3 # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. @@ -48,7 +48,7 @@ def __init__(self, CP): # Default for most HKG else: - self.STEER_MAX = 384 + self.STEER_MAX = 500 #up from 384 class HyundaiFlags(IntFlag): diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 309194f70a0505..8ab60d786ecbed 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -14,7 +14,6 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.simple_kalman import KF1D, get_kalman_gain from openpilot.common.numpy_fast import clip -from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG from openpilot.selfdrive.car.values import PLATFORMS @@ -22,6 +21,8 @@ from openpilot.selfdrive.controls.lib.events import Events from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel +from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles, params, params_memory + ButtonType = car.CarState.ButtonEvent.Type FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter @@ -223,23 +224,20 @@ def __init__(self, CP, CarController, CarState): self.CC: CarControllerBase = CarController(dbc_name, CP, self.VM) # FrogPilot variables - self.params = Params() - self.params_memory = Params("/dev/shm/params") + self.frogpilot_toggles = get_frogpilot_toggles() eps_firmware = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), "")) comma_nnff_supported = self.check_comma_nn_ff_support(CP.carFingerprint) nnff_supported = self.initialize_lat_torque_nn(CP.carFingerprint, eps_firmware) - lateral_tune = self.params.get_bool("LateralTune") - self.use_nnff = not comma_nnff_supported and nnff_supported and lateral_tune and self.params.get_bool("NNFF") - self.use_nnff_lite = not self.use_nnff and lateral_tune and self.params.get_bool("NNFFLite") + self.use_nnff = not comma_nnff_supported and nnff_supported and self.frogpilot_toggles.nnff + self.use_nnff_lite = not self.use_nnff and self.frogpilot_toggles.nnff_lite self.always_on_lateral_disabled = False self.belowSteerSpeed_shown = False self.disable_belowSteerSpeed = False self.disable_resumeRequired = False - self.is_gm = self.CP.carName == "gm" self.prev_distance_button = False self.resumeRequired_shown = False self.traffic_mode_active = False @@ -247,6 +245,8 @@ def __init__(self, CP, CarController, CarState): self.gap_counter = 0 + self.is_gm = self.CP.carName == "gm" + def get_ff_nn(self, x): return self.lat_torque_nn_model.evaluate(x) @@ -263,7 +263,7 @@ def apply(self, c: car.CarControl, now_nanos: int, frogpilot_toggles) -> tuple[c return self.CC.update(c, self.CS, now_nanos, frogpilot_toggles) @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_toggles): + def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX @classmethod @@ -274,7 +274,7 @@ def get_non_essential_params(cls, candidate: str): return cls.get_params(candidate, gen_empty_fingerprint(), list(), False, False, False) @classmethod - def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], disable_openpilot_long: bool, experimental_long: bool, params: Params, docs: bool): + def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], disable_openpilot_long: bool, experimental_long: bool, params: params, docs: bool): ret = CarInterfaceBase.get_std_params(candidate) platform = PLATFORMS[candidate] @@ -287,7 +287,7 @@ def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_ ret.tireStiffnessFactor = platform.config.specs.tireStiffnessFactor ret.flags |= int(platform.config.flags) - ret = cls._get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params) + ret = cls._get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs) # Enable torque controller for all cars that do not use angle based steering if ret.steerControlType != car.CarParams.SteerControlType.angle and params.get_bool("LateralTune") and params.get_bool("NNFF"): @@ -509,7 +509,7 @@ def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_ return events def frogpilot_distance_functions(self, frogpilot_toggles): - distance_button = self.CS.distance_button or self.params_memory.get_bool("OnroadDistanceButtonPressed") + distance_button = self.CS.distance_button or params_memory.get_bool("OnroadDistanceButtonPressed") if distance_button: self.gap_counter += 1 @@ -518,12 +518,12 @@ def frogpilot_distance_functions(self, frogpilot_toggles): if self.gap_counter == CRUISE_LONG_PRESS * (1.5 if self.is_gm else 1) and frogpilot_toggles.experimental_mode_via_distance or self.traffic_mode_changed: if frogpilot_toggles.conditional_experimental_mode: - conditional_status = self.params_memory.get_int("CEStatus") + conditional_status = params_memory.get_int("CEStatus") override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 1 if conditional_status >= 7 else 2 - self.params_memory.put_int("CEStatus", override_value) + params_memory.put_int("CEStatus", override_value) else: - experimental_mode = self.params.get_bool("ExperimentalMode") - self.params.put_bool("ExperimentalMode", not experimental_mode) + experimental_mode = params.get_bool("ExperimentalMode") + params.put_bool("ExperimentalMode", not experimental_mode) self.traffic_mode_changed = False if self.gap_counter == CRUISE_LONG_PRESS * 5: diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 2a27229948af69..9f46274b74d83d 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -12,7 +12,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "mazda" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarUnavailable = True diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index bb602374248435..414bcd4553921d 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -12,7 +12,7 @@ def __init__(self, CP, CarController, CarState): self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']) @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "mock" ret.mass = 1700. ret.wheelbase = 2.70 diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 39af7cfbab81f3..e1addf0e22a04c 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -11,7 +11,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "nissan" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] ret.autoResumeSng = False diff --git a/selfdrive/car/secoc.py b/selfdrive/car/secoc.py new file mode 100644 index 00000000000000..971ea36a192a90 --- /dev/null +++ b/selfdrive/car/secoc.py @@ -0,0 +1,47 @@ +import struct + +from Crypto.Hash import CMAC +from Crypto.Cipher import AES + +def add_mac(key, trip_cnt, reset_cnt, msg_cnt, msg): + # TODO: clean up conversion to and from hex + + addr, payload, bus = msg + reset_flag = reset_cnt & 0b11 + msg_cnt_flag = msg_cnt & 0b11 + payload = payload[:4] + + # Step 1: Build Freshness Value (48 bits) + # [Trip Counter (16 bit)][[Reset Counter (20 bit)][Message Counter (8 bit)][Reset Flag (2 bit)][Padding (2 bit)] + freshness_value = struct.pack('>HI', trip_cnt, (reset_cnt << 12) | ((msg_cnt & 0xff) << 4) | (reset_flag << 2)) + + # Step 2: Build data to authenticate (96 bits) + # [Message ID (16 bits)][Payload (32 bits)][Freshness Value (48 bits)] + to_auth = struct.pack('>H', addr) + payload + freshness_value + + # Step 3: Calculate CMAC (28 bit) + cmac = CMAC.new(key, ciphermod=AES) + cmac.update(to_auth) + mac = cmac.digest().hex()[:7] # truncated MAC + + # Step 4: Build message + # [Payload (32 bit)][Message Counter Flag (2 bit)][Reset Flag (2 bit)][Authenticator (28 bit)] + msg_cnt_rst_flag = struct.pack('>B', (msg_cnt_flag << 2) | reset_flag).hex()[1] + msg = payload.hex() + msg_cnt_rst_flag + mac + payload = bytes.fromhex(msg) + + return (addr, payload, bus) + +def build_sync_mac(key, trip_cnt, reset_cnt, id_=0xf): + id_ = struct.pack('>H', id_) # 16 + trip_cnt = struct.pack('>H', trip_cnt) # 16 + reset_cnt = struct.pack('>I', reset_cnt << 12)[:-1] # 20 + 4 padding + + to_auth = id_ + trip_cnt + reset_cnt # SecOC 11.4.1.1 page 138 + + cmac = CMAC.new(key, ciphermod=AES) + cmac.update(to_auth) + + msg = "0" + cmac.digest().hex()[:7] + msg = bytes.fromhex(msg) + return struct.unpack('>I', msg)[0] diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index ede2980fcaf9a2..2e05a763c9c738 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -10,7 +10,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate: CAR, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate: CAR, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): # FrogPilot variables crosstrek_torque_increase = params.get_bool("CrosstrekTorque") diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index f2d0544a46519f..64d8182963f9bf 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -3,11 +3,12 @@ from cereal import car from panda.python import uds -from openpilot.common.params import Params from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Tool, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 +from openpilot.selfdrive.frogpilot.frogpilot_variables import params + Ecu = car.CarParams.Ecu @@ -26,7 +27,7 @@ def __init__(self, CP): self.STEER_DELTA_DOWN = 40 elif CP.carFingerprint == CAR.SUBARU_IMPREZA_2020: self.STEER_MAX = 1439 - elif CP.carFingerprint == CAR.SUBARU_IMPREZA and Params().get_bool("CrosstrekTorque"): + elif CP.carFingerprint == CAR.SUBARU_IMPREZA and params.get_bool("CrosstrekTorque"): self.STEER_MAX = 3071 else: self.STEER_MAX = 2047 diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 04c36b7f4df4fa..0c8a1d69c81082 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -8,7 +8,7 @@ class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "tesla" # There is no safe way to do steer blending with user torque, diff --git a/selfdrive/car/torque_data/substitute.toml b/selfdrive/car/torque_data/substitute.toml index 08c5783f98c726..1525bf11411fd6 100644 --- a/selfdrive/car/torque_data/substitute.toml +++ b/selfdrive/car/torque_data/substitute.toml @@ -9,6 +9,8 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "TOYOTA_ALPHARD_TSS2" = "TOYOTA_SIENNA" "TOYOTA_PRIUS_V" = "TOYOTA_PRIUS" +"TOYOTA_RAV4_PRIME" = "TOYOTA_RAV4_TSS2" +"TOYOTA_SIENNA_4TH_GEN" = "TOYOTA_RAV4_TSS2" "LEXUS_IS" = "LEXUS_NX" "LEXUS_CTH" = "LEXUS_NX" "LEXUS_ES" = "TOYOTA_CAMRY" diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 032860e6ffa42e..4c56efaed430b9 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -2,13 +2,13 @@ from cereal import car from openpilot.common.numpy_fast import clip, interp -from openpilot.common.params import Params from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, create_gas_interceptor_command, make_can_msg, rate_limit from openpilot.selfdrive.car.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 opendbc.can.packer import CANPacker from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_acceleration import get_max_allowed_accel @@ -54,15 +54,17 @@ def __init__(self, dbc_name, CP, VM): self.distance_button = 0 self.pcm_accel_compensation = 0.0 - self.permit_braking = 0.0 + self.permit_braking = True self.packer = CANPacker(dbc_name) self.accel = 0 # FrogPilot variables - params = Params() - self.doors_locked = False + self.reverse_cruise_active = False + + self.cruise_timer = 0 + self.previous_set_speed = 0 def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators @@ -141,10 +143,13 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): 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 - # TODO: sometimes when switching from brake to gas quickly, CLUTCH->ACCEL_NET shows a slow unwind. make it go to 0 immediately - if (self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT or self.CP.flags & ToyotaFlags.NEW_TOYOTA_TUNE) and CC.longActive and not CS.out.cruiseState.standstill: + 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 - accel_due_to_pitch = math.sin(CS.slope_angle) * ACCELERATION_DUE_TO_GRAVITY + 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 @@ -171,9 +176,9 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): 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_accel, get_max_allowed_accel(CS.out.vEgo))) + 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_accel, self.params.ACCEL_MAX)) + pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, self.params.ACCEL_MAX)) # on entering standstill, send standstill request if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor): @@ -208,10 +213,10 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): pcm_accel_cmd = min(pcm_accel_cmd, self.accel + ACCEL_WINDUP_LIMIT) if CC.longActive else 0.0 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, frogpilot_toggles)) + 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, frogpilot_toggles)) + 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. @@ -255,6 +260,14 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): new_actuators.steeringAngleDeg = self.last_angle new_actuators.accel = self.accel + # FrogPilot Toyota carcontroller functions + if False: #self.previous_set_speed != CS.out.cruiseState.speedCluster: + self.cruise_timer = CRUISE_LONG_PRESS + elif self.cruise_timer > 0: + self.cruise_timer -= 1 + else: + self.previous_set_speed = CS.out.cruiseState.speedCluster + # Lock doors when in drive / unlock doors when in park if not self.doors_locked and CS.out.gearShifter != PARK: if frogpilot_toggles.lock_doors: @@ -265,5 +278,8 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): can_sends.append(make_can_msg(0x750, UNLOCK_CMD, 0)) self.doors_locked = False + self.reverse_cruise_active = frogpilot_toggles.reverse_cruise_increase + self.reverse_cruise_active &= self.cruise_timer <= 0 + self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 75130c2fd9b039..55a8dedbb4f42a 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -24,21 +24,22 @@ # - prolonged high driver torque: 17 (permanent) PERM_STEER_FAULTS = (3, 17) + # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! @staticmethod def calculate_speed_limit(cp_cam, frogpilot_toggles): signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] traffic_signals = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} - tsgn1 = traffic_signals.get("TSGN1", None) - spdval1 = traffic_signals.get("SPDVAL1", None) + tsgn1 = traffic_signals.get("TSGN1") + spdval1 = traffic_signals.get("SPDVAL1") if tsgn1 == 1 and not frogpilot_toggles.force_mph_dashboard: return spdval1 * CV.KPH_TO_MS elif tsgn1 == 36 or frogpilot_toggles.force_mph_dashboard: return spdval1 * CV.MPH_TO_MS - else: - return 0 + return 0 + class CarState(CarStateBase): def __init__(self, CP): @@ -64,7 +65,6 @@ def __init__(self, CP): self.acc_type = 1 self.lkas_hud = {} self.pcm_accel_net = 0.0 - self.slope_angle = 0.0 # FrogPilot variables self.latActive_previous = False @@ -80,11 +80,8 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): # Describes the acceleration request from the PCM if on flat ground, may be higher or lower if pitched # CLUTCH->ACCEL_NET is only accurate for gas, PCM_CRUISE->ACCEL_NET is only accurate for brake # These signals only have meaning when ACC is active - if self.CP.flags & ToyotaFlags.NEW_TOYOTA_TUNE or self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: - if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: - self.pcm_accel_net = max(cp.vl["CLUTCH"]["ACCEL_NET"], 0.0) - else: - self.pcm_accel_net = max(cp.vl["PCM_CRUISE"]["ACCEL_NET"], 0.0) + if 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"]: @@ -273,7 +270,6 @@ def get_can_parser(CP): ("BODY_CONTROL_STATE", 3), ("BODY_CONTROL_STATE_2", 2), ("ESP_CONTROL", 3), - ("VSC1S07", 20), ("EPS_STATUS", 25), ("BRAKE_MODULE", 40), ("WHEEL_SPEEDS", 80), diff --git a/selfdrive/car/toyota/fingerprints.py b/selfdrive/car/toyota/fingerprints.py index 292be96fc85e73..b8942cd59d3df9 100644 --- a/selfdrive/car/toyota/fingerprints.py +++ b/selfdrive/car/toyota/fingerprints.py @@ -127,6 +127,7 @@ b'\x018966333X0000\x00\x00\x00\x00', b'\x018966333X4000\x00\x00\x00\x00', b'\x01896633T16000\x00\x00\x00\x00', + b'\x01896633TA2000\x00\x00\x00\x00', b'\x028966306B2100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', b'\x028966306B2300\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', b'\x028966306B2500\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', @@ -321,6 +322,7 @@ b'F1526F4073\x00\x00\x00\x00\x00\x00', b'F1526F4121\x00\x00\x00\x00\x00\x00', b'F1526F4122\x00\x00\x00\x00\x00\x00', + b'F1526F4190\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B10011\x00\x00\x00\x00\x00\x00', @@ -337,6 +339,7 @@ b'\x033F401200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', + b'\x033F435000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F0W01000 ', @@ -391,6 +394,7 @@ (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646FF410200\x00\x00\x00\x008646GF408200\x00\x00\x00\x00', b'\x028646FF411100\x00\x00\x00\x008646GF409000\x00\x00\x00\x00', + b'\x028646FF413000\x00\x00\x00\x008646GF411000\x00\x00\x00\x00', b'\x028646FF413100\x00\x00\x00\x008646GF411100\x00\x00\x00\x00', ], }, @@ -431,6 +435,7 @@ CAR.TOYOTA_COROLLA_TSS2: { (Ecu.engine, 0x700, None): [ b'\x01896630A22000\x00\x00\x00\x00', + b'\x01896630A42000\x00\x00\x00\x00', b'\x01896630ZG2000\x00\x00\x00\x00', b'\x01896630ZG5000\x00\x00\x00\x00', b'\x01896630ZG5100\x00\x00\x00\x00', @@ -525,6 +530,7 @@ b'8965B16011\x00\x00\x00\x00\x00\x00', b'8965B16101\x00\x00\x00\x00\x00\x00', b'8965B16170\x00\x00\x00\x00\x00\x00', + b'8965B16260\x00\x00\x00\x00\x00\x00', b'8965B76012\x00\x00\x00\x00\x00\x00', b'8965B76050\x00\x00\x00\x00\x00\x00', b'8965B76091\x00\x00\x00\x00\x00\x00', @@ -539,6 +545,7 @@ b'\x01F15260A010\x00\x00\x00\x00\x00\x00', b'\x01F15260A050\x00\x00\x00\x00\x00\x00', b'\x01F15260A070\x00\x00\x00\x00\x00\x00', + b'\x01F15260A33000\x00\x00\x00\x00', b'\x01F152612641\x00\x00\x00\x00\x00\x00', b'\x01F152612651\x00\x00\x00\x00\x00\x00', b'\x01F152612862\x00\x00\x00\x00\x00\x00', @@ -598,6 +605,7 @@ b'\x028646F1601100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F1601200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F1601300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F1601500\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F76020C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F7603100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', @@ -716,11 +724,13 @@ b'\x01896630EE7000\x00\x00\x00\x00', b'\x01896630EF8000\x00\x00\x00\x00', b'\x01896630EG3000\x00\x00\x00\x00', + b'\x01896630EG3100\x00\x00\x00\x00', b'\x01896630EG5000\x00\x00\x00\x00', b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630E66100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896630EB3100\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896630EB3200\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', @@ -1005,6 +1015,33 @@ b'8646F4204000\x00\x00\x00\x00', ], }, + CAR.TOYOTA_RAV4_PRIME: { + (Ecu.engine, 0x700, None): [ + b'\x018966342S7000\x00\x00\x00\x00', + b'\x018966342Z1000\x00\x00\x00\x00', + b'\x018966342Z1100\x00\x00\x00\x00', + b'\x01896634AJ7000\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'\x01F15264228300\x00\x00\x00\x00', + b'\x01F15264228500\x00\x00\x00\x00', + b'\x01F15264284100\x00\x00\x00\x00', + b'\x01F152642F3000\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'\x018965B4209000\x00\x00\x00\x00', + b'\x018965B4233100\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301400\x00\x00\x00\x00', + b'\x018821F6201400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4205200\x00\x00\x00\x008646G4202000\x00\x00\x00\x00', + b'\x028646F4205300\x00\x00\x00\x008646G4202100\x00\x00\x00\x00', + b'\x028646F4210100\x00\x00\x00\x008646G3305000\x00\x00\x00\x00', + ], + }, CAR.TOYOTA_RAV4_TSS2: { (Ecu.engine, 0x700, None): [ b'\x01896630R58000\x00\x00\x00\x00', @@ -1187,6 +1224,7 @@ (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F0R05100\x00\x00\x00\x008646G0R02100\x00\x00\x00\x00', b'\x028646F0R05200\x00\x00\x00\x008646G0R02200\x00\x00\x00\x00', + b'\x028646F0R05300\x00\x00\x00\x008646G0R02300\x00\x00\x00\x00', b'\x028646F0R11000\x00\x00\x00\x008646G0R04000\x00\x00\x00\x00', ], }, @@ -1226,21 +1264,46 @@ b'8646F0801100\x00\x00\x00\x00', ], }, + CAR.TOYOTA_SIENNA_4TH_GEN: { + (Ecu.engine, 0x700, None): [ + b'\x01896630841000\x00\x00\x00\x00', + b'\x01896630857101\x00\x00\x00\x00', + b'\x01896630864000\x00\x00\x00\x00', + ], + (Ecu.abs, 0x7b0, None): [ + b'\x01F15260815100\x00\x00\x00\x00', + b'\x01F15260815300\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'\x018965B4509100\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301500\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F0802200\x00\x00\x00\x008646G4202100\x00\x00\x00\x00', + b'\x028646F0802300\x00\x00\x00\x008646G4202100\x00\x00\x00\x00', + b'\x028646F0802400\x00\x00\x00\x008646G4202100\x00\x00\x00\x00', + ], + }, CAR.LEXUS_CTH: { (Ecu.dsu, 0x791, None): [ b'881517601100\x00\x00\x00\x00', + b'881517602000\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'F152676144\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\x0237635000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0237641000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F4702300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'8646F7601100\x00\x00\x00\x00', + b'8646F7601200\x00\x00\x00\x00', ], }, CAR.LEXUS_ES_TSS2: { @@ -1252,6 +1315,7 @@ b'\x01896633T07000\x00\x00\x00\x00', b'\x01896633T38000\x00\x00\x00\x00', b'\x01896633T58000\x00\x00\x00\x00', + b'\x01896633T63000\x00\x00\x00\x00', b'\x028966333S8000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', b'\x028966333S8000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x028966333T0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', @@ -1275,6 +1339,7 @@ b'8965B33252\x00\x00\x00\x00\x00\x00', b'8965B33590\x00\x00\x00\x00\x00\x00', b'8965B33690\x00\x00\x00\x00\x00\x00', + b'8965B33702\x00\x00\x00\x00\x00\x00', b'8965B33721\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ @@ -1296,6 +1361,7 @@ b'\x028646F3304300\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', b'\x028646F3309100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', b'\x028646F3309100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + b'\x028646F3309400\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, CAR.LEXUS_ES: { @@ -1644,6 +1710,7 @@ b'8965B47070\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301300\x00\x00\x00\x00', b'\x018821F3301400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 21c8670093be48..99d58b2280107d 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -7,20 +7,20 @@ from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase +from openpilot.selfdrive.frogpilot.frogpilot_variables import params + ButtonType = car.CarState.ButtonEvent.Type FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type EventName = car.CarEvent.EventName SteerControlType = car.CarParams.SteerControlType - class CarInterface(CarInterfaceBase): @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_toggles): - CCP = CarControllerParams(CP) - return CCP.ACCEL_MIN, CCP.ACCEL_MAX + def get_pid_accel_limits(CP, current_speed, cruise_speed): + return CarControllerParams(CP).ACCEL_MIN, CarControllerParams(CP).ACCEL_MAX @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "toyota" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)] ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] @@ -60,9 +60,6 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp if candidate == CAR.LEXUS_ES_TSS2 and Ecu.hybrid not in found_ecus: ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value - if params.get_bool("NewToyotaTune"): - ret.flags |= ToyotaFlags.NEW_TOYOTA_TUNE.value - if candidate == CAR.TOYOTA_PRIUS: # Only give steer angle deadzone to for bad angle sensor prius for fw in car_fw: @@ -140,7 +137,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp ret.minEnableSpeed = -1. if (candidate in STOP_AND_GO_CAR or ret.enableGasInterceptor) else MIN_ACC_SPEED tune = ret.longitudinalTuning - if candidate in TSS2_CAR or ret.flags & ToyotaFlags.NEW_TOYOTA_TUNE: + if candidate in TSS2_CAR: tune.kpV = [0.0] tune.kiV = [0.5] ret.vEgoStopping = 0.25 @@ -149,16 +146,13 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp # Since we compensate for imprecise acceleration in carcontroller, we can be less aggressive with tuning # This also prevents unnecessary request windup due to internal car jerk limits - if ret.flags & ToyotaFlags.NEW_TOYOTA_TUNE or ret.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: + 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] if params.get_bool("FrogsGoMoosTweak"): - if ret.flags & ToyotaFlags.NEW_TOYOTA_TUNE or ret.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: - tune.kiV = [0.3] - ret.stoppingDecelRate = 0.1 # reach stopping target smoothly ret.vEgoStopping = 0.15 ret.vEgoStarting = 0.15 diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 8b183fa50f16b2..fd3887efab3f49 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -33,7 +33,7 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, return packer.make_can_msg("STEERING_LTA", 0, values) -def create_accel_command(packer, accel, pcm_cancel, permit_braking, standstill_req, lead, acc_type, fcw_alert, distance, frogpilot_toggles): +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 @@ -43,7 +43,7 @@ def create_accel_command(packer, accel, pcm_cancel, permit_braking, standstill_r "PERMIT_BRAKING": permit_braking, "RELEASE_STANDSTILL": not standstill_req, "CANCEL_REQ": pcm_cancel, - "ALLOW_LONG_PRESS": 2 if frogpilot_toggles.reverse_cruise_increase else 1, + "ALLOW_LONG_PRESS": 2 if reverse_cruise_active else 1, "ACC_CUT_IN": fcw_alert, # only shown when ACC enabled } return packer.make_can_msg("ACC_CONTROL", 0, values) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 96bae1420bd5f6..1ae1effd05e238 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -36,7 +36,7 @@ def __init__(self, CP): self.ACCEL_MAX = 1.5 # m/s2, lower than allowed 2.0 m/s^2 for tuning reasons self.ACCEL_MIN = -3.5 # m/s2 - if CP.lateralTuning.which == 'torque': + if CP.lateralTuning.which() == 'torque': self.STEER_DELTA_UP = 15 # 1.0s time to peak torque self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) else: @@ -47,9 +47,7 @@ def __init__(self, CP): class ToyotaFlags(IntFlag): # Detected flags HYBRID = 1 - SMART_DSU = 2 DISABLE_RADAR = 4 - RADAR_CAN_FILTER = 2048 # Static flags TSS2 = 8 @@ -59,14 +57,16 @@ class ToyotaFlags(IntFlag): # these cars use the Lane Tracing Assist (LTA) message for lateral control ANGLE_CONTROL = 128 NO_STOP_TIMER = 256 - # these cars are speculated to allow stop and go when the DSU is unplugged or disabled with sDSU + # these cars are speculated to allow stop and go when the DSU is unplugged SNG_WITHOUT_DSU = 512 # these cars can utilize 2.0 m/s^2 RAISED_ACCEL_LIMIT = 1024 + SECOC = 2048 # FrogPilot Toyota flags - NEW_TOYOTA_TUNE = 4096 - ZSS = 8192 + RADAR_CAN_FILTER = 4096 + SMART_DSU = 8192 + ZSS = 16384 class Footnote(Enum): CAMRY = CarFootnote( @@ -254,6 +254,14 @@ class CAR(Platforms): TOYOTA_RAV4_TSS2.specs, flags=ToyotaFlags.RADAR_ACC | ToyotaFlags.ANGLE_CONTROL, ) + TOYOTA_RAV4_PRIME = PlatformConfig( + # TODO: Enable this docs entry when it can be suppressed from openpilot CARS.md + # [ToyotaCarDocs("Toyota RAV4 Prime 2021-23", min_enable_speed=MIN_ACC_SPEED)], + [], + CarSpecs(mass=4372. * CV.LB_TO_KG, wheelbase=2.68, steerRatio=16.88, tireStiffnessFactor=0.5533), + dbc_dict('toyota_rav4_prime_generated', 'toyota_tss2_adas'), + flags=ToyotaFlags.TSS2 | ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.NO_DSU | ToyotaFlags.SECOC, + ) TOYOTA_MIRAI = ToyotaTSS2PlatformConfig( # TSS 2.5 [ToyotaCarDocs("Toyota Mirai 2021")], CarSpecs(mass=4300. * CV.LB_TO_KG, wheelbase=2.91, steerRatio=14.8, tireStiffnessFactor=0.8), @@ -264,6 +272,14 @@ class CAR(Platforms): dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), flags=ToyotaFlags.NO_STOP_TIMER, ) + TOYOTA_SIENNA_4TH_GEN = PlatformConfig( + # TODO: Enable this docs entry when it can be suppressed from openpilot CARS.md + # [ToyotaCarDocs("Toyota Sienna 2021-23", min_enable_speed=MIN_ACC_SPEED)], + [], + CarSpecs(mass=4625. * CV.LB_TO_KG, wheelbase=3.06, steerRatio=17.8, tireStiffnessFactor=0.444), + dbc_dict('toyota_rav4_prime_generated', 'toyota_tss2_adas'), + flags=ToyotaFlags.TSS2 | ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.NO_DSU | ToyotaFlags.SECOC, + ) # Lexus LEXUS_CTH = PlatformConfig( @@ -547,6 +563,7 @@ def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str # Hybrid control computer can be on 0x7e2 (KWP) or 0x7d2 (UDS) depending on platform (Ecu.hybrid, 0x7e2, None), # Hybrid Control Assembly & Computer + (Ecu.hybrid, 0x7d2, None), # Hybrid Control Assembly & Computer (Ecu.srs, 0x780, None), # SRS Airbag # Transmission is combined with engine on some platforms, such as TSS-P RAV4 (Ecu.transmission, 0x701, None), @@ -577,6 +594,8 @@ def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str ANGLE_CONTROL_CAR = CAR.with_flags(ToyotaFlags.ANGLE_CONTROL) +SECOC_CAR = CAR.with_flags(ToyotaFlags.SECOC) + # no resume button press required NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index e7e7ee1f77dca4..802a0ce4eea009 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -83,9 +83,9 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl: acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) if frogpilot_toggles.sport_plus: - accel = clip(actuators.accel, self.CCP.ACCEL_MIN, min(frogpilot_toggles.max_desired_accel, get_max_allowed_accel(CS.out.vEgo))) if CC.longActive else 0 + accel = clip(actuators.accel, self.CCP.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, get_max_allowed_accel(CS.out.vEgo))) if CC.longActive else 0 else: - accel = clip(actuators.accel, self.CCP.ACCEL_MIN, min(frogpilot_toggles.max_desired_accel, self.CCP.ACCEL_MAX)) if CC.longActive else 0 + accel = clip(actuators.accel, self.CCP.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, self.CCP.ACCEL_MAX)) if CC.longActive else 0 stopping = actuators.longControlState == LongCtrlState.stopping starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < self.CP.vEgoStopping) can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, CS.acc_type, CC.longActive, accel, diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index fd41a402e85bb8..ce35ac84078485 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -20,7 +20,7 @@ def __init__(self, CP, CarController, CarState): self.cp_ext = self.cp_cam @staticmethod - def _get_params(ret, candidate: CAR, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + def _get_params(ret, candidate: CAR, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): ret.carName = "volkswagen" ret.radarUnavailable = True diff --git a/selfdrive/classic_modeld/classic_modeld.py b/selfdrive/classic_modeld/classic_modeld.py index 06c7cf8112c26a..07e05dcffee49b 100755 --- a/selfdrive/classic_modeld/classic_modeld.py +++ b/selfdrive/classic_modeld/classic_modeld.py @@ -25,9 +25,7 @@ from openpilot.selfdrive.classic_modeld.constants import ModelConstants from openpilot.selfdrive.classic_modeld.models.commonmodel_pyx import ModelFrame, CLContext -from openpilot.selfdrive.frogpilot.assets.model_manager import DEFAULT_MODEL -from openpilot.selfdrive.frogpilot.frogpilot_functions import MODELS_PATH -from openpilot.selfdrive.frogpilot.frogpilot_variables import FrogPilotVariables +from openpilot.selfdrive.frogpilot.frogpilot_variables import DEFAULT_CLASSIC_MODEL, MODELS_PATH, get_frogpilot_toggles PROCESS_NAME = "selfdrive.classic_modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -61,7 +59,7 @@ def __init__(self, context: CLContext, frogpilot_toggles: SimpleNamespace): self.radarless = frogpilot_toggles.radarless_model model_path = Path(__file__).parent / f'{MODELS_PATH}/{frogpilot_toggles.model}.thneed' - if frogpilot_toggles.model != DEFAULT_MODEL and model_path.exists(): + if frogpilot_toggles.model != DEFAULT_CLASSIC_MODEL and model_path.exists(): MODEL_PATHS[ModelRunner.THNEED] = model_path self.frame = ModelFrame(context) @@ -136,14 +134,11 @@ def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_ def main(demo=False): # FrogPilot variables - frogpilot_toggles = FrogPilotVariables.toggles - FrogPilotVariables.update_frogpilot_params() + frogpilot_toggles = get_frogpilot_toggles() enable_navigation = not frogpilot_toggles.navigationless_model radarless = frogpilot_toggles.radarless_model - update_toggles = False - cloudlog.warning("classic_modeld init") sentry.set_tag("daemon", PROCESS_NAME) @@ -347,11 +342,8 @@ def main(demo=False): last_vipc_frame_id = meta_main.frame_id # Update FrogPilot parameters - if FrogPilotVariables.toggles_updated: - update_toggles = True - elif update_toggles: - FrogPilotVariables.update_frogpilot_params() - update_toggles = False + if sm['frogpilotPlan'].togglesUpdated: + frogpilot_toggles = get_frogpilot_toggles() if __name__ == "__main__": try: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4ce4dfcb43359b..e46a7bb828b37d 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -32,7 +32,7 @@ from openpilot.system.hardware import HARDWARE from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_acceleration import get_max_allowed_accel -from openpilot.selfdrive.frogpilot.frogpilot_variables import NON_DRIVING_GEARS, FrogPilotVariables +from openpilot.selfdrive.frogpilot.frogpilot_variables import NON_DRIVING_GEARS, get_frogpilot_toggles, params_memory SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS @@ -80,7 +80,6 @@ def __init__(self, CI=None): # Ensure the current branch is cached, otherwise the first iteration of controlsd lags self.branch = get_short_branch() - self.block_user = self.branch == "FrogPilot-Development" and self.params.get("DongleId", encoding='utf-8') != "FrogsGoMoo" # Setup sockets self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents', 'frogpilotCarControl']) @@ -99,7 +98,7 @@ def __init__(self, CI=None): if REPLAY: # no vipc in replay will make them ignored anyways ignore += ['roadCameraState', 'wideRoadCameraState'] - if FrogPilotVariables.toggles.radarless_model: + if get_frogpilot_toggles().radarless_model: ignore += ['radarState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', @@ -164,7 +163,10 @@ def __init__(self, CI=None): self.can_log_mono_time = 0 - self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0, self.block_user, FrogPilotVariables.toggles) + if car_recognized and not self.CP.passive and self.CP.secOcRequired and not self.CP.secOcKeyAvailable: + self.startup_event = EventName.startupNoSecOcKey + else: + self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0) if not sounds_available: self.events.add(EventName.soundsUnavailable, static=True) @@ -181,23 +183,19 @@ def __init__(self, CI=None): self.rk = Ratekeeper(100, print_delay_threshold=None) # FrogPilot variables - self.frogpilot_toggles = FrogPilotVariables.toggles - FrogPilotVariables.update_frogpilot_params() - - self.params_memory = Params("/dev/shm/params") + self.frogpilot_toggles = get_frogpilot_toggles() self.always_on_lateral_active = False self.always_on_lateral_active_previously = False self.fcw_event_triggered = False self.no_entry_alert_triggered = False self.onroad_distance_pressed = False - self.radarless_model = self.frogpilot_toggles.radarless_model self.resume_pressed = False self.resume_previously_pressed = False self.steer_saturated_event_triggered = False - self.update_toggles = False - self.use_old_long = self.CP.carName == "hyundai" and not self.params.get_bool("NewLongAPI") - self.use_old_long |= self.CP.carName == "gm" and not self.params.get_bool("NewLongAPIGM") + + self.radarless_model = self.frogpilot_toggles.radarless_model + self.use_old_long = self.frogpilot_toggles.old_long_api self.display_timer = 0 @@ -401,7 +399,8 @@ def update_events(self, CS): planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled if (planner_fcw or model_fcw) and not (self.CP.notCar and self.joystick_mode): self.events.add(EventName.fcw) - self.fcw_event_triggered = True + if self.frogpilot_toggles.random_events: + self.fcw_event_triggered = True for m in messaging.drain_sock(self.log_sock, wait_for_one=False): try: @@ -429,8 +428,8 @@ def update_events(self, CS): # Add FrogPilot events self.events.add_from_msg(self.sm['frogpilotPlan'].frogpilotEvents) - if self.block_user: - return EventName.blockUser + if self.frogpilot_toggles.block_user: + self.events.add(EventName.blockUser, static=True) def data_sample(self): """Receive data from sockets""" @@ -547,7 +546,8 @@ def state_transition(self, CS): if self.events.contains(ET.ENABLE): if self.events.contains(ET.NO_ENTRY): self.current_alert_types.append(ET.NO_ENTRY) - self.no_entry_alert_triggered = True + if self.frogpilot_toggles.random_events: + self.no_entry_alert_triggered = True else: if self.events.contains(ET.PRE_ENABLE): @@ -571,7 +571,7 @@ def state_control(self, CS): # Update VehicleModel lp = self.sm['liveParameters'] x = max(lp.stiffnessFactor, 0.1) - sr = max(self.frogpilot_toggles.steer_ratio, 0.1) if self.frogpilot_toggles.use_custom_steer_ratio else max(lp.steerRatio, 0.1) + sr = max(self.frogpilot_toggles.steer_ratio if self.frogpilot_toggles.use_custom_steer_ratio else lp.steerRatio, 0.1) self.VM.update_params(x, sr) # Update Torque Params @@ -618,7 +618,7 @@ def state_control(self, CS): if not self.joystick_mode: # accel PID loop - pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS, self.frogpilot_toggles) + pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS) if self.frogpilot_toggles.sport_plus: pid_accel_limits = (pid_accel_limits[0], get_max_allowed_accel(CS.vEgo)) @@ -626,7 +626,7 @@ def state_control(self, CS): t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL actuators.accel = self.LoC.update_old_long(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) else: - actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits) + actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop or self.sm['frogpilotPlan'].forcingStopLength < 1, pid_accel_limits) if len(long_plan.speeds): actuators.speed = long_plan.speeds[-1] @@ -673,8 +673,9 @@ def state_control(self, CS): good_speed = CS.vEgo > 5 max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99 if undershooting and turning and good_speed and max_torque: - lac_log.active and self.events.add(EventName.goatSteerSaturated if self.frogpilot_toggles.goat_scream else EventName.steerSaturated) - self.steer_saturated_event_triggered = True + lac_log.active and self.events.add(EventName.goatSteerSaturated if self.frogpilot_toggles.goat_scream_alert else EventName.steerSaturated) + if self.frogpilot_toggles.random_events: + self.steer_saturated_event_triggered = True else: self.steer_saturated_event_triggered = False elif lac_log.saturated: @@ -708,11 +709,11 @@ def state_control(self, CS): if self.CP.openpilotLongitudinalControl: if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents) or self.onroad_distance_pressed: menu_open = self.display_timer > 0 or not self.sm['frogpilotCarState'].hasMenu - if not (self.sm['frogpilotCarState'].distanceLongPressed or self.params_memory.get_bool("OnroadDistanceButtonPressed")) and menu_open: + if not (self.sm['frogpilotCarState'].distanceLongPressed or params_memory.get_bool("OnroadDistanceButtonPressed")) and menu_open: self.personality = (self.personality - 1) % 3 self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) self.display_timer = 350 - self.onroad_distance_pressed = self.params_memory.get_bool("OnroadDistanceButtonPressed") + self.onroad_distance_pressed = params_memory.get_bool("OnroadDistanceButtonPressed") self.display_timer -= 1 @@ -722,7 +723,7 @@ def state_control(self, CS): def update_frogpilot_variables(self, CS): self.always_on_lateral_active |= self.frogpilot_toggles.always_on_lateral_main or CS.cruiseState.enabled - self.always_on_lateral_active &= self.frogpilot_toggles.always_on_lateral and CS.cruiseState.available + self.always_on_lateral_active &= self.frogpilot_toggles.always_on_lateral_set and CS.cruiseState.available self.always_on_lateral_active &= CS.gearShifter not in NON_DRIVING_GEARS self.always_on_lateral_active &= self.sm['frogpilotPlan'].lateralCheck self.always_on_lateral_active &= self.sm['liveCalibration'].calPerc >= 1 @@ -730,15 +731,15 @@ def update_frogpilot_variables(self, CS): self.always_on_lateral_active &= not (CS.brakePressed and CS.vEgo < self.frogpilot_toggles.always_on_lateral_pause_speed) or CS.standstill self.always_on_lateral_active = bool(self.always_on_lateral_active) - if self.frogpilot_toggles.conditional_experimental_mode or self.frogpilot_toggles.slc_fallback_experimental: + if self.frogpilot_toggles.conditional_experimental_mode or self.frogpilot_toggles.slc_fallback_experimental_mode: self.experimental_mode = self.sm['frogpilotPlan'].experimentalMode if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents): if self.frogpilot_toggles.experimental_mode_via_lkas and self.enabled: if self.frogpilot_toggles.conditional_experimental_mode: - conditional_status = self.params_memory.get_int("CEStatus") + conditional_status = params_memory.get_int("CEStatus") override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 3 if conditional_status >= 7 else 4 - self.params_memory.put_int("CEStatus", override_value) + params_memory.put_int("CEStatus", override_value) else: self.experimental_mode = not self.experimental_mode self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode) @@ -814,7 +815,7 @@ def publish_logs(self, CS, start_time, CC, lac_log, FPCC): if self.enabled: clear_event_types.add(ET.NO_ENTRY) - alerts = self.events.create_alerts(self.current_alert_types, [self.CP, CS, self.sm, self.is_metric, self.soft_disable_timer]) + alerts = self.events.create_alerts(self.current_alert_types, [self.CP, CS, self.sm, self.is_metric, self.soft_disable_timer, self.frogpilot_toggles]) self.AM.add_many(self.sm.frame, alerts) current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types) if current_alert: @@ -941,11 +942,8 @@ def params_thread(self, evt): time.sleep(0.1) # Update FrogPilot parameters - if FrogPilotVariables.toggles_updated: - self.update_toggles = True - elif self.update_toggles: - FrogPilotVariables.update_frogpilot_params() - self.update_toggles = False + if self.sm['frogpilotPlan'].togglesUpdated: + self.frogpilot_toggles = get_frogpilot_toggles() def controlsd_thread(self): e = threading.Event() diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 2089bb55b5b4cf..0a669e85ebb985 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -49,7 +49,7 @@ def __init__(self): # FrogPilot variables self.lane_change_completed = False - self.lane_change_wait_timer = 0.0 + self.lane_change_wait_timer = 0 self.turn_direction = TurnDirection.none @@ -68,7 +68,7 @@ def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan, frog self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.turn_direction = TurnDirection.none - elif one_blinker and below_lane_change_speed and not carstate.standstill and frogpilot_toggles.turn_desires: + elif one_blinker and below_lane_change_speed and not carstate.standstill and frogpilot_toggles.use_turn_desires: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 7fcf99a16e01b1..d39bd5d6c8f044 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -3,7 +3,6 @@ from cereal import car, log from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp -from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL # WARNING: this value was determined based on the model's training distribution, diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index b0ccd98c1535e2..1b7523d7c17f1c 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -4,15 +4,17 @@ import os from enum import IntEnum from collections.abc import Callable +from types import SimpleNamespace from cereal import log, car import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV from openpilot.common.git import get_short_branch -from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER +from openpilot.selfdrive.frogpilot.frogpilot_variables import params + AlertSize = log.ControlsState.AlertSize AlertStatus = log.ControlsState.AlertStatus VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -212,31 +214,31 @@ def get_display_speed(speed_ms: float, metric: bool) -> str: def soft_disable_alert(alert_text_2: str) -> AlertCallbackType: - def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: if soft_disable_time < int(0.5 / DT_CTRL): return ImmediateDisableAlert(alert_text_2) return SoftDisableAlert(alert_text_2) return func def user_soft_disable_alert(alert_text_2: str) -> AlertCallbackType: - def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: if soft_disable_time < int(0.5 / DT_CTRL): return ImmediateDisableAlert(alert_text_2) return UserSoftDisableAlert(alert_text_2) return func -def startup_master_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def startup_master_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: branch = get_short_branch() # Ensure get_short_branch is cached to avoid lags on startup if "REPLAY" in os.environ: branch = "replay" return StartupAlert("WARNING: This branch is not tested", branch, alert_status=AlertStatus.userPrompt) -def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: return NoEntryAlert(f"Drive above {get_display_speed(CP.minEnableSpeed, metric)} to engage") -def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: return Alert( f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}", "", @@ -244,7 +246,7 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4) -def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: first_word = 'Recalibration' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibration' return Alert( f"{first_word} in Progress: {sm['liveCalibration'].calPerc:.0f}%", @@ -253,54 +255,39 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) -def torque_nn_load_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: - model_name = Params().get("NNFFModelName", encoding='utf-8') - if model_name == "": - return Alert( - "NNFF Torque Controller not available", - "Donate logs to Twilsonco to get your car supported!", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 10.0) - else: - return Alert( - "NNFF Torque Controller loaded", - model_name, - AlertStatus.frogpilot, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.engage, 5.0) - # *** debug alerts *** -def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: full_perc = round(100. - sm['deviceState'].freeSpacePercent) return NormalPermanentAlert("Out of Storage", f"{full_perc}% full") -def posenet_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def posenet_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: mdl = sm['modelV2'].velocity.x[0] if len(sm['modelV2'].velocity.x) else math.nan err = CS.vEgo - mdl msg = f"Speed Error: {err:.1f} m/s" return NoEntryAlert(msg, alert_text_1="Posenet Speed Invalid") -def process_not_running_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def process_not_running_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning] msg = ', '.join(not_running) return NoEntryAlert(msg, alert_text_1="Process Not Running") -def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: bs = [s for s in sm.data.keys() if not sm.all_checks([s, ])] msg = ', '.join(bs[:4]) # can't fit too many on one line return NoEntryAlert(msg, alert_text_1="Communication Issue Between Processes") -def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') bad_cams = [s.replace('State', '') for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])] return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams)) -def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: rpy = sm['liveCalibration'].rpyCalib yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan) pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan) @@ -308,34 +295,34 @@ def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging return NormalPermanentAlert("Calibration Invalid", angles) -def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: cpu = max(sm['deviceState'].cpuTempC, default=0.) gpu = max(sm['deviceState'].gpuTempC, default=0.) temp = max((cpu, gpu, sm['deviceState'].memoryTempC)) return NormalPermanentAlert("System Overheated", f"{temp:.0f} °C") -def low_memory_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def low_memory_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: return NormalPermanentAlert("Low Memory", f"{sm['deviceState'].memoryUsagePercent}% used") -def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: x = max(sm['deviceState'].cpuUsagePercent, default=0.) return NormalPermanentAlert("High CPU Usage", f"{x}% used") -def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: return NormalPermanentAlert("Driving Model Lagging", f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped") -def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: text = "Enable Adaptive Cruise to Engage" if CP.carName == "honda": text = "Enable Main Switch to Engage" return NoEntryAlert(text) -def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: +def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: axes = sm['testJoystick'].axes gb, steer = list(axes)[:2] if len(axes) else (0., 0.) vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" @@ -343,11 +330,22 @@ def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, # FrogPilot Alerts -def custom_startup_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: - params = Params() - return StartupAlert(params.get("StartupMessageTop", encoding='utf-8') or "", params.get("StartupMessageBottom", encoding='utf-8') or "", alert_status=AlertStatus.frogpilot) +def custom_startup_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: + return StartupAlert(frogpilot_toggles.startup_alert_top, frogpilot_toggles.startup_alert_bottom, alert_status=AlertStatus.frogpilot) + + +def forcing_stop_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: + model_length = sm['frogpilotPlan'].forcingStopLength + model_length_msg = f"{model_length:.1f} meters" if metric else f"{model_length * CV.METER_TO_FOOT:.1f} feet" + + return Alert( + f"Forcing the car to stop in {model_length_msg}", + "Press the gas pedal or 'Resume' button to override", + AlertStatus.frogpilot, AlertSize.mid, + Priority.MID, VisualAlert.none, AudibleAlert.prompt, 1.) -def holiday_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + +def holiday_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: holiday_messages = { "new_years": ("Happy New Year! 🎉", "newYearsDayAlert"), "valentines": ("Happy Valentine's Day! ❤️", "valentinesDayAlert"), @@ -362,7 +360,7 @@ def holiday_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, "christmas_week": ("Merry Christmas! 🎄", "christmasAlert") } - holiday_name = Params().get("CurrentHolidayTheme", encoding='utf-8') + holiday_name = frogpilot_toggles.current_holiday_theme message, alert_type = holiday_messages.get(holiday_name, ("", "")) return Alert( @@ -371,7 +369,8 @@ def holiday_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, AlertStatus.normal, AlertSize.small, Priority.LOWEST, VisualAlert.none, AudibleAlert.engage, 5.) -def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + +def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: lane_width = sm['frogpilotPlan'].laneWidthLeft if CS.leftBlinker else sm['frogpilotPlan'].laneWidthRight lane_width_msg = f"{lane_width:.1f} meters" if metric else f"{lane_width * CV.METER_TO_FOOT:.1f} feet" @@ -381,6 +380,23 @@ def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S AlertStatus.normal, AlertSize.mid, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) + +def torque_nn_load_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert: + model_name = params.get("NNFFModelName", encoding='utf-8') + if model_name == "": + return Alert( + "NNFF Torque Controller not available", + "Donate logs to Twilsonco to get your car supported!", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 10.0) + else: + return Alert( + "NNFF Torque Controller loaded", + model_name, + AlertStatus.frogpilot, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.engage, 5.0) + + EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # ********** events with no alerts ********** @@ -423,6 +439,12 @@ def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S alert_status=AlertStatus.userPrompt), }, + EventName.startupNoSecOcKey: { + ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", + "Security Key Not Available", + priority=Priority.HIGH), + }, + EventName.dashcamMode: { ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", priority=Priority.LOWEST), @@ -1008,8 +1030,8 @@ def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S # FrogPilot Events EventName.blockUser: { - ET.NO_ENTRY: Alert( - "Please don't use the 'Development' branch!", + ET.PERMANENT: Alert( + "Don't use the 'Development' branch!", "Forcing you into 'Dashcam Mode' for your safety", AlertStatus.userPrompt, AlertSize.mid, Priority.HIGHEST, VisualAlert.none, AudibleAlert.none, 1.), @@ -1020,11 +1042,7 @@ def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S }, EventName.forcingStop: { - ET.WARNING: Alert( - "Forcing the car to stop", - "Press the gas pedal or 'Resume' button to override", - AlertStatus.frogpilot, AlertSize.mid, - Priority.MID, VisualAlert.none, AudibleAlert.prompt, 1.), + ET.WARNING: forcing_stop_alert, }, EventName.goatSteerSaturated: { @@ -1072,7 +1090,7 @@ def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S "openpilot crashed", "Please post the 'Error Log' in the FrogPilot Discord!", AlertStatus.normal, AlertSize.mid, - Priority.HIGH, VisualAlert.none, AudibleAlert.none, 10.), + Priority.HIGHEST, VisualAlert.none, AudibleAlert.prompt, 10.), }, EventName.pedalInterceptorNoBrake: { @@ -1091,6 +1109,14 @@ def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.), }, + EventName.thisIsFineSteerSaturated: { + ET.WARNING: Alert( + "This is fine", + "☕", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.thisIsFine, 2.), + }, + EventName.torqueNNLoad: { ET.PERMANENT: torque_nn_load_alert, }, diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 315dc2bf7cc4d9..096a8dcd584645 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -1,4 +1,5 @@ from collections import deque + import math import numpy as np diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 7e4a780263c449..dc1d9267af6532 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -89,7 +89,7 @@ def long_control_state_trans_old_long(CP, active, long_control_state, v_ego, v_t class LongControl: def __init__(self, CP): self.CP = CP - self.long_control_state = LongCtrlState.off # initialized to off + self.long_control_state = LongCtrlState.off self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index b9cd1907433e9b..a144574304a7e2 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -326,10 +326,10 @@ def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv - def process_lead(self, lead, increased_distance=0): + def process_lead(self, lead): v_ego = self.x0[1] if lead is not None and lead.status: - x_lead = lead.dRel - increased_distance + x_lead = lead.dRel v_lead = lead.vLead a_lead = lead.aLeadK a_lead_tau = lead.aLeadTau @@ -355,12 +355,11 @@ def set_accel_limits(self, min_a, max_a): self.cruise_min_a = min_a self.max_a = max_a - def update(self, lead_one, lead_two, v_cruise, x, v, a, j, radarless_model, t_follow, trafficModeActive, frogpilot_toggles, personality=log.LongitudinalPersonality.standard): + def update(self, lead_one, lead_two, v_cruise, x, v, a, j, radarless_model, t_follow, trafficModeActive, personality=log.LongitudinalPersonality.standard): v_ego = self.x0[1] self.status = lead_one.status or lead_two.status - increased_distance = max(frogpilot_toggles.increase_stopped_distance + min(10 - v_ego, 0), 0) if not trafficModeActive else 0 - lead_xv_0 = self.process_lead(lead_one, increased_distance) + lead_xv_0 = self.process_lead(lead_one) lead_xv_1 = self.process_lead(lead_two) # To estimate a safe distance from a moving lead, we calculate how much stopping @@ -370,7 +369,8 @@ def update(self, lead_one, lead_two, v_cruise, x, v, a, j, radarless_model, t_fo lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) self.params[:,0] = ACCEL_MIN - self.params[:,1] = self.max_a + # negative accel constraint causes problems because negative speed is not allowed + self.params[:,1] = max(0.0, self.max_a) # Update in ACC mode or ACC/e2e blend if self.mode == 'acc': @@ -379,6 +379,7 @@ def update(self, lead_one, lead_two, v_cruise, x, v, a, j, radarless_model, t_fo # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) + # TODO does this make sense when max_a is negative? v_upper = v_ego + (T_IDXS * self.max_a * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c087005bb9fa24..c710cb79ffc56a 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -22,7 +22,7 @@ A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] ALLOW_THROTTLE_THRESHOLD = 0.5 -ACCEL_LIMIT_MARGIN = 0.05 +MIN_ALLOW_THROTTLE_SPEED = 2.5 # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] @@ -58,7 +58,10 @@ def get_accel_from_plan(CP, speeds, accels): a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, accels) v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds) - a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now + if v_target != v_target_now: + a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now + else: + a_target = a_target_now v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds) else: @@ -159,8 +162,8 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): @staticmethod def parse_model(model_msg, model_error, v_ego, taco_tune): if (len(model_msg.position.x) == ModelConstants.IDX_N and - len(model_msg.velocity.x) == ModelConstants.IDX_N and - len(model_msg.acceleration.x) == ModelConstants.IDX_N): + len(model_msg.velocity.x) == ModelConstants.IDX_N and + len(model_msg.acceleration.x) == ModelConstants.IDX_N): x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) @@ -183,7 +186,7 @@ def parse_model(model_msg, model_error, v_ego, taco_tune): throttle_prob = 1.0 return x, v, a, j, throttle_prob - def update(self, radarless_model, secretgoodopenpilot_model, sm, frogpilot_toggles): + def update(self, classic_model, radarless_model, sm, frogpilot_toggles): self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' if len(sm['carControl'].orientationNED) == 3: @@ -209,7 +212,8 @@ def update(self, radarless_model, secretgoodopenpilot_model, sm, frogpilot_toggl accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration] if self.mpc.mode == 'acc': - accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) + steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg + accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) else: accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] @@ -223,12 +227,13 @@ def update(self, radarless_model, secretgoodopenpilot_model, sm, frogpilot_toggl # Compute model v_ego error self.v_model_error = get_speed_error(sm['modelV2'], v_ego) x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, frogpilot_toggles.taco_tune) - self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD + # Don't clip at low speeds since throttle_prob doesn't account for creep + self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED - if not self.allow_throttle and v_ego > 5.0 and secretgoodopenpilot_model: # Don't clip at low speeds since throttle_prob doesn't account for creep - # MPC breaks when accel limits would cause negative velocity within the MPC horizon, so we clip the max accel limit at vEgo/T_MAX plus a bit of margin - clipped_accel_coast = max(accel_coast, accel_limits_turns[0], -v_ego / T_IDXS_MPC[-1] + ACCEL_LIMIT_MARGIN) - accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast) + if not self.allow_throttle and not classic_model: + clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) + clipped_accel_coast_interp = interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast]) + accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp) if force_slow_decel: v_cruise = 0.0 @@ -242,8 +247,9 @@ def update(self, radarless_model, secretgoodopenpilot_model, sm, frogpilot_toggl lead_states = [self.lead_one, self.lead_two] for index in range(len(lead_states)): if len(model_leads) > index: + distance_offset = max(frogpilot_toggles.increased_stopped_distance + min(10 - v_ego, 0), 0) if not sm['frogpilotCarState'].trafficModeActive else 0 model_lead = model_leads[index] - lead_states[index].update(model_lead.x[0], model_lead.y[0], model_lead.v[0], model_lead.a[0], model_lead.prob) + lead_states[index].update(model_lead.x[0] - distance_offset, model_lead.y[0], model_lead.v[0], model_lead.a[0], model_lead.prob) else: lead_states[index].reset() else: @@ -254,7 +260,7 @@ def update(self, radarless_model, secretgoodopenpilot_model, sm, frogpilot_toggl self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) self.mpc.update(self.lead_one, self.lead_two, sm['frogpilotPlan'].vCruise, x, v, a, j, radarless_model, sm['frogpilotPlan'].tFollow, - sm['frogpilotCarState'].trafficModeActive, frogpilot_toggles, personality=sm['controlsState'].personality) + sm['frogpilotCarState'].trafficModeActive, personality=sm['controlsState'].personality) self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index cfe4b2ab31340e..7c242e7c549878 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -59,15 +59,13 @@ def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0 if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) else: - i = self.i + error * self.k_i * self.i_rate - control = self.p + i + self.d + self.f - - # Update when changing i will move the control away from the limits - # or when i will move towards the sign of the error - if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or - (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ - not freeze_integrator: - self.i = i + if not freeze_integrator: + self.i = self.i + error * self.k_i * self.i_rate + + # Clip i to prevent exceeding control limits + control_no_i = self.p + self.d + self.f + control_no_i = clip(control_no_i, self.neg_limit, self.pos_limit) + self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i) control = self.p + self.i + self.d + self.f diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 3bae4ca0a12229..c151c816076908 100644 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -6,7 +6,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner import cereal.messaging as messaging -from openpilot.selfdrive.frogpilot.frogpilot_variables import FrogPilotVariables +from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles def publish_ui_plan(sm, pm, longitudinal_planner): ui_send = messaging.new_message('uiPlan') @@ -30,31 +30,26 @@ def plannerd_thread(): longitudinal_planner = LongitudinalPlanner(CP) pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'frogpilotCarControl', 'frogpilotCarState', 'frogpilotPlan'], + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', + 'frogpilotCarState', 'frogpilotPlan'], poll='modelV2', ignore_avg_freq=['radarState']) # FrogPilot variables - frogpilot_toggles = FrogPilotVariables.toggles - FrogPilotVariables.update_frogpilot_params() + frogpilot_toggles = get_frogpilot_toggles() + classic_model = frogpilot_toggles.classic_model radarless_model = frogpilot_toggles.radarless_model - secretgoodopenpilot_model = frogpilot_toggles.secretgoodopenpilot_model - - update_toggles = False while True: sm.update() if sm.updated['modelV2']: - longitudinal_planner.update(radarless_model, secretgoodopenpilot_model, sm, frogpilot_toggles) + longitudinal_planner.update(classic_model, radarless_model, sm, frogpilot_toggles) longitudinal_planner.publish(sm, pm) publish_ui_plan(sm, pm, longitudinal_planner) # Update FrogPilot parameters - if FrogPilotVariables.toggles_updated: - update_toggles = True - elif update_toggles: - FrogPilotVariables.update_frogpilot_params() - update_toggles = False + if sm['frogpilotPlan'].togglesUpdated: + frogpilot_toggles = get_frogpilot_toggles() def main(): plannerd_thread() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 2f44a8d6a032ca..bdb986428abfa8 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -2,6 +2,7 @@ import importlib import math from collections import deque +from types import SimpleNamespace from typing import Any import capnp @@ -13,7 +14,7 @@ from openpilot.common.simple_kalman import KF1D -from openpilot.selfdrive.frogpilot.frogpilot_variables import FrogPilotVariables +from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles # Default lead acceleration decay set to 50% at 1s _LEAD_ACCEL_TAU = 1.5 @@ -108,6 +109,19 @@ def get_RadarState(self, model_prob: float = 0.0): "radarTrackId": self.identifier, } + def potential_adjacent_lead(self, far: bool, lane_width: float, left: bool, model_data: capnp._DynamicStructReader): + adjacent_lane_max = float('inf') if far else lane_width * 1.5 + adjacent_lane_min = max(lane_width * 1.5, 4.5) if far else max(lane_width * 0.5, 1.5) + + y_delta = self.yRel + interp(self.dRel, model_data.position.x, model_data.position.y) + + if left and adjacent_lane_min < y_delta < adjacent_lane_max: + return True + elif not left and adjacent_lane_min < -y_delta < adjacent_lane_max: + return True + else: + return False + def potential_low_speed_lead(self, v_ego: float): # stop for stuff in front of you and low speed, even without model confirmation # Radar points closer than 0.75, are almost always glitches on toyota radars @@ -168,9 +182,9 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, lead_detection_threshold: float, low_speed_override: bool = True) -> dict[str, Any]: + model_v_ego: float, frogpilot_toggles: SimpleNamespace, frogpilotCarState: capnp._DynamicStructReader, low_speed_override: bool = True) -> dict[str, Any]: # Determine leads, this is where the essential logic happens - if len(tracks) > 0 and ready and lead_msg.prob > lead_detection_threshold: + if len(tracks) > 0 and ready and lead_msg.prob > frogpilot_toggles.lead_detection_probability: track = match_vision_to_track(v_ego, lead_msg, tracks) else: track = None @@ -178,7 +192,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn lead_dict = {'status': False} if track is not None: lead_dict = track.get_RadarState(lead_msg.prob) - elif (track is None) and ready and (lead_msg.prob > lead_detection_threshold): + elif (track is None) and ready and (lead_msg.prob > frogpilot_toggles.lead_detection_probability): lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if low_speed_override: @@ -190,11 +204,25 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']): lead_dict = closest_track.get_RadarState() + if 'dRel' in lead_dict: + lead_dict['dRel'] -= frogpilot_toggles.increased_stopped_distance if not frogpilotCarState.trafficModeActive else 0 + + return lead_dict + + +def get_lead_adjacent(tracks: dict[int, Track], model_data: capnp._DynamicStructReader, lane_width: float, left: bool = True, far: bool = False) -> dict[str, Any]: + lead_dict = {'status': False} + + adjacent_tracks = [c for c in tracks.values() if c.vLead > 1 and c.potential_adjacent_lead(far, lane_width, left, model_data)] + if len(adjacent_tracks) > 0: + closest_track = min(adjacent_tracks, key=lambda c: c.dRel) + lead_dict = closest_track.get_RadarState() + return lead_dict class RadarD: - def __init__(self, radar_ts: float, delay: int = 0): + def __init__(self, frogpilot_toggles, radar_ts: float, delay: int = 0): self.points: dict[int, tuple[float, float, float]] = {} self.current_time = 0.0 @@ -213,12 +241,9 @@ def __init__(self, radar_ts: float, delay: int = 0): self.ready = False # FrogPilot variables - self.frogpilot_toggles = FrogPilotVariables.toggles - FrogPilotVariables.update_frogpilot_params() - - self.velocity_model = self.frogpilot_toggles.velocity_model + self.frogpilot_toggles = frogpilot_toggles - self.update_toggles = False + self.classic_model = self.frogpilot_toggles.classic_model def update(self, sm: messaging.SubMaster, rr): self.ready = sm.seen['modelV2'] @@ -263,23 +288,26 @@ def update(self, sm: messaging.SubMaster, rr): self.radar_state.radarErrors = list(radar_errors) self.radar_state.carStateMonoTime = sm.logMonoTime['carState'] - if self.velocity_model and len(sm['modelV2'].velocity.x): - model_v_ego = sm['modelV2'].velocity.x[0] - elif len(sm['modelV2'].temporalPose.trans): + if self.classic_model and len(sm['modelV2'].temporalPose.trans): model_v_ego = sm['modelV2'].temporalPose.trans[0] + elif len(sm['modelV2'].velocity.x): + model_v_ego = sm['modelV2'].velocity.x[0] else: model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.frogpilot_toggles.lead_detection_threshold, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.frogpilot_toggles.lead_detection_threshold, low_speed_override=False) + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.frogpilot_toggles, sm['frogpilotCarState'], low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.frogpilot_toggles, sm['frogpilotCarState'], low_speed_override=False) + + if self.frogpilot_toggles.adjacent_lead_tracking and self.ready: + self.radar_state.leadLeft = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True) + 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) # Update FrogPilot parameters - if FrogPilotVariables.toggles_updated: - self.update_toggles = True - elif self.update_toggles: - FrogPilotVariables.update_frogpilot_params() - self.update_toggles = False + if sm['frogpilotPlan'].togglesUpdated: + self.frogpilot_toggles = get_frogpilot_toggles() def publish(self, pm: messaging.PubMaster, lag_ms: float): assert self.radar_state is not None @@ -348,14 +376,14 @@ def main(): RI = RadarInterface(CP) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) - RD = RadarD(CP.radarTimeStep, RI.delay) # FrogPilot variables - frogpilot_toggles = FrogPilotVariables.toggles - FrogPilotVariables.update_frogpilot_params() + frogpilot_toggles = get_frogpilot_toggles() + + RD = RadarD(frogpilot_toggles, CP.radarTimeStep, RI.delay) if not frogpilot_toggles.radarless_model: - sm = messaging.SubMaster(['modelV2', 'carState'], frequency=int(1./DT_CTRL)) + sm = messaging.SubMaster(['modelV2', 'carState', 'frogpilotCarState', 'frogpilotPlan'], frequency=int(1./DT_CTRL)) pm = messaging.PubMaster(['radarState', 'liveTracks']) while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) diff --git a/selfdrive/frogpilot/assets/active_theme/colors/colors.json b/selfdrive/frogpilot/assets/active_theme/colors/colors.json index 80e70f351c2180..bf527d51eb243e 100644 --- a/selfdrive/frogpilot/assets/active_theme/colors/colors.json +++ b/selfdrive/frogpilot/assets/active_theme/colors/colors.json @@ -23,12 +23,6 @@ "blue": 54, "alpha": 242 }, - "RoadEdges": { - "red": 23, - "green": 134, - "blue": 68, - "alpha": 242 - }, "Sidebar1": { "red": 23, "green": 134, diff --git a/selfdrive/frogpilot/assets/download_functions.py b/selfdrive/frogpilot/assets/download_functions.py index 9057c2dbce593d..7ae574fc1d1870 100644 --- a/selfdrive/frogpilot/assets/download_functions.py +++ b/selfdrive/frogpilot/assets/download_functions.py @@ -1,12 +1,12 @@ import os import requests -from openpilot.selfdrive.frogpilot.frogpilot_functions import delete_file, is_url_pingable +from openpilot.selfdrive.frogpilot.frogpilot_utilities import delete_file, is_url_pingable GITHUB_URL = "https://raw.githubusercontent.com/FrogAi/FrogPilot-Resources/" GITLAB_URL = "https://gitlab.com/FrogAi/FrogPilot-Resources/-/raw/" -def download_file(cancel_param, destination, progress_param, url, download_param, params_memory): +def download_file(cancel_param, destination, temp_destination, progress_param, url, download_param, params_memory): try: os.makedirs(os.path.dirname(destination), exist_ok=True) total_size = get_remote_file_size(url) @@ -14,11 +14,11 @@ def download_file(cancel_param, destination, progress_param, url, download_param return downloaded_size = 0 - with requests.get(url, stream=True, timeout=5) as response, open(destination, 'wb') as file: + with requests.get(url, stream=True, timeout=5) as response, open(temp_destination, 'wb') as file: response.raise_for_status() for chunk in response.iter_content(chunk_size=8192): if params_memory.get_bool(cancel_param): - handle_error(destination, "Download cancelled.", "Download cancelled.", download_param, progress_param, params_memory) + handle_error(temp_destination, "Download cancelled", "Download cancelled", download_param, progress_param, params_memory) return if chunk: @@ -31,29 +31,28 @@ def download_file(cancel_param, destination, progress_param, url, download_param else: params_memory.put(progress_param, "Verifying authenticity...") except Exception as e: - handle_request_error(e, destination, download_param, progress_param, params_memory) + handle_request_error(e, temp_destination, download_param, progress_param, params_memory) def handle_error(destination, error_message, error, download_param, progress_param, params_memory): - print(f"Error occurred: {error}") if destination: delete_file(destination) + if download_param: params_memory.remove(download_param) - if progress_param: + + if progress_param and "404" not in error_message: + print(f"Error occurred: {error}") params_memory.put(progress_param, error_message) def handle_request_error(error, destination, download_param, progress_param, params_memory): error_map = { - requests.HTTPError: lambda e: f"Server error ({e.response.status_code})" if e.response else "Server error.", - requests.ConnectionError: "Connection dropped.", - requests.Timeout: "Download timed out.", - requests.RequestException: "Network request error. Check connection." + requests.ConnectionError: "Connection dropped", + requests.HTTPError: lambda e: f"Server error ({e.response.status_code})" if e.response else "Server error", + requests.RequestException: "Network request error. Check connection", + requests.Timeout: "Download timed out" } - error_message = error_map.get(type(error), "Unexpected error.") - if isinstance(error, requests.HTTPError) and error.response and error.response.status_code == 404: - return - + error_message = error_map.get(type(error), "Unexpected error") handle_error(destination, f"Failed: {error_message}", error, download_param, progress_param, params_memory) def get_remote_file_size(url): @@ -61,12 +60,18 @@ def get_remote_file_size(url): response = requests.head(url, headers={'Accept-Encoding': 'identity'}, timeout=5) if response.status_code == 404: print(f"URL not found: {url}") - return None + return 0 response.raise_for_status() return int(response.headers.get('Content-Length', 0)) + except requests.HTTPError as e: + if e.response and e.response.status_code == 404: + print(f"URL not found: {url}") + return 0 + handle_request_error(e, None, None, None, None) + return 0 except Exception as e: handle_request_error(e, None, None, None, None) - return None + return 0 def get_repository_url(): if is_url_pingable("https://github.com"): @@ -75,35 +80,23 @@ def get_repository_url(): return GITLAB_URL return None -def link_valid(url): - try: - response = requests.head(url, allow_redirects=True, timeout=5) - response.raise_for_status() - return True - except requests.HTTPError as e: - if e.response.status_code != 404: - handle_request_error(e, None, None, None, None) - return False - except Exception as e: - handle_request_error(e, None, None, None, None) - return False - -def verify_download(file_path, url, initial_download=True): +def verify_download(file_path, temp_file_path, url, initial_download=True): remote_file_size = get_remote_file_size(url) if remote_file_size is None: print(f"Error fetching remote size for {file_path}") return False if initial_download else True - if not os.path.isfile(file_path): - print(f"File not found: {file_path}") + if not os.path.isfile(temp_file_path): + print(f"File not found: {temp_file_path}") return False try: - if remote_file_size != os.path.getsize(file_path): - print(f"File size mismatch for {file_path}") + if remote_file_size != os.path.getsize(temp_file_path): + print(f"File size mismatch for {temp_file_path}") return False except Exception as e: - print(f"An unexpected error occurred while trying to verify the {file_path} download: {e}") + print(f"An unexpected error occurred while trying to verify the {temp_file_path} download: {e}") return False + os.rename(temp_file_path, file_path) return True diff --git a/selfdrive/frogpilot/assets/holiday_themes/halloween_week/colors/colors.json b/selfdrive/frogpilot/assets/holiday_themes/halloween_week/colors/colors.json index 9fca352351cac6..91690ba0b21a35 100644 --- a/selfdrive/frogpilot/assets/holiday_themes/halloween_week/colors/colors.json +++ b/selfdrive/frogpilot/assets/holiday_themes/halloween_week/colors/colors.json @@ -23,12 +23,6 @@ "blue": 255, "alpha": 255 }, - "RoadEdges": { - "red": 242, - "green": 117, - "blue": 3, - "alpha": 255 - }, "Sidebar1": { "red": 134, "green": 47, diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/colors/colors.json b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/colors/colors.json new file mode 100644 index 00000000000000..6a91eb709410e1 --- /dev/null +++ b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/colors/colors.json @@ -0,0 +1,44 @@ +{ + "LaneLines": { + "red": 237, + "green": 116, + "blue": 46, + "alpha": 255 + }, + "LeadMarker": { + "red": 237, + "green": 116, + "blue": 46, + "alpha": 255 + }, + "Path": { + "red": 248, + "green": 177, + "blue": 44, + "alpha": 255 + }, + "PathEdge": { + "red": 198, + "green": 142, + "blue": 35, + "alpha": 255 + }, + "Sidebar1": { + "red": 153, + "green": 99, + "blue": 54, + "alpha": 255 + }, + "Sidebar2": { + "red": 248, + "green": 177, + "blue": 44, + "alpha": 255 + }, + "Sidebar3": { + "red": 237, + "green": 116, + "blue": 46, + "alpha": 255 + } +} diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.gif b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.gif new file mode 100644 index 00000000000000..2ff77fb021fb95 Binary files /dev/null and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.gif differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.png b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.png deleted file mode 100644 index 627af8aff329cd..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_flag.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.gif b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.gif new file mode 100644 index 00000000000000..2ff77fb021fb95 Binary files /dev/null and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.gif differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.png b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.png deleted file mode 100644 index 627af8aff329cd..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_home.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_settings.png b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_settings.png index 55927599758a9b..c6e453c1f0deff 100644 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_settings.png and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/icons/button_settings.png differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/disengage.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/disengage.wav index ba583c41f362d3..b7ab60e075466d 100644 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/disengage.wav and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/disengage.wav differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/engage.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/engage.wav index 41e9b2d588d4aa..5a1395feb3e49c 100644 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/engage.wav and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/engage.wav differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt.wav deleted file mode 100644 index 1ae77051eb5722..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt_distracted.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt_distracted.wav deleted file mode 100644 index c3d4475caa70ce..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/prompt_distracted.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/refuse.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/refuse.wav deleted file mode 100644 index 0e80f7d127dcd4..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/refuse.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_immediate.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_immediate.wav deleted file mode 100644 index b1815a95867b9e..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_immediate.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_soft.wav b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_soft.wav deleted file mode 100644 index 261c7e1376c672..00000000000000 Binary files a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/sounds/warning_soft.wav and /dev/null differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/steering_wheel/wheel.png b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/steering_wheel/wheel.png new file mode 100644 index 00000000000000..32c30aacfc333d Binary files /dev/null and b/selfdrive/frogpilot/assets/holiday_themes/thanksgiving_week/steering_wheel/wheel.png differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/colors/colors.json b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/colors/colors.json new file mode 100644 index 00000000000000..bf527d51eb243e --- /dev/null +++ b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/colors/colors.json @@ -0,0 +1,44 @@ +{ + "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/holiday_themes/halloween_week/distance_icons/aggressive.gif b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/distance_icons/aggressive.gif similarity index 100% rename from selfdrive/frogpilot/assets/holiday_themes/halloween_week/distance_icons/aggressive.gif rename to selfdrive/frogpilot/assets/holiday_themes/world_frog_day/distance_icons/aggressive.gif diff --git a/selfdrive/frogpilot/assets/holiday_themes/halloween_week/distance_icons/relaxed.gif b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/distance_icons/relaxed.gif similarity index 100% rename from selfdrive/frogpilot/assets/holiday_themes/halloween_week/distance_icons/relaxed.gif rename to selfdrive/frogpilot/assets/holiday_themes/world_frog_day/distance_icons/relaxed.gif diff --git a/selfdrive/frogpilot/assets/holiday_themes/halloween_week/distance_icons/standard.gif b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/distance_icons/standard.gif similarity index 100% rename from selfdrive/frogpilot/assets/holiday_themes/halloween_week/distance_icons/standard.gif rename to selfdrive/frogpilot/assets/holiday_themes/world_frog_day/distance_icons/standard.gif diff --git a/selfdrive/frogpilot/assets/holiday_themes/halloween_week/distance_icons/traffic.gif b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/distance_icons/traffic.gif similarity index 100% rename from selfdrive/frogpilot/assets/holiday_themes/halloween_week/distance_icons/traffic.gif rename to selfdrive/frogpilot/assets/holiday_themes/world_frog_day/distance_icons/traffic.gif diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/button_home.gif b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/button_home.gif index 6b20a726e4ce2f..133d8d34ed1c00 100644 Binary files a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/button_home.gif and b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/icons/button_home.gif differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/traditional_100 b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/traditional_100 new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_1.png b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_1.png index 43e0b446f8ea9d..8c2f7a54ed26a3 100644 Binary files a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_1.png and b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_1.png differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_2.png b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_2.png index e8e147976b7330..3c7d2d9eb8a0ad 100644 Binary files a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_2.png and b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_2.png differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_3.png b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_3.png index b59b003f95eb2f..b495eeae190cbc 100644 Binary files a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_3.png and b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_3.png differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_4.png b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_4.png index c3c1d204e68375..024a0b03ea6c08 100644 Binary files a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_4.png and b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_4.png differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_5.png b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_5.png new file mode 100644 index 00000000000000..91eb05f12f4579 Binary files /dev/null and b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_5.png differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_6.png b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_6.png new file mode 100644 index 00000000000000..3c7d2d9eb8a0ad Binary files /dev/null and b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_6.png differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_blindspot.png b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_blindspot.png new file mode 100644 index 00000000000000..b721266fb2f1b1 Binary files /dev/null and b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/signals/turn_signal_blindspot.png differ diff --git a/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/steering_wheel/wheel.png b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/steering_wheel/wheel.png new file mode 100644 index 00000000000000..712255984600c0 Binary files /dev/null and b/selfdrive/frogpilot/assets/holiday_themes/world_frog_day/steering_wheel/wheel.png differ diff --git a/selfdrive/frogpilot/assets/model_manager.py b/selfdrive/frogpilot/assets/model_manager.py index b4913ac61d347b..62a96ee860d8f1 100644 --- a/selfdrive/frogpilot/assets/model_manager.py +++ b/selfdrive/frogpilot/assets/model_manager.py @@ -1,25 +1,20 @@ import json import os import re +import requests import shutil import time +import urllib.parse import urllib.request from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params, UnknownKeyName +from openpilot.common.params import Params -from openpilot.selfdrive.frogpilot.assets.download_functions import GITHUB_URL, GITLAB_URL, download_file, get_remote_file_size, get_repository_url, handle_error, handle_request_error, verify_download -from openpilot.selfdrive.frogpilot.frogpilot_functions import MODELS_PATH, delete_file +from openpilot.selfdrive.frogpilot.assets.download_functions import GITHUB_URL, GITLAB_URL, download_file, get_repository_url, handle_error, handle_request_error, verify_download +from openpilot.selfdrive.frogpilot.frogpilot_utilities import delete_file +from openpilot.selfdrive.frogpilot.frogpilot_variables import DEFAULT_MODEL, DEFAULT_MODEL_NAME, DEFAULT_CLASSIC_MODEL, DEFAULT_CLASSIC_MODEL_NAME, MODELS_PATH -VERSION = "v9" - -DEFAULT_MODEL = "north-dakota" -DEFAULT_MODEL_NAME = "North Dakota (Default)" - -def process_model_name(model_name): - cleaned_name = re.sub(r'[🗺️👀📡]', '', model_name) - cleaned_name = re.sub(r'[^a-zA-Z0-9()-]', '', cleaned_name) - return cleaned_name.replace(' ', '').replace('(Default)', '').replace('-', '') +VERSION = "v10" class ModelManager: def __init__(self): @@ -30,100 +25,148 @@ def __init__(self): self.download_param = "ModelToDownload" self.download_progress_param = "ModelDownloadProgress" - def handle_verification_failure(self, model, model_path): + @staticmethod + def fetch_models(url): + try: + with urllib.request.urlopen(url, timeout=10) as response: + return json.loads(response.read().decode('utf-8'))['models'] + except Exception as error: + handle_request_error(error, None, None, None, None) + return [] + + @staticmethod + def fetch_all_model_sizes(repo_url): + project_path = "FrogAi/FrogPilot-Resources" + branch = "Models" + + if "github" in repo_url: + api_url = f"https://api.github.com/repos/{project_path}/contents?ref={branch}" + elif "gitlab" in repo_url: + api_url = f"https://gitlab.com/api/v4/projects/{urllib.parse.quote_plus(project_path)}/repository/tree?ref={branch}" + else: + return {} + + try: + response = requests.get(api_url) + response.raise_for_status() + thneed_files = [file for file in response.json() if file['name'].endswith('.thneed')] + + if "gitlab" in repo_url: + model_sizes = {} + for file in thneed_files: + file_path = file['path'] + metadata_url = f"https://gitlab.com/api/v4/projects/{urllib.parse.quote_plus(project_path)}/repository/files/{urllib.parse.quote_plus(file_path)}/raw?ref={branch}" + metadata_response = requests.head(metadata_url) + metadata_response.raise_for_status() + model_sizes[file['name'].replace('.thneed', '')] = int(metadata_response.headers.get('content-length', 0)) + return model_sizes + else: + return {file['name'].replace('.thneed', ''): file['size'] for file in thneed_files if 'size' in file} + except Exception as e: + handle_request_error(f"Failed to fetch model sizes from {'GitHub' if 'github' in repo_url else 'GitLab'}: {e}", None, None, None, None) + return {} + + @staticmethod + def copy_default_model(): + classic_default_model_path = os.path.join(MODELS_PATH, f"{DEFAULT_CLASSIC_MODEL}.thneed") + source_path = os.path.join(BASEDIR, "selfdrive", "classic_modeld", "models", "supercombo.thneed") + + if os.path.isfile(source_path): + shutil.copyfile(source_path, classic_default_model_path) + print(f"Copied the classic default model from {source_path} to {classic_default_model_path}") + + default_model_path = os.path.join(MODELS_PATH, f"{DEFAULT_MODEL}.thneed") + source_path = os.path.join(BASEDIR, "selfdrive", "modeld", "models", "supercombo.thneed") + + if os.path.isfile(source_path): + shutil.copyfile(source_path, default_model_path) + print(f"Copied the default model from {source_path} to {default_model_path}") + + def handle_verification_failure(self, model, model_path, temp_model_path): if self.params_memory.get_bool(self.cancel_download_param): return print(f"Verification failed for model {model}. Retrying from GitLab...") model_url = f"{GITLAB_URL}Models/{model}.thneed" - download_file(self.cancel_download_param, model_path, self.download_progress_param, model_url, self.download_param, self.params_memory) + download_file(self.cancel_download_param, model_path, temp_model_path, self.download_progress_param, model_url, self.download_param, self.params_memory) - if verify_download(model_path, model_url): - print(f"Model {model} redownloaded and verified successfully from GitLab.") + if verify_download(model_path, temp_model_path, model_url): + print(f"Model {model} redownloaded and verified successfully from GitLab") else: handle_error(model_path, "GitLab verification failed", "Verification failed", self.download_param, self.download_progress_param, self.params_memory) def download_model(self, model_to_download): model_path = os.path.join(MODELS_PATH, f"{model_to_download}.thneed") + temp_model_path = f"{os.path.splitext(model_path)[0]}_temp.thneed" if os.path.isfile(model_path): handle_error(model_path, "Model already exists...", "Model already exists...", self.download_param, self.download_progress_param, self.params_memory) return repo_url = get_repository_url() if not repo_url: - handle_error(model_path, "GitHub and GitLab are offline...", "Repository unavailable", self.download_param, self.download_progress_param, self.params_memory) + handle_error(temp_model_path, "GitHub and GitLab are offline...", "Repository unavailable", self.download_param, self.download_progress_param, self.params_memory) return model_url = f"{repo_url}Models/{model_to_download}.thneed" print(f"Downloading model: {model_to_download}") - download_file(self.cancel_download_param, model_path, self.download_progress_param, model_url, self.download_param, self.params_memory) + download_file(self.cancel_download_param, model_path, temp_model_path, self.download_progress_param, model_url, self.download_param, self.params_memory) - if verify_download(model_path, model_url): + if verify_download(model_path, temp_model_path, model_url): print(f"Model {model_to_download} downloaded and verified successfully!") self.params_memory.put(self.download_progress_param, "Downloaded!") self.params_memory.remove(self.download_param) else: - self.handle_verification_failure(model_to_download, model_path) + self.handle_verification_failure(model_to_download, model_path, temp_model_path) - def fetch_models(self, url): - try: - with urllib.request.urlopen(url, timeout=10) as response: - return json.loads(response.read().decode('utf-8'))['models'] - except Exception as error: - handle_request_error(error, None, None, None, None) - return [] + def queue_model_download(self, model, model_name=None): + while self.params_memory.get(self.download_param, encoding='utf-8'): + time.sleep(1) - def update_model_params(self, model_info, repo_url): - available_models, available_model_names, experimental_models, navigation_models, radarless_models, velocity_models = [], [], [], [], [], [] + self.params_memory.put(self.download_param, model) + if model_name: + self.params_memory.put(self.download_progress_param, f"Downloading {model_name}...") + def update_model_params(self, model_info, repo_url): + available_models = [] for model in model_info: available_models.append(model['id']) - available_model_names.append(model['name']) - - if model.get("e2e_longitudinal", False): - e2e_longitudinal_models.append(model['id']) - if model.get("experimental", False): - experimental_models.append(model['id']) - if model.get("gas_brake", False): - gas_brake_models.append(model['id']) - if model.get("velocity", False): - velocity_models.append(model['id']) - if "🗺️" in model['name']: - navigation_models.append(model['id']) - if "📡" not in model['name']: - radarless_models.append(model['id']) self.params.put_nonblocking("AvailableModels", ','.join(available_models)) - self.params.put_nonblocking("AvailableModelsNames", ','.join(available_model_names)) - self.params.put_nonblocking("ExperimentalModels", ','.join(experimental_models)) - self.params.put_nonblocking("NavigationModels", ','.join(navigation_models)) - self.params.put_nonblocking("RadarlessModels", ','.join(radarless_models)) - self.params.put_nonblocking("VelocityModels", ','.join(velocity_models)) - print("Models list updated successfully.") + self.params.put_nonblocking("AvailableModelsNames", ','.join([model['name'] for model in model_info])) + self.params.put_nonblocking("ClassicModels", ','.join([model['id'] for model in model_info if model.get("classic_model", False)])) + self.params.put_nonblocking("ExperimentalModels", ','.join([model['id'] for model in model_info if model.get("experimental", False)])) + self.params.put_nonblocking("NavigationModels", ','.join([model['id'] for model in model_info if "🗺️" in model['name']])) + self.params.put_nonblocking("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, available_model_names, repo_url) + models_downloaded = self.are_all_models_downloaded(available_models, repo_url) self.params.put_bool_nonblocking("ModelsDownloaded", models_downloaded) - def are_all_models_downloaded(self, available_models, available_model_names, repo_url): + def are_all_models_downloaded(self, available_models, repo_url): + available_models = set(available_models) - {DEFAULT_MODEL, DEFAULT_CLASSIC_MODEL} + automatically_update_models = self.params.get_bool("AutomaticallyUpdateModels") all_models_downloaded = True + 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") - model_url = f"{repo_url}Models/{model}.thneed" + expected_size = model_sizes.get(model) + + if expected_size is None: + print(f"Size data for {model} not available.") + continue if os.path.isfile(model_path): - if automatically_update_models: - verify_result = verify_download(model_path, model_url, False) - if verify_result is None: - all_models_downloaded = False - elif not verify_result: - print(f"Model {model} is outdated. Re-downloading...") - delete_file(model_path) - download_queue.append(model) - all_models_downloaded = False + local_size = os.path.getsize(model_path) + if automatically_update_models and local_size != expected_size: + print(f"Model {model} is outdated. Re-downloading...") + delete_file(model_path) + download_queue.append(model) + all_models_downloaded = False else: if automatically_update_models: print(f"Model {model} isn't downloaded. Downloading...") @@ -135,19 +178,11 @@ def are_all_models_downloaded(self, available_models, available_model_names, rep return all_models_downloaded - def queue_model_download(self, model, model_name=None): - while self.params_memory.get(self.download_param, encoding='utf-8'): - time.sleep(1) - - self.params_memory.put(self.download_param, model) - if model_name: - self.params_memory.put(self.download_progress_param, f"Downloading {model_name}...") - def validate_models(self): current_model = self.params.get("Model", encoding='utf-8') current_model_name = self.params.get("ModelName", encoding='utf-8') - if "(Default)" in current_model_name and current_model_name != DEFAULT_MODEL_NAME: + if "(Default)" in current_model_name and current_model_name != DEFAULT_CLASSIC_MODEL_NAME: self.params.put_nonblocking("ModelName", current_model_name.replace(" (Default)", "")) available_models = self.params.get("AvailableModels", encoding='utf-8') @@ -162,31 +197,11 @@ def validate_models(self): for model_file in os.listdir(MODELS_PATH): model_name = model_file.replace(".thneed", "") if model_name not in available_models.split(','): - reason = "Model is not in the list of available models" if model_name == current_model: - self.params.put_nonblocking("Model", DEFAULT_MODEL) - self.params.put_nonblocking("ModelName", DEFAULT_MODEL_NAME) + self.params.put_nonblocking("Model", DEFAULT_CLASSIC_MODEL) + self.params.put_nonblocking("ModelName", DEFAULT_CLASSIC_MODEL_NAME) delete_file(os.path.join(MODELS_PATH, model_file)) - print(f"Deleted model file: {model_file} - Reason: {reason}") - - def copy_default_model(self): - default_model_path = os.path.join(MODELS_PATH, f"{DEFAULT_MODEL}.thneed") - source_path = os.path.join(BASEDIR, "selfdrive", "classic_modeld", "models", "supercombo.thneed") - - if os.path.isfile(source_path) and not os.path.isfile(default_model_path): - shutil.copyfile(source_path, default_model_path) - print(f"Copied default model from {source_path} to {default_model_path}") - else: - print(f"Source default model not found at {source_path}. Exiting...") - - sgo_model_path = os.path.join(MODELS_PATH, "secret-good-openpilot.thneed") - source_path = os.path.join(BASEDIR, "selfdrive", "modeld", "models", "supercombo.thneed") - - if os.path.isfile(source_path) and not os.path.isfile(sgo_model_path): - shutil.copyfile(source_path, sgo_model_path) - print(f"Copied 'secret-good-openpilot' model from {source_path} to {sgo_model_path}") - else: - print(f"Source 'secret-good-openpilot' model not found at {source_path}. Exiting...") + print(f"Deleted model file: {model_file} - Reason: Model is not in the list of available models") def update_models(self, boot_run=False): if boot_run: @@ -197,12 +212,12 @@ def update_models(self, boot_run=False): print("GitHub and GitLab are offline...") return + model_info = self.fetch_models(f"{repo_url}Versions/model_names_{VERSION}.json") + if model_info: + self.update_model_params(model_info, repo_url) + if boot_run: self.validate_models() - else: - model_info = self.fetch_models(f"{repo_url}Versions/model_names_{VERSION}.json") - if model_info: - self.update_model_params(model_info, repo_url) def download_all_models(self): repo_url = get_repository_url() @@ -230,10 +245,8 @@ def download_all_models(self): if not os.path.isfile(os.path.join(MODELS_PATH, f"{model}.thneed")): model_index = available_models.index(model) model_name = available_model_names[model_index] - cleaned_model_name = re.sub(r'[🗺️👀📡]', '', model_name).strip() print(f"Downloading model: {cleaned_model_name}") - self.queue_model_download(model, cleaned_model_name) while self.params_memory.get(self.download_param, encoding='utf-8'): diff --git a/selfdrive/frogpilot/assets/random_events/icons/this_is_fine.gif b/selfdrive/frogpilot/assets/random_events/icons/this_is_fine.gif new file mode 100644 index 00000000000000..3cc427f760cb64 Binary files /dev/null and b/selfdrive/frogpilot/assets/random_events/icons/this_is_fine.gif differ diff --git a/selfdrive/frogpilot/assets/random_events/sounds/this_is_fine.wav b/selfdrive/frogpilot/assets/random_events/sounds/this_is_fine.wav new file mode 100644 index 00000000000000..e43f4e59ccf6a8 Binary files /dev/null and b/selfdrive/frogpilot/assets/random_events/sounds/this_is_fine.wav differ diff --git a/selfdrive/frogpilot/assets/theme_manager.py b/selfdrive/frogpilot/assets/theme_manager.py index dabf060885eb6d..c5b9a874e97a99 100644 --- a/selfdrive/frogpilot/assets/theme_manager.py +++ b/selfdrive/frogpilot/assets/theme_manager.py @@ -1,4 +1,5 @@ import glob +import json import os import re import requests @@ -9,15 +10,15 @@ from dateutil import easter from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.selfdrive.frogpilot.assets.download_functions import GITHUB_URL, GITLAB_URL, download_file, get_repository_url, handle_error, handle_request_error, link_valid, verify_download -from openpilot.selfdrive.frogpilot.frogpilot_functions import ACTIVE_THEME_PATH, THEME_SAVE_PATH, update_frogpilot_toggles +from openpilot.selfdrive.frogpilot.assets.download_functions import GITHUB_URL, GITLAB_URL, download_file, get_repository_url, handle_error, handle_request_error, verify_download +from openpilot.selfdrive.frogpilot.frogpilot_variables import ACTIVE_THEME_PATH, THEME_SAVE_PATH, params, params_memory, update_frogpilot_toggles CANCEL_DOWNLOAD_PARAM = "CancelThemeDownload" DOWNLOAD_PROGRESS_PARAM = "ThemeDownloadProgress" -def update_theme_asset(asset_type, theme, holiday_theme, params): + +def update_theme_asset(asset_type, theme, holiday_theme): save_location = os.path.join(ACTIVE_THEME_PATH, asset_type) if holiday_theme: @@ -49,6 +50,7 @@ def update_theme_asset(asset_type, theme, holiday_theme, params): shutil.copytree(asset_location, save_location) print(f"Copied {asset_location} to {save_location}") + def update_wheel_image(image, holiday_theme=None, random_event=True): wheel_save_location = os.path.join(ACTIVE_THEME_PATH, "steering_wheel") @@ -74,24 +76,99 @@ def update_wheel_image(image, holiday_theme=None, random_event=True): print(f"Copied {source_file} to {destination_file}") break + class ThemeManager: def __init__(self): - self.params = Params() - self.params_memory = Params("/dev/shm/params") - self.previous_assets = {} @staticmethod def calculate_thanksgiving(year): november_first = date(year, 11, 1) - day_of_week = november_first.weekday() - return november_first + timedelta(days=(3 - day_of_week + 21) % 7 + 21) + days_to_thursday = (3 - november_first.weekday()) % 7 + first_thursday = november_first + timedelta(days=days_to_thursday) + thanksgiving_date = first_thursday + timedelta(days=21) + return thanksgiving_date @staticmethod def is_within_week_of(target_date, now): start_of_week = target_date - timedelta(days=target_date.weekday()) - end_of_week = start_of_week + timedelta(days=6) - return start_of_week <= now <= end_of_week + return start_of_week <= now < target_date + + @staticmethod + def fetch_files(url): + try: + response = requests.get(url, timeout=10) + response.raise_for_status() + return [name for name in re.findall(r'href="[^"]*\/blob\/[^"]*\/([^"]*)"', response.text) if name.lower() != "license"] + except Exception as error: + handle_request_error(error, None, None, None, None) + return [] + + @staticmethod + def fetch_assets(repo_url): + repo = "FrogAi/FrogPilot-Resources" + branches = ["Themes", "Distance-Icons", "Steering-Wheels"] + + assets = { + "themes": {}, + "distance_icons": [], + "wheels": [] + } + + if "github" in repo_url: + base_api_url = "https://api.github.com/repos" + elif "gitlab" in repo_url: + base_api_url = "https://gitlab.com/api/v4/projects" + repo = repo.replace("/", "%2F") + else: + print(f"Unsupported repository URL: {repo_url}") + return assets + + for branch in branches: + if "github" in repo_url: + api_url = f"{base_api_url}/{repo}/git/trees/{branch}?recursive=1" + elif "gitlab" in repo_url: + api_url = f"{base_api_url}/{repo}/repository/tree?ref={branch}&recursive=true" + + try: + print(f"Fetching assets from branch '{branch}': {api_url}") + response = requests.get(api_url, timeout=10) + response.raise_for_status() + content = response.json() + + if "github" in repo_url: + items = content.get('tree', []) + elif "gitlab" in repo_url: + items = content + + for item in items: + if item["type"] != "blob": + continue + + item_path = item["path"].lower() + if branch == "Themes": + theme_name = item["path"].split('/')[0] + assets["themes"].setdefault(theme_name, set()) + if "icons" in item_path: + assets["themes"][theme_name].add("icons") + elif "signals" in item_path: + assets["themes"][theme_name].add("signals") + elif "sounds" in item_path: + assets["themes"][theme_name].add("sounds") + else: + assets["themes"][theme_name].add("colors") + + elif branch == "Distance-Icons": + assets["distance_icons"].append(item["path"]) + + elif branch == "Steering-Wheels": + assets["wheels"].append(item["path"]) + + except requests.exceptions.RequestException as error: + print(f"Error occurred when fetching from branch '{branch}': {error}") + + assets["themes"] = {k: list(v) for k, v in assets["themes"].items()} + return assets def update_holiday(self): now = date.today() @@ -115,30 +192,30 @@ def update_holiday(self): for holiday, holiday_date in holidays.items(): if (holiday.endswith("_week") and self.is_within_week_of(holiday_date, now)) or (now == holiday_date): if holiday != self.previous_assets.get("holiday_theme"): - self.params.put("CurrentHolidayTheme", holiday) - self.params_memory.put_bool("UpdateTheme", True) + params.put("CurrentHolidayTheme", holiday) + params_memory.put_bool("UpdateTheme", True) return if "holiday_theme" in self.previous_assets: - self.params.remove("CurrentHolidayTheme") - self.params_memory.put_bool("UpdateTheme", True) + params.remove("CurrentHolidayTheme") + params_memory.put_bool("UpdateTheme", True) self.previous_assets.pop("holiday_theme") def update_active_theme(self): if not os.path.exists(THEME_SAVE_PATH): return - holiday_themes = self.params.get_bool("HolidayThemes") - current_holiday_theme = self.params.get("CurrentHolidayTheme", encoding='utf-8') if holiday_themes else None + holiday_themes = params.get_bool("HolidayThemes") + current_holiday_theme = params.get("CurrentHolidayTheme", encoding='utf-8') if holiday_themes else None - if not current_holiday_theme and self.params.get_bool("PersonalizeOpenpilot"): + if not current_holiday_theme and params.get_bool("PersonalizeOpenpilot"): asset_mappings = { - "color_scheme": ("colors", self.params.get("CustomColors", encoding='utf-8')), - "distance_icons": ("distance_icons", self.params.get("CustomDistanceIcons", encoding='utf-8')), - "icon_pack": ("icons", self.params.get("CustomIcons", encoding='utf-8')), - "sound_pack": ("sounds", self.params.get("CustomSounds", encoding='utf-8')), - "turn_signal_pack": ("signals", self.params.get("CustomSignals", encoding='utf-8')), - "wheel_image": ("wheel_image", self.params.get("WheelIcon", encoding='utf-8')) + "color_scheme": ("colors", params.get("CustomColors", encoding='utf-8')), + "distance_icons": ("distance_icons", params.get("CustomDistanceIcons", encoding='utf-8')), + "icon_pack": ("icons", params.get("CustomIcons", encoding='utf-8')), + "sound_pack": ("sounds", params.get("CustomSounds", encoding='utf-8')), + "turn_signal_pack": ("signals", params.get("CustomSignals", encoding='utf-8')), + "wheel_image": ("wheel_image", params.get("WheelIcon", encoding='utf-8')) } else: asset_mappings = { @@ -150,6 +227,7 @@ def update_active_theme(self): "wheel_image": ("wheel_image", "stock") } + theme_changed = False for asset, (asset_type, current_value) in asset_mappings.items(): if current_value != self.previous_assets.get(asset) or current_holiday_theme != self.previous_assets.get("holiday_theme"): print(f"Updating {asset}: {asset_type} with value {current_holiday_theme if current_holiday_theme else current_value}") @@ -157,26 +235,29 @@ def update_active_theme(self): if asset_type == "wheel_image": update_wheel_image(current_value, current_holiday_theme, random_event=False) else: - update_theme_asset(asset_type, current_value, current_holiday_theme, self.params) + update_theme_asset(asset_type, current_value, current_holiday_theme) self.previous_assets[asset] = current_value + theme_changed = True - self.previous_assets["holiday_theme"] = current_holiday_theme - update_frogpilot_toggles() + if theme_changed: + if current_holiday_theme: + self.previous_assets["holiday_theme"] = current_holiday_theme + update_frogpilot_toggles() def extract_zip(self, zip_file, extract_path): print(f"Extracting {zip_file} to {extract_path}") with zipfile.ZipFile(zip_file, 'r') as zip_ref: zip_ref.extractall(extract_path) os.remove(zip_file) - print(f"Extraction completed and zip file deleted.") + print(f"Extraction completed and zip file deleted") def handle_existing_theme(self, theme_name, theme_param): print(f"Theme {theme_name} already exists, skipping download...") - self.params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Theme already exists...") - self.params_memory.remove(theme_param) + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Theme already exists...") + params_memory.remove(theme_param) - def handle_verification_failure(self, extentions, theme_component, theme_name, theme_param, download_path): + def handle_verification_failure(self, extensions, theme_component, theme_name, theme_param, download_path): if theme_component == "distance_icons": download_link = f"{GITLAB_URL}Distance-Icons/{theme_name}" elif theme_component == "steering_wheels": @@ -184,83 +265,66 @@ def handle_verification_failure(self, extentions, theme_component, theme_name, t else: download_link = f"{GITLAB_URL}Themes/{theme_name}/{theme_component}" - for ext in extentions: + for ext in extensions: theme_path = download_path + ext + temp_theme_path = f"{os.path.splitext(theme_path)[0]}_temp{ext}" theme_url = download_link + ext print(f"Downloading theme from GitLab: {theme_name}") - download_file(CANCEL_DOWNLOAD_PARAM, theme_path, DOWNLOAD_PROGRESS_PARAM, theme_url, theme_param, self.params_memory) + download_file(CANCEL_DOWNLOAD_PARAM, theme_path, temp_theme_path, DOWNLOAD_PROGRESS_PARAM, theme_url, theme_param, params_memory) - if verify_download(theme_path, theme_url): + if verify_download(theme_path, temp_theme_path, theme_url): print(f"Theme {theme_name} downloaded and verified successfully from GitLab!") if ext == ".zip": - self.params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Unpacking theme...") + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Unpacking theme...") self.extract_zip(theme_path, os.path.join(THEME_SAVE_PATH, theme_name)) - self.params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") - self.params_memory.remove(theme_param) + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") + params_memory.remove(theme_param) return True - handle_error(download_path, "GitLab verification failed", "Verification failed", theme_param, DOWNLOAD_PROGRESS_PARAM, self.params_memory) + handle_error(download_path, "GitLab verification failed", "Verification failed", theme_param, DOWNLOAD_PROGRESS_PARAM, params_memory) return False def download_theme(self, theme_component, theme_name, theme_param): repo_url = get_repository_url() if not repo_url: - handle_error(None, "GitHub and GitLab are offline...", "Repository unavailable", theme_param, DOWNLOAD_PROGRESS_PARAM, self.params_memory) + handle_error(None, "GitHub and GitLab are offline...", "Repository unavailable", theme_param, DOWNLOAD_PROGRESS_PARAM, params_memory) return if theme_component == "distance_icons": download_link = f"{repo_url}Distance-Icons/{theme_name}" download_path = os.path.join(THEME_SAVE_PATH, theme_component, theme_name) - extentions = [".zip"] + extensions = [".zip"] elif theme_component == "steering_wheels": download_link = f"{repo_url}Steering-Wheels/{theme_name}" download_path = os.path.join(THEME_SAVE_PATH, theme_component, theme_name) - extentions = [".gif", ".png"] + extensions = [".gif", ".png"] else: download_link = f"{repo_url}Themes/{theme_name}/{theme_component}" download_path = os.path.join(THEME_SAVE_PATH, "theme_packs", theme_name, theme_component) - extentions = [".zip"] + extensions = [".zip"] - for ext in extentions: + for ext in extensions: theme_path = download_path + ext + temp_theme_path = f"{os.path.splitext(theme_path)[0]}_temp{ext}" + if os.path.isfile(theme_path): - handle_error(theme_path, "Theme already exists...", "Theme already exists...", theme_param, DOWNLOAD_PROGRESS_PARAM, self.params_memory) + handle_error(theme_path, "Theme already exists...", "Theme already exists...", theme_param, DOWNLOAD_PROGRESS_PARAM, params_memory) return theme_url = download_link + ext print(f"Downloading theme from GitHub: {theme_name}") - download_file(CANCEL_DOWNLOAD_PARAM, theme_path, DOWNLOAD_PROGRESS_PARAM, theme_url, theme_param, self.params_memory) + download_file(CANCEL_DOWNLOAD_PARAM, theme_path, temp_theme_path, DOWNLOAD_PROGRESS_PARAM, theme_url, theme_param, params_memory) - if verify_download(theme_path, theme_url): + if verify_download(theme_path, temp_theme_path, theme_url): print(f"Theme {theme_name} downloaded and verified successfully from GitHub!") if ext == ".zip": - self.params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Unpacking theme...") + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Unpacking theme...") self.extract_zip(theme_path, download_path) - self.params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") - self.params_memory.remove(theme_param) + params_memory.put(DOWNLOAD_PROGRESS_PARAM, "Downloaded!") + params_memory.remove(theme_param) return - self.handle_verification_failure(extentions, theme_component, theme_name, theme_param, download_path) - - @staticmethod - def fetch_files(url): - try: - response = requests.get(url, timeout=10) - response.raise_for_status() - return [name for name in re.findall(r'href="[^"]*\/blob\/[^"]*\/([^"]*)"', response.text) if name.lower() != "license"] - except Exception as error: - handle_request_error(error, None, None, None, None) - return [] - - @staticmethod - def fetch_folders(url): - try: - response = requests.get(url, timeout=10) - response.raise_for_status() - return re.findall(r'href="[^"]*\/tree\/[^"]*\/([^"]*)"', response.text) - except Exception as error: - handle_request_error(error, None, None, None, None) - return [] + self.handle_verification_failure(extensions, theme_component, theme_name, theme_param, download_path) def update_theme_params(self, downloadable_colors, downloadable_distance_icons, downloadable_icons, downloadable_signals, downloadable_sounds, downloadable_wheels): def filter_existing_assets(assets, subfolder): @@ -271,27 +335,27 @@ def filter_existing_assets(assets, subfolder): } return sorted(set(assets) - existing_themes) - self.params.put("DownloadableColors", ','.join(filter_existing_assets(downloadable_colors, "colors"))) - print("Colors list updated successfully.") + params.put("DownloadableColors", ','.join(filter_existing_assets(downloadable_colors, "colors"))) + print("Colors list updated successfully") distance_icons_directory = os.path.join(THEME_SAVE_PATH, "distance_icons") - self.params.put("DownloadableDistanceIcons", ','.join(sorted(set(downloadable_distance_icons) - { + params.put("DownloadableDistanceIcons", ','.join(sorted(set(downloadable_distance_icons) - { distance_icons.replace('_', ' ').split('.')[0].title() for distance_icons in os.listdir(distance_icons_directory) })) ) - self.params.put("DownloadableIcons", ','.join(filter_existing_assets(downloadable_icons, "icons"))) - print("Icons list updated successfully.") + params.put("DownloadableIcons", ','.join(filter_existing_assets(downloadable_icons, "icons"))) + print("Icons list updated successfully") - self.params.put("DownloadableSignals", ','.join(filter_existing_assets(downloadable_signals, "signals"))) - print("Signals list updated successfully.") + params.put("DownloadableSignals", ','.join(filter_existing_assets(downloadable_signals, "signals"))) + print("Signals list updated successfully") - self.params.put("DownloadableSounds", ','.join(filter_existing_assets(downloadable_sounds, "sounds"))) - print("Sounds list updated successfully.") + params.put("DownloadableSounds", ','.join(filter_existing_assets(downloadable_sounds, "sounds"))) + print("Sounds list updated successfully") wheel_directory = os.path.join(THEME_SAVE_PATH, "steering_wheels") - self.params.put("DownloadableWheels", ','.join(sorted(set(downloadable_wheels) - { + params.put("DownloadableWheels", ','.join(sorted(set(downloadable_wheels) - { wheel.replace('_', ' ').split('.')[0].title() for wheel in os.listdir(wheel_directory) if wheel != "img_chffr_wheel.png" })) @@ -308,7 +372,7 @@ def validate_themes(self): } for theme_param, theme_component in asset_mappings.items(): - theme_name = self.params.get(theme_param, encoding='utf-8') + theme_name = params.get(theme_param, encoding='utf-8') if not theme_name or theme_name == "stock": continue @@ -333,6 +397,7 @@ def validate_themes(self): def update_themes(self, boot_run=False): if not os.path.exists(THEME_SAVE_PATH): + print(f"Theme save path does not exist: {THEME_SAVE_PATH}") return repo_url = get_repository_url() @@ -343,34 +408,34 @@ def update_themes(self, boot_run=False): if boot_run: self.validate_themes() - if repo_url == GITHUB_URL: - base_url = "https://github.com/FrogAi/FrogPilot-Resources/blob/Themes/" - distance_icons_files = self.fetch_files("https://github.com/FrogAi/FrogPilot-Resources/blob/Distance-Icons") - wheel_files = self.fetch_files("https://github.com/FrogAi/FrogPilot-Resources/blob/Steering-Wheels") - else: - base_url = "https://gitlab.com/FrogAi/FrogPilot-Resources/-/blob/Themes/" - distance_icons_files = self.fetch_files("https://github.com/FrogAi/FrogPilot-Resources/blob/Distance-Icons") - wheel_files = self.fetch_files("https://gitlab.com/FrogAi/FrogPilot-Resources/-/blob/Steering-Wheels") + assets = self.fetch_assets(repo_url) - theme_folders = self.fetch_folders(base_url) downloadable_colors = [] downloadable_icons = [] downloadable_signals = [] downloadable_sounds = [] - for theme in theme_folders: + for theme, available_assets in assets["themes"].items(): theme_name = theme.replace('_', ' ').split('.')[0].title() + print(f"Checking theme: {theme_name}") - if link_valid(f"{base_url}{theme}/colors.zip"): + if "colors" in available_assets: downloadable_colors.append(theme_name) - if link_valid(f"{base_url}{theme}/icons.zip"): + if "icons" in available_assets: downloadable_icons.append(theme_name) - if link_valid(f"{base_url}{theme}/signals.zip"): + if "signals" in available_assets: downloadable_signals.append(theme_name) - if link_valid(f"{base_url}{theme}/sounds.zip"): + if "sounds" in available_assets: downloadable_sounds.append(theme_name) - downloadable_distance_icons = [distance_icons.replace('_', ' ').split('.')[0].title() for distance_icons in distance_icons_files] - downloadable_wheels = [wheel.replace('_', ' ').split('.')[0].title() for wheel in wheel_files] + downloadable_distance_icons = [distance_icon.replace('_', ' ').split('.')[0].title() for distance_icon in assets["distance_icons"]] + downloadable_wheels = [wheel.replace('_', ' ').split('.')[0].title() for wheel in assets["wheels"]] + + print(f"Downloadable Colors: {downloadable_colors}") + print(f"Downloadable Icons: {downloadable_icons}") + print(f"Downloadable Signals: {downloadable_signals}") + print(f"Downloadable Sounds: {downloadable_sounds}") + print(f"Downloadable Distance Icons: {downloadable_distance_icons}") + print(f"Downloadable Wheels: {downloadable_wheels}") self.update_theme_params(downloadable_colors, downloadable_distance_icons, downloadable_icons, downloadable_signals, downloadable_sounds, downloadable_wheels) diff --git a/selfdrive/frogpilot/assets/toggle_icons/advanced_quality_of_life.png b/selfdrive/frogpilot/assets/toggle_icons/advanced_quality_of_life.png deleted file mode 100644 index e84f90ac0e939c..00000000000000 Binary files a/selfdrive/frogpilot/assets/toggle_icons/advanced_quality_of_life.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_accessibility.png b/selfdrive/frogpilot/assets/toggle_icons/icon_accessibility.png new file mode 100644 index 00000000000000..cf80f5ef96b4b9 Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_accessibility.png differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced.png b/selfdrive/frogpilot/assets/toggle_icons/icon_advanced.png deleted file mode 100644 index 1b3c52390d8c6a..00000000000000 Binary files a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_calibration.png b/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_calibration.png deleted file mode 100644 index 25e9f247c6e5e0..00000000000000 Binary files a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_calibration.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_device.png b/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_device.png index 6d577060820f17..f849ec1324c716 100644 Binary files a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_device.png and b/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_device.png differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_longitudinal_tune.png b/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_longitudinal_tune.png deleted file mode 100644 index 72a7e4057ac14e..00000000000000 Binary files a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_longitudinal_tune.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_model.png b/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_model.png deleted file mode 100644 index c299a1e793e24f..00000000000000 Binary files a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_model.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_personality.png b/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_personality.png deleted file mode 100644 index 2ce09a567a1c7b..00000000000000 Binary files a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_personality.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_road.png b/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_road.png deleted file mode 100644 index 76513344845127..00000000000000 Binary files a/selfdrive/frogpilot/assets/toggle_icons/icon_advanced_road.png and /dev/null differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_customization.png b/selfdrive/frogpilot/assets/toggle_icons/icon_customization.png new file mode 100644 index 00000000000000..8c2fa95ca96586 Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_customization.png differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_green_light.png b/selfdrive/frogpilot/assets/toggle_icons/icon_green_light.png index cb47d7a81d4d58..b0c06f82134287 100644 Binary files a/selfdrive/frogpilot/assets/toggle_icons/icon_green_light.png and b/selfdrive/frogpilot/assets/toggle_icons/icon_green_light.png differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_personality.png b/selfdrive/frogpilot/assets/toggle_icons/icon_personality.png new file mode 100644 index 00000000000000..4f552c59b4752b Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_personality.png differ diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 53789076f6756b..313b3bf6fbbccd 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -1,7 +1,6 @@ import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV -from openpilot.common.params import Params from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_UNSET from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE @@ -12,13 +11,11 @@ from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_events import FrogPilotEvents from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_following import FrogPilotFollowing from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_vcruise import FrogPilotVCruise -from openpilot.selfdrive.frogpilot.frogpilot_functions import MovingAverageCalculator, calculate_lane_width, calculate_road_curvature, update_frogpilot_toggles +from openpilot.selfdrive.frogpilot.frogpilot_utilities import MovingAverageCalculator, calculate_lane_width, calculate_road_curvature from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, MODEL_LENGTH, NON_DRIVING_GEARS, PLANNER_TIME, THRESHOLD class FrogPilotPlanner: def __init__(self): - self.params_memory = Params("/dev/shm/params") - self.cem = ConditionalExperimentalMode(self) self.frogpilot_acceleration = FrogPilotAcceleration(self) self.frogpilot_events = FrogPilotEvents(self) @@ -29,23 +26,21 @@ def __init__(self): self.tracking_lead_mac = MovingAverageCalculator() self.lateral_check = False - self.lead_departing = False self.model_stopped = False self.slower_lead = False - self.taking_curve_quickly = False self.tracking_lead = False self.model_length = 0 self.road_curvature = 1 - self.tracking_lead_distance = 0 self.v_cruise = 0 def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarless_model, radarState, frogpilot_toggles): if radarless_model: model_leads = list(modelData.leadsV3) if len(model_leads) > 0: + distance_offset = frogpilot_toggles.increased_stopped_distance if not frogpilotCarState.trafficModeActive else 0 model_lead = model_leads[0] - self.lead_one.update(model_lead.x[0], model_lead.y[0], model_lead.v[0], model_lead.a[0], model_lead.prob) + self.lead_one.update(model_lead.x[0] - distance_offset, model_lead.y[0], model_lead.v[0], model_lead.a[0], model_lead.prob) else: self.lead_one.reset() else: @@ -55,41 +50,25 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState v_ego = max(carState.vEgo, 0) v_lead = self.lead_one.vLead - driving_gear = carState.gearShifter not in NON_DRIVING_GEARS - - distance_offset = max(frogpilot_toggles.increase_stopped_distance + min(10 - v_ego, 0), 0) if not frogpilotCarState.trafficModeActive else 0 - lead_distance = self.lead_one.dRel - distance_offset - stopping_distance = STOP_DISTANCE + distance_offset - self.frogpilot_acceleration.update(controlsState, frogpilotCarState, v_cruise, 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 driving_gear: - self.cem.update(carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles) + if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateralActive) and carState.gearShifter not in NON_DRIVING_GEARS: + self.cem.update(carState, frogpilotCarState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles) else: self.cem.stop_light_detected = False - self.frogpilot_events.update(carState, controlsState, frogpilotCarControl, frogpilotCarState, modelData, frogpilot_toggles) - self.frogpilot_following.update(carState.aEgo, controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles) + self.frogpilot_events.update(carState, controlsState, frogpilotCarControl, frogpilotCarState, self.lead_one.dRel, modelData, v_lead, frogpilot_toggles) + self.frogpilot_following.update(carState.aEgo, controlsState, frogpilotCarState, self.lead_one.dRel, v_ego, v_lead, frogpilot_toggles) - check_lane_width = frogpilot_toggles.adjacent_lanes or frogpilot_toggles.adjacent_path_metrics or frogpilot_toggles.blind_spot_path or frogpilot_toggles.lane_detection - if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed: + check_lane_width = frogpilot_toggles.adjacent_paths or frogpilot_toggles.adjacent_path_metrics or frogpilot_toggles.blind_spot_path or frogpilot_toggles.lane_detection + if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed or frogpilot_toggles.adjacent_lead_tracking: self.lane_width_left = calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0]) self.lane_width_right = calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1]) else: self.lane_width_left = 0 self.lane_width_right = 0 - if frogpilot_toggles.lead_departing_alert and self.tracking_lead and driving_gear and carState.standstill: - if self.tracking_lead_distance == 0: - self.tracking_lead_distance = lead_distance - - self.lead_departing = lead_distance - self.tracking_lead_distance > 1 - self.lead_departing &= v_lead > 1 - else: - self.lead_departing = False - self.tracking_lead_distance = 0 - self.lateral_check = v_ego >= frogpilot_toggles.pause_lateral_below_speed self.lateral_check |= frogpilot_toggles.pause_lateral_below_signal and not (carState.leftBlinker or carState.rightBlinker) self.lateral_check |= carState.standstill @@ -100,26 +79,20 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState self.road_curvature = calculate_road_curvature(modelData, v_ego) if not carState.standstill else 1 - if frogpilot_toggles.random_events and v_ego > CRUISING_SPEED and driving_gear: - self.taking_curve_quickly = v_ego > (1 / self.road_curvature)**0.75 * 2 > CRUISING_SPEED * 2 and abs(carState.steeringAngleDeg) > 30 - else: - self.taking_curve_quickly = False - - self.tracking_lead = self.set_lead_status(lead_distance, stopping_distance, v_ego) + 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) - if self.frogpilot_events.frame == 1: # Force update to check the current state of "Always On Lateral" and holiday theme - update_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 - def set_lead_status(self, lead_distance, stopping_distance, v_ego): following_lead = self.lead_one.status - following_lead &= 1 < lead_distance < self.model_length + stopping_distance + following_lead &= 1 < self.lead_one.dRel < self.model_length + STOP_DISTANCE + distance_offset following_lead &= v_ego > CRUISING_SPEED or self.tracking_lead self.tracking_lead_mac.add_data(following_lead) return self.tracking_lead_mac.get_moving_average() >= THRESHOLD - def publish(self, sm, pm, frogpilot_toggles): + def publish(self, sm, pm, frogpilot_toggles, toggles_updated): frogpilot_plan_send = messaging.new_message('frogpilotPlan') frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotPlan = frogpilot_plan_send.frogpilotPlan @@ -142,6 +115,7 @@ def publish(self, sm, pm, frogpilot_toggles): frogpilotPlan.experimentalMode = self.cem.experimental_mode or self.frogpilot_vcruise.slc.experimental_mode frogpilotPlan.forcingStop = self.frogpilot_vcruise.forcing_stop + frogpilotPlan.forcingStopLength = self.frogpilot_vcruise.tracked_model_length frogpilotPlan.frogpilotEvents = self.frogpilot_events.events.to_msg() @@ -162,6 +136,8 @@ def publish(self, sm, pm, frogpilot_toggles): frogpilotPlan.speedLimitChanged = self.frogpilot_vcruise.speed_limit_changed frogpilotPlan.unconfirmedSlcSpeedLimit = self.frogpilot_vcruise.slc.desired_speed_limit + frogpilotPlan.togglesUpdated = toggles_updated + frogpilotPlan.vCruise = self.v_cruise pm.send('frogpilotPlan', frogpilot_plan_send) diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py index 4930774b3b66dc..2a1e292bde444d 100644 --- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -1,31 +1,28 @@ -from openpilot.common.params import Params - -from openpilot.selfdrive.frogpilot.frogpilot_functions import MovingAverageCalculator -from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD +from openpilot.selfdrive.frogpilot.frogpilot_utilities import MovingAverageCalculator +from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory class ConditionalExperimentalMode: def __init__(self, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner - self.params_memory = Params("/dev/shm/params") - self.curvature_mac = MovingAverageCalculator() + self.slow_lead_mac = MovingAverageCalculator() self.stop_light_mac = MovingAverageCalculator() self.curve_detected = False self.experimental_mode = False self.stop_light_detected = False - def update(self, carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles): + def update(self, carState, frogpilotCarState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles): if frogpilot_toggles.experimental_mode_via_press: - self.status_value = self.params_memory.get_int("CEStatus") + self.status_value = params_memory.get_int("CEStatus") else: self.status_value = 0 if self.status_value not in {1, 2, 3, 4, 5, 6} and not carState.standstill: - self.update_conditions(self.frogpilot_planner.tracking_lead, v_ego, v_lead, frogpilot_toggles) + self.update_conditions(frogpilotCarState, self.frogpilot_planner.tracking_lead, v_ego, v_lead, frogpilot_toggles) self.experimental_mode = self.check_conditions(carState, frogpilotNavigation, modelData, self.frogpilot_planner.frogpilot_following.following_lead, v_ego, v_lead, frogpilot_toggles) - self.params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else 0) + params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else 0) else: self.experimental_mode = self.status_value in {2, 4, 6} or carState.standstill and self.experimental_mode self.stop_light_detected &= self.status_value not in {1, 2, 3, 4, 5, 6} @@ -63,32 +60,37 @@ def check_conditions(self, carState, frogpilotNavigation, modelData, following_l if self.frogpilot_planner.frogpilot_vcruise.slc.experimental_mode: self.status_value = 17 return True - return False - def update_conditions(self, tracking_lead, v_ego, v_lead, frogpilot_toggles): + def update_conditions(self, frogpilotCarState, tracking_lead, v_ego, v_lead, frogpilot_toggles): self.curve_detection(tracking_lead, v_ego, frogpilot_toggles) self.slow_lead(tracking_lead, v_lead, frogpilot_toggles) - self.stop_sign_and_light(tracking_lead, v_ego, frogpilot_toggles) + self.stop_sign_and_light(frogpilotCarState, tracking_lead, v_ego, frogpilot_toggles) def curve_detection(self, tracking_lead, v_ego, frogpilot_toggles): - curve_detected = (1 / self.frogpilot_planner.road_curvature)**0.5 < v_ego - curve_active = (0.9 / self.frogpilot_planner.road_curvature)**0.5 < v_ego and self.curve_detected + if v_ego > CRUISING_SPEED: + curve_detected = (1 / self.frogpilot_planner.road_curvature)**0.5 < v_ego + curve_active = (0.9 / self.frogpilot_planner.road_curvature)**0.5 < v_ego and self.curve_detected - self.curvature_mac.add_data(curve_detected or curve_active) - self.curve_detected = self.curvature_mac.get_moving_average() >= THRESHOLD + self.curvature_mac.add_data(curve_detected or curve_active) + self.curve_detected = self.curvature_mac.get_moving_average() >= THRESHOLD + else: + self.curvature_mac.reset_data() + self.curve_detected = False def slow_lead(self, tracking_lead, v_lead, frogpilot_toggles): if tracking_lead: - slower_lead = self.frogpilot_planner.frogpilot_following.slower_lead and frogpilot_toggles.conditional_slower_lead + slower_lead = frogpilot_toggles.conditional_slower_lead and self.frogpilot_planner.frogpilot_following.slower_lead stopped_lead = frogpilot_toggles.conditional_stopped_lead and v_lead < 1 - self.slow_lead_detected = slower_lead or stopped_lead + self.slow_lead_mac.add_data(slower_lead or stopped_lead) + self.slow_lead_detected = self.slow_lead_mac.get_moving_average() >= THRESHOLD else: + self.slow_lead_mac.reset_data() self.slow_lead_detected = False - def stop_sign_and_light(self, tracking_lead, v_ego, frogpilot_toggles): - if not (self.curve_detected or tracking_lead): + def stop_sign_and_light(self, frogpilotCarState, tracking_lead, v_ego, frogpilot_toggles): + if not (self.curve_detected or tracking_lead or frogpilotCarState.trafficModeActive): model_stopping = self.frogpilot_planner.model_length < v_ego * frogpilot_toggles.conditional_model_stop_time self.stop_light_mac.add_data(self.frogpilot_planner.model_stopped or model_stopping) diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py b/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py index e3d7c70b5d0e74..a81afdf242cbdb 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py @@ -3,16 +3,16 @@ from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel -from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED +from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT -A_CRUISE_MIN_ECO = A_CRUISE_MIN / 4 -A_CRUISE_MIN_SPORT = A_CRUISE_MIN / 2 +A_CRUISE_MIN_ECO = A_CRUISE_MIN / 2 +A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2 - # MPH = [ 0., 11, 22, 34, 45, 56, 89] -A_CRUISE_MAX_BP_CUSTOM = [ 0., 5., 10., 15., 20., 25., 40.] + # MPH = [0.0, 11, 22, 34, 45, 56, 89] +A_CRUISE_MAX_BP_CUSTOM = [0.0, 5., 10., 15., 20., 25., 40.] A_CRUISE_MAX_VALS_ECO = [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2] A_CRUISE_MAX_VALS_SPORT = [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6] -A_CRUISE_MAX_VALS_SPORT_PLUS = [4.0, 3.5, 3.0, 2.0, 1.0, 0.8, 0.6] +A_CRUISE_MAX_VALS_SPORT_PLUS = [4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] def get_max_accel_eco(v_ego): return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO) @@ -23,11 +23,11 @@ def get_max_accel_sport(v_ego): def get_max_accel_sport_plus(v_ego): return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS) -def get_max_accel_ramp_off(max_accel, v_cruise, v_ego): - return interp(v_ego, [0., v_cruise * 0.5, v_cruise * 0.75, v_cruise], [max_accel, max_accel, max_accel / 2, max_accel / 4]) +def get_max_accel_low_speeds(max_accel, v_cruise): + return interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel]) -def get_max_accel_ramp_off_highway(max_accel, v_cruise, v_ego): - return interp(v_ego, [0., v_cruise * 0.75, v_cruise], [max_accel, max_accel, max_accel / 2]) +def get_max_accel_ramp_off(max_accel, v_cruise, v_ego): + return interp(v_cruise - v_ego, [0., 1., 5., 10.], [0., 0.5, 1.0, max_accel]) def get_max_allowed_accel(v_ego): return interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0]) # ISO 15622:2018 @@ -36,17 +36,8 @@ class FrogPilotAcceleration: def __init__(self, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner - self.acceleration_jerk = 0 - self.base_acceleration_jerk = 0 - self.base_speed_jerk = 0 - self.danger_jerk = 0 self.max_accel = 0 self.min_accel = 0 - self.safe_obstacle_distance = 0 - self.safe_obstacle_distance_stock = 0 - self.speed_jerk = 0 - self.stopped_equivalence_factor = 0 - self.t_follow = 0 def update(self, controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles): eco_gear = frogpilotCarState.ecoGear @@ -77,17 +68,11 @@ def update(self, controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_to if frogpilot_toggles.human_acceleration: if self.frogpilot_planner.frogpilot_following.following_lead and not frogpilotCarState.trafficModeActive: self.max_accel = clip(self.frogpilot_planner.lead_one.aLeadK, get_max_accel_sport_plus(v_ego), get_max_allowed_accel(v_ego)) - if self.frogpilot_planner.v_cruise < CITY_SPEED_LIMIT: - self.max_accel = get_max_accel_ramp_off(self.max_accel, self.frogpilot_planner.v_cruise, v_ego) - else: - self.max_accel = get_max_accel_ramp_off_highway(self.max_accel, self.frogpilot_planner.v_cruise, v_ego) - - self.max_accel = min(self.max_accel, frogpilot_toggles.max_desired_accel) + self.max_accel = min(get_max_accel_low_speeds(self.max_accel, self.frogpilot_planner.v_cruise), self.max_accel) + self.max_accel = min(get_max_accel_ramp_off(self.max_accel, self.frogpilot_planner.v_cruise, v_ego), self.max_accel) if controlsState.experimentalMode: self.min_accel = ACCEL_MIN - elif min(self.frogpilot_planner.frogpilot_vcruise.mtsc_target, self.frogpilot_planner.frogpilot_vcruise.vtsc_target) < v_cruise: - self.min_accel = A_CRUISE_MIN elif frogpilot_toggles.map_deceleration and (eco_gear or sport_gear): if eco_gear: self.min_accel = A_CRUISE_MIN_ECO diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_events.py b/selfdrive/frogpilot/controls/lib/frogpilot_events.py index 3793ed7c6bd0dc..d20c3d3db74299 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_events.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_events.py @@ -4,25 +4,22 @@ import openpilot.system.sentry as sentry from openpilot.common.conversions import Conversions as CV -from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.controlsd import Desire from openpilot.selfdrive.controls.lib.events import EventName, Events from openpilot.selfdrive.frogpilot.assets.theme_manager import update_wheel_image -from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED +from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, params, params_memory class FrogPilotEvents: def __init__(self, FrogPilotPlanner): - self.params = Params() - self.params_memory = Params("/dev/shm/params") - self.events = Events() self.frogpilot_planner = FrogPilotPlanner self.accel30_played = False self.accel35_played = False self.accel40_played = False + self.always_on_lateral_active_previously = False self.dejaVu_played = False self.fcw_played = False self.firefox_played = False @@ -33,23 +30,23 @@ def __init__(self, FrogPilotPlanner): self.previous_traffic_mode = False self.random_event_played = False self.stopped_for_light = False + self.this_is_fine_played = False self.vCruise69_played = False self.youveGotMail_played = False self.frame = 0 self.max_acceleration = 0 self.random_event_timer = 0 + self.tracking_lead_distance = 0 - def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, modelData, frogpilot_toggles): - v_cruise = max(controlsState.vCruise, controlsState.vCruiseCluster) - + def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, lead_distance, modelData, v_lead, frogpilot_toggles): self.events.clear() if self.random_event_played: self.random_event_timer += DT_MDL if self.random_event_timer >= 4: update_wheel_image(frogpilot_toggles.wheel_image, frogpilot_toggles.current_holiday_theme, False) - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.random_event_played = False self.random_event_timer = 0 @@ -63,12 +60,21 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState else: self.stopped_for_light = False - if not self.holiday_theme_played and frogpilot_toggles.current_holiday_theme != None and self.frame >= 10: + if not self.holiday_theme_played and frogpilot_toggles.current_holiday_theme is not None and self.frame >= 10: self.events.add(EventName.holidayActive) self.holiday_theme_played = True - if self.frogpilot_planner.lead_departing: - self.events.add(EventName.leadDeparting) + if frogpilot_toggles.lead_departing_alert and self.frogpilot_planner.tracking_lead and carState.standstill: + if self.tracking_lead_distance == 0: + self.tracking_lead_distance = lead_distance + + lead_departing = lead_distance - self.tracking_lead_distance > 1 + lead_departing &= v_lead > 1 + + if lead_departing: + self.events.add(EventName.leadDeparting) + 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 frogpilot_toggles.random_events: @@ -88,7 +94,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState if not self.accel30_played and 3.5 > self.max_acceleration >= 3.0 and acceleration < 1.5: self.events.add(EventName.accel30) update_wheel_image("weeb_wheel") - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.accel30_played = True self.random_event_played = True self.max_acceleration = 0 @@ -96,7 +102,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState elif not self.accel35_played and 4.0 > self.max_acceleration >= 3.5 and acceleration < 1.5: self.events.add(EventName.accel35) update_wheel_image("tree_fiddy") - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.accel35_played = True self.random_event_played = True self.max_acceleration = 0 @@ -104,15 +110,16 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState elif not self.accel40_played and self.max_acceleration >= 4.0 and acceleration < 1.5: self.events.add(EventName.accel40) update_wheel_image("great_scott") - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.accel40_played = True self.random_event_played = True self.max_acceleration = 0 - if not self.dejaVu_played and self.frogpilot_planner.taking_curve_quickly: - self.events.add(EventName.dejaVuCurve) - self.dejaVu_played = True - self.random_event_played = True + if not self.dejaVu_played and carState.vEgo > CRUISING_SPEED * 2: + if carState.vEgo > (1 / self.frogpilot_planner.road_curvature)**0.75 * 2 > CRUISING_SPEED * 2 and abs(carState.steeringAngleDeg) > 30: + self.events.add(EventName.dejaVuCurve) + self.dejaVu_played = True + self.random_event_played = True if not self.no_entry_alert_played and frogpilotCarControl.noEntryEventTriggered: self.events.add(EventName.hal9000) @@ -125,22 +132,29 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState event_choices.append("firefoxSteerSaturated") if not self.goat_played: event_choices.append("goatSteerSaturated") + if not self.this_is_fine_played: + event_choices.append("thisIsFineSteerSaturated") if self.frame % 100 == 0 and event_choices: event_choice = random.choice(event_choices) if event_choice == "firefoxSteerSaturated": self.events.add(EventName.firefoxSteerSaturated) update_wheel_image("firefox") - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.firefox_played = True elif event_choice == "goatSteerSaturated": self.events.add(EventName.goatSteerSaturated) update_wheel_image("goat") - self.params_memory.put_bool("UpdateWheelImage", True) + params_memory.put_bool("UpdateWheelImage", True) self.goat_played = True + elif event_choice == "thisIsFineSteerSaturated": + self.events.add(EventName.thisIsFineSteerSaturated) + update_wheel_image("this_is_fine") + params_memory.put_bool("UpdateWheelImage", True) + self.this_is_fine_played = True self.random_event_played = True - if not self.vCruise69_played and 70 > v_cruise * (1 if frogpilot_toggles.is_metric else CV.KPH_TO_MPH) >= 69: + if not self.vCruise69_played and 70 > max(controlsState.vCruise, controlsState.vCruiseCluster) * (1 if frogpilot_toggles.is_metric else CV.KPH_TO_MPH) >= 69: self.events.add(EventName.vCruise69) self.vCruise69_played = True self.random_event_played = True @@ -157,10 +171,10 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState self.random_event_played = True self.always_on_lateral_active_previously = frogpilotCarControl.alwaysOnLateralActive - if frogpilot_toggles.speed_limit_alert and self.frogpilot_planner.frogpilot_vcruise.speed_limit_changed: + if frogpilot_toggles.speed_limit_changed_alert and self.frogpilot_planner.frogpilot_vcruise.speed_limit_changed: self.events.add(EventName.speedLimitChanged) - if self.frame == 4 and self.params.get("NNFFModelName", encoding='utf-8') is not None: + if self.frame == 4 and params.get("NNFFModelName", encoding='utf-8') is not None: self.events.add(EventName.torqueNNLoad) if frogpilotCarState.trafficModeActive != self.previous_traffic_mode: diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_following.py b/selfdrive/frogpilot/controls/lib/frogpilot_following.py index 2be1a0b9056522..28619a2b5e6a77 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_following.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_following.py @@ -1,6 +1,5 @@ from openpilot.common.numpy_fast import clip, interp - -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, STOP_DISTANCE, get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED @@ -23,7 +22,7 @@ def __init__(self, FrogPilotPlanner): self.stopped_equivalence_factor = 0 self.t_follow = 0 - def update(self, aEgo, controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles): + def update(self, aEgo, controlsState, frogpilotCarState, lead_distance, v_ego, v_lead, frogpilot_toggles): if frogpilotCarState.trafficModeActive: if aEgo >= 0: self.base_acceleration_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_acceleration) @@ -68,17 +67,17 @@ def update(self, aEgo, controlsState, frogpilotCarState, lead_distance, stopping self.safe_obstacle_distance = int(get_safe_obstacle_distance(v_ego, self.t_follow)) self.safe_obstacle_distance_stock = self.safe_obstacle_distance self.stopped_equivalence_factor = int(get_stopped_equivalence_factor(v_lead)) - self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles) + self.update_follow_values(lead_distance, v_ego, v_lead, frogpilot_toggles) else: self.safe_obstacle_distance = 0 self.safe_obstacle_distance_stock = 0 self.stopped_equivalence_factor = 0 - def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles): + 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(stopping_distance - v_ego, 0) * max(v_lead - v_ego, 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 @@ -87,10 +86,10 @@ def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead, # 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: distance_factor = max(lead_distance - (v_lead * self.t_follow), 1) - far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - stopping_distance + (v_lead - CITY_SPEED_LIMIT), 0) + 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) if frogpilot_toggles.human_following: - self.acceleration_jerk *= braking_offset - self.speed_jerk *= braking_offset + self.acceleration_jerk *= braking_offset if far_lead_offset != 0 else max(braking_offset / COMFORT_BRAKE, 1) + self.speed_jerk *= braking_offset if far_lead_offset != 0 else max(braking_offset / COMFORT_BRAKE, 1) self.t_follow /= braking_offset self.slower_lead = braking_offset - far_lead_offset > 1 diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_tracking.py b/selfdrive/frogpilot/controls/lib/frogpilot_tracking.py index d23ce9665241c5..8feb61079eaa24 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_tracking.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_tracking.py @@ -10,16 +10,18 @@ def __init__(self): self.total_minutes = self.params_tracking.get_float("FrogPilotMinutes") self.drive_added = False + self.enabled = False self.drive_distance = 0 self.drive_time = 0 - self.starting_total_minutes = self.total_minutes - def update(self, carState): + def update(self, carState, controlsState, frogpilotCarControl): + self.enabled |= controlsState.enabled or frogpilotCarControl.alwaysOnLateralActive + self.drive_distance += carState.vEgo * DT_MDL self.drive_time += DT_MDL - if self.drive_time > 60 and carState.standstill: + if self.drive_time > 60 and carState.standstill and self.enabled: self.total_kilometers += self.drive_distance / 1000 self.params_tracking.put_float_nonblocking("FrogPilotKilometers", self.total_kilometers) self.drive_distance = 0 @@ -28,7 +30,7 @@ def update(self, carState): self.params_tracking.put_float_nonblocking("FrogPilotMinutes", self.total_minutes) self.drive_time = 0 - if not self.drive_added and (self.total_minutes - self.starting_total_minutes > 15): + if not self.drive_added: self.total_drives += 1 self.params_tracking.put_int_nonblocking("FrogPilotDrives", self.total_drives) self.drive_added = True diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py b/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py index cc1b4a56169030..49a386038c82a4 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py @@ -7,7 +7,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController -from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME +from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, params_memory TARGET_LAT_A = 1.9 @@ -15,8 +15,6 @@ class FrogPilotVCruise: def __init__(self, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner - self.params_memory = self.frogpilot_planner.params_memory - self.mtsc = MapTurnSpeedController() self.slc = SpeedLimitController() @@ -36,16 +34,18 @@ def __init__(self, FrogPilotPlanner): self.vtsc_target = 0 def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles): - force_stop_enabled = frogpilot_toggles.force_stops and self.frogpilot_planner.cem.stop_light_detected and controlsState.enabled - force_stop_enabled &= self.frogpilot_planner.model_length < 100 - force_stop_enabled &= self.override_force_stop_timer <= 0 + 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 + + self.force_stop_timer = self.force_stop_timer + DT_MDL if force_stop else 0 - self.force_stop_timer = self.force_stop_timer + DT_MDL if force_stop_enabled else 0 + force_stop_enabled = self.force_stop_timer >= 1 self.override_force_stop |= not frogpilot_toggles.force_standstill and carState.standstill and self.frogpilot_planner.tracking_lead self.override_force_stop |= carState.gasPressed self.override_force_stop |= frogpilotCarControl.resumePressed - self.override_force_stop &= self.force_stop_timer >= 1 + self.override_force_stop &= force_stop_enabled if self.override_force_stop: self.override_force_stop_timer = 10 @@ -79,22 +79,22 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState self.slc.update(frogpilotCarState.dashboardSpeedLimit, controlsState.enabled, frogpilotNavigation.navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles) unconfirmed_slc_target = self.slc.desired_speed_limit - if (frogpilot_toggles.speed_limit_alert or frogpilot_toggles.speed_limit_confirmation_lower or frogpilot_toggles.speed_limit_confirmation_higher) 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 + if (frogpilot_toggles.speed_limit_changed_alert or frogpilot_toggles.speed_limit_confirmation) and self.slc_target != 0: + self.speed_limit_changed = unconfirmed_slc_target != self.previous_speed_limit and abs(self.slc_target - unconfirmed_slc_target) > 1 and unconfirmed_slc_target > 1 speed_limit_decreased = self.speed_limit_changed and self.slc_target > unconfirmed_slc_target speed_limit_increased = self.speed_limit_changed and self.slc_target < unconfirmed_slc_target - accepted_via_ui = self.params_memory.get_bool("SLCConfirmedPressed") and self.params_memory.get_bool("SLCConfirmed") - denied_via_ui = self.params_memory.get_bool("SLCConfirmedPressed") and not self.params_memory.get_bool("SLCConfirmed") + accepted_via_ui = params_memory.get_bool("SLCConfirmedPressed") and params_memory.get_bool("SLCConfirmed") + denied_via_ui = params_memory.get_bool("SLCConfirmedPressed") and not params_memory.get_bool("SLCConfirmed") - speed_limit_accepted = frogpilotCarControl.resumePressed or accepted_via_ui - speed_limit_denied = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents) or denied_via_ui or self.speed_limit_timer >= 10 + speed_limit_accepted = frogpilotCarControl.resumePressed and controlsState.enabled or accepted_via_ui + speed_limit_denied = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents) and controlsState.enabled or denied_via_ui or self.speed_limit_timer >= 10 if speed_limit_accepted or speed_limit_denied: self.previous_speed_limit = unconfirmed_slc_target - self.params_memory.put_bool("SLCConfirmed", False) - self.params_memory.put_bool("SLCConfirmedPressed", False) + params_memory.put_bool("SLCConfirmed", False) + params_memory.put_bool("SLCConfirmedPressed", False) if speed_limit_decreased: speed_limit_confirmed = not frogpilot_toggles.speed_limit_confirmation_lower or speed_limit_accepted @@ -142,13 +142,10 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState self.forcing_stop = True v_cruise = -1 - elif self.force_stop_timer >= 1 and not self.override_force_stop: - if self.tracked_model_length == 0: - self.tracked_model_length = self.frogpilot_planner.model_length - - self.forcing_stop = True - self.tracked_model_length -= v_ego * DT_MDL - v_cruise = min((self.tracked_model_length / PLANNER_TIME) - 1, v_cruise) + elif force_stop_enabled and not self.override_force_stop: + self.forcing_stop |= not carState.standstill + self.tracked_model_length = max(self.tracked_model_length - v_ego * DT_MDL, 0) + v_cruise = min((self.tracked_model_length // PLANNER_TIME), v_cruise) else: if not self.frogpilot_planner.cem.stop_light_detected: @@ -156,7 +153,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState self.forcing_stop = False - self.tracked_model_length = 0 + self.tracked_model_length = self.frogpilot_planner.model_length 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])) diff --git a/selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py b/selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py index a29e9108047730..38623843fdfa17 100644 --- a/selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py +++ b/selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py @@ -2,14 +2,11 @@ import json import math -from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import interp -from openpilot.common.params import Params -params_memory = Params("/dev/shm/params") +from openpilot.selfdrive.frogpilot.frogpilot_utilities import calculate_distance_to_point +from openpilot.selfdrive.frogpilot.frogpilot_variables import TO_RADIANS, params_memory -R = 6373000.0 # approximate radius of earth in meters -TO_RADIANS = math.pi / 180 TARGET_JERK = -0.6 # m/s^3 should match up with the long planner TARGET_ACCEL = -1.2 # m/s^2 should match up with the long planner TARGET_OFFSET = 1.0 # seconds - This controls how soon before the curve you reach the target velocity. It also helps @@ -25,15 +22,6 @@ def calculate_velocity(t, target_jerk, a_ego, v_ego): def calculate_distance(t, target_jerk, a_ego, v_ego): return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3) - -# points should be in radians -# output is meters -def distance_to_point(ax, ay, bx, by): - a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2) - c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) - - return R * c # in meters - class MapTurnSpeedController: def __init__(self): self.target_lat = 0.0 @@ -62,7 +50,7 @@ def target_speed(self, v_ego, a_ego, frogpilot_toggles) -> float: target_velocity = target_velocities[i] tlat = target_velocity["latitude"] tlon = target_velocity["longitude"] - d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS) + d = calculate_distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS) distances.append(d) if d < min_dist: min_dist = d diff --git a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py index 1e9dab502ea31c..c7044ded094ffd 100644 --- a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py +++ b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py @@ -1,128 +1,98 @@ # PFEIFER - SLC - Modified by FrogAi for FrogPilot import json -import math -from openpilot.common.conversions import Conversions as CV -from openpilot.common.params import Params - -from openpilot.selfdrive.frogpilot.frogpilot_variables import FrogPilotVariables - -R = 6373000.0 # approximate radius of earth in meters -TO_RADIANS = math.pi / 180 - -# points should be in radians -# output is meters -def distance_to_point(ax, ay, bx, by): - a = math.sin((bx - ax) / 2) * math.sin((bx - ax) / 2) + math.cos(ax) * math.cos(bx) * math.sin((by - ay) / 2) * math.sin((by - ay) / 2) - c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a)) - return R * c # in meters +from openpilot.selfdrive.frogpilot.frogpilot_utilities import calculate_distance_to_point +from openpilot.selfdrive.frogpilot.frogpilot_variables import TO_RADIANS, params, params_memory class SpeedLimitController: def __init__(self): - self.frogpilot_toggles = FrogPilotVariables.toggles - FrogPilotVariables.update_frogpilot_params() - - self.params = Params() - self.params_memory = Params("/dev/shm/params") + self.experimental_mode = False - self.car_speed_limit = 0 # m/s - self.map_speed_limit = 0 # m/s - self.max_speed_limit = 0 # m/s - self.nav_speed_limit = 0 # m/s - self.prv_speed_limit = self.params.get_float("PreviousSpeedLimit") + self.desired_speed_limit = 0 + self.offset = 0 + self.speed_limit = 0 - def get_param_memory(self, key, is_json=False): - param_value = self.params_memory.get(key) - if param_value is None: - return {} if is_json else 0.0 - return json.loads(param_value) if is_json else float(param_value) + self.previous_speed_limit = params.get_float("PreviousSpeedLimit") - def update_previous_limit(self, speed_limit): - if self.prv_speed_limit != speed_limit: - self.params.put_float_nonblocking("PreviousSpeedLimit", speed_limit) - self.prv_speed_limit = speed_limit + def update(self, dashboard_speed_limit, enabled, navigation_speed_limit, v_cruise, v_ego, frogpilot_toggles): + map_speed_limit = self.get_map_speed_limit(v_ego, frogpilot_toggles) + max_speed_limit = v_cruise if enabled else 0 - def update(self, dashboardSpeedLimit, enabled, navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles): - self.car_speed_limit = dashboardSpeedLimit - self.write_map_state(v_ego) - self.nav_speed_limit = navigationSpeedLimit + self.speed_limit = self.get_speed_limit(dashboard_speed_limit, map_speed_limit, max_speed_limit, navigation_speed_limit, frogpilot_toggles) + self.offset = self.get_offset(frogpilot_toggles) + self.desired_speed_limit = self.get_desired_speed_limit() - self.max_speed_limit = v_cruise if enabled else 0 + self.experimental_mode = self.speed_limit == 0 and frogpilot_toggles.slc_fallback_experimental_mode - self.frogpilot_toggles = frogpilot_toggles + def get_desired_speed_limit(self): + if self.speed_limit > 1: + if self.previous_speed_limit != self.speed_limit: + params.put_float_nonblocking("PreviousSpeedLimit", self.speed_limit) + self.previous_speed_limit = self.speed_limit + return self.speed_limit + self.offset + return 0 - def write_map_state(self, v_ego): - self.map_speed_limit = self.get_param_memory("MapSpeedLimit") + def get_map_speed_limit(self, v_ego, frogpilot_toggles): + map_speed_limit = params_memory.get_float("MapSpeedLimit") - next_map_speed_limit = self.get_param_memory("NextMapSpeedLimit", is_json=True) - next_map_speed_limit_value = next_map_speed_limit.get("speedlimit", 0) + next_map_speed_limit = json.loads(params_memory.get("NextMapSpeedLimit", "{}")) next_map_speed_limit_lat = next_map_speed_limit.get("latitude", 0) next_map_speed_limit_lon = next_map_speed_limit.get("longitude", 0) + next_map_speed_limit_value = next_map_speed_limit.get("speedlimit", 0) - position = self.get_param_memory("LastGPSPosition", is_json=True) + position = json.loads(params_memory.get("LastGPSPosition", "{}")) lat = position.get("latitude", 0) lon = position.get("longitude", 0) if next_map_speed_limit_value > 1: - d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, next_map_speed_limit_lat * TO_RADIANS, next_map_speed_limit_lon * TO_RADIANS) + d = calculate_distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, next_map_speed_limit_lat * TO_RADIANS, next_map_speed_limit_lon * TO_RADIANS) - if self.prv_speed_limit < next_map_speed_limit_value: - max_d = self.frogpilot_toggles.map_speed_lookahead_higher * v_ego + if self.previous_speed_limit < next_map_speed_limit_value: + max_d = frogpilot_toggles.map_speed_lookahead_higher * v_ego else: - max_d = self.frogpilot_toggles.map_speed_lookahead_lower * v_ego + max_d = frogpilot_toggles.map_speed_lookahead_lower * v_ego if d < max_d: - self.map_speed_limit = next_map_speed_limit_value - - @property - def experimental_mode(self): - return self.speed_limit == 0 and self.frogpilot_toggles.slc_fallback_experimental + map_speed_limit = next_map_speed_limit_value - @property - def desired_speed_limit(self): - if self.speed_limit > 1: - self.update_previous_limit(self.speed_limit) - return self.speed_limit + self.offset - return 0 + return map_speed_limit - @property - def offset(self): + def get_offset(self, frogpilot_toggles): if self.speed_limit < 13.5: - return self.frogpilot_toggles.offset1 + return frogpilot_toggles.speed_limit_offset1 if self.speed_limit < 24: - return self.frogpilot_toggles.offset2 + return frogpilot_toggles.speed_limit_offset2 if self.speed_limit < 29: - return self.frogpilot_toggles.offset3 - return self.frogpilot_toggles.offset4 + return frogpilot_toggles.speed_limit_offset3 + return frogpilot_toggles.speed_limit_offset4 - @property - def speed_limit(self): - limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit] + def get_speed_limit(self, dashboard_speed_limit, map_speed_limit, max_speed_limit, navigation_speed_limit, frogpilot_toggles): + limits = [dashboard_speed_limit, map_speed_limit, navigation_speed_limit] filtered_limits = [float(limit) for limit in limits if limit > 1] - if self.frogpilot_toggles.speed_limit_priority_highest and filtered_limits: + if frogpilot_toggles.speed_limit_priority_highest and filtered_limits: return max(filtered_limits) - if self.frogpilot_toggles.speed_limit_priority_lowest and filtered_limits: + if frogpilot_toggles.speed_limit_priority_lowest and filtered_limits: return min(filtered_limits) speed_limits = { - "Dashboard": self.car_speed_limit, - "Offline Maps": self.map_speed_limit, - "Navigation": self.nav_speed_limit, + "Dashboard": dashboard_speed_limit, + "Offline Maps": map_speed_limit, + "Navigation": navigation_speed_limit, } for priority in [ - self.frogpilot_toggles.speed_limit_priority1, - self.frogpilot_toggles.speed_limit_priority2, - self.frogpilot_toggles.speed_limit_priority3, + frogpilot_toggles.speed_limit_priority1, + frogpilot_toggles.speed_limit_priority2, + frogpilot_toggles.speed_limit_priority3, ]: - if speed_limits.get(priority, 0) in filtered_limits: + if speed_limits.get(priority) in filtered_limits: return speed_limits[priority] - if self.frogpilot_toggles.slc_fallback_previous: - return self.prv_speed_limit + if frogpilot_toggles.slc_fallback_previous_speed_limit: + return self.previous_speed_limit - if self.frogpilot_toggles.use_set_speed: - return self.max_speed_limit + if frogpilot_toggles.slc_fallback_set_speed: + return max_speed_limit return 0 diff --git a/selfdrive/frogpilot/fleetmanager/fleet_manager.py b/selfdrive/frogpilot/fleetmanager/fleet_manager.py index 34aff472bed086..38f7535ecd5ef4 100644 --- a/selfdrive/frogpilot/fleetmanager/fleet_manager.py +++ b/selfdrive/frogpilot/fleetmanager/fleet_manager.py @@ -35,7 +35,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.system.hardware.hw import Paths -from openpilot.selfdrive.frogpilot.frogpilot_variables import FrogPilotVariables +from openpilot.selfdrive.frogpilot.frogpilot_variables import has_prime app = Flask(__name__) @@ -190,7 +190,7 @@ def addr_input(): return redirect(url_for('nav_confirmation', addr=addr, lon=lon, lat=lat)) else: return render_template("error.html") - elif FrogPilotVariables.has_prime: + elif has_prime(): return render_template("prime.html") # amap stuff elif SearchInput == 1: diff --git a/selfdrive/frogpilot/fleetmanager/helpers.py b/selfdrive/frogpilot/fleetmanager/helpers.py index b0bfd7c6d3c637..0745ce6ec8dacd 100644 --- a/selfdrive/frogpilot/fleetmanager/helpers.py +++ b/selfdrive/frogpilot/fleetmanager/helpers.py @@ -20,6 +20,7 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +import base64 import json import math import os @@ -43,13 +44,15 @@ from urllib.parse import parse_qs, quote import openpilot.system.sentry as sentry +from openpilot.selfdrive.frogpilot.frogpilot_variables import params, update_frogpilot_toggles + +XOR_KEY = "s8#pL3*Xj!aZ@dWq" + pi = 3.1415926535897932384626 x_pi = 3.14159265358979324 * 3000.0 / 180.0 a = 6378245.0 ee = 0.00669342162296594323 -params = Params() -params_memory = Params("/dev/shm/params") params_storage = Params("/persist/params") PRESERVE_ATTR_NAME = 'user.preserve' @@ -436,6 +439,20 @@ def transform_lng(lng, lat): ret += (150.0 * math.sin(lng / 12.0 * pi) + 300.0 * math.sin(lng / 30.0 * pi)) * 2.0 / 3.0 return ret +def xor_encrypt_decrypt(data: str, key: str) -> str: + return ''.join(chr(ord(c) ^ ord(key[i % len(key)])) for i, c in enumerate(data)) + +def encode_parameters(params_dict): + serialized_data = json.dumps(params_dict) + obfuscated_data = xor_encrypt_decrypt(serialized_data, XOR_KEY) + encoded_data = base64.b64encode(obfuscated_data.encode('utf-8')).decode('utf-8') + return encoded_data + +def decode_parameters(encoded_string): + obfuscated_data = base64.b64decode(encoded_string.encode('utf-8')).decode('utf-8') + decrypted_data = xor_encrypt_decrypt(obfuscated_data, XOR_KEY) + return json.loads(decrypted_data) + def get_all_toggle_values(): toggle_values = {} @@ -451,18 +468,29 @@ def get_all_toggle_values(): value = "0" toggle_values[key] = value if value is not None else "0" - return toggle_values + return encode_parameters(toggle_values) + +def store_toggle_values(request_data): + current_parameters = { + key.decode('utf-8') if isinstance(key, bytes) else key: None + for key in params.all_keys() if params.get_key_type(key) & ParamKeyType.FROGPILOT_STORAGE + } + decoded_values = decode_parameters(request_data['data']) -def store_toggle_values(updated_values): - for key, value in updated_values.items(): + for key in current_parameters: + print(f"Processing key: {key}") + value = decoded_values.get(key, "0") try: if isinstance(value, (int, float)): value = str(value) - params.put(key, value.encode('utf-8')) - params_storage.put(key, value.encode('utf-8')) + print(f"value: {value}") + params.put(key, value) + params_storage.put(key, value) except Exception as e: print(f"Failed to update {key}: {e}") - params_memory.put_bool("FrogPilotTogglesUpdated", True) - time.sleep(1) - params_memory.put_bool("FrogPilotTogglesUpdated", False) + extra_keys = set(decoded_values.keys()) - set(current_parameters.keys()) + if extra_keys: + print(f"Warning: Ignoring extra keys: {extra_keys}") + + update_frogpilot_toggles() diff --git a/selfdrive/frogpilot/fleetmanager/templates/tools.html b/selfdrive/frogpilot/fleetmanager/templates/tools.html index 12ac34798190c2..04347d0c1d93d0 100644 --- a/selfdrive/frogpilot/fleetmanager/templates/tools.html +++ b/selfdrive/frogpilot/fleetmanager/templates/tools.html @@ -29,40 +29,33 @@

Toggle Values

- + - +