From e08da675f2ce08b22fe6967e4ea643b2208d4b01 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 3 Dec 2024 12:20:42 -0700 Subject: [PATCH] December 7th, 2024 Patch --- cereal/log.capnp | 1 + common/params.cc | 103 ++++---- panda/board/safety/safety_toyota.h | 22 +- selfdrive/car/__init__.py | 5 + selfdrive/car/interfaces.py | 2 +- selfdrive/car/toyota/carcontroller.py | 230 +++++++++++++----- selfdrive/car/toyota/carstate.py | 82 ++++--- selfdrive/car/toyota/fingerprints.py | 8 + selfdrive/car/toyota/interface.py | 33 ++- selfdrive/car/toyota/toyotacan.py | 9 +- selfdrive/car/toyota/values.py | 2 +- selfdrive/controls/controlsd.py | 6 +- selfdrive/controls/radard.py | 13 + selfdrive/frogpilot/assets/model_manager.py | 30 +-- selfdrive/frogpilot/assets/theme_manager.py | 15 +- .../frogpilot/controls/frogpilot_planner.py | 10 +- .../controls/lib/frogpilot_acceleration.py | 2 +- .../controls/lib/frogpilot_events.py | 25 +- .../controls/lib/frogpilot_following.py | 12 +- .../controls/lib/frogpilot_vcruise.py | 39 +-- .../controls/lib/speed_limit_controller.py | 8 +- selfdrive/frogpilot/frogpilot_functions.py | 67 ++--- selfdrive/frogpilot/frogpilot_process.py | 91 ++++--- selfdrive/frogpilot/frogpilot_variables.py | 58 +++-- selfdrive/frogpilot/navigation/mapd.py | 90 +++---- .../frogpilot/navigation/ui/maps_settings.cc | 2 +- .../frogpilot/navigation/ui/maps_settings.h | 2 +- .../frogpilot/screenrecorder/omx_encoder.cc | 17 +- .../screenrecorder/screenrecorder.cc | 82 +++++-- .../frogpilot/screenrecorder/screenrecorder.h | 26 +- .../ui/qt/offroad/frogpilot_settings.cc | 75 +++--- .../ui/qt/offroad/frogpilot_settings.h | 1 + .../ui/qt/offroad/lateral_settings.cc | 58 ++++- .../ui/qt/offroad/longitudinal_settings.cc | 40 ++- .../frogpilot/ui/qt/offroad/model_settings.cc | 28 +-- .../frogpilot/ui/qt/offroad/model_settings.h | 2 +- .../frogpilot/ui/qt/offroad/theme_settings.cc | 60 ++--- .../frogpilot/ui/qt/offroad/theme_settings.h | 2 +- .../frogpilot/ui/qt/offroad/utilities.cc | 12 +- selfdrive/frogpilot/ui/qt/offroad/utilities.h | 2 +- .../ui/qt/widgets/frogpilot_controls.cc | 4 +- .../ui/qt/widgets/frogpilot_controls.h | 21 +- .../frogpilot/ui/qt/widgets/model_reviewer.cc | 2 +- .../frogpilot/ui/qt/widgets/model_reviewer.h | 2 +- selfdrive/ui/qt/offroad/settings.cc | 9 +- selfdrive/ui/qt/offroad/settings.h | 5 +- selfdrive/ui/qt/offroad/software_settings.cc | 13 +- selfdrive/ui/qt/onroad/annotated_camera.cc | 91 ++++--- selfdrive/ui/qt/onroad/annotated_camera.h | 6 +- selfdrive/ui/qt/onroad/buttons.cc | 8 + selfdrive/ui/qt/onroad/onroad_home.cc | 1 - selfdrive/ui/qt/sidebar.cc | 16 +- selfdrive/ui/qt/widgets/controls.h | 23 +- selfdrive/ui/ui.cc | 14 +- selfdrive/ui/ui.h | 5 +- system/manager/manager.py | 12 +- system/manager/process_config.py | 2 +- system/sentry.py | 2 +- system/updated/updated.py | 4 +- 59 files changed, 963 insertions(+), 649 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 9a944eac3429f6..77ddeb835aa923 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -622,6 +622,7 @@ struct RadarState @0x9a185389d6fdd05f { leadRight @14 :LeadData; leadLeftFar @15 :LeadData; leadRightFar @16 :LeadData; + leadsLead @17 :LeadData; cumLagMs @5 :Float32; struct LeadData { diff --git a/common/params.cc b/common/params.cc index 6d64a1d120ccfa..b52c2517c8105c 100644 --- a/common/params.cc +++ b/common/params.cc @@ -103,10 +103,10 @@ std::unordered_map keys = { {"CarBatteryCapacity", PERSISTENT}, {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"CarParamsCache", CLEAR_ON_MANAGER_START}, - {"CarParamsPersistent", PERSISTENT}, + {"CarParamsPersistent", PERSISTENT | FROGPILOT_STORAGE}, {"CarParamsPrevRoute", PERSISTENT}, {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, - {"CompletedTrainingVersion", PERSISTENT}, + {"CompletedTrainingVersion", PERSISTENT | FROGPILOT_STORAGE}, {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"CurrentBootlog", PERSISTENT}, {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, @@ -121,21 +121,21 @@ std::unordered_map keys = { {"DoUninstall", CLEAR_ON_MANAGER_START}, {"ExperimentalLongitudinalEnabled", PERSISTENT | DEVELOPMENT_ONLY | FROGPILOT_STORAGE}, {"ExperimentalMode", PERSISTENT}, - {"ExperimentalModeConfirmed", PERSISTENT}, + {"ExperimentalModeConfirmed", PERSISTENT | FROGPILOT_STORAGE}, {"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"ForcePowerDown", PERSISTENT}, {"GitBranch", PERSISTENT}, {"GitCommit", PERSISTENT}, {"GitCommitDate", PERSISTENT}, {"GitDiff", PERSISTENT}, - {"GithubSshKeys", PERSISTENT}, - {"GithubUsername", PERSISTENT}, + {"GithubSshKeys", PERSISTENT | FROGPILOT_STORAGE}, + {"GithubUsername", PERSISTENT | FROGPILOT_STORAGE}, {"GitRemote", PERSISTENT}, - {"GsmApn", PERSISTENT}, - {"GsmMetered", PERSISTENT}, - {"GsmRoaming", PERSISTENT}, + {"GsmApn", PERSISTENT | FROGPILOT_STORAGE}, + {"GsmMetered", PERSISTENT | FROGPILOT_STORAGE}, + {"GsmRoaming", PERSISTENT | FROGPILOT_STORAGE}, {"HardwareSerial", PERSISTENT}, - {"HasAcceptedTerms", PERSISTENT}, + {"HasAcceptedTerms", PERSISTENT | FROGPILOT_STORAGE}, {"IMEI", PERSISTENT}, {"InstallDate", PERSISTENT}, {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START}, @@ -163,7 +163,7 @@ std::unordered_map keys = { {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"NavPastDestinations", PERSISTENT}, - {"NavSettingLeftSide", PERSISTENT}, + {"NavSettingLeftSide", PERSISTENT | FROGPILOT_STORAGE}, {"NavSettingTime24h", PERSISTENT}, {"NetworkMetered", PERSISTENT}, {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, @@ -180,25 +180,25 @@ std::unordered_map keys = { {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START}, {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START}, {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, - {"OpenpilotEnabledToggle", PERSISTENT}, + {"OpenpilotEnabledToggle", PERSISTENT | FROGPILOT_STORAGE}, {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"PandaSomResetTriggered", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"PandaSignatures", CLEAR_ON_MANAGER_START}, {"PrimeType", PERSISTENT}, - {"RecordFront", PERSISTENT}, + {"RecordFront", PERSISTENT | FROGPILOT_STORAGE}, {"RecordFrontLock", PERSISTENT}, // for the internal fleet {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"RouteCount", PERSISTENT}, - {"SecOCKey", PERSISTENT | DONT_LOG}, + {"SecOCKey", PERSISTENT | DONT_LOG | FROGPILOT_STORAGE}, {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, - {"SshEnabled", PERSISTENT}, + {"SshEnabled", PERSISTENT | FROGPILOT_STORAGE}, {"TermsVersion", PERSISTENT}, {"Timezone", PERSISTENT}, {"TrainingVersion", PERSISTENT}, {"UbloxAvailable", PERSISTENT}, {"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, - {"UpdaterAvailableBranches", PERSISTENT}, + {"UpdaterAvailableBranches", PERSISTENT | FROGPILOT_STORAGE}, {"UpdaterCurrentDescription", CLEAR_ON_MANAGER_START}, {"UpdaterCurrentReleaseNotes", CLEAR_ON_MANAGER_START}, {"UpdaterFetchAvailable", CLEAR_ON_MANAGER_START}, @@ -228,13 +228,13 @@ std::unordered_map keys = { {"AlwaysOnLateral", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"AlwaysOnLateralLKAS", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"AlwaysOnLateralMain", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"AMapKey1", PERSISTENT}, - {"AMapKey2", PERSISTENT}, - {"ApiCache_DriveStats", PERSISTENT}, + {"AMapKey1", PERSISTENT | FROGPILOT_STORAGE}, + {"AMapKey2", PERSISTENT | FROGPILOT_STORAGE}, + {"ApiCache_DriveStats", PERSISTENT | FROGPILOT_STORAGE}, {"AutomaticallyUpdateModels", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"AutomaticUpdates", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, - {"AvailableModels", PERSISTENT}, - {"AvailableModelsNames", PERSISTENT}, + {"AvailableModels", PERSISTENT | FROGPILOT_STORAGE}, + {"AvailableModelsNames", PERSISTENT | FROGPILOT_STORAGE}, {"BigMap", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"BlacklistedModels", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"BlindSpotMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -243,9 +243,9 @@ std::unordered_map keys = { {"CameraView", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"CancelModelDownload", PERSISTENT}, {"CancelThemeDownload", PERSISTENT}, - {"CarMake", PERSISTENT}, - {"CarModel", PERSISTENT}, - {"CarModelName", PERSISTENT}, + {"CarMake", PERSISTENT | FROGPILOT_STORAGE}, + {"CarModel", PERSISTENT | FROGPILOT_STORAGE}, + {"CarModelName", PERSISTENT | FROGPILOT_STORAGE}, {"CECurves", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CECurvesLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CELead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -263,7 +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}, + {"ClassicModels", PERSISTENT | FROGPILOT_STORAGE}, {"ClusterOffset", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"ColorToDownload", PERSISTENT}, {"Compass", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -279,8 +279,8 @@ std::unordered_map keys = { {"CustomCruiseLong", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CustomDistanceIcons", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CustomIcons", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"CustomizationLevel", PERSISTENT}, - {"CustomizationLevelConfirmed", PERSISTENT}, + {"CustomizationLevel", PERSISTENT | FROGPILOT_STORAGE}, + {"CustomizationLevelConfirmed", PERSISTENT | FROGPILOT_STORAGE}, {"CustomPersonalities", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CustomSignals", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"CustomSounds", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -297,12 +297,12 @@ std::unordered_map keys = { {"DistanceIconToDownload", PERSISTENT}, {"DisengageVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"DoToggleReset", PERSISTENT}, - {"DownloadableColors", PERSISTENT}, - {"DownloadableDistanceIcons", PERSISTENT}, - {"DownloadableIcons", PERSISTENT}, - {"DownloadableSignals", PERSISTENT}, - {"DownloadableSounds", PERSISTENT}, - {"DownloadableWheels", PERSISTENT}, + {"DownloadableColors", PERSISTENT | FROGPILOT_STORAGE}, + {"DownloadableDistanceIcons", PERSISTENT | FROGPILOT_STORAGE}, + {"DownloadableIcons", PERSISTENT | FROGPILOT_STORAGE}, + {"DownloadableSignals", PERSISTENT | FROGPILOT_STORAGE}, + {"DownloadableSounds", PERSISTENT | FROGPILOT_STORAGE}, + {"DownloadableWheels", PERSISTENT | FROGPILOT_STORAGE}, {"DownloadAllModels", PERSISTENT}, {"DragonRiderDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"DragonRiderScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -314,7 +314,7 @@ std::unordered_map keys = { {"EngageVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"ExperimentalGMTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"ExperimentalModeActivation", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"ExperimentalModels", PERSISTENT}, + {"ExperimentalModels", PERSISTENT | FROGPILOT_STORAGE}, {"ExperimentalModeViaDistance", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ExperimentalModeViaLKAS", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ExperimentalModeViaTap", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -332,12 +332,12 @@ std::unordered_map keys = { {"FrogPilotDrives", PERSISTENT | FROGPILOT_TRACKING}, {"FrogPilotKilometers", PERSISTENT | FROGPILOT_TRACKING}, {"FrogPilotMinutes", PERSISTENT | FROGPILOT_TRACKING}, - {"FrogPilotToggles", PERSISTENT}, + {"FrogPilotToggles", CLEAR_ON_MANAGER_START}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"FrogsGoMoosTweak", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"FullMap", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"GasRegenCmd", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, - {"GMapKey", PERSISTENT}, + {"GMapKey", PERSISTENT | FROGPILOT_STORAGE}, {"GoatScream", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"GreenLightAlert", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"HideAlerts", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -376,8 +376,8 @@ std::unordered_map keys = { {"LowVoltageShutdown", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ManualUpdateInitiated", PERSISTENT}, {"MapAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"MapboxPublicKey", PERSISTENT}, - {"MapboxSecretKey", PERSISTENT}, + {"MapboxPublicKey", PERSISTENT | FROGPILOT_STORAGE}, + {"MapboxSecretKey", PERSISTENT | FROGPILOT_STORAGE}, {"MapDeceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"MapGears", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"MapsSelected", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, @@ -396,7 +396,7 @@ std::unordered_map keys = { {"ModelUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"MTSCCurvatureCheck", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"MTSCEnabled", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"NavigationModels", PERSISTENT}, + {"NavigationModels", PERSISTENT | FROGPILOT_STORAGE}, {"NavigationUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"NextMapSpeedLimit", PERSISTENT}, {"NewLongAPI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, @@ -438,13 +438,13 @@ std::unordered_map keys = { {"QOLLateral", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"QOLLongitudinal", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"QOLVisuals", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"RadarlessModels", PERSISTENT}, - {"RadicalTurtleDrives", PERSISTENT | FROGPILOT_CONTROLS}, - {"RadicalTurtleScore", PERSISTENT | FROGPILOT_CONTROLS}, + {"RadarlessModels", PERSISTENT | FROGPILOT_STORAGE}, + {"RadicalTurtleDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"RadicalTurtleScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"RainbowPath", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"RandomEvents", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"RecertifiedHerbalistDrives", PERSISTENT | FROGPILOT_CONTROLS}, - {"RecertifiedHerbalistScore", PERSISTENT | FROGPILOT_CONTROLS}, + {"RecertifiedHerbalistDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"RecertifiedHerbalistScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"RefuseVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"RelaxedFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"RelaxedJerkAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -466,8 +466,8 @@ std::unordered_map keys = { {"ScreenTimeout", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"ScreenTimeoutOnroad", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"SearchInput", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, - {"SecretGoodOpenpilotDrives", PERSISTENT | FROGPILOT_CONTROLS}, - {"SecretGoodOpenpilotScore", PERSISTENT | FROGPILOT_CONTROLS}, + {"SecretGoodOpenpilotDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SecretGoodOpenpilotScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SetSpeedLimit", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SetSpeedOffset", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"ShowCPU", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, @@ -488,7 +488,6 @@ std::unordered_map keys = { {"SLCConfirmationHigher", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SLCConfirmationLower", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SLCConfirmed", PERSISTENT}, - {"SLCConfirmedPressed", PERSISTENT}, {"SLCLookaheadHigher", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SLCLookaheadLower", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"SLCFallback", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, @@ -513,13 +512,13 @@ std::unordered_map keys = { {"StartupMessageTop", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"StaticPedalsOnUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"SteerFriction", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"SteerFrictionStock", PERSISTENT}, + {"SteerFrictionStock", PERSISTENT | FROGPILOT_STORAGE}, {"SteerLatAccel", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"SteerLatAccelStock", PERSISTENT}, + {"SteerLatAccelStock", PERSISTENT | FROGPILOT_STORAGE}, {"SteerKP", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"SteerKPStock", PERSISTENT}, + {"SteerKPStock", PERSISTENT | FROGPILOT_STORAGE}, {"SteerRatio", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, - {"SteerRatioStock", PERSISTENT}, + {"SteerRatioStock", PERSISTENT | FROGPILOT_STORAGE}, {"StoppedTimer", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"TacoTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"TestingSound", PERSISTENT}, @@ -547,8 +546,8 @@ std::unordered_map keys = { {"VoltSNG", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"WarningImmediateVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"WarningSoftVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, - {"WD40Drives", PERSISTENT | FROGPILOT_CONTROLS}, - {"WD40Score", PERSISTENT | FROGPILOT_CONTROLS}, + {"WD40Drives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"WD40Score", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"WheelIcon", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"WheelSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"WheelToDownload", PERSISTENT}, diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 208d0e08980efb..36b236ceabcfdd 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -49,10 +49,18 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805; #define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks // Stock longitudinal -#define TOYOTA_COMMON_TX_MSGS \ - {0x2E4, 0, 5}, {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \ +#define TOYOTA_BASE_TX_MSGS \ + {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \ {0x750, 0, 8}, /* white list 0x750 for Enhanced Diagnostic Request */ \ +#define TOYOTA_COMMON_TX_MSGS \ + TOYOTA_BASE_TX_MSGS \ + {0x2E4, 0, 5}, \ + +#define TOYOTA_COMMON_SECOC_TX_MSGS \ + TOYOTA_BASE_TX_MSGS \ + {0x2E4, 0, 8}, {0x131, 0, 8}, \ + #define TOYOTA_COMMON_LONG_TX_MSGS \ TOYOTA_COMMON_TX_MSGS \ {0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8}, /* DSU bus 0 */ \ @@ -66,7 +74,7 @@ const CanMsg TOYOTA_TX_MSGS[] = { }; const CanMsg TOYOTA_SECOC_TX_MSGS[] = { - TOYOTA_COMMON_TX_MSGS + TOYOTA_COMMON_SECOC_TX_MSGS }; const CanMsg TOYOTA_LONG_TX_MSGS[] = { @@ -81,9 +89,11 @@ const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = { #define TOYOTA_COMMON_RX_CHECKS(lta) \ {.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}}, \ {.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \ - {.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}}, \ - {.msg = {{0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \ - {0x226, 0, 8, .check_checksum = false, .frequency = 40U}, { 0 }}}, \ + {.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, \ + {0x176, 0, 8, .check_checksum = true, .frequency = 32U}, { 0 }}}, \ + {.msg = {{0x101, 0, 8, .check_checksum = false, .frequency = 50U}, \ + {0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \ + {0x226, 0, 8, .check_checksum = false, .frequency = 40U}}}, \ RxCheck toyota_lka_rx_checks[] = { TOYOTA_COMMON_RX_CHECKS(false) diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 307b5df0c3468e..b918b684abc5a3 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -1,4 +1,5 @@ # functions common among cars +import logging from collections import namedtuple from dataclasses import dataclass from enum import IntFlag, ReprEnum, EnumType @@ -11,6 +12,10 @@ from openpilot.common.utils import Freezable from openpilot.selfdrive.car.docs_definitions import CarDocs +# set up logging +carlog = logging.getLogger('carlog') +carlog.setLevel(logging.INFO) +carlog.propagate = False # kg of standard extra cargo to count for drive, gas, etc... STD_CARGO_KG = 136. diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 8ab60d786ecbed..4871ff5a426171 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -295,7 +295,7 @@ def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_ eps_firmware = str(next((fw.fwVersion for fw in car_fw if fw.ecu == "eps"), "")) model = get_nn_model_path(candidate, eps_firmware) if model is not None: - params.put_nonblocking("NNFFModelName", candidate.replace("_", " ")) + params.put("NNFFModelName", candidate.replace("_", " ")) # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload if not ret.notCar: diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 4c56efaed430b9..795fafd4cd4ae8 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,14 +1,18 @@ import math from cereal import car +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.numpy_fast import clip, interp -from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, create_gas_interceptor_command, make_can_msg, rate_limit +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.car import carlog, apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, create_gas_interceptor_command, make_can_msg, rate_limit +from openpilot.selfdrive.car.secoc import add_mac, build_sync_mac from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.toyota import toyotacan from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \ UNSUPPORTED_DSU_CAR, STOP_AND_GO_CAR from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS +from openpilot.selfdrive.controls.lib.pid import PIDController from opendbc.can.packer import CANPacker from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_acceleration import get_max_allowed_accel @@ -19,7 +23,10 @@ ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2 -ACCEL_WINDUP_LIMIT = 0.5 # m/s^2 / frame +# The up limit allows the brakes/gas to unwind quickly leaving a stop, +# the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup +ACCEL_WINDUP_LIMIT = 4.0 * DT_CTRL * 3 # m/s^2 / frame +ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3 # m/s^2 / frame # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long @@ -40,6 +47,23 @@ PARK = car.CarState.GearShifter.park +def get_long_tune(CP, params): + kiBP = [0.] + kdBP = [0.] + kdV = [0.] + if CP.carFingerprint in TSS2_CAR: + kiV = [0.25] + kdV = [0.25 / 4] + + else: + kiBP = [0., 5., 35.] + kiV = [3.6, 2.4, 1.5] + + return PIDController(0.0, (kiBP, kiV), k_f=1.0, k_d=(kdBP, kdV), + pos_limit=params.ACCEL_MAX, neg_limit=params.ACCEL_MIN, + rate=1 / (DT_CTRL * 3)) + + class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP @@ -50,31 +74,81 @@ def __init__(self, dbc_name, CP, VM): self.alert_active = False self.last_standstill = False self.standstill_req = False + self.permit_braking = True self.steer_rate_counter = 0 self.distance_button = 0 - self.pcm_accel_compensation = 0.0 - self.permit_braking = True + # *** start long control state *** + self.long_pid = get_long_tune(self.CP, self.params) + + self.error_rate = FirstOrderFilter(0.0, 0.5, DT_CTRL * 3) + self.prev_error = 0.0 + + self.aego = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3) + + self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL) - self.packer = CANPacker(dbc_name) self.accel = 0 + self.prev_accel = 0 + # *** end long control state *** + + self.packer = CANPacker(dbc_name) + + self.secoc_lka_message_counter = 0 + self.secoc_lta_message_counter = 0 + self.secoc_prev_reset_counter = 0 + self.secoc_mismatch_counter = 0 # FrogPilot variables + self.stock_max_accel = self.params.ACCEL_MAX + self.doors_locked = False self.reverse_cruise_active = False + self.updated_pid = False self.cruise_timer = 0 self.previous_set_speed = 0 def update(self, CC, CS, now_nanos, frogpilot_toggles): + if frogpilot_toggles.sport_plus: + if not self.updated_pid: + self.params.ACCEL_MAX = get_max_allowed_accel(0) + self.long_pid = get_long_tune(self.CP, self.params) + self.updated_pid = True + + self.params.ACCEL_MAX = min(frogpilot_toggles.max_desired_acceleration, get_max_allowed_accel(CS.out.vEgo)) + else: + if self.updated_pid: + self.params.ACCEL_MAX = self.stock_max_accel + self.long_pid = get_long_tune(self.CP, self.params) + self.updated_pid = False + + self.params.ACCEL_MAX = min(frogpilot_toggles.max_desired_acceleration, self.stock_max_accel) + actuators = CC.actuators + stopping = actuators.longControlState == LongCtrlState.stopping hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE + if len(CC.orientationNED) == 3: + self.pitch.update(CC.orientationNED[1]) + # *** control msgs *** can_sends = [] + # *** handle secoc reset counter increase *** + if self.CP.flags & ToyotaFlags.SECOC.value: + if CS.secoc_synchronization['RESET_CNT'] != self.secoc_prev_reset_counter: + self.secoc_lka_message_counter = 0 + self.secoc_lta_message_counter = 0 + self.secoc_prev_reset_counter = CS.secoc_synchronization['RESET_CNT'] + + expected_mac = build_sync_mac(self.secoc_key, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT'])) + if int(CS.secoc_synchronization['AUTHENTICATOR']) != expected_mac and self.secoc_mismatch_counter < 100: + carlog.error("SecOC synchronization MAC mismatch, wrong key?") + self.secoc_mismatch_counter += 1 + # *** steer torque *** new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params) @@ -108,7 +182,16 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): # toyota can trace shows STEERING_LKA at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages - can_sends.append(toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req)) + steer_command = toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req) + if self.CP.flags & ToyotaFlags.SECOC.value: + # TODO: check if this slow and needs to be done by the CANPacker + steer_command = add_mac(self.secoc_key, + int(CS.secoc_synchronization['TRIP_CNT']), + int(CS.secoc_synchronization['RESET_CNT']), + self.secoc_lka_message_counter, + steer_command) + self.secoc_lka_message_counter += 1 + can_sends.append(steer_command) # STEERING_LTA does not seem to allow more rate by sending faster, and may wind up easier if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR: @@ -123,6 +206,16 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.CP.steerControlType, self.last_angle, lta_active, self.frame // 2, torque_wind_down)) + if self.CP.flags & ToyotaFlags.SECOC.value: + lta_steer_2 = toyotacan.create_lta_steer_command_2(self.packer, self.frame // 2) + lta_steer_2 = add_mac(self.secoc_key, + int(CS.secoc_synchronization['TRIP_CNT']), + int(CS.secoc_synchronization['RESET_CNT']), + self.secoc_lta_message_counter, + lta_steer_2) + self.secoc_lta_message_counter += 1 + can_sends.append(lta_steer_2) + # *** gas and brake *** if self.CP.enableGasInterceptor and CC.longActive and self.CP.carFingerprint not in STOP_AND_GO_CAR: MAX_INTERCEPTOR_GAS = 0.5 @@ -142,43 +235,10 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): else: interceptor_gas_cmd = 0. - # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking - if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill: - # calculate amount of acceleration PCM should apply to reach target, given pitch - if len(CC.orientationNED) == 3: - accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY - else: - accel_due_to_pitch = 0.0 - - net_acceleration_request = actuators.accel + accel_due_to_pitch - - # let PCM handle stopping for now - pcm_accel_compensation = 0.0 - if actuators.longControlState != LongCtrlState.stopping: - pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - net_acceleration_request) - - # prevent compensation windup - pcm_accel_compensation = clip(pcm_accel_compensation, actuators.accel - self.params.ACCEL_MAX, - actuators.accel - self.params.ACCEL_MIN) - - self.pcm_accel_compensation = rate_limit(pcm_accel_compensation, self.pcm_accel_compensation, -0.01, 0.01) - pcm_accel_cmd = actuators.accel - self.pcm_accel_compensation - - # Along with rate limiting positive jerk below, this greatly improves gas response time - # Consider the net acceleration request that the PCM should be applying (pitch included) - if net_acceleration_request < 0.1: - self.permit_braking = True - elif net_acceleration_request > 0.2: - self.permit_braking = False - else: - self.pcm_accel_compensation = 0.0 - pcm_accel_cmd = actuators.accel - self.permit_braking = True - - if frogpilot_toggles.sport_plus: - pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, get_max_allowed_accel(CS.out.vEgo))) - else: - pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, min(frogpilot_toggles.max_desired_acceleration, self.params.ACCEL_MAX)) + if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: + # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. + # This prevents unexpected pedal range rescaling + can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2)) # on entering standstill, send standstill request if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor): @@ -192,36 +252,76 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): # handle UI messages fcw_alert = hud_control.visualAlert == VisualAlert.fcw steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) + lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged + + if self.CP.openpilotLongitudinalControl: + if self.frame % 3 == 0: + # Press distance button until we are at the correct bar length. Only change while enabled to avoid skipping startup popup + if self.frame % 6 == 0 and self.CP.openpilotLongitudinalControl: + desired_distance = 4 - hud_control.leadDistanceBars + if CS.out.cruiseState.enabled and CS.pcm_follow_distance != desired_distance: + self.distance_button = not self.distance_button + else: + self.distance_button = 0 - # we can spam can to cancel the system even if we are using lat only control - if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: - lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged - - # Press distance button until we are at the correct bar length. Only change while enabled to avoid skipping startup popup - if self.frame % 6 == 0 and self.CP.openpilotLongitudinalControl: - desired_distance = 4 - hud_control.leadDistanceBars - if CS.out.cruiseState.enabled and CS.pcm_follow_distance != desired_distance: - self.distance_button = not self.distance_button + # internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit + pcm_accel_cmd = actuators.accel + if CC.longActive: + pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT) + self.prev_accel = pcm_accel_cmd + + # calculate amount of acceleration PCM should apply to reach target, given pitch + accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY + # TODO: on uphills this sometimes sets PERMIT_BRAKING low not considering the creep force + net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch + + # GVC does not overshoot ego acceleration when starting from stop, but still has a similar delay + if not self.CP.flags & ToyotaFlags.SECOC.value: + a_ego_blended = interp(CS.out.vEgo, [1.0, 2.0], [CS.gvc, CS.out.aEgo]) + else: + a_ego_blended = CS.out.aEgo + + # wind down integral when approaching target for step changes and smooth ramps to reduce overshoot + prev_aego = self.aego.x + self.aego.update(a_ego_blended) + j_ego = (self.aego.x - prev_aego) / (DT_CTRL * 3) + a_ego_future = a_ego_blended + j_ego * 0.5 + + if actuators.longControlState == LongCtrlState.pid: + error = pcm_accel_cmd - a_ego_blended + self.error_rate.update((error - self.prev_error) / (DT_CTRL * 3)) + self.prev_error = error + + error_future = pcm_accel_cmd - a_ego_future + pcm_accel_cmd = self.long_pid.update(error_future, error_rate=self.error_rate.x, + speed=CS.out.vEgo, + feedforward=pcm_accel_cmd) else: - self.distance_button = 0 + self.long_pid.reset() + self.error_rate.x = 0.0 + self.prev_error = 0.0 - # Lexus IS uses a different cancellation message - if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: - can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) - elif self.CP.openpilotLongitudinalControl: - # internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit - pcm_accel_cmd = min(pcm_accel_cmd, self.accel + ACCEL_WINDUP_LIMIT) if CC.longActive else 0.0 + # Along with rate limiting positive jerk above, this greatly improves gas response time + # Consider the net acceleration request that the PCM should be applying (pitch included) + net_acceleration_request_min = min(actuators.accel + accel_due_to_pitch, net_acceleration_request) + if net_acceleration_request_min < 0.2 or stopping or not CC.longActive: + self.permit_braking = True + elif net_acceleration_request_min > 0.3: + self.permit_braking = False + + pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX) can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead, CS.acc_type, fcw_alert, self.distance_button, self.reverse_cruise_active)) self.accel = pcm_accel_cmd - else: - can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, True, False, lead, CS.acc_type, False, self.distance_button, self.reverse_cruise_active)) - if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: - # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. - # This prevents unexpected pedal range rescaling - can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2)) + else: + # we can spam can to cancel the system even if we are using lat only control + if pcm_cancel_cmd: + if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: + can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) + else: + can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, True, False, lead, CS.acc_type, False, self.distance_button, self.reverse_cruise_active)) # *** hud ui *** if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 55a8dedbb4f42a..716b7db63c1f4e 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -9,7 +9,7 @@ from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ - TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR + TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, SECOC_CAR SteerControlType = car.CarParams.SteerControlType @@ -45,11 +45,15 @@ class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) - self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"] self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2. self.cluster_min_speed = CV.KPH_TO_MS / 2. + if CP.flags & ToyotaFlags.SECOC.value: + self.shifter_values = can_define.dv["GEAR_PACKET_HYBRID"]["GEAR"] + else: + self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"] + # On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # the signal is zeroed to where the steering angle is at start. # Need to apply an offset as soon as the steering angle measurements are both received @@ -64,7 +68,8 @@ def __init__(self, CP): self.low_speed_lockout = False self.acc_type = 1 self.lkas_hud = {} - self.pcm_accel_net = 0.0 + self.gvc = 0.0 + self.secoc_synchronization = None # FrogPilot variables self.latActive_previous = False @@ -76,21 +81,10 @@ def __init__(self, CP): def update(self, cp, cp_cam, CC, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() + cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp - # Describes the acceleration request from the PCM if on flat ground, may be higher or lower if pitched - # CLUTCH->ACCEL_NET is only accurate for gas, PCM_CRUISE->ACCEL_NET is only accurate for brake - # These signals only have meaning when ACC is active - if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: - self.pcm_accel_net = max(cp.vl["CLUTCH"]["ACCEL_NET"], 0.0) - - # Sometimes ACC_BRAKING can be 1 while showing we're applying gas already - if cp.vl["PCM_CRUISE"]["ACC_BRAKING"]: - self.pcm_accel_net += min(cp.vl["PCM_CRUISE"]["ACCEL_NET"], 0.0) - - # add creeping force at low speeds only for braking, CLUTCH->ACCEL_NET already shows this - neutral_accel = max(cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] / self.CP.mass, 0.0) - if self.pcm_accel_net + neutral_accel < 0.0: - self.pcm_accel_net += neutral_accel + if not self.CP.flags & ToyotaFlags.SECOC.value: + self.gvc = cp.vl["VSC1S07"]["GVC"] ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]]) @@ -100,12 +94,22 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0 ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1 - if self.CP.enableGasInterceptor: - ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2 - ret.gasPressed = ret.gas > 805 + if self.CP.flags & ToyotaFlags.SECOC.value: + self.secoc_synchronization = copy.copy(cp.vl["SECOC_SYNCHRONIZATION"]) + ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"] + ret.gasPressed = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"] > 0 + can_gear = int(cp.vl["GEAR_PACKET_HYBRID"]["GEAR"]) else: - # TODO: find a common gas pedal percentage signal - ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 + if self.CP.enableGasInterceptor: + ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2 + ret.gasPressed = ret.gas > 805 + else: + ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 # TODO: these also have GAS_PEDAL, come back and unify + can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"]) + if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: + ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5) + if self.CP.carFingerprint != CAR.TOYOTA_MIRAI: + ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"] ret.wheelSpeeds = self.get_wheel_speeds( cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"], @@ -136,14 +140,10 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): ret.steeringAngleOffsetDeg = self.angle_offset.x ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x - can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1 ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2 - if self.CP.carFingerprint != CAR.TOYOTA_MIRAI: - ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"] - ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale # we could use the override bit from dbc, but it's triggered at too high torque values @@ -174,8 +174,6 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): conversion_factor = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor - cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp - if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: if not (self.CP.flags & ToyotaFlags.SMART_DSU.value): self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"] @@ -199,9 +197,6 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"]) ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0 - if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: - ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5) - if self.CP.enableBsm: ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1) ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1) @@ -264,7 +259,6 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles): @staticmethod def get_can_parser(CP): messages = [ - ("GEAR_PACKET", 1), ("LIGHT_STALK", 1), ("BLINKERS_STATE", 0.15), ("BODY_CONTROL_STATE", 3), @@ -279,11 +273,20 @@ def get_can_parser(CP): ("STEER_TORQUE_SENSOR", 50), ] - if CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: - messages.append(("CLUTCH", 15)) + if CP.flags & ToyotaFlags.SECOC.value: + messages += [ + ("GEAR_PACKET_HYBRID", 60), + ("SECOC_SYNCHRONIZATION", 10), + ("GAS_PEDAL", 42), + ] + else: + messages.append(("VSC1S07", 20)) + if CP.carFingerprint not in [CAR.TOYOTA_MIRAI]: + messages.append(("ENGINE_RPM", 42)) - if CP.carFingerprint != CAR.TOYOTA_MIRAI: - messages.append(("ENGINE_RPM", 42)) + messages += [ + ("GEAR_PACKET", 1), + ] if CP.carFingerprint in UNSUPPORTED_DSU_CAR: messages.append(("DSU_CRUISE", 5)) @@ -338,9 +341,14 @@ def get_cam_can_parser(CP): if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): messages += [ - ("PRE_COLLISION", 33), ("ACC_CONTROL", 33), ("PCS_HUD", 1), ] + # TODO: Figure out new layout of the PRE_COLLISION message + if not CP.flags & ToyotaFlags.SECOC.value: + messages += [ + ("PRE_COLLISION", 33), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2) diff --git a/selfdrive/car/toyota/fingerprints.py b/selfdrive/car/toyota/fingerprints.py index b8942cd59d3df9..ec9cdbb073e8df 100644 --- a/selfdrive/car/toyota/fingerprints.py +++ b/selfdrive/car/toyota/fingerprints.py @@ -110,6 +110,7 @@ b'\x018966306Q6000\x00\x00\x00\x00', b'\x018966333N1100\x00\x00\x00\x00', b'\x018966333N4300\x00\x00\x00\x00', + b'\x018966333P3000\x00\x00\x00\x00', b'\x018966333P3100\x00\x00\x00\x00', b'\x018966333P3200\x00\x00\x00\x00', b'\x018966333P4200\x00\x00\x00\x00', @@ -160,6 +161,7 @@ b'8821F0605200 ', b'8821F0606200 ', b'8821F0607200 ', + b'8821F0607300 ', b'8821F0608000 ', b'8821F0608200 ', b'8821F0608300 ', @@ -192,6 +194,7 @@ b'8965B33581\x00\x00\x00\x00\x00\x00', b'8965B33611\x00\x00\x00\x00\x00\x00', b'8965B33621\x00\x00\x00\x00\x00\x00', + b'8965B33630\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F0601200 ', @@ -207,6 +210,7 @@ b'8821F0605200 ', b'8821F0606200 ', b'8821F0607200 ', + b'8821F0607300 ', b'8821F0608000 ', b'8821F0608200 ', b'8821F0608300 ', @@ -754,6 +758,7 @@ b'\x018966353Q2300\x00\x00\x00\x00', b'\x018966353Q4000\x00\x00\x00\x00', b'\x018966353R1100\x00\x00\x00\x00', + b'\x018966353R7000\x00\x00\x00\x00', b'\x018966353R7100\x00\x00\x00\x00', b'\x018966353R8000\x00\x00\x00\x00', b'\x018966353R8100\x00\x00\x00\x00', @@ -1353,6 +1358,7 @@ ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F0610000\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', + b'\x028646F0610100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', @@ -1550,6 +1556,7 @@ b'\x01896630E36100\x00\x00\x00\x00', b'\x01896630E36200\x00\x00\x00\x00', b'\x01896630E36300\x00\x00\x00\x00', + b'\x01896630E36600\x00\x00\x00\x00', b'\x01896630E37100\x00\x00\x00\x00', b'\x01896630E37200\x00\x00\x00\x00', b'\x01896630E37300\x00\x00\x00\x00', @@ -1656,6 +1663,7 @@ b'\x02348X8000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02348Y3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0234D14000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0234D15000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0234D16000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 084bcb14bc52d1..172e0d65bfcb01 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -29,6 +29,10 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp if DBC[candidate]["pt"] == "toyota_new_mc_pt_generated": ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE + if ret.flags & ToyotaFlags.SECOC.value: + ret.secOcRequired = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_SECOC + if candidate in ANGLE_CONTROL_CAR: ret.steerControlType = SteerControlType.angle ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA @@ -57,7 +61,11 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \ and not (ret.flags & ToyotaFlags.SMART_DSU) - if candidate == CAR.LEXUS_ES_TSS2 and Ecu.hybrid not in found_ecus: + if Ecu.hybrid in found_ecus: + ret.flags |= ToyotaFlags.HYBRID.value + + # TODO: expand to the rest of the cars + if candidate in (CAR.LEXUS_ES_TSS2,) and not (ret.flags & ToyotaFlags.HYBRID.value): ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value if candidate == CAR.TOYOTA_PRIUS: @@ -73,7 +81,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2): ret.wheelSpeedFactor = 1.035 - elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023): + elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023, CAR.TOYOTA_RAV4_PRIME, CAR.TOYOTA_SIENNA_4TH_GEN): ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP = [0.0] ret.lateralTuning.pid.kpBP = [0.0] @@ -121,8 +129,13 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp # - TSS2 radar ACC cars w/ smartDSU installed # - TSS2 radar ACC cars w/o smartDSU installed (disables radar) # - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet) - ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) + + if ret.flags & ToyotaFlags.SECOC.value: + ret.openpilotLongitudinalControl = False + else: + ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) ret.openpilotLongitudinalControl &= not disable_openpilot_long + ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR ret.enableGasInterceptor = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl @@ -136,27 +149,21 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp # to a negative value, so it won't matter. ret.minEnableSpeed = -1. if (candidate in STOP_AND_GO_CAR or ret.enableGasInterceptor) else MIN_ACC_SPEED - tune = ret.longitudinalTuning if ret.enableGasInterceptor: + tune = ret.longitudinalTuning tune.kiBP = [0., 5., 20.] tune.kiV = [1.3, 1.0, 0.7] ret.vEgoStopping = 0.25 ret.vEgoStarting = 0.25 ret.stoppingDecelRate = 0.3 # reach stopping target smoothly elif candidate in TSS2_CAR: - tune.kpV = [0.0] - tune.kiV = [0.5] ret.vEgoStopping = 0.25 ret.vEgoStarting = 0.25 ret.stoppingDecelRate = 0.3 # reach stopping target smoothly - # Since we compensate for imprecise acceleration in carcontroller, we can be less aggressive with tuning - # This also prevents unnecessary request windup due to internal car jerk limits - if ret.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: - tune.kiV = [0.25] - else: - tune.kiBP = [0., 5., 35.] - tune.kiV = [3.6, 2.4, 1.5] + # Hybrids have much quicker longitudinal actuator response + if ret.flags & ToyotaFlags.HYBRID.value: + ret.longitudinalActuatorDelay = 0.05 if params.get_bool("FrogsGoMoosTweak"): ret.stoppingDecelRate = 0.1 # reach stopping target smoothly diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index fd3887efab3f49..5d633fc952aa00 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -33,10 +33,17 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, return packer.make_can_msg("STEERING_LTA", 0, values) +def create_lta_steer_command_2(packer, frame): + values = { + "COUNTER": frame + 128, + } + return packer.make_can_msg("STEERING_LTA_2", 0, values) + + def create_accel_command(packer, accel, pcm_cancel, permit_braking, standstill_req, lead, acc_type, fcw_alert, distance, reverse_cruise_active): # TODO: find the exact canceling bit that does not create a chime values = { - "ACCEL_CMD": accel, # compensated accel command + "ACCEL_CMD": accel, "ACC_TYPE": acc_type, "DISTANCE": distance, "MINI_CAR": lead, diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 1ae1effd05e238..6fd0885c696550 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -298,7 +298,7 @@ class CAR(Platforms): LEXUS_ES_TSS2 = ToyotaTSS2PlatformConfig( [ ToyotaCarDocs("Lexus ES 2019-24"), - ToyotaCarDocs("Lexus ES Hybrid 2019-24", video_link="https://youtu.be/BZ29osRVJeg?t=12"), + ToyotaCarDocs("Lexus ES Hybrid 2019-25", video_link="https://youtu.be/BZ29osRVJeg?t=12"), ], LEXUS_ES.specs, ) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e46a7bb828b37d..f182040008f498 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -15,7 +15,7 @@ from openpilot.common.git import get_short_branch from openpilot.common.numpy_fast import clip from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL +from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL, DT_MDL from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event @@ -579,7 +579,7 @@ def state_control(self, CS): torque_params = self.sm['liveTorqueParameters'] friction = self.frogpilot_toggles.steer_friction if self.frogpilot_toggles.use_custom_steer_friction else torque_params.frictionCoefficientFiltered lat_accel_factor = self.frogpilot_toggles.steer_lat_accel_factor if self.frogpilot_toggles.use_custom_lat_accel_factor else torque_params.latAccelFactorFiltered - if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.frogpilot_toggles.force_auto_tune) and not self.frogpilot_toggles.force_auto_tune_off: + if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.frogpilot_toggles.force_auto_tune): self.LaC.update_live_torque_params(lat_accel_factor, torque_params.latAccelOffsetFiltered, friction) @@ -744,7 +744,7 @@ def update_frogpilot_variables(self, CS): self.experimental_mode = not self.experimental_mode self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode) - if self.sm.frame % 10 == 0 or self.resume_pressed: + if self.sm.frame * DT_CTRL % DT_MDL == 0 or self.resume_pressed: self.resume_previously_pressed = self.resume_pressed FPCC = custom.FrogPilotCarControl.new_message() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index bdb986428abfa8..cae152992a6cb1 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -221,6 +221,18 @@ def get_lead_adjacent(tracks: dict[int, Track], model_data: capnp._DynamicStruct return lead_dict +def get_leads_lead(tracks: dict[int, Track], leadOne) -> dict[str, Any]: + lead_dict = {'status': False} + + toyota_corolla_length = 4.6355 + leads_lead_tracks = [c for c in tracks.values() if leadOne.status and c.dRel > leadOne.dRel + toyota_corolla_length] + if len(leads_lead_tracks) > 0: + closest_track = min(leads_lead_tracks, key=lambda c: c.dRel) + lead_dict = closest_track.get_RadarState() + + return lead_dict + + class RadarD: def __init__(self, frogpilot_toggles, radar_ts: float, delay: int = 0): self.points: dict[int, tuple[float, float, float]] = {} @@ -304,6 +316,7 @@ def update(self, sm: messaging.SubMaster, rr): self.radar_state.leadLeftFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True, far=True) self.radar_state.leadRight = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthRight, left=False) self.radar_state.leadRightFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthRight, left=False, far=True) + self.radar_state.leadsLead = get_leads_lead(self.tracks, self.radar_state.leadOne) # Update FrogPilot parameters if sm['frogpilotPlan'].togglesUpdated: diff --git a/selfdrive/frogpilot/assets/model_manager.py b/selfdrive/frogpilot/assets/model_manager.py index d7fa1162422bc5..999d35f167bd10 100644 --- a/selfdrive/frogpilot/assets/model_manager.py +++ b/selfdrive/frogpilot/assets/model_manager.py @@ -127,27 +127,29 @@ def update_model_params(self, model_info, repo_url): for model in model_info: available_models.append(model['id']) - params.put_nonblocking("AvailableModels", ','.join(available_models)) - params.put_nonblocking("AvailableModelsNames", ','.join([model['name'] for model in model_info])) - params.put_nonblocking("ClassicModels", ','.join([model['id'] for model in model_info if model.get("classic_model", False)])) - params.put_nonblocking("ExperimentalModels", ','.join([model['id'] for model in model_info if model.get("experimental", False)])) - params.put_nonblocking("NavigationModels", ','.join([model['id'] for model in model_info if "🗺️" in model['name']])) - params.put_nonblocking("RadarlessModels", ','.join([model['id'] for model in model_info if "📡" not in model['name']])) + params.put("AvailableModels", ','.join(available_models)) + params.put("AvailableModelsNames", ','.join([model['name'] for model in model_info])) + params.put("ClassicModels", ','.join([model['id'] for model in model_info if model.get("classic_model", False)])) + params.put("ExperimentalModels", ','.join([model['id'] for model in model_info if model.get("experimental", False)])) + params.put("NavigationModels", ','.join([model['id'] for model in model_info if "🗺️" in model['name']])) + params.put("RadarlessModels", ','.join([model['id'] for model in model_info if "📡" not in model['name']])) print("Models list updated successfully") if available_models: models_downloaded = self.are_all_models_downloaded(available_models, repo_url) - params.put_bool_nonblocking("ModelsDownloaded", models_downloaded) + params.put_bool("ModelsDownloaded", models_downloaded) def are_all_models_downloaded(self, available_models, repo_url): + model_sizes = self.fetch_all_model_sizes(repo_url) + if not model_sizes: + return + available_models = set(available_models) - {DEFAULT_MODEL, DEFAULT_CLASSIC_MODEL} - automatically_update_models = params.get_bool("AutomaticallyUpdateModels") all_models_downloaded = True + automatically_update_models = params.get_bool("AutomaticallyUpdateModels") - model_sizes = self.fetch_all_model_sizes(repo_url) download_queue = [] - for model in available_models: model_path = os.path.join(MODELS_PATH, f"{model}.thneed") expected_size = model_sizes.get(model) @@ -179,7 +181,7 @@ def validate_models(self): current_model_name = params.get("ModelName", encoding='utf-8') if "(Default)" in current_model_name and current_model_name != DEFAULT_CLASSIC_MODEL_NAME: - params.put_nonblocking("ModelName", current_model_name.replace(" (Default)", "")) + params.put("ModelName", current_model_name.replace(" (Default)", "")) available_models = params.get("AvailableModels", encoding='utf-8') if not available_models: @@ -194,8 +196,8 @@ def validate_models(self): model_name = model_file.replace(".thneed", "") if model_name not in available_models.split(','): if model_name == current_model: - params.put_nonblocking("Model", DEFAULT_CLASSIC_MODEL) - params.put_nonblocking("ModelName", DEFAULT_CLASSIC_MODEL_NAME) + params.put("Model", DEFAULT_CLASSIC_MODEL) + params.put("ModelName", DEFAULT_CLASSIC_MODEL_NAME) delete_file(os.path.join(MODELS_PATH, model_file)) print(f"Deleted model file: {model_file} - Reason: Model is not in the list of available models") @@ -253,4 +255,4 @@ def download_all_models(self): params_memory.put(self.download_progress_param, "All models downloaded!") params_memory.remove("DownloadAllModels") - params.put_bool_nonblocking("ModelsDownloaded", True) + params.put_bool("ModelsDownloaded", True) diff --git a/selfdrive/frogpilot/assets/theme_manager.py b/selfdrive/frogpilot/assets/theme_manager.py index 2d25eacfa086ca..e3ad1ec752e1b3 100644 --- a/selfdrive/frogpilot/assets/theme_manager.py +++ b/selfdrive/frogpilot/assets/theme_manager.py @@ -23,7 +23,7 @@ def update_theme_asset(asset_type, theme, holiday_theme): save_location = os.path.join(ACTIVE_THEME_PATH, asset_type) - if holiday_theme: + if holiday_theme is not None: asset_location = os.path.join(HOLIDAY_THEME_PATH, holiday_theme, asset_type) elif asset_type == "distance_icons": asset_location = os.path.join(THEME_SAVE_PATH, "distance_icons", theme) @@ -51,11 +51,14 @@ def update_theme_asset(asset_type, theme, holiday_theme): os.makedirs(os.path.dirname(save_location), exist_ok=True) + if os.path.islink(save_location): + os.unlink(save_location) + if os.path.exists(save_location): - if os.path.islink(save_location): - os.unlink(save_location) - elif os.path.isdir(save_location): + if os.path.isdir(save_location): shutil.rmtree(save_location) + else: + os.remove(save_location) os.symlink(asset_location, save_location, target_is_directory=True) print(f"Linked {save_location} to {asset_location}") @@ -63,7 +66,7 @@ def update_theme_asset(asset_type, theme, holiday_theme): def update_wheel_image(image, holiday_theme=None, random_event=True): wheel_save_location = os.path.join(ACTIVE_THEME_PATH, "steering_wheel") - if holiday_theme: + if holiday_theme is not None: wheel_location = os.path.join(HOLIDAY_THEME_PATH, holiday_theme, "steering_wheel") elif random_event: wheel_location = os.path.join(RANDOM_EVENTS_PATH, "icons") @@ -426,6 +429,8 @@ def update_themes(self, frogpilot_toggles, boot_run=False): self.validate_themes(frogpilot_toggles) assets = self.fetch_assets(repo_url) + if not assets["themes"] and not assets["distance_icons"] and not assets["wheels"]: + return downloadable_colors = [] downloadable_icons = [] diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index dde238a451b926..85c945a5d92b2a 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -15,7 +15,9 @@ from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, MODEL_LENGTH, NON_DRIVING_GEARS, PLANNER_TIME, THRESHOLD class FrogPilotPlanner: - def __init__(self): + def __init__(self, error_log): + self.error_log = error_log + self.cem = ConditionalExperimentalMode(self) self.frogpilot_acceleration = FrogPilotAcceleration(self) self.frogpilot_events = FrogPilotEvents(self) @@ -34,7 +36,7 @@ def __init__(self): self.road_curvature = 1 self.v_cruise = 0 - def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarless_model, radarState, frogpilot_toggles): + def update(self, carControl, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarless_model, radarState, frogpilot_toggles): if radarless_model: model_leads = list(modelData.leadsV3) if len(model_leads) > 0: @@ -50,7 +52,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState v_ego = max(carState.vEgo, 0) v_lead = self.lead_one.vLead - self.frogpilot_acceleration.update(controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles) + self.frogpilot_acceleration.update(controlsState, frogpilotCarState, v_ego, frogpilot_toggles) run_cem = frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.force_stops or frogpilot_toggles.green_light_alert or frogpilot_toggles.show_stopping_point if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateralActive) and carState.gearShifter not in NON_DRIVING_GEARS: @@ -80,7 +82,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState self.road_curvature = calculate_road_curvature(modelData, v_ego) if not carState.standstill else 1 self.tracking_lead = self.set_lead_status(frogpilotCarState, v_ego, frogpilot_toggles) - self.v_cruise = self.frogpilot_vcruise.update(carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles) + self.v_cruise = self.frogpilot_vcruise.update(carControl, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, v_cruise, v_ego, frogpilot_toggles) def set_lead_status(self, frogpilotCarState, v_ego, frogpilot_toggles): distance_offset = frogpilot_toggles.increased_stopped_distance if not frogpilotCarState.trafficModeActive else 0 diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py b/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py index e4f04953da99f7..776e00e6425747 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py @@ -39,7 +39,7 @@ def __init__(self, FrogPilotPlanner): self.max_accel = 0 self.min_accel = 0 - def update(self, controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles): + def update(self, controlsState, frogpilotCarState, v_ego, frogpilot_toggles): eco_gear = frogpilotCarState.ecoGear sport_gear = frogpilotCarState.sportGear diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_events.py b/selfdrive/frogpilot/controls/lib/frogpilot_events.py index 5838aed82dc4ad..ce0d33531e03d2 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_events.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_events.py @@ -1,8 +1,6 @@ import os import random -import openpilot.system.sentry as sentry - from openpilot.common.conversions import Conversions as CV from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.controlsd import Desire @@ -11,11 +9,14 @@ from openpilot.selfdrive.frogpilot.assets.theme_manager import update_wheel_image from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, params, params_memory +RANDOM_EVENTS_CHANCE = 0.01 * DT_MDL + class FrogPilotEvents: def __init__(self, FrogPilotPlanner): - self.events = Events() self.frogpilot_planner = FrogPilotPlanner + self.events = Events() + self.accel30_played = False self.accel35_played = False self.accel40_played = False @@ -76,7 +77,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState else: self.tracking_lead_distance = 0 - if not self.openpilot_crashed_played and os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')): + if not self.openpilot_crashed_played and os.path.isfile(self.frogpilot_planner.error_log): if frogpilot_toggles.random_events: self.events.add(EventName.openpilotCrashedRandomEvent) else: @@ -135,7 +136,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState if not self.this_is_fine_played: event_choices.append("thisIsFineSteerSaturated") - if self.frame % 100 == 0 and event_choices: + if random.random() < RANDOM_EVENTS_CHANCE and event_choices: event_choice = random.choice(event_choices) if event_choice == "firefoxSteerSaturated": self.events.add(EventName.firefoxSteerSaturated) @@ -161,17 +162,17 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState if not self.fcw_played and frogpilotCarControl.fcwEventTriggered: event_choices = ["toBeContinued", "yourFrogTriedToKillMe"] - event_choice = random.choice(event_choices) - if random.random() < 0.1: + if random.random() < RANDOM_EVENTS_CHANCE: + event_choice = random.choice(event_choices) if event_choice == "toBeContinued": self.events.add(EventName.toBeContinued) elif event_choice == "yourFrogTriedToKillMe": self.events.add(EventName.yourFrogTriedToKillMe) - self.fcw_played = True - self.random_event_played = True + self.fcw_played = True + self.random_event_played = True if not self.youveGotMail_played and frogpilotCarControl.alwaysOnLateralActive and not self.always_on_lateral_active_previously: - if random.random() < 0.01 and not carState.standstill: + if random.random() < RANDOM_EVENTS_CHANCE and not carState.standstill: self.events.add(EventName.youveGotMail) self.youveGotMail_played = True self.random_event_played = True @@ -180,7 +181,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState if frogpilot_toggles.speed_limit_changed_alert and self.frogpilot_planner.frogpilot_vcruise.speed_limit_changed: self.events.add(EventName.speedLimitChanged) - if self.frame == 4 and params.get("NNFFModelName", encoding='utf-8') is not None: + if 5 > self.frame > 4 and params.get("NNFFModelName", encoding='utf-8') is not None: self.events.add(EventName.torqueNNLoad) if frogpilotCarState.trafficModeActive != self.previous_traffic_mode: @@ -195,4 +196,4 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState elif modelData.meta.turnDirection == Desire.turnRight: self.events.add(EventName.turningRight) - self.frame = round(self.frame + DT_MDL, 2) + self.frame += DT_MDL diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_following.py b/selfdrive/frogpilot/controls/lib/frogpilot_following.py index a29c4577700dbf..600b17947a5dbd 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_following.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_following.py @@ -76,17 +76,17 @@ def update_follow_values(self, lead_distance, v_ego, v_lead, frogpilot_toggles): # Offset by FrogAi for FrogPilot for a more natural approach to a faster lead if frogpilot_toggles.human_following and v_lead > v_ego: distance_factor = max(lead_distance - (v_ego * self.t_follow), 1) - standstill_offset = max(STOP_DISTANCE - v_ego, 0) * max(v_lead - v_ego, 1) - acceleration_offset = clip((v_lead - v_ego) + standstill_offset - COMFORT_BRAKE, 1, distance_factor) - self.acceleration_jerk /= acceleration_offset - self.speed_jerk /= acceleration_offset + standstill_offset = max((v_lead - v_ego) * (STOP_DISTANCE - v_ego), 1) + acceleration_offset = clip((v_lead - v_ego) * standstill_offset - COMFORT_BRAKE, 1, distance_factor) + self.acceleration_jerk /= standstill_offset + self.speed_jerk /= standstill_offset self.t_follow /= acceleration_offset # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead if (frogpilot_toggles.conditional_slower_lead or frogpilot_toggles.human_following) and v_lead < v_ego > CRUISING_SPEED: distance_factor = max(lead_distance - (v_lead * self.t_follow), 1) - far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - STOP_DISTANCE + (v_lead - CITY_SPEED_LIMIT), 0) - braking_offset = clip((v_ego - v_lead) + far_lead_offset - COMFORT_BRAKE, 1, distance_factor) + far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - STOP_DISTANCE + (v_lead - CITY_SPEED_LIMIT), 1) + braking_offset = clip((v_ego - v_lead) * far_lead_offset - COMFORT_BRAKE, 1, distance_factor) if frogpilot_toggles.human_following: self.t_follow /= braking_offset self.slower_lead = braking_offset - far_lead_offset > 1 diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py b/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py index fc4a918abf28b0..8b3da09a35e886 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py @@ -33,7 +33,7 @@ def __init__(self, FrogPilotPlanner): self.tracked_model_length = 0 self.vtsc_target = 0 - def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles): + def update(self, carControl, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, v_cruise, v_ego, frogpilot_toggles): force_stop = frogpilot_toggles.force_stops and self.frogpilot_planner.cem.stop_light_detected and controlsState.enabled force_stop &= self.frogpilot_planner.model_length < 100 force_stop &= self.override_force_stop_timer <= 0 @@ -52,11 +52,14 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState elif self.override_force_stop_timer > 0: self.override_force_stop_timer -= DT_MDL + v_cruise_cluster = max(controlsState.vCruiseCluster * CV.KPH_TO_MS, v_cruise) + v_cruise_diff = v_cruise_cluster - v_cruise + v_ego_cluster = max(carState.vEgoCluster, v_ego) v_ego_diff = v_ego_cluster - v_ego # Pfeiferj's Map Turn Speed Controller - if frogpilot_toggles.map_turn_speed_controller and v_ego > CRUISING_SPEED and controlsState.enabled: + if frogpilot_toggles.map_turn_speed_controller and v_ego > CRUISING_SPEED and carControl.longActive: mtsc_active = self.mtsc_target < v_cruise self.mtsc_target = clip(self.mtsc.target_speed(v_ego, carState.aEgo, frogpilot_toggles), CRUISING_SPEED, v_cruise) @@ -77,21 +80,13 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState unconfirmed_slc_target = self.slc.desired_speed_limit if (frogpilot_toggles.speed_limit_changed_alert or frogpilot_toggles.speed_limit_confirmation) and self.slc_target != 0: - self.speed_limit_changed = unconfirmed_slc_target != self.previous_speed_limit and abs(self.slc_target - unconfirmed_slc_target) > 1 and unconfirmed_slc_target > 1 + self.speed_limit_changed |= unconfirmed_slc_target != self.previous_speed_limit and abs(self.slc_target - unconfirmed_slc_target) > 1 speed_limit_decreased = self.speed_limit_changed and self.slc_target > unconfirmed_slc_target speed_limit_increased = self.speed_limit_changed and self.slc_target < unconfirmed_slc_target - accepted_via_ui = params_memory.get_bool("SLCConfirmedPressed") and params_memory.get_bool("SLCConfirmed") - denied_via_ui = params_memory.get_bool("SLCConfirmedPressed") and not params_memory.get_bool("SLCConfirmed") - - speed_limit_accepted = frogpilotCarControl.resumePressed and controlsState.enabled or accepted_via_ui - speed_limit_denied = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents) and controlsState.enabled or denied_via_ui or self.speed_limit_timer >= 30 - - if speed_limit_accepted or speed_limit_denied: - self.previous_speed_limit = unconfirmed_slc_target - params_memory.put_bool("SLCConfirmed", False) - params_memory.put_bool("SLCConfirmedPressed", False) + speed_limit_accepted = frogpilotCarControl.resumePressed and carControl.longActive or params_memory.get_bool("SLCConfirmed") + speed_limit_denied = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents) and carControl.longActive or self.speed_limit_timer >= 30 if speed_limit_decreased: speed_limit_confirmed = not frogpilot_toggles.speed_limit_confirmation_lower or speed_limit_accepted @@ -100,15 +95,18 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState else: speed_limit_confirmed = False + if speed_limit_confirmed: + self.slc_target = unconfirmed_slc_target + + if speed_limit_denied or self.slc_target == unconfirmed_slc_target: + self.speed_limit_changed = False + if self.speed_limit_changed: self.speed_limit_timer += DT_MDL else: self.speed_limit_timer = 0 - if speed_limit_confirmed: - self.slc_target = unconfirmed_slc_target - elif self.slc_target == unconfirmed_slc_target: - self.speed_limit_changed = False + self.previous_speed_limit = unconfirmed_slc_target else: self.slc_target = unconfirmed_slc_target @@ -122,14 +120,14 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState self.overridden_speed = v_ego_cluster self.overridden_speed = clip(self.overridden_speed, self.slc_target, v_cruise) elif frogpilot_toggles.speed_limit_controller_override_set_speed: - self.overridden_speed = v_cruise + self.overridden_speed = v_cruise_cluster else: self.overridden_speed = 0 else: self.slc_target = 0 # Pfeiferj's Vision Turn Controller - if frogpilot_toggles.vision_turn_controller and v_ego > CRUISING_SPEED and controlsState.enabled: + if frogpilot_toggles.vision_turn_controller and v_ego > CRUISING_SPEED and carControl.longActive: self.vtsc_target = ((TARGET_LAT_A * frogpilot_toggles.turn_aggressiveness) / (self.frogpilot_planner.road_curvature * frogpilot_toggles.curve_sensitivity))**0.5 self.vtsc_target = clip(self.vtsc_target, CRUISING_SPEED, v_cruise) else: @@ -155,4 +153,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff, self.vtsc_target] v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets])) + self.mtsc_target += v_cruise_diff + self.vtsc_target += v_cruise_diff + return v_cruise diff --git a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py index 9315ad9f3f776b..33298bcef2713f 100644 --- a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py +++ b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py @@ -85,8 +85,12 @@ def get_speed_limit(self, dashboard_speed_limit, max_speed_limit, navigation_spe self.source = min(filtered_limits, key=filtered_limits.get) return filtered_limits[self.source] - for priority in [frogpilot_toggles.speed_limit_priority1, frogpilot_toggles.speed_limit_priority2, frogpilot_toggles.speed_limit_priority3]: - if priority in filtered_limits: + for priority in [ + frogpilot_toggles.speed_limit_priority1, + frogpilot_toggles.speed_limit_priority2, + frogpilot_toggles.speed_limit_priority3 + ]: + if priority is not None and priority in filtered_limits: self.source = priority return filtered_limits[priority] diff --git a/selfdrive/frogpilot/frogpilot_functions.py b/selfdrive/frogpilot/frogpilot_functions.py index a02bf219aa2cf1..ae5b6dc467a1aa 100644 --- a/selfdrive/frogpilot/frogpilot_functions.py +++ b/selfdrive/frogpilot/frogpilot_functions.py @@ -8,15 +8,15 @@ import time from openpilot.common.basedir import BASEDIR -from openpilot.common.params_pyx import ParamKeyType, UnknownKeyName +from openpilot.common.params_pyx import Params, ParamKeyType, UnknownKeyName from openpilot.common.time import system_time_valid from openpilot.system.hardware import HARDWARE from openpilot.selfdrive.frogpilot.frogpilot_utilities import copy_if_exists, run_cmd -from openpilot.selfdrive.frogpilot.frogpilot_variables import ACTIVE_THEME_PATH, MODELS_PATH, THEME_SAVE_PATH, FrogPilotVariables +from openpilot.selfdrive.frogpilot.frogpilot_variables import ACTIVE_THEME_PATH, MODELS_PATH, THEME_SAVE_PATH, FrogPilotVariables, update_frogpilot_toggles, params -def backup_directory(backup, destination, success_message, fail_message, minimum_backup_size=0, params=None, compressed=False): +def backup_directory(backup, destination, success_message, fail_message, minimum_backup_size=0, compressed=False): if not compressed: if os.path.exists(destination): print("Backup already exists. Aborting") @@ -53,29 +53,31 @@ def backup_directory(backup, destination, success_message, fail_message, minimum params.put_int("MinimumBackupSize", compressed_backup_size) -def cleanup_backups(directory, limit, minimum_backup_size=0, compressed=False): +def cleanup_backups(directory, limit, compressed=False): os.makedirs(directory, exist_ok=True) backups = sorted(glob.glob(os.path.join(directory, "*_auto*")), key=os.path.getmtime, reverse=True) for backup in backups[:]: - if backup.endswith("_in_progress"): - run_cmd(["sudo", "rm", "-rf", backup], f"Deleted in-progress backup: {os.path.basename(backup)}", f"Failed to delete in-progress backup: {os.path.basename(backup)}") + if backup.endswith("_in_progress") or backup.endswith("_in_progress.tar.gz"): + if os.path.isdir(backup): + shutil.rmtree(backup) + elif os.path.isfile(backup): + os.remove(backup) + backups.remove(backup) - if compressed: - for backup in backups[:]: - if os.path.getsize(backup) < minimum_backup_size: - run_cmd(["sudo", "rm", "-rf", backup], f"Deleted incomplete backup: {os.path.basename(backup)}", f"Failed to delete incomplete backup: {os.path.basename(backup)}") + for oldest_backup in backups[limit:]: + if os.path.isdir(oldest_backup): + shutil.rmtree(oldest_backup) + elif os.path.isfile(oldest_backup): + os.remove(oldest_backup) - for old_backup in backups[limit - 1:]: - run_cmd(["sudo", "rm", "-rf", old_backup], f"Deleted oldest backup: {os.path.basename(old_backup)}", f"Failed to delete backup: {os.path.basename(old_backup)}") - -def backup_frogpilot(build_metadata, params): +def backup_frogpilot(build_metadata): backup_path = os.path.join("/data", "backups") maximum_backups = 5 minimum_backup_size = params.get_int("MinimumBackupSize") - cleanup_backups(backup_path, maximum_backups, minimum_backup_size, compressed=True) + cleanup_backups(backup_path, maximum_backups) _, _, free = shutil.disk_usage(backup_path) required_free_space = minimum_backup_size * maximum_backups @@ -84,10 +86,10 @@ def backup_frogpilot(build_metadata, params): branch = build_metadata.channel commit = build_metadata.openpilot.git_commit_date[12:-16] backup_dir = os.path.join(backup_path, f"{branch}_{commit}_auto") - backup_directory(BASEDIR, backup_dir, f"Successfully backed up FrogPilot to {backup_dir}.", f"Failed to backup FrogPilot to {backup_dir}.", minimum_backup_size, params, True) + backup_directory(BASEDIR, backup_dir, f"Successfully backed up FrogPilot to {backup_dir}.", f"Failed to backup FrogPilot to {backup_dir}.", minimum_backup_size, True) -def backup_toggles(params, params_storage): +def backup_toggles(params_storage): for key in params.all_keys(): if params.get_key_type(key) & ParamKeyType.FROGPILOT_STORAGE: value = params.get(key) @@ -103,7 +105,7 @@ def backup_toggles(params, params_storage): backup_directory(os.path.join("/data", "params", "d"), backup_dir, f"Successfully backed up toggles to {backup_dir}.", f"Failed to backup toggles to {backup_dir}.") -def convert_params(params, params_storage): +def convert_params(params_storage): print("Starting to convert params") required_type = str @@ -159,7 +161,17 @@ def decrease_param(key): print("Param conversion completed") -def frogpilot_boot_functions(build_metadata, params, params_storage): +def frogpilot_boot_functions(build_metadata, params_storage): + if params.get_int("CustomizationLevel") == 2 or params_storage.get_int("CustomizationLevel") == 2: + if Params("/persist/tracking").get_int("FrogPilotMinutes") / 60 < 100: + params.put_int("CustomizationLevel", 1) + params_storage.put_int("CustomizationLevel", 1) + + update_frogpilot_toggles() + + if params.get_bool("HasAcceptedTerms"): + params_storage.clear_all() + old_screenrecordings = os.path.join("/data", "media", "0", "videos") new_screenrecordings = os.path.join("/data", "media", "screen_recordings") @@ -171,15 +183,16 @@ def frogpilot_boot_functions(build_metadata, params, params_storage): print("Waiting for system time to become valid...") time.sleep(1) - backup_frogpilot(build_metadata, params) - backup_toggles(params, params_storage) + if params.get("UpdaterAvailableBranches") is None: + os.system("pkill -SIGUSR1 -f system.updated.updated") + + backup_frogpilot(build_metadata) -def setup_frogpilot(build_metadata, params): +def setup_frogpilot(build_metadata): FrogPilotVariables().update(started=False) - remount_persist = ["sudo", "mount", "-o", "remount,rw", "/persist"] - run_cmd(remount_persist, "Successfully remounted /persist as read-write.", "Failed to remount /persist.") + run_cmd(["sudo", "mount", "-o", "remount,rw", "/persist"], "Successfully remounted /persist as read-write.", "Failed to remount /persist.") os.makedirs("/persist/params", exist_ok=True) os.makedirs(MODELS_PATH, exist_ok=True) @@ -227,8 +240,7 @@ def setup_frogpilot(build_metadata, params): if not os.path.exists(frog_steering_wheel_destination): copy_if_exists(frog_steering_wheel_source, frog_steering_wheel_destination, single_file_name="frog.png") - remount_root = ["sudo", "mount", "-o", "remount,rw", "/"] - run_cmd(remount_root, "File system remounted as read-write.", "Failed to remount file system.") + run_cmd(["sudo", "mount", "-o", "remount,rw", "/"], "File system remounted as read-write.", "Failed to remount file system.") boot_logo_location = "/usr/comma/bg.jpg" boot_logo_save_location = os.path.join(BASEDIR, "selfdrive", "frogpilot", "assets", "other_images", "original_bg.jpg") @@ -246,7 +258,6 @@ def uninstall_frogpilot(): boot_logo_location = "/usr/comma/bg.jpg" boot_logo_restore_location = os.path.join(BASEDIR, "selfdrive", "frogpilot", "assets", "other_images", "original_bg.jpg") - copy_cmd = ["sudo", "cp", boot_logo_restore_location, boot_logo_location] - run_cmd(copy_cmd, "Successfully restored the original boot logo.", "Failed to restore the original boot logo.") + run_cmd(["sudo", "cp", boot_logo_restore_location, boot_logo_location], "Successfully restored the original boot logo.", "Failed to restore the original boot logo.") HARDWARE.uninstall() diff --git a/selfdrive/frogpilot/frogpilot_process.py b/selfdrive/frogpilot/frogpilot_process.py index 570183b14f3e22..2b089441192f8a 100644 --- a/selfdrive/frogpilot/frogpilot_process.py +++ b/selfdrive/frogpilot/frogpilot_process.py @@ -18,6 +18,7 @@ from openpilot.selfdrive.frogpilot.frogpilot_functions import backup_toggles from openpilot.selfdrive.frogpilot.frogpilot_utilities import is_url_pingable from openpilot.selfdrive.frogpilot.frogpilot_variables import FrogPilotVariables, get_frogpilot_toggles, params, params_memory +from openpilot.selfdrive.frogpilot.navigation.mapd import update_mapd locks = { "backup_toggles": threading.Lock(), @@ -26,13 +27,13 @@ "download_theme": threading.Lock(), "update_active_theme": threading.Lock(), "update_checks": threading.Lock(), + "update_mapd": threading.Lock(), "update_models": threading.Lock(), "update_themes": threading.Lock() } running_threads = {} - def run_thread_with_lock(name, target, args=()): if not running_threads.get(name, threading.Thread()).is_alive(): with locks[name]: @@ -40,27 +41,22 @@ def run_thread_with_lock(name, target, args=()): thread.start() running_threads[name] = thread +def automatic_update_check(): + os.system("pkill -SIGUSR1 -f system.updated.updated") + while params.get("UpdaterState", encoding="utf8") != "idle": + time.sleep(60) + + if not params.get_bool("UpdaterFetchAvailable"): + return -def automatic_update_check(frogs_go_moo, started): - update_available = params.get_bool("UpdaterFetchAvailable") - update_ready = params.get_bool("UpdateAvailable") - update_state_idle = params.get("UpdaterState", encoding='utf8') == "idle" + os.system("pkill -SIGHUP -f system.updated.updated") + while not params.get_bool("UpdateAvailable"): + time.sleep(60) - if update_ready and not started: - if not frogs_go_moo: - os.system("pkill -SIGUSR1 -f system.updated.updated") - time.sleep(30) - if params.get_bool("UpdaterFetchAvailable"): - os.system("pkill -SIGHUP -f system.updated.updated") - time.sleep(300) - HARDWARE.reboot() - elif update_available: - os.system("pkill -SIGUSR1 -f system.updated.updated") - time.sleep(30) - os.system("pkill -SIGHUP -f system.updated.updated") - elif update_state_idle: - os.system("pkill -SIGUSR1 -f system.updated.updated") + while params.get_bool("IsOnroad"): + time.sleep(60) + HARDWARE.reboot() def check_assets(model_manager, theme_manager, frogpilot_toggles): if params_memory.get_bool("DownloadAllModels"): @@ -84,19 +80,19 @@ def check_assets(model_manager, theme_manager, frogpilot_toggles): if asset_to_download is not None: run_thread_with_lock("download_theme", theme_manager.download_theme, (asset_type, asset_to_download, param)) - -def update_checks(automatic_updates, frogs_go_moo, model_manager, now, screen_off, started, theme_manager, time_validated): +def update_checks(model_manager, now, theme_manager, time_validated, frogpilot_toggles): if not (is_url_pingable("https://github.com") or is_url_pingable("https://gitlab.com")): return - if automatic_updates and (screen_off or frogs_go_moo): - automatic_update_check(frogs_go_moo, started) + if frogpilot_toggles.automatic_updates: + automatic_update_check() if time_validated: update_maps(now) + run_thread_with_lock("update_mapd", update_mapd()) run_thread_with_lock("update_models", model_manager.update_models) - + run_thread_with_lock("update_themes", theme_manager.update_themes(frogpilot_toggles)) def update_maps(now): maps_selected = params.get("MapsSelected", encoding='utf8') @@ -120,15 +116,18 @@ def update_maps(now): if params.get("OSMDownloadProgress", encoding='utf-8') is None: params_memory.put("OSMDownloadLocations", maps_selected) - params.put_nonblocking("LastMapsUpdate", todays_date) - + params.put("LastMapsUpdate", todays_date) def frogpilot_thread(): config_realtime_process(5, Priority.CTRL_LOW) + error_log = os.path.join(sentry.CRASHES_DIR, 'error.txt') + if os.path.isfile(error_log): + os.remove(error_log) + params_storage = Params("/persist/params") - frogpilot_planner = FrogPilotPlanner() + frogpilot_planner = FrogPilotPlanner(error_log) frogpilot_tracking = FrogPilotTracking() frogpilot_variables = FrogPilotVariables() model_manager = ModelManager() @@ -145,35 +144,34 @@ def frogpilot_thread(): toggles_last_updated = datetime.datetime.now() - error_log = os.path.join(sentry.CRASHES_DIR, 'error.txt') - if os.path.isfile(error_log): - os.remove(error_log) - pm = messaging.PubMaster(['frogpilotPlan']) - sm = messaging.SubMaster(['carState', 'controlsState', 'deviceState', 'modelV2', 'radarState', + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'deviceState', 'modelV2', 'radarState', 'frogpilotCarControl', 'frogpilotCarState', 'frogpilotNavigation'], poll='modelV2', ignore_avg_freq=['radarState']) + run_thread_with_lock("update_active_theme", theme_manager.update_active_theme, (frogpilot_toggles,)) + while True: sm.update() now = datetime.datetime.now() - deviceState = sm['deviceState'] - screen_off = deviceState.screenBrightnessPercent == 0 - started = deviceState.started + + started = sm['deviceState'].started if params_memory.get_bool("FrogPilotTogglesUpdated"): frogpilot_variables.update(started) frogpilot_toggles = get_frogpilot_toggles() if time_validated: - run_thread_with_lock("backup_toggles", backup_toggles, (params, params_storage)) + run_thread_with_lock("backup_toggles", backup_toggles, (params_storage,)) run_thread_with_lock("update_active_theme", theme_manager.update_active_theme, (frogpilot_toggles,)) toggles_last_updated = now toggles_updated = (now - toggles_last_updated).total_seconds() <= 1 if not started and started_previously: - frogpilot_planner = FrogPilotPlanner() + frogpilot_planner = FrogPilotPlanner(error_log) frogpilot_tracking = FrogPilotTracking() + + run_update_checks = True elif started and not started_previously: radarless_model = frogpilot_toggles.radarless_model @@ -181,7 +179,7 @@ def frogpilot_thread(): os.remove(error_log) if started and sm.updated['modelV2']: - frogpilot_planner.update(sm['carState'], sm['controlsState'], sm['frogpilotCarControl'], sm['frogpilotCarState'], + frogpilot_planner.update(sm['carControl'], sm['carState'], sm['controlsState'], sm['frogpilotCarControl'], sm['frogpilotCarState'], sm['frogpilotNavigation'], sm['modelV2'], radarless_model, sm['radarState'], frogpilot_toggles) frogpilot_planner.publish(sm, pm, frogpilot_toggles, toggles_updated) @@ -193,21 +191,19 @@ def frogpilot_thread(): started_previously = started - check_assets(model_manager, theme_manager, frogpilot_toggles) + if now.second % 2 == 0: + check_assets(model_manager, theme_manager, frogpilot_toggles) + + run_update_checks |= params_memory.get_bool("ManualUpdateInitiated") + run_update_checks |= now.second == 0 and (now.minute % 60 == 0 or frogpilot_toggles.frogs_go_moo) - if params_memory.get_bool("ManualUpdateInitiated"): - run_thread_with_lock("update_checks", update_checks, (False, frogpilot_toggles.frogs_go_moo, model_manager, now, screen_off, started, theme_manager, time_validated)) - run_thread_with_lock("update_themes", theme_manager.update_themes(frogpilot_toggles)) - elif now.second == 0: - run_update_checks = not screen_off and not started or now.minute % 15 == 0 or frogpilot_toggles.frogs_go_moo - elif run_update_checks: - run_thread_with_lock("update_checks", update_checks, (frogpilot_toggles.automatic_updates, frogpilot_toggles.frogs_go_moo, model_manager, now, screen_off, started, theme_manager, time_validated)) + if run_update_checks: + run_thread_with_lock("update_checks", update_checks, (model_manager, now, theme_manager, time_validated, frogpilot_toggles)) if time_validated: theme_manager.update_holiday() run_update_checks = False - elif not time_validated: time_validated = system_time_valid() if not time_validated: @@ -219,6 +215,5 @@ def frogpilot_thread(): def main(): frogpilot_thread() - if __name__ == "__main__": main() diff --git a/selfdrive/frogpilot/frogpilot_variables.py b/selfdrive/frogpilot/frogpilot_variables.py index a41ba95513e0cd..f5596c79e2776c 100644 --- a/selfdrive/frogpilot/frogpilot_variables.py +++ b/selfdrive/frogpilot/frogpilot_variables.py @@ -35,8 +35,8 @@ RANDOM_EVENTS_PATH = os.path.join(BASEDIR, "selfdrive", "frogpilot", "assets", "random_events") THEME_SAVE_PATH = os.path.join("/data", "themes") -DEFAULT_MODEL = "alabama" -DEFAULT_MODEL_NAME = "Alabama" +DEFAULT_MODEL = "playstation" +DEFAULT_MODEL_NAME = "PlayStation®" DEFAULT_CLASSIC_MODEL = "north-dakota" DEFAULT_CLASSIC_MODEL_NAME = "North Dakota (Default)" @@ -157,7 +157,6 @@ def update_frogpilot_toggles(): ("ForceStandstill", 0), ("ForceStops", 0), ("FPSCounter", 1), - ("FrogPilotToggles", ""), ("FrogsGoMoosTweak", 1), ("FullMap", 0), ("GasRegenCmd", 1), @@ -297,9 +296,9 @@ def update_frogpilot_toggles(): ("SLCLookaheadHigher", 5), ("SLCLookaheadLower", 5), ("SLCOverride", 1), - ("SLCPriority1", "Dashboard"), - ("SLCPriority2", "Navigation"), - ("SLCPriority3", "Map Data"), + ("SLCPriority1", "Navigation"), + ("SLCPriority2", "Map Data"), + ("SLCPriority3", "Dashboard"), ("SNGHack", 1), ("SpeedLimitChangedAlert", 1), ("SpeedLimitController", 1), @@ -315,14 +314,14 @@ def update_frogpilot_toggles(): ("StandardPersonalityProfile", 1), ("StandbyMode", 0), ("StaticPedalsOnUI", 0), - ("SteerFriction", 0.1), - ("SteerFrictionStock", 0.1), - ("SteerLatAccel", 2.5), - ("SteerLatAccelStock", 2.5), - ("SteerKP", 1), - ("SteerKPStock", 1), - ("SteerRatio", 15), - ("SteerRatioStock", 15), + ("SteerFriction", 0), + ("SteerFrictionStock", 0), + ("SteerKP", 0), + ("SteerKPStock", 0), + ("SteerLatAccel", 0), + ("SteerLatAccelStock", 0), + ("SteerRatio", 0), + ("SteerRatioStock", 0), ("StoppedTimer", 0), ("TacoTune", 0), ("ToyotaDoors", 1), @@ -368,14 +367,14 @@ def update(self, started): if msg_bytes: with car.CarParams.from_bytes(msg_bytes) as CP: - always_on_lateral_set = key == "CarParams" and CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL + always_on_lateral_set = CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL car_make = CP.carName car_model = CP.carFingerprint has_auto_tune = (car_model == "hyundai" or car_model == "toyota") and CP.lateralTuning.which == "torque" has_bsm = CP.enableBsm has_radar = not CP.radarUnavailable is_pid_car = CP.lateralTuning.which == "pid" - max_acceleration_enabled = key == "CarParams" and CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX + max_acceleration_enabled = CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX openpilot_longitudinal = CP.openpilotLongitudinalControl pcm_cruise = CP.pcmCruise else: @@ -407,20 +406,20 @@ def update(self, started): toggle.use_wheel_speed = toggle.advanced_custom_onroad_ui and params.get_bool("WheelSpeed") toggle.advanced_lateral_tuning = params.get_bool("AdvancedLateralTune") + toggle.force_auto_tune = toggle.advanced_lateral_tuning and not has_auto_tune and not is_pid_car and params.get_bool("ForceAutoTune") + toggle.force_auto_tune_off = toggle.advanced_lateral_tuning and has_auto_tune and not is_pid_car and params.get_bool("ForceAutoTuneOff") stock_steer_friction = params.get_float("SteerFrictionStock") toggle.steer_friction = params.get_float("SteerFriction") if toggle.advanced_lateral_tuning else stock_steer_friction - toggle.use_custom_steer_friction = toggle.steer_friction != stock_steer_friction and not is_pid_car + toggle.use_custom_steer_friction = toggle.steer_friction != stock_steer_friction and not is_pid_car and not toggle.force_auto_tune or toggle.force_auto_tune_off stock_steer_kp = params.get_float("SteerKPStock") toggle.steer_kp = params.get_float("SteerKP") if toggle.advanced_lateral_tuning else stock_steer_kp toggle.use_custom_kp = toggle.steer_kp != stock_steer_kp and not is_pid_car stock_steer_lat_accel_factor = params.get_float("SteerLatAccelStock") toggle.steer_lat_accel_factor = params.get_float("SteerLatAccel") if toggle.advanced_lateral_tuning else stock_steer_lat_accel_factor - toggle.use_custom_lat_accel_factor = toggle.steer_lat_accel_factor != stock_steer_lat_accel_factor and not is_pid_car + toggle.use_custom_lat_accel_factor = toggle.steer_lat_accel_factor != stock_steer_lat_accel_factor and not is_pid_car and not toggle.force_auto_tune or toggle.force_auto_tune_off stock_steer_ratio = params.get_float("SteerRatioStock") toggle.steer_ratio = params.get_float("SteerRatio") if toggle.advanced_lateral_tuning else stock_steer_ratio - toggle.use_custom_steer_ratio = toggle.steer_ratio != stock_steer_ratio - toggle.force_auto_tune = toggle.advanced_lateral_tuning and not has_auto_tune and not is_pid_car and params.get_bool("ForceAutoTune") - toggle.force_auto_tune_off = toggle.advanced_lateral_tuning and has_auto_tune and not is_pid_car and params.get_bool("ForceAutoTuneOff") + toggle.use_custom_steer_ratio = toggle.steer_ratio != stock_steer_ratio and not toggle.force_auto_tune or toggle.force_auto_tune_off toggle.alert_volume_control = params.get_bool("AlertVolumeControl") toggle.disengage_volume = params.get_int("DisengageVolume") if toggle.alert_volume_control else 101 @@ -562,7 +561,7 @@ def update(self, started): toggle.frogsgomoo_tweak = openpilot_longitudinal and car_make == "toyota" and params.get_bool("FrogsGoMoosTweak") toggle.holiday_themes = params.get_bool("HolidayThemes") - toggle.current_holiday_theme = params.get("CurrentHolidayTheme", encoding='utf-8') if params.get_bool("HolidayThemes") else None + toggle.current_holiday_theme = params.get("CurrentHolidayTheme", encoding='utf-8') if toggle.holiday_themes else None toggle.lane_change_customizations = params.get_bool("LaneChangeCustomizations") toggle.lane_change_delay = params.get_float("LaneChangeTime") if toggle.lane_change_customizations else 0 @@ -661,7 +660,7 @@ def update(self, started): toggle.quality_of_life_visuals = params.get_bool("QOLVisuals") toggle.camera_view = params.get_int("CameraView") if toggle.quality_of_life_visuals else 0 toggle.driver_camera_in_reverse = toggle.quality_of_life_visuals and params.get_bool("DriverCamera") - toggle.onroad_distance_button = toggle.quality_of_life_visuals and params.get_bool("OnroadDistanceButton") + toggle.onroad_distance_button = openpilot_longitudinal and toggle.quality_of_life_visuals and params.get_bool("OnroadDistanceButton") toggle.standby_mode = toggle.quality_of_life_visuals and params.get_bool("StandbyMode") toggle.stopped_timer = toggle.quality_of_life_visuals and params.get_bool("StoppedTimer") @@ -704,8 +703,8 @@ def update(self, started): toggle.speed_limit_priority_lowest = toggle.speed_limit_priority1 == "Lowest" toggle.speed_limit_sources = toggle.speed_limit_controller and params.get_bool("SpeedLimitSources") - toggle.startup_alert_top = params.get("StartupMessageTop", encoding='utf-8') or "" - toggle.startup_alert_bottom = params.get("StartupMessageBottom", encoding='utf-8') or "" + toggle.startup_alert_top = params.get("StartupMessageTop", encoding='utf-8') or str(self.default_frogpilot_toggles.StartupMessageTop) + toggle.startup_alert_bottom = params.get("StartupMessageBottom", encoding='utf-8') or str(self.default_frogpilot_toggles.StartupMessageTop) toggle.tethering_config = params.get_int("TetheringEnabled") @@ -717,6 +716,11 @@ def update(self, started): customization_level = params.get_int("CustomizationLevel") if params.get_bool("CustomizationLevelConfirmed") else 2 + if customization_level != 2: + distance_conversion = CV.FOOT_TO_METER + small_distance_conversion = CV.INCH_TO_CM + speed_conversion = CV.MPH_TO_MS + if customization_level == 0: toggle.advanced_custom_onroad_ui = bool(self.default_frogpilot_toggles.AdvancedCustomUI) toggle.hide_alerts = bool(toggle.advanced_custom_onroad_ui and self.default_frogpilot_toggles.HideAlerts) @@ -926,7 +930,7 @@ def update(self, started): toggle.map_acceleration = bool(toggle.map_gears and self.default_frogpilot_toggles.MapAcceleration) toggle.map_deceleration = bool(toggle.map_gears and self.default_frogpilot_toggles.MapDeceleration) toggle.reverse_cruise_increase = bool(toggle.quality_of_life_longitudinal and pcm_cruise and self.default_frogpilot_toggles.ReverseCruise) - toggle.set_speed_offset = int(self.default_frogpilot_toggles.SetSpeedOffset) * (1 if toggle.is_metric else CV.MPH_TO_KPH) if toggle.quality_of_life_longitudinal and not pcm_cruise else 0 + toggle.set_speed_offset = int(self.default_frogpilot_toggles.SetSpeedOffset) * CV.MPH_TO_KPH if toggle.quality_of_life_longitudinal and not pcm_cruise else 0 toggle.camera_view = int(self.default_frogpilot_toggles.CameraView) if toggle.quality_of_life_visuals else 0 toggle.driver_camera_in_reverse = bool(toggle.quality_of_life_visuals and self.default_frogpilot_toggles.DriverCamera) @@ -1131,7 +1135,7 @@ def update(self, started): toggle.force_standstill = bool(toggle.quality_of_life_longitudinal and self.default_frogpilot_toggles.ForceStandstill) toggle.force_stops = bool(toggle.quality_of_life_longitudinal and self.default_frogpilot_toggles.ForceStops) toggle.reverse_cruise_increase = bool(toggle.quality_of_life_longitudinal and pcm_cruise and self.default_frogpilot_toggles.ReverseCruise) - toggle.set_speed_offset = int(self.default_frogpilot_toggles.SetSpeedOffset) * (1 if toggle.is_metric else CV.MPH_TO_KPH) if toggle.quality_of_life_longitudinal and not pcm_cruise else 0 + toggle.set_speed_offset = int(self.default_frogpilot_toggles.SetSpeedOffset) * CV.MPH_TO_KPH if toggle.quality_of_life_longitudinal and not pcm_cruise else 0 toggle.camera_view = int(self.default_frogpilot_toggles.CameraView) if toggle.quality_of_life_visuals else 0 toggle.standby_mode = bool(toggle.quality_of_life_visuals and self.default_frogpilot_toggles.StandbyMode) diff --git a/selfdrive/frogpilot/navigation/mapd.py b/selfdrive/frogpilot/navigation/mapd.py index 4bb08eb147b871..5f4330387a6fe4 100644 --- a/selfdrive/frogpilot/navigation/mapd.py +++ b/selfdrive/frogpilot/navigation/mapd.py @@ -6,10 +6,6 @@ import time import urllib.request -from openpilot.common.realtime import Ratekeeper - -from openpilot.selfdrive.frogpilot.frogpilot_utilities import is_url_pingable - VERSION = 'v1' GITHUB_VERSION_URL = f"https://github.com/FrogAi/FrogPilot-Resources/raw/Versions/mapd_version_{VERSION}.json" @@ -18,16 +14,6 @@ MAPD_PATH = '/data/media/0/osm/mapd' VERSION_PATH = '/data/media/0/osm/mapd_version' -def get_latest_version(): - for url in [GITHUB_VERSION_URL, GITLAB_VERSION_URL]: - try: - with urllib.request.urlopen(url, timeout=5) as response: - return json.loads(response.read().decode('utf-8'))['version'] - except Exception as e: - print(f"Error fetching version from {url}: {e}") - print("Failed to get the latest version from both sources.") - return None - def download(current_version): urls = [ f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{current_version}/mapd", @@ -45,49 +31,65 @@ def download(current_version): with open(VERSION_PATH, 'w') as version_file: version_file.write(current_version) - print(f"Successfully downloaded mapd from {url}") return True except Exception as e: - print(f"Failed to download from {url}: {e}") + print(f"Failed to download mapd from {url}: {e}") - print(f"Failed to download mapd for version {current_version} from both sources.") + print(f"Failed to download mapd for version {current_version}") return False -def ensure_mapd_is_running(): - while True: - if os.path.exists(MAPD_PATH): - try: - subprocess.run([MAPD_PATH], check=True) - except Exception as e: - print(f"Error running mapd process: {e}") - else: - print(f"Error: {MAPD_PATH} does not exist.") - time.sleep(1) +def get_installed_version(): + try: + with open(VERSION_PATH, 'r') as version_file: + return version_file.read().strip() + except FileNotFoundError: + return None + except Exception as e: + print(f"Error reading installed version: {e}") + return None -def mapd_thread(): - rk = Ratekeeper(0.05) +def get_latest_version(): + for url in [GITHUB_VERSION_URL, GITLAB_VERSION_URL]: + try: + with urllib.request.urlopen(url, timeout=5) as response: + return json.loads(response.read().decode('utf-8'))['version'] + except Exception as e: + print(f"Error fetching mapd version from {url}: {e}") + print("Failed to get the latest mapd version") + return None - while True: +def update_mapd(): + installed_version = get_installed_version() + latest_version = get_latest_version() + + if latest_version is None: + print("Could not get the latest mapd version") + return + + if installed_version != latest_version: + print("New version available, stopping mapd for update...") try: - if is_url_pingable("https://github.com"): - current_version = get_latest_version() - if current_version: - if not os.path.exists(MAPD_PATH) or not os.path.exists(VERSION_PATH) or open(VERSION_PATH).read() != current_version: - if download(current_version): - continue - ensure_mapd_is_running() + subprocess.run(["pkill", "-f", MAPD_PATH], stdout=subprocess.PIPE, stderr=subprocess.PIPE) except Exception as e: - print(f"Exception in mapd_thread: {e}") - time.sleep(1) + print(f"Error stopping mapd process: {e}") + if download(latest_version): + print(f"Updated mapd to version {latest_version}") + else: + print("Failed to update mapd") + else: + print("Mapd is up to date") - rk.keep_time() +def ensure_mapd_is_running(): + while True: + try: + subprocess.run([MAPD_PATH], check=True) + except Exception as e: + print(f"Error running mapd process: {e}") + time.sleep(60) def main(): - try: - mapd_thread() - except Exception as e: - print(f"Unhandled exception in main: {e}") + ensure_mapd_is_running() if __name__ == "__main__": main() diff --git a/selfdrive/frogpilot/navigation/ui/maps_settings.cc b/selfdrive/frogpilot/navigation/ui/maps_settings.cc index 7bc573c1f42832..7f5c53d209faa5 100644 --- a/selfdrive/frogpilot/navigation/ui/maps_settings.cc +++ b/selfdrive/frogpilot/navigation/ui/maps_settings.cc @@ -162,7 +162,7 @@ void FrogPilotMapsPanel::downloadMaps() { return; } - paramsMemory.put("OSMDownloadLocations", params.get("MapsSelected")); + params_memory.put("OSMDownloadLocations", params.get("MapsSelected")); downloadActive = true; diff --git a/selfdrive/frogpilot/navigation/ui/maps_settings.h b/selfdrive/frogpilot/navigation/ui/maps_settings.h index 2373c1ed198fcc..ce0b524946745f 100644 --- a/selfdrive/frogpilot/navigation/ui/maps_settings.h +++ b/selfdrive/frogpilot/navigation/ui/maps_settings.h @@ -65,7 +65,7 @@ class FrogPilotMapsPanel : public FrogPilotListWidget { MapSelectionControl *westMaps; Params params; - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; bool countriesOpen; bool downloadActive; diff --git a/selfdrive/frogpilot/screenrecorder/omx_encoder.cc b/selfdrive/frogpilot/screenrecorder/omx_encoder.cc index 0700cd7cf0defb..69e3f5c6ec874a 100644 --- a/selfdrive/frogpilot/screenrecorder/omx_encoder.cc +++ b/selfdrive/frogpilot/screenrecorder/omx_encoder.cc @@ -209,10 +209,18 @@ OmxEncoder::OmxEncoder(const char* path, int width, int height, int fps, int bit this->height = height; this->fps = fps; + OMX_ERRORTYPE err = OMX_Init(); + if (err != OMX_ErrorNone) { + LOGE("OMX_Init failed: %x", err); + return; + } + OMX_STRING component = (OMX_STRING)("OMX.qcom.video.encoder.avc"); - int err = OMX_GetHandle(&handle, component, this, &omx_callbacks); + err = OMX_GetHandle(&handle, component, this, &omx_callbacks); if (err != OMX_ErrorNone) { - LOGE("error getting codec: %x", err); + LOGE("Error getting codec: %x", err); + OMX_Deinit(); + return; } // setup input port @@ -543,6 +551,11 @@ OmxEncoder::~OmxEncoder() { OMX_CHECK(OMX_FreeHandle(handle)); + OMX_ERRORTYPE err = OMX_Deinit(); + if (err != OMX_ErrorNone) { + LOGE("OMX_Deinit failed: %x", err); + } + OMX_BUFFERHEADERTYPE *out_buf; while (free_in.try_pop(out_buf)); while (done_out.try_pop(out_buf)); diff --git a/selfdrive/frogpilot/screenrecorder/screenrecorder.cc b/selfdrive/frogpilot/screenrecorder/screenrecorder.cc index 906063bfc57f71..be6307d7a1cc0a 100644 --- a/selfdrive/frogpilot/screenrecorder/screenrecorder.cc +++ b/selfdrive/frogpilot/screenrecorder/screenrecorder.cc @@ -5,30 +5,48 @@ #include "selfdrive/frogpilot/screenrecorder/screenrecorder.h" namespace { - inline long long milliseconds() { + long long currentMilliseconds() { return std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); } + + uint64_t nanosSinceBoot() { + return std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + } } -ScreenRecorder::ScreenRecorder(QWidget *parent) : QPushButton(parent), recording(false) { +ScreenRecorder::ScreenRecorder(QWidget *parent) : QPushButton(parent) { setFixedSize(btn_size, btn_size); - encoder = std::make_unique("/data/media/screen_recordings", screenWidth, screenHeight, UI_FREQ, 8 * 1024 * 1024); + std::thread encoderInitThread([this]() { + try { + encoder = std::make_unique("/data/media/screen_recordings", screenWidth, screenHeight, UI_FREQ, 8 * 1024 * 1024); + encoderReady = true; + } catch (const std::exception &e) { + std::cerr << "Error initializing OmxEncoder: " << e.what() << std::endl; + } + }); + encoderInitThread.detach(); + rgbScaleBuffer = std::make_unique(screenWidth * screenHeight * 4); QObject::connect(this, &QPushButton::clicked, this, &ScreenRecorder::toggleRecording); } ScreenRecorder::~ScreenRecorder() { - stop(); + stopRecording(); } void ScreenRecorder::toggleRecording() { - recording ? stop() : start(); + if (recording) { + stopRecording(); + } else { + startRecording(); + } } -void ScreenRecorder::start() { - if (recording) { +void ScreenRecorder::startRecording() { + if (recording || !encoderReady) { + std::cerr << "Recording already in progress or encoder not ready." << std::endl; return; } @@ -48,10 +66,10 @@ void ScreenRecorder::start() { recording = false; } - started_time = milliseconds(); + startedTime = currentMilliseconds(); } -void ScreenRecorder::stop() { +void ScreenRecorder::stopRecording() { if (!recording) { return; } @@ -69,7 +87,7 @@ void ScreenRecorder::stop() { } imageQueue.clear(); - rgbScaleBuffer.reset(new uint8_t[screenWidth * screenHeight * 4]); + rgbScaleBuffer = std::make_unique(screenWidth * screenHeight * 4); } void ScreenRecorder::openEncoder(const std::string &filename) { @@ -85,20 +103,29 @@ void ScreenRecorder::closeEncoder() { } void ScreenRecorder::encodingThreadFunction() { - uint64_t start_time = nanos_since_boot(); + uint64_t startTime = nanosSinceBoot(); while (recording) { - if (!encoder) { - break; - } - - QImage popImage; - if (imageQueue.pop_wait_for(popImage, std::chrono::milliseconds(10))) { - QImage image = popImage.convertToFormat(QImage::Format_RGBA8888); - libyuv::ARGBScale(image.bits(), image.width() * 4, image.width(), image.height(), - rgbScaleBuffer.get(), screenWidth * 4, screenWidth, screenHeight, - libyuv::kFilterBilinear); - encoder->encode_frame_rgba(rgbScaleBuffer.get(), screenWidth, screenHeight, nanos_since_boot() - start_time); + QImage image; + if (imageQueue.pop_wait_for(image, std::chrono::milliseconds(10))) { + QImage convertedImage = image.convertToFormat(QImage::Format_RGBA8888); + libyuv::ARGBScale( + convertedImage.bits(), + convertedImage.width() * 4, + convertedImage.width(), + convertedImage.height(), + rgbScaleBuffer.get(), + screenWidth * 4, + screenWidth, + screenHeight, + libyuv::kFilterBilinear + ); + encoder->encode_frame_rgba( + rgbScaleBuffer.get(), + screenWidth, + screenHeight, + nanosSinceBoot() - startTime + ); } } } @@ -109,13 +136,14 @@ void ScreenRecorder::updateScreen(double fps, bool started) { } if (!started) { - stop(); + stopRecording(); return; } - if (milliseconds() - started_time > 1000 * 60 * 3) { - stop(); - start(); + static long long recordingDurationLimit = 1000 * 60 * 3; + if (currentMilliseconds() - startedTime > recordingDurationLimit) { + stopRecording(); + startRecording(); return; } @@ -157,7 +185,7 @@ void ScreenRecorder::paintEvent(QPaintEvent *event) { painter.setPen(QPen(whiteColor(), 6)); painter.drawText(textRect, Qt::AlignLeft | Qt::AlignVCenter, tr("RECORD")); - if (recording && ((milliseconds() - started_time) / 1000) % 2 == 0) { + if (recording && ((currentMilliseconds() - startedTime) / 1000) % 2 == 0) { painter.setPen(Qt::NoPen); painter.drawEllipse(QPoint(buttonRect.right() - btn_size / 10 - centeringOffset, buttonRect.center().y()), btn_size / 10, btn_size / 10); } diff --git a/selfdrive/frogpilot/screenrecorder/screenrecorder.h b/selfdrive/frogpilot/screenrecorder/screenrecorder.h index a3f4d536e0608a..4747d6269c5759 100644 --- a/selfdrive/frogpilot/screenrecorder/screenrecorder.h +++ b/selfdrive/frogpilot/screenrecorder/screenrecorder.h @@ -9,39 +9,43 @@ class ScreenRecorder : public QPushButton { Q_OBJECT public: - explicit ScreenRecorder(QWidget *parent = 0); + explicit ScreenRecorder(QWidget *parent = nullptr); ~ScreenRecorder() override; void updateScreen(double fps, bool started); +protected: + void paintEvent(QPaintEvent *event) override; + +private slots: + void toggleRecording(); + private: void closeEncoder(); void encodingThreadFunction(); void openEncoder(const std::string &filename); - void paintEvent(QPaintEvent *event) override; - void start(); - void stop(); - void toggleRecording(); + void startRecording(); + void stopRecording(); BlockingQueue imageQueue{UI_FREQ}; - BlockingQueue encodeQueue{UI_FREQ}; inline QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); } inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); } - QWidget *rootWidget; - - bool recording; + QWidget *rootWidget = nullptr; int screenHeight = 1080; int screenWidth = 2160; - long long started_time; + long long startedTime = 0; - std::thread encodingThread; + std::atomic encoderReady{false}; + std::atomic recording{false}; std::unique_ptr encoder; std::unique_ptr rgbScaleBuffer; + + std::thread encodingThread; }; diff --git a/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.cc index 211fdc20ca89c5..9e9616a7627281 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.cc @@ -44,15 +44,36 @@ FrogPilotSettingsWindow::FrogPilotSettingsWindow(SettingsWindow *parent) : QFram FrogPilotListWidget *list = new FrogPilotListWidget(frogpilotSettingsWidget); - std::vector toggle_presets{tr("Basic"), tr("Standard"), tr("Advanced")}; - ButtonParamControl *toggle_preset = new ButtonParamControl("CustomizationLevel", tr("Customization Level"), - tr("Choose your preferred customization level. 'Standard' is recommended for most users, offering a balanced experience and automatically managing more 'Advanced' features," - " while 'Basic' is designed for those new to customization or seeking simplicity."), - "../frogpilot/assets/toggle_icons/icon_customization.png", - toggle_presets); - QObject::connect(toggle_preset, &ButtonParamControl::buttonClicked, this, &FrogPilotSettingsWindow::updatePanelVisibility); - QObject::connect(toggle_preset, &ButtonParamControl::buttonClicked, this, &updateFrogPilotToggles); - list->addItem(toggle_preset); + std::vector togglePresets{tr("Stock-Like"), tr("Standard"), tr("Advanced"), tr("Expert")}; + ButtonParamControl *togglePreset = new ButtonParamControl("CustomizationLevel", tr("Tuning Level"), + tr("Select the tuning level that best suits your needs. 'Basic' is ideal for those who prefer simplicity and ease of use, " + "'Standard' is recommended for most users, offering a balanced experience, " + "'Advanced' provides more control for experienced users, " + "while 'Expert' unlocks highly customizable settings designed for seasoned enthusiasts."), + "../frogpilot/assets/toggle_icons/icon_customization.png", + togglePresets); + int timeTo100FPHours = 100 - (paramsTracking.getInt("FrogPilotMinutes") / 60); + int timeTo250OPHours = 250 - (params.getInt("openpilotMinutes") / 60); + togglePreset->setEnabledButtons(3, timeTo100FPHours <= 0 || timeTo250OPHours <= 0); + QObject::connect(togglePreset, &ButtonParamControl::buttonClicked, [=](int id) { + if (id == 3) { + FrogPilotConfirmationDialog::toggleAlert( + tr("WARNING: This unlocks some potentially dangerous settings that can DRASTICALLY alter your driving experience!"), + tr("I understand the risks."), this + ); + } + updateFrogPilotToggles(); + updatePanelVisibility(); + }); + QObject::connect(togglePreset, &ButtonParamControl::disabledButtonClicked, [=](int id) { + if (id == 3) { + FrogPilotConfirmationDialog::toggleAlert( + tr("The 'Expert' preset is only available for users with either over 100 hours on FrogPilot, or 250 hours with openpilot."), + tr("Okay"), this + ); + } + }); + list->addItem(togglePreset); FrogPilotDevicePanel *frogpilotDevicePanel = new FrogPilotDevicePanel(this); QObject::connect(frogpilotDevicePanel, &FrogPilotDevicePanel::openParentToggle, this, &FrogPilotSettingsWindow::openParentToggle); @@ -136,12 +157,12 @@ FrogPilotSettingsWindow::FrogPilotSettingsWindow(SettingsWindow *parent) : QFram QObject::connect(parent, &SettingsWindow::closeParentToggle, this, &FrogPilotSettingsWindow::closeParentToggle); QObject::connect(parent, &SettingsWindow::closeSubParentToggle, this, &FrogPilotSettingsWindow::closeSubParentToggle); QObject::connect(parent, &SettingsWindow::updateMetric, this, &FrogPilotSettingsWindow::updateMetric); - QObject::connect(uiState(), &UIState::offroadTransition, this, &FrogPilotSettingsWindow::updateCarVariables); closeParentToggle(); } void FrogPilotSettingsWindow::showEvent(QShowEvent *event) { + updateCarVariables(); updatePanelVisibility(); } @@ -175,12 +196,8 @@ void FrogPilotSettingsWindow::updatePanelVisibility() { } void FrogPilotSettingsWindow::updateCarVariables() { - std::thread([this] { - std::string carParams = params.get("CarParamsPersistent"); - if (carParams.empty()) { - carParams = params.get("CarParams", true); - } - + std::string carParams = params.get("CarParamsPersistent"); + if (!carParams.empty()) { AlignedBuffer aligned_buf; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(carParams.data(), carParams.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); @@ -219,31 +236,31 @@ void FrogPilotSettingsWindow::updateCarVariables() { float currentRatioStock = params.getFloat("SteerRatioStock"); if (currentFrictionStock != steerFrictionStock && steerFrictionStock != 0) { - if (params.getFloat("SteerFriction") == currentFrictionStock) { - params.putFloatNonBlocking("SteerFriction", steerFrictionStock); + if (params.getFloat("SteerFriction") == currentFrictionStock || currentFrictionStock == 0) { + params.putFloat("SteerFriction", steerFrictionStock); } - params.putFloatNonBlocking("SteerFrictionStock", steerFrictionStock); + params.putFloat("SteerFrictionStock", steerFrictionStock); } if (currentKPStock != steerKPStock && currentKPStock != 0) { - if (params.getFloat("SteerKP") == currentKPStock) { - params.putFloatNonBlocking("SteerKP", steerKPStock); + if (params.getFloat("SteerKP") == currentKPStock || currentKPStock == 0) { + params.putFloat("SteerKP", steerKPStock); } - params.putFloatNonBlocking("SteerKPStock", steerKPStock); + params.putFloat("SteerKPStock", steerKPStock); } if (currentLatAccelStock != steerLatAccelStock && steerLatAccelStock != 0) { - if (params.getFloat("SteerLatAccel") == steerLatAccelStock) { - params.putFloatNonBlocking("SteerLatAccel", steerLatAccelStock); + if (params.getFloat("SteerLatAccel") == steerLatAccelStock || steerLatAccelStock == 0) { + params.putFloat("SteerLatAccel", steerLatAccelStock); } - params.putFloatNonBlocking("SteerLatAccelStock", steerLatAccelStock); + params.putFloat("SteerLatAccelStock", steerLatAccelStock); } if (currentRatioStock != steerRatioStock && steerRatioStock != 0) { - if (params.getFloat("SteerRatio") == steerRatioStock) { - params.putFloatNonBlocking("SteerRatio", steerRatioStock); + if (params.getFloat("SteerRatio") == steerRatioStock || steerRatioStock == 0) { + params.putFloat("SteerRatio", steerRatioStock); } - params.putFloatNonBlocking("SteerRatioStock", steerRatioStock); + params.putFloat("SteerRatioStock", steerRatioStock); } uiState()->scene.has_auto_tune = hasAutoTune || forcingAutoTune; @@ -265,7 +282,7 @@ void FrogPilotSettingsWindow::updateCarVariables() { } emit updateCarToggles(); - }).detach(); + } } void FrogPilotSettingsWindow::addPanelControl(FrogPilotListWidget *list, QString &title, QString &desc, std::vector &button_labels, QString &icon, std::vector &panels, QString ¤tPanel) { diff --git a/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.h b/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.h index a8c27615fb4e44..3a98da2f93f307 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/frogpilot_settings.h @@ -62,6 +62,7 @@ class FrogPilotSettingsWindow : public QFrame { FrogPilotButtonsControl *systemButton; Params params; + Params paramsTracking{"/persist/tracking"}; QStackedLayout *mainLayout; diff --git a/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.cc index dcaa426e298993..0e5b5d6e08d33e 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/lateral_settings.cc @@ -40,23 +40,27 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : QObject::connect(advancedLateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, [this]() { std::set modifiedAdvancedLateralTuneKeys = advancedLateralTuneKeys; - bool usingNNFF = hasNNFFLog && params.getBool("LateralTune") && params.getBool("NNFF"); - if (usingNNFF) { + bool forcingAutoTune = params.getBool("ForceAutoTune"); + bool forcingAutoTuneOff = params.getBool("ForceAutoTuneOff"); + if (!hasAutoTune && forcingAutoTune || hasAutoTune && !forcingAutoTuneOff) { + modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); + modifiedAdvancedLateralTuneKeys.erase("SteerKP"); + modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); + modifiedAdvancedLateralTuneKeys.erase("SteerRatio"); + } + + if (hasAutoTune) { modifiedAdvancedLateralTuneKeys.erase("ForceAutoTune"); + } else if (isPIDCar) { modifiedAdvancedLateralTuneKeys.erase("ForceAutoTuneOff"); + modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); + modifiedAdvancedLateralTuneKeys.erase("SteerKP"); + modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); } else { - if (hasAutoTune) { - modifiedAdvancedLateralTuneKeys.erase("ForceAutoTune"); - } else if (isPIDCar) { - modifiedAdvancedLateralTuneKeys.erase("ForceAutoTuneOff"); - modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); - modifiedAdvancedLateralTuneKeys.erase("SteerKP"); - modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); - } else { - modifiedAdvancedLateralTuneKeys.erase("ForceAutoTuneOff"); - } + modifiedAdvancedLateralTuneKeys.erase("ForceAutoTuneOff"); } + bool usingNNFF = hasNNFFLog && params.getBool("LateralTune") && params.getBool("NNFF"); if (!liveValid || usingNNFF) { modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); @@ -175,6 +179,36 @@ FrogPilotLateralPanel::FrogPilotLateralPanel(FrogPilotSettingsWindow *parent) : } }); + QObject::connect(static_cast(toggles["ForceAutoTune"]), &ToggleControl::toggleFlipped, [this](bool state) { + std::set modifiedAdvancedLateralTuneKeys = advancedLateralTuneKeys; + + modifiedAdvancedLateralTuneKeys.erase("ForceAutoTuneOff"); + + if (!hasAutoTune && state) { + modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); + modifiedAdvancedLateralTuneKeys.erase("SteerKP"); + modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); + modifiedAdvancedLateralTuneKeys.erase("SteerRatio"); + } + + showToggles(modifiedAdvancedLateralTuneKeys); + }); + + QObject::connect(static_cast(toggles["ForceAutoTuneOff"]), &ToggleControl::toggleFlipped, [this](bool state) { + std::set modifiedAdvancedLateralTuneKeys = advancedLateralTuneKeys; + + modifiedAdvancedLateralTuneKeys.erase("ForceAutoTune"); + + if (hasAutoTune && !state) { + modifiedAdvancedLateralTuneKeys.erase("SteerFriction"); + modifiedAdvancedLateralTuneKeys.erase("SteerKP"); + modifiedAdvancedLateralTuneKeys.erase("SteerLatAccel"); + modifiedAdvancedLateralTuneKeys.erase("SteerRatio"); + } + + showToggles(modifiedAdvancedLateralTuneKeys); + }); + QObject::connect(static_cast(toggles["NNFF"]), &ToggleControl::toggleFlipped, [this](bool state) { std::set modifiedLateralTuneKeys = lateralTuneKeys; diff --git a/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.cc index c4220fdb7d8e23..247be5612f2c1f 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/longitudinal_settings.cc @@ -247,6 +247,14 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * curveDetectionBtn->setCheckedButton(1, !vtscEnabled); } }); + QObject::connect(curveDetectionBtn, &FrogPilotButtonsControl::disabledButtonClicked, [=](int id) { + if (id == 0) { + FrogPilotConfirmationDialog::toggleAlert( + tr("The 'Map Based' option is only available when some 'Map Data' has been downloaded!"), + tr("Okay"), this + ); + } + }); longitudinalToggle = curveDetectionBtn; } else if (param == "CurveSensitivity" || param == "TurnAggressiveness") { longitudinalToggle = new FrogPilotParamValueControl(param, title, desc, icon, 1, 200, "%"); @@ -372,43 +380,35 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * longitudinalToggle = overrideSelection; } else if (param == "SLCPriority") { ButtonControl *slcPriorityButton = new ButtonControl(title, tr("SELECT"), desc); - QStringList primaryPriorities = {tr("None"), tr("Dashboard"), tr("Map Data"), tr("Navigation"), tr("Highest"), tr("Lowest")}; - QStringList secondaryTertiaryPriorities = {tr("None"), tr("Dashboard"), tr("Map Data"), tr("Navigation")}; + QStringList primaryPriorities = {tr("Dashboard"), tr("Map Data"), tr("Navigation"), tr("Highest"), tr("Lowest")}; + QStringList otherPriorities = {tr("None"), tr("Dashboard"), tr("Map Data"), tr("Navigation")}; QStringList priorityPrompts = {tr("Select your primary priority"), tr("Select your secondary priority"), tr("Select your tertiary priority")}; QObject::connect(slcPriorityButton, &ButtonControl::clicked, [=]() { QStringList selectedPriorities; for (int i = 1; i <= 3; ++i) { - QStringList currentPriorities = (i == 1) ? primaryPriorities : secondaryTertiaryPriorities; - QStringList prioritiesToDisplay = currentPriorities; - - for (QString selectedPriority : qAsConst(selectedPriorities)) { - prioritiesToDisplay.removeAll(selectedPriority); - } + QStringList availablePriorities = (i == 1) ? primaryPriorities : otherPriorities; + availablePriorities = availablePriorities.toSet().subtract(selectedPriorities.toSet()).toList(); if (!hasDashSpeedLimits) { - prioritiesToDisplay.removeAll(tr("Dashboard")); + availablePriorities.removeAll(tr("Dashboard")); } - - if (prioritiesToDisplay.size() == 1 && prioritiesToDisplay.contains(tr("None"))) { + if (availablePriorities.size() == 1 && availablePriorities.contains(tr("None"))) { break; } - QString priorityKey = QString("SLCPriority%1").arg(i); - QString selection = MultiOptionDialog::getSelection(priorityPrompts[i - 1], prioritiesToDisplay, "", this); - + QString selection = MultiOptionDialog::getSelection(priorityPrompts[i - 1], availablePriorities, "", this); if (selection.isEmpty()) { break; } - params.putNonBlocking(priorityKey.toStdString(), selection.toStdString()); + params.put(QString("SLCPriority%1").arg(i).toStdString(), selection.toStdString()); selectedPriorities.append(selection); if (selection == tr("None")) { for (int j = i + 1; j <= 3; ++j) { - QString priorityKeyNext = QString("SLCPriority%1").arg(j); - params.putNonBlocking(priorityKeyNext.toStdString(), "None"); + params.put(QString("SLCPriority%1").arg(j).toStdString(), tr("None").toStdString()); } break; } @@ -426,10 +426,8 @@ FrogPilotLongitudinalPanel::FrogPilotLongitudinalPanel(FrogPilotSettingsWindow * QStringList initialPriorities; for (int i = 1; i <= 3; ++i) { - QString priorityKey = QString("SLCPriority%1").arg(i); - QString priority = QString::fromStdString(params.get(priorityKey.toStdString())); - - if (!priority.isEmpty() && primaryPriorities.contains(priority) && priority != tr("None")) { + QString priority = QString::fromStdString(params.get(QString("SLCPriority%1").arg(i).toStdString())); + if (!priority.isEmpty() && priority != tr("None") && primaryPriorities.contains(priority)) { initialPriorities.append(priority); } } diff --git a/selfdrive/frogpilot/ui/qt/offroad/model_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/model_settings.cc index cf23ec66d0ac3a..d229c1d209f73d 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/model_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/model_settings.cc @@ -144,8 +144,8 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog downloadModelBtn = new ButtonControl(title, tr("DOWNLOAD"), desc); QObject::connect(downloadModelBtn, &ButtonControl::clicked, [this]() { if (downloadModelBtn->text() == tr("CANCEL")) { - paramsMemory.remove("ModelToDownload"); - paramsMemory.putBool("CancelModelDownload", true); + params_memory.remove("ModelToDownload"); + params_memory.putBool("CancelModelDownload", true); cancellingDownload = true; } else { QMap labelToModelMap; @@ -163,8 +163,8 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog QString modelToDownload = MultiOptionDialog::getSelection(tr("Select a driving model to download"), downloadableModels, "", this); if (!modelToDownload.isEmpty()) { modelDownloading = true; - paramsMemory.put("ModelToDownload", labelToModelMap.value(modelToDownload).toStdString()); - paramsMemory.put("ModelDownloadProgress", "0%"); + params_memory.put("ModelToDownload", labelToModelMap.value(modelToDownload).toStdString()); + params_memory.put("ModelDownloadProgress", "0%"); downloadModelBtn->setValue(tr("Downloading %1...").arg(modelToDownload.remove(QRegularExpression("[🗺️👀📡]")).trimmed())); @@ -172,7 +172,7 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog progressTimer->setInterval(100); QObject::connect(progressTimer, &QTimer::timeout, this, [=]() { - QString progress = QString::fromStdString(paramsMemory.get("ModelDownloadProgress")); + QString progress = QString::fromStdString(params_memory.get("ModelDownloadProgress")); bool downloadComplete = progress.contains(QRegularExpression("downloaded", QRegularExpression::CaseInsensitiveOption)); bool downloadFailed = progress.contains(QRegularExpression("cancelled|exists|failed|offline", QRegularExpression::CaseInsensitiveOption)); @@ -199,8 +199,8 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog downloadModelBtn->setValue(progress); - paramsMemory.remove("CancelModelDownload"); - paramsMemory.remove("ModelDownloadProgress"); + params_memory.remove("CancelModelDownload"); + params_memory.remove("ModelDownloadProgress"); progressTimer->stop(); progressTimer->deleteLater(); @@ -229,8 +229,8 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog downloadAllModelsBtn = new ButtonControl(title, tr("DOWNLOAD"), desc); QObject::connect(downloadAllModelsBtn, &ButtonControl::clicked, [this]() { if (downloadAllModelsBtn->text() == tr("CANCEL")) { - paramsMemory.remove("DownloadAllModels"); - paramsMemory.putBool("CancelModelDownload", true); + params_memory.remove("DownloadAllModels"); + params_memory.putBool("CancelModelDownload", true); cancellingDownload = true; } else { startDownloadAllModels(); @@ -340,7 +340,7 @@ void FrogPilotModelPanel::startDownloadAllModels() { allModelsDownloading = true; modelDownloading = true; - paramsMemory.putBool("DownloadAllModels", true); + params_memory.putBool("DownloadAllModels", true); downloadAllModelsBtn->setValue(tr("Downloading models...")); @@ -348,7 +348,7 @@ void FrogPilotModelPanel::startDownloadAllModels() { progressTimer->setInterval(100); QObject::connect(progressTimer, &QTimer::timeout, this, [=]() { - QString progress = QString::fromStdString(paramsMemory.get("ModelDownloadProgress")); + QString progress = QString::fromStdString(params_memory.get("ModelDownloadProgress")); bool downloadComplete = progress.contains(QRegularExpression("All models downloaded!", QRegularExpression::CaseInsensitiveOption)); bool downloadFailed = progress.contains(QRegularExpression("cancelled|exists|failed|offline", QRegularExpression::CaseInsensitiveOption)); @@ -364,8 +364,8 @@ void FrogPilotModelPanel::startDownloadAllModels() { downloadAllModelsBtn->setValue(progress); - paramsMemory.remove("CancelModelDownload"); - paramsMemory.remove("ModelDownloadProgress"); + params_memory.remove("CancelModelDownload"); + params_memory.remove("ModelDownloadProgress"); progressTimer->stop(); progressTimer->deleteLater(); @@ -374,7 +374,7 @@ void FrogPilotModelPanel::startDownloadAllModels() { cancellingDownload = false; modelDownloading = false; - paramsMemory.remove("DownloadAllModels"); + params_memory.remove("DownloadAllModels"); downloadAllModelsBtn->setValue(""); }); diff --git a/selfdrive/frogpilot/ui/qt/offroad/model_settings.h b/selfdrive/frogpilot/ui/qt/offroad/model_settings.h index cc3229e6c7d048..379c192c82acef 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/model_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/model_settings.h @@ -33,7 +33,7 @@ class FrogPilotModelPanel : public FrogPilotListWidget { ButtonControl *selectModelBtn; Params params; - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; Params paramsStorage{"/persist/params"}; QDir modelDir{"/data/models/"}; diff --git a/selfdrive/frogpilot/ui/qt/offroad/theme_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/theme_settings.cc index 6fe782ed9a80d1..08dddee948508e 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/theme_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/theme_settings.cc @@ -113,11 +113,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (colorDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; colorDownloading = false; themeDownloading = false; @@ -128,9 +128,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!colorSchemeToDownload.isEmpty()) { QString convertedColorScheme = formatColorNameForStorage(colorSchemeToDownload); - paramsMemory.put("ColorToDownload", convertedColorScheme.toStdString()); + params_memory.put("ColorToDownload", convertedColorScheme.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); colorDownloading = true; themeDownloading = true; @@ -241,11 +241,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (distanceIconDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; distanceIconDownloading = false; themeDownloading = false; @@ -256,9 +256,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!iconPackToDownload.isEmpty()) { QString convertedIconPack = formatIconNameForStorage(iconPackToDownload); - paramsMemory.put("DistanceIconToDownload", convertedIconPack.toStdString()); + params_memory.put("DistanceIconToDownload", convertedIconPack.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); distanceIconDownloading = true; themeDownloading = true; @@ -370,11 +370,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (iconDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; iconDownloading = false; themeDownloading = false; @@ -385,9 +385,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!iconPackToDownload.isEmpty()) { QString convertedIconPack = formatIconNameForStorage(iconPackToDownload); - paramsMemory.put("IconToDownload", convertedIconPack.toStdString()); + params_memory.put("IconToDownload", convertedIconPack.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); iconDownloading = true; themeDownloading = true; @@ -465,12 +465,12 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr availableSignals << formatSignalName(dirInfo.fileName()); } } - availableSignals.append("Stock"); + availableSignals.append("None"); std::sort(availableSignals.begin(), availableSignals.end()); if (id == 0) { QStringList customSignalsList = availableSignals; - customSignalsList.removeAll("Stock"); + customSignalsList.removeAll("None"); customSignalsList.removeAll(currentSignal); QString signalPackToDelete = MultiOptionDialog::getSelection(tr("Select a signal pack to delete"), customSignalsList, "", this); @@ -499,11 +499,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (signalDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; signalDownloading = false; themeDownloading = false; @@ -514,9 +514,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!signalPackToDownload.isEmpty()) { QString convertedSignalPack = formatSignalNameForStorage(signalPackToDownload); - paramsMemory.put("SignalToDownload", convertedSignalPack.toStdString()); + params_memory.put("SignalToDownload", convertedSignalPack.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); signalDownloading = true; themeDownloading = true; @@ -628,11 +628,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (soundDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; soundDownloading = false; themeDownloading = false; @@ -643,9 +643,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!soundSchemeToDownload.isEmpty()) { QString convertedSoundScheme = formatSoundNameForStorage(soundSchemeToDownload); - paramsMemory.put("SoundToDownload", convertedSoundScheme.toStdString()); + params_memory.put("SoundToDownload", convertedSoundScheme.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); soundDownloading = true; themeDownloading = true; @@ -757,11 +757,11 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } } else if (id == 1) { if (wheelDownloading) { - paramsMemory.putBool("CancelThemeDownload", true); + params_memory.putBool("CancelThemeDownload", true); cancellingDownload = true; QTimer::singleShot(2000, [=]() { - paramsMemory.putBool("CancelThemeDownload", false); + params_memory.putBool("CancelThemeDownload", false); cancellingDownload = false; wheelDownloading = false; themeDownloading = false; @@ -772,9 +772,9 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr if (!wheelToDownload.isEmpty()) { QString convertedImage = formatWheelNameForStorage(wheelToDownload); - paramsMemory.put("WheelToDownload", convertedImage.toStdString()); + params_memory.put("WheelToDownload", convertedImage.toStdString()); downloadStatusLabel->setText("Downloading..."); - paramsMemory.put("ThemeDownloadProgress", "Downloading..."); + params_memory.put("ThemeDownloadProgress", "Downloading..."); themeDownloading = true; wheelDownloading = true; @@ -831,10 +831,10 @@ FrogPilotThemesPanel::FrogPilotThemesPanel(FrogPilotSettingsWindow *parent) : Fr } else if (id == 2) { QString newTop = InputDialog::getText(tr("Enter the text for the top half"), this, tr("Characters: 0/%1").arg(maxLengthTop), false, -1, currentTop, maxLengthTop).trimmed(); if (newTop.length() > 0) { - params.putNonBlocking("StartupMessageTop", newTop.toStdString()); + params.put("StartupMessageTop", newTop.toStdString()); QString newBottom = InputDialog::getText(tr("Enter the text for the bottom half"), this, tr("Characters: 0/%1").arg(maxLengthBottom), false, -1, currentBottom, maxLengthBottom).trimmed(); if (newBottom.length() > 0) { - params.putNonBlocking("StartupMessageBottom", newBottom.toStdString()); + params.put("StartupMessageBottom", newBottom.toStdString()); } } } else if (id == 3) { @@ -891,7 +891,7 @@ void FrogPilotThemesPanel::updateState(const UIState &s) { if (personalizeOpenpilotOpen) { if (themeDownloading) { - QString progress = QString::fromStdString(paramsMemory.get("ThemeDownloadProgress")); + QString progress = QString::fromStdString(params_memory.get("ThemeDownloadProgress")); bool downloadFailed = progress.contains(QRegularExpression("cancelled|exists|Failed|offline", QRegularExpression::CaseInsensitiveOption)); if (progress != "Downloading...") { @@ -904,7 +904,7 @@ void FrogPilotThemesPanel::updateState(const UIState &s) { downloadStatusLabel->setText("Idle"); } }); - paramsMemory.remove("ThemeDownloadProgress"); + params_memory.remove("ThemeDownloadProgress"); colorDownloading = false; distanceIconDownloading = false; iconDownloading = false; diff --git a/selfdrive/frogpilot/ui/qt/offroad/theme_settings.h b/selfdrive/frogpilot/ui/qt/offroad/theme_settings.h index 9ddfe7b6aa3914..65fae972542461 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/theme_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/theme_settings.h @@ -35,7 +35,7 @@ class FrogPilotThemesPanel : public FrogPilotListWidget { LabelControl *downloadStatusLabel; Params params; - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; bool cancellingDownload; bool colorDownloading; diff --git a/selfdrive/frogpilot/ui/qt/offroad/utilities.cc b/selfdrive/frogpilot/ui/qt/offroad/utilities.cc index eb6a5c10868922..3e837dcd6a3d85 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/utilities.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/utilities.cc @@ -29,14 +29,14 @@ UtilitiesPanel::UtilitiesPanel(FrogPilotSettingsWindow *parent) : FrogPilotListW forceStartedBtn = new FrogPilotButtonsControl(tr("Force Started State"), tr("Forces openpilot either offroad or onroad."), {tr("OFFROAD"), tr("ONROAD"), tr("OFF")}, true); QObject::connect(forceStartedBtn, &FrogPilotButtonsControl::buttonClicked, [=](int id) { if (id == 0) { - paramsMemory.putBool("ForceOffroad", true); - paramsMemory.putBool("ForceOnroad", false); + params_memory.putBool("ForceOffroad", true); + params_memory.putBool("ForceOnroad", false); } else if (id == 1) { - paramsMemory.putBool("ForceOffroad", false); - paramsMemory.putBool("ForceOnroad", true); + params_memory.putBool("ForceOffroad", false); + params_memory.putBool("ForceOnroad", true); } else if (id == 2) { - paramsMemory.putBool("ForceOffroad", false); - paramsMemory.putBool("ForceOnroad", false); + params_memory.putBool("ForceOffroad", false); + params_memory.putBool("ForceOnroad", false); } forceStartedBtn->setCheckedButton(id); }); diff --git a/selfdrive/frogpilot/ui/qt/offroad/utilities.h b/selfdrive/frogpilot/ui/qt/offroad/utilities.h index 93f5481c9b1e85..4be541371c7685 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/utilities.h +++ b/selfdrive/frogpilot/ui/qt/offroad/utilities.h @@ -12,5 +12,5 @@ class UtilitiesPanel : public FrogPilotListWidget { FrogPilotButtonsControl *forceStartedBtn; Params params; - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; }; diff --git a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc index e44bce01e52dd0..bc82cbda65c918 100644 --- a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc +++ b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc @@ -5,8 +5,8 @@ #include "selfdrive/ui/ui.h" void updateFrogPilotToggles() { - static Params paramsMemory{"/dev/shm/params"}; - paramsMemory.putBool("FrogPilotTogglesUpdated", true); + static Params params_memory{"/dev/shm/params"}; + params_memory.putBool("FrogPilotTogglesUpdated", true); } QColor loadThemeColors(const QString &colorKey, bool clearCache) { diff --git a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h index b4c78b959b91fb..27c02af39599f9 100644 --- a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h +++ b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h @@ -112,6 +112,7 @@ class FrogPilotButtonsControl : public AbstractControl { button->setMinimumWidth(minimumButtonWidth); hlayout->addWidget(button); buttonGroup->addButton(button, i); + button->installEventFilter(this); } QObject::connect(buttonGroup, QOverload::of(&QButtonGroup::buttonClicked), [=](int id) { @@ -151,6 +152,18 @@ class FrogPilotButtonsControl : public AbstractControl { signals: void buttonClicked(int id); + void disabledButtonClicked(int id); + +protected: + bool eventFilter(QObject *obj, QEvent *event) override { + if (event->type() == QEvent::MouseButtonPress) { + QPushButton *button = qobject_cast(obj); + if (button && !button->isEnabled()) { + emit disabledButtonClicked(buttonGroup->id(button)); + } + } + return AbstractControl::eventFilter(obj, event); + } private: QButtonGroup *buttonGroup; @@ -203,12 +216,6 @@ class FrogPilotButtonToggleControl : public ParamControl { } } - void setEnabledButtons(int id, bool enable) { - if (QAbstractButton *button = buttonGroup->button(id)) { - button->setEnabled(enable); - } - } - void setVisibleButton(int id, bool visible) { if (QAbstractButton *button = buttonGroup->button(id)) { button->setVisible(visible); @@ -343,7 +350,7 @@ private slots: void onButtonReleased() { if (instantUpdate) { - params.putFloatNonBlocking(key, value); + params.putFloat(key, value); } float lastValue = value; diff --git a/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.cc b/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.cc index 5bc2d310c55300..cc34f895e822e2 100644 --- a/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.cc +++ b/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.cc @@ -110,7 +110,7 @@ QPushButton *ModelReview::createButton(const QString &text, const QString &type, } void ModelReview::showEvent(QShowEvent *event) { - currentModel = QString::fromStdString(paramsMemory.get("CurrentModelName")); + currentModel = QString::fromStdString(params_memory.get("CurrentModelName")); currentModelFiltered = processModelName(currentModel); mainLayout->setCurrentIndex(modelRated ? 1 : 0); diff --git a/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.h b/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.h index 4f5af5c0164dc2..f3593d2c00da94 100644 --- a/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.h +++ b/selfdrive/frogpilot/ui/qt/widgets/model_reviewer.h @@ -43,7 +43,7 @@ private slots: QPushButton *blacklistButton; Params params; - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; QString currentModel; QString currentModelFiltered; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 1801b4eeeda9e7..7c682fbed9b498 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -233,6 +233,8 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { if (ConfirmationDialog::confirm(tr("Are you sure you want to reset calibration?"), tr("Reset"), this)) { params.remove("CalibrationParams"); params.remove("LiveTorqueParameters"); + paramsStorage.remove("CalibrationParams"); + paramsStorage.remove("LiveTorqueParameters"); } }); addItem(resetCalibBtn); @@ -504,7 +506,7 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { params.putBool("CustomizationLevelConfirmed", true); params.putInt("CustomizationLevel", 0); } - } else if (frogpilotHours < 100) { + } else { if (openpilotHours >= 100 && frogpilotHours < 100) { if (FrogPilotConfirmationDialog::toggleAlert(tr("Since you're experienced with openpilot, the 'Standard' toggle preset has been applied, but you can change this at any time via the 'Customization Level' button!"), tr("Sounds good!"), this, true)) { params.putBool("CustomizationLevelConfirmed", true); @@ -516,11 +518,6 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { params.putInt("CustomizationLevel", 1); } } - } else if (frogpilotHours >= 100) { - if (FrogPilotConfirmationDialog::toggleAlert(tr("Since you're very experienced with FrogPilot, the 'Advanced' toggle preset has been applied, but you can change this at any time via the 'Customization Level' button!"), tr("Sounds good!"), this, true)) { - params.putBool("CustomizationLevelConfirmed", true); - params.putInt("CustomizationLevel", 2); - } } } } diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index e2ea3ac5a04624..a862106c85cc3e 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -77,6 +77,9 @@ private slots: private: Params params; ButtonControl *pair_device; + + // FrogPilot variables + Params paramsStorage{"/persist/params"}; }; class TogglesPanel : public ListWidget { @@ -125,5 +128,5 @@ class SoftwarePanel : public ListWidget { ParamWatcher *fs_watch; // FrogPilot variables - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; }; diff --git a/selfdrive/ui/qt/offroad/software_settings.cc b/selfdrive/ui/qt/offroad/software_settings.cc index 081e2ea5e3cd1d..8c6992d076fb43 100644 --- a/selfdrive/ui/qt/offroad/software_settings.cc +++ b/selfdrive/ui/qt/offroad/software_settings.cc @@ -44,7 +44,7 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { } else { std::system("pkill -SIGHUP -f system.updated.updated"); } - paramsMemory.putBool("ManualUpdateInitiated", true); + params_memory.putBool("ManualUpdateInitiated", true); }); addItem(downloadBtn); @@ -89,10 +89,9 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { auto uninstallBtn = new ButtonControl(tr("Uninstall %1").arg(getBrand()), tr("UNINSTALL")); connect(uninstallBtn, &ButtonControl::clicked, [&]() { if (ConfirmationDialog::confirm(tr("Are you sure you want to uninstall?"), tr("Uninstall"), this)) { - if (FrogPilotConfirmationDialog::yesorno(tr("Do you want to delete deep storage FrogPilot assets? This is 100% unrecoverable and includes FrogPilot stats and toggle settings for quick reinstalls."), this)) { - if (FrogPilotConfirmationDialog::yesorno(tr("Are you sure? This is 100% unrecoverable and includes FrogPilot stats and toggle settings for quick reinstalls."), this)) { + if (FrogPilotConfirmationDialog::yesorno(tr("Do you want to delete deep storage FrogPilot assets? This includes your toggle settings for quick reinstalls."), this)) { + if (FrogPilotConfirmationDialog::yesorno(tr("Are you sure? This is 100% unrecoverable and if you reinstall FrogPilot you'll lose all your previous settings!"), this)) { std::system("rm -rf /persist/params"); - std::system("rm -rf /persist/tracking"); } } params.putBool("DoUninstall", true); @@ -143,10 +142,10 @@ void SoftwarePanel::updateLabels() { } // updater only runs offroad or when parked - bool parked = scene.parked; + bool parked = scene.parked || scene.frogs_go_moo; - onroadLbl->setVisible(is_onroad && !parked && !scene.frogs_go_moo); - downloadBtn->setVisible(!is_onroad || parked || scene.frogs_go_moo); + onroadLbl->setVisible(is_onroad && !parked); + downloadBtn->setVisible(!is_onroad || parked); // download update QString updater_state = QString::fromStdString(params.get("UpdaterState")); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 8a817bc47a5dca..8c6d9929345223 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -209,29 +209,24 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.drawText(rect.adjusted(20, 0, 0, 0), Qt::AlignVCenter | Qt::AlignLeft, speedStr); }; - static float previousSetSpeed = 0; - if (setSpeed == previousSetSpeed) { - QRect curveSpeedRect(QPoint(set_speed_rect.right() + 25, set_speed_rect.top()), QSize(default_size.width() * 1.25, default_size.width() * 1.25)); - QPixmap scaledCurveSpeedIcon = (leftCurve ? curveSpeedLeftIcon : curveSpeedRightIcon).scaled(curveSpeedRect.size(), Qt::KeepAspectRatio, Qt::SmoothTransformation); + QRect curveSpeedRect(QPoint(set_speed_rect.right() + 25, set_speed_rect.top()), QSize(default_size.width() * 1.25, default_size.width() * 1.25)); + QPixmap scaledCurveSpeedIcon = (leftCurve ? curveSpeedLeftIcon : curveSpeedRightIcon).scaled(curveSpeedRect.size(), Qt::KeepAspectRatio, Qt::SmoothTransformation); - p.setOpacity(1.0); - p.setRenderHint(QPainter::Antialiasing); - p.drawPixmap(curveSpeedRect, scaledCurveSpeedIcon); + p.setOpacity(1.0); + p.setRenderHint(QPainter::Antialiasing); + p.drawPixmap(curveSpeedRect, scaledCurveSpeedIcon); - if (mtscEnabled) { - QRect mtscRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), vtscControllingCurve ? 50 : 100)); + if (mtscEnabled) { + QRect mtscRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), vtscControllingCurve ? 50 : 100)); drawCurveSpeedControl(mtscRect, mtscSpeedStr, true); - if (vtscEnabled) { - QRect vtscRect(mtscRect.topLeft() + QPoint(0, mtscRect.height() + 20), QSize(mtscRect.width(), vtscControllingCurve ? 100 : 50)); - drawCurveSpeedControl(vtscRect, vtscSpeedStr, false); - } - } else if (vtscEnabled) { - QRect vtscRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), 150)); + if (vtscEnabled) { + QRect vtscRect(mtscRect.topLeft() + QPoint(0, mtscRect.height() + 20), QSize(mtscRect.width(), vtscControllingCurve ? 100 : 50)); drawCurveSpeedControl(vtscRect, vtscSpeedStr, false); } - } else { - previousSetSpeed = setSpeed; + } else if (vtscEnabled) { + QRect vtscRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), 150)); + drawCurveSpeedControl(vtscRect, vtscSpeedStr, false); } } @@ -560,7 +555,7 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s, f if (scene.show_stopping_point_metrics) { QFont font = InterFont(35, QFont::DemiBold); - QString text = QString::number(modelLength * distanceConversion) + leadDistanceUnit; + QString text = QString::number(std::nearbyint(modelLength * distanceConversion)) + leadDistanceUnit; int text_width = QFontMetrics(font).horizontalAdvance(text); QPointF text_position = last_point - QPointF(text_width / 2, stopSignImg.height() + 35); @@ -741,10 +736,10 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState // chevron QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}}; - if (useStockColors) { + if (useStockColors && !adjacent) { painter.setBrush(redColor(fillAlpha)); } else { - painter.setBrush(lead_marker_color); + painter.setBrush(QColor(lead_marker_color.red(), lead_marker_color.green(), lead_marker_color.blue(), fillAlpha)); } painter.drawPolygon(chevron, std::size(chevron)); @@ -864,21 +859,25 @@ void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) { auto lead_right = radar_state.getLeadRight(); auto lead_left_far = radar_state.getLeadLeftFar(); auto lead_right_far = radar_state.getLeadRightFar(); + auto leads_lead = radar_state.getLeadsLead(); if (lead_left.getStatus()) { drawLead(painter, lead_left, s->scene.lead_vertices[2], v_ego, blueColor(), true); } if (lead_right.getStatus()) { - drawLead(painter, lead_right, s->scene.lead_vertices[3], v_ego, redColor(), true); + drawLead(painter, lead_right, s->scene.lead_vertices[3], v_ego, orangeColor(), true); } if (lead_left_far.getStatus()) { - drawLead(painter, lead_left_far, s->scene.lead_vertices[4], v_ego, greenColor(), true); + drawLead(painter, lead_left_far, s->scene.lead_vertices[4], v_ego, purpleColor(), true); } if (lead_right_far.getStatus()) { - drawLead(painter, lead_right_far, s->scene.lead_vertices[5], v_ego, whiteColor(), true); + drawLead(painter, lead_right_far, s->scene.lead_vertices[5], v_ego, yellowColor(), true); } if (lead_two.getStatus()) { drawLead(painter, lead_two, s->scene.lead_vertices[1], v_ego, s->scene.lead_marker_color); } else if (lead_one.getStatus()) { + if (leads_lead.getStatus()) { + drawLead(painter, leads_lead, s->scene.lead_vertices[6], v_ego, s->scene.lead_marker_color); + } drawLead(painter, lead_one, s->scene.lead_vertices[0], v_ego, s->scene.lead_marker_color); } else { lead_x = 0; @@ -921,26 +920,6 @@ void AnnotatedCameraWidget::showEvent(QShowEvent *event) { prev_draw_t = millis_since_boot(); // FrogPilot variables - if (is_metric || useSI) { - accelerationUnit = tr("m/s²"); - leadDistanceUnit = tr(mapOpen ? "m" : "meters"); - leadSpeedUnit = useSI ? tr("m/s") : tr("kph"); - - accelerationConversion = 1.0f; - distanceConversion = 1.0f; - speedConversion = is_metric ? MS_TO_KPH : MS_TO_MPH; - speedConversionMetrics = useSI ? 1.0f : MS_TO_KPH; - } else { - accelerationUnit = tr("ft/s²"); - leadDistanceUnit = tr(mapOpen ? "ft" : "feet"); - leadSpeedUnit = tr("mph"); - - accelerationConversion = METER_TO_FOOT; - distanceConversion = METER_TO_FOOT; - speedConversion = MS_TO_MPH; - speedConversionMetrics = MS_TO_MPH; - } - distance_btn->updateIcon(); experimental_btn->updateIcon(); updateSignals(); @@ -1038,6 +1017,26 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { } void AnnotatedCameraWidget::updateFrogPilotVariables(int alert_height, const UIScene &scene) { + if (is_metric || useSI) { + accelerationUnit = tr("m/s²"); + leadDistanceUnit = tr(mapOpen ? "m" : "meters"); + leadSpeedUnit = useSI ? tr("m/s") : tr("kph"); + + accelerationConversion = 1.0f; + distanceConversion = 1.0f; + speedConversion = is_metric ? MS_TO_KPH : MS_TO_MPH; + speedConversionMetrics = useSI ? 1.0f : MS_TO_KPH; + } else { + accelerationUnit = tr("ft/s²"); + leadDistanceUnit = tr(mapOpen ? "ft" : "feet"); + leadSpeedUnit = tr("mph"); + + accelerationConversion = METER_TO_FOOT; + distanceConversion = METER_TO_FOOT; + speedConversion = MS_TO_MPH; + speedConversionMetrics = MS_TO_MPH; + } + alertHeight = alert_height; alwaysOnLateralActive = scene.always_on_lateral_active; @@ -1093,7 +1092,7 @@ void AnnotatedCameraWidget::updateFrogPilotVariables(int alert_height, const UIS modelLength = scene.model_length; mtscEnabled = scene.mtsc_enabled; - mtscSpeed = scene.mtsc_speed * speedConversion + vCruiseDiff; + mtscSpeed = mtscEnabled ? scene.mtsc_speed * speedConversion : setSpeed; onroadDistanceButton = scene.onroad_distance_button; bool enableDistanceButton = onroadDistanceButton && !hideBottomIcons; @@ -1151,11 +1150,9 @@ void AnnotatedCameraWidget::updateFrogPilotVariables(int alert_height, const UIS useStockColors = scene.use_stock_colors; - vCruiseDiff = scene.v_cruise_diff; - vtscControllingCurve = scene.vtsc_controlling_curve; vtscEnabled = scene.vtsc_enabled; - vtscSpeed = scene.vtsc_speed * speedConversion + vCruiseDiff; + vtscSpeed = vtscEnabled ? scene.vtsc_speed * speedConversion : setSpeed; } void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter) { diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 56ab666315f343..7ba29548ea6135 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -179,7 +179,6 @@ class AnnotatedCameraWidget : public CameraWidget { float speedConversionMetrics; float unconfirmedSpeedLimit; float upcomingSpeedLimit; - float vCruiseDiff; float vtscSpeed; int alertHeight; @@ -203,8 +202,11 @@ class AnnotatedCameraWidget : public CameraWidget { std::string speedLimitSource; - inline QColor blueColor(int alpha = 255) { return QColor(0, 150, 255, alpha); } + inline QColor blueColor(int alpha = 255) { return QColor(0, 0, 255, alpha); } inline QColor greenColor(int alpha = 242) { return QColor(23, 134, 68, alpha); } + inline QColor orangeColor(int alpha = 255) { return QColor(255, 165, 0, alpha); } + inline QColor purpleColor(int alpha = 255) { return QColor(128, 0, 128, alpha); } + inline QColor yellowColor(int alpha = 255) { return QColor(255, 255, 0, alpha); } protected: void paintGL() override; diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index 2f0722ccb58747..e524a0dbc9a088 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -39,6 +39,7 @@ ExperimentalButton::~ExperimentalButton() { if (gif != nullptr) { gif->stop(); delete gif; + gif = nullptr; gif_label->hide(); } } @@ -126,12 +127,19 @@ void ExperimentalButton::updateIcon() { if (gif != nullptr) { gif->stop(); delete gif; + gif = nullptr; gif_label->hide(); } if (QFile::exists(wheel_gif_path)) { gif = new QMovie(wheel_gif_path); + if (!gif->isValid()) { + delete gif; + gif = nullptr; + return; + } + gif_label->setMovie(gif); gif_label->resize(img_size, img_size); gif_label->move((btn_size - img_size) / 2, (btn_size - img_size) / 2 + y_offset); diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 0749dd2cc708d0..d4deac8fc3557d 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -125,7 +125,6 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { if (scene.speed_limit_changed && nvg->newSpeedLimitRect.contains(pos)) { paramsMemory.putBool("SLCConfirmed", true); - paramsMemory.putBool("SLCConfirmedPressed", true); return; } diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index abb864eb3676a4..bd2a843f6598ab 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -63,24 +63,28 @@ void Sidebar::updateIcon(QLabel *&label, QMovie *&gif, const QString &gifPath, c gif->stop(); delete gif; gif = nullptr; - label->hide(); + if (label) { + label->hide(); + } } if (QFile::exists(selectedGifPath)) { gif = new QMovie(selectedGifPath); - if (!gif->fileName().isEmpty()) { + if (gif->isValid()) { gif->setScaledSize(btnRect.size()); - label->setGeometry(btnRect); - label->setMovie(gif); - label->show(); + if (label) { + label->setGeometry(btnRect); + label->setMovie(gif); + label->show(); + } gif->start(); - isGif = true; } else { delete gif; + gif = nullptr; isGif = false; } } else { diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h index 1a5b2c38be70a7..cd6b1e82799dba 100644 --- a/selfdrive/ui/qt/widgets/controls.h +++ b/selfdrive/ui/qt/widgets/controls.h @@ -225,11 +225,12 @@ class ButtonParamControl : public AbstractControl { button->setMinimumWidth(minimum_button_width); hlayout->addWidget(button); button_group->addButton(button, i); + button->installEventFilter(this); } QObject::connect(button_group, QOverload::of(&QButtonGroup::buttonClicked), [=](int id) { params.put(key, std::to_string(id)); - emit buttonClicked(); + emit buttonClicked(id); }); } @@ -239,6 +240,12 @@ class ButtonParamControl : public AbstractControl { } } + void setEnabledButtons(int id, bool enable) { + if (QAbstractButton *button = button_group->button(id)) { + button->setEnabled(enable); + } + } + void setCheckedButton(int id) { button_group->button(id)->setChecked(true); } @@ -253,7 +260,19 @@ class ButtonParamControl : public AbstractControl { } signals: - void buttonClicked(); + void buttonClicked(int id); + void disabledButtonClicked(int id); + +protected: + bool eventFilter(QObject *obj, QEvent *event) override { + if (event->type() == QEvent::MouseButtonPress) { + QPushButton *button = qobject_cast(obj); + if (button && !button->isEnabled()) { + emit disabledButtonClicked(button_group->id(button)); + } + } + return AbstractControl::eventFilter(obj, event); + } private: std::string key; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 6e52e9139d1a1c..35f3cd9169ffb6 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -48,16 +48,17 @@ void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, con SubMaster &sm = *(s->sm); float path_offset_z = sm["liveCalibration"].getLiveCalibration().getHeight()[0]; - cereal::RadarState::LeadData::Reader (cereal::RadarState::Reader::*get_lead_data[6])() const = { + cereal::RadarState::LeadData::Reader (cereal::RadarState::Reader::*get_lead_data[7])() const = { &cereal::RadarState::Reader::getLeadOne, &cereal::RadarState::Reader::getLeadTwo, &cereal::RadarState::Reader::getLeadLeft, &cereal::RadarState::Reader::getLeadRight, &cereal::RadarState::Reader::getLeadLeftFar, - &cereal::RadarState::Reader::getLeadRightFar + &cereal::RadarState::Reader::getLeadRightFar, + &cereal::RadarState::Reader::getLeadsLead }; - for (int i = 0; i < 6; ++i) { + for (int i = 0; i < 7; ++i) { auto lead_data = (radar_state.*get_lead_data[i])(); if (lead_data.getStatus()) { float z = line.getZ()[get_path_length_idx(line, lead_data.getDRel())]; @@ -271,7 +272,6 @@ static void update_state(UIState *s) { auto controlsState = sm["controlsState"].getControlsState(); scene.enabled = controlsState.getEnabled(); scene.experimental_mode = scene.enabled && controlsState.getExperimentalMode(); - scene.v_cruise_diff = controlsState.getVCruiseCluster() - controlsState.getVCruise(); } if (sm.updated("deviceState")) { auto deviceState = sm["deviceState"].getDeviceState(); @@ -490,7 +490,7 @@ void UIState::updateStatus() { } scene.started |= scene.force_onroad; - scene.started &= !paramsMemory.getBool("ForceOffroad"); + scene.started &= !params_memory.getBool("ForceOffroad"); // Handle onroad/offroad transition if (scene.started != started_prev || sm->frame == 1) { @@ -548,9 +548,9 @@ void UIState::update() { emit uiUpdate(*this); // FrogPilot variables - scene.conditional_status = scene.conditional_experimental && scene.enabled ? paramsMemory.getInt("CEStatus") : 0; + scene.conditional_status = scene.conditional_experimental && scene.enabled ? params_memory.getInt("CEStatus") : 0; scene.driver_camera_timer = scene.driver_camera_in_reverse && scene.reverse ? scene.driver_camera_timer + 1 : 0; - scene.force_onroad = paramsMemory.getBool("ForceOnroad"); + scene.force_onroad = params_memory.getBool("ForceOnroad"); scene.started_timer = scene.started || started_prev ? scene.started_timer + 1 : 0; if (scene.keep_screen_on) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 9cccb54f61d090..e4c143b7f6ad2e 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -104,7 +104,7 @@ typedef struct UIScene { QPolygonF road_edge_vertices[2]; // lead - QPointF lead_vertices[6]; + QPointF lead_vertices[7]; // DMoji state float driver_pose_vals[3]; @@ -257,7 +257,6 @@ typedef struct UIScene { float steer; float unconfirmed_speed_limit; float upcoming_speed_limit; - float v_cruise_diff; float vtsc_speed; int bearing_deg; @@ -332,7 +331,7 @@ private slots: PrimeType prime_type = PrimeType::UNKNOWN; // FrogPilot variables - Params paramsMemory{"/dev/shm/params"}; + Params params_memory{"/dev/shm/params"}; }; UIState *uiState(); diff --git a/system/manager/manager.py b/system/manager/manager.py index 29dcb0efe072f3..341d12fc391569 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -29,20 +29,20 @@ def manager_init() -> None: build_metadata = get_build_metadata() params = Params() - setup_frogpilot(build_metadata, params) - params_storage = Params("/persist/params") params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) if build_metadata.release_channel: params.clear_all(ParamKeyType.DEVELOPMENT_ONLY) - convert_params(params, params_storage) - threading.Thread(target=frogpilot_boot_functions, args=(build_metadata, params, params_storage,)).start() + # FrogPilot variables + setup_frogpilot(build_metadata) + params_storage = Params("/persist/params") + convert_params(params_storage) + threading.Thread(target=frogpilot_boot_functions, args=(build_metadata, params_storage,)).start() default_params: list[tuple[str, str | bytes]] = [ ("AlwaysOnDM", "0"), - ("CalibrationParams", ""), ("CarParamsPersistent", ""), ("CompletedTrainingVersion", "0"), ("DisengageOnAccelerator", "0"), @@ -58,13 +58,13 @@ def manager_init() -> None: ("IsLdwEnabled", "0"), ("IsMetric", "0"), ("LanguageSetting", "main_en"), - ("LiveTorqueParameters", ""), ("NavSettingLeftSide", "0"), ("NavSettingTime24h", "0"), ("OpenpilotEnabledToggle", "1"), ("RecordFront", "0"), ("SshEnabled", "0"), ("TetheringEnabled", "0"), + ("UpdaterAvailableBranches", ""), ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), ] if not PC: diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 39d418c33e61f9..115b951e7556a7 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -72,7 +72,7 @@ def run_new_modeld(started, params, CP: car.CarParams, classic_model, frogpilot_ NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], only_onroad), PythonProcess("navmodeld", "selfdrive.modeld.navmodeld", only_onroad), NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC), - NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(30 if not PC else None)), + NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)), PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad), NativeProcess("locationd", "selfdrive/locationd", ["./locationd"], only_onroad), NativeProcess("pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), diff --git a/system/sentry.py b/system/sentry.py index 35bf570e6f1612..7a2dd7e4e971ed 100644 --- a/system/sentry.py +++ b/system/sentry.py @@ -96,7 +96,7 @@ def capture_fingerprint(candidate, params, blocked=False): sentry_sdk.capture_message("Blocked user from using the development branch", level='error') else: sentry_sdk.capture_message(f"Fingerprinted {candidate}", level='info') - params.put_bool_nonblocking("FingerprintLogged", True) + params.put_bool("FingerprintLogged", True) sentry_sdk.flush() diff --git a/system/updated/updated.py b/system/updated/updated.py index 180a4a54e08ec6..40fd8b898e8f0a 100755 --- a/system/updated/updated.py +++ b/system/updated/updated.py @@ -411,7 +411,7 @@ def fetch_update(self) -> None: cloudlog.info("finalize success!") # Format "Updated" to Phoenix time zone - self.params.put_nonblocking("Updated", datetime.datetime.now().astimezone(ZoneInfo('America/Phoenix')).strftime("%B %d, %Y - %I:%M%p").encode('utf8')) + self.params.put("Updated", datetime.datetime.now().astimezone(ZoneInfo('America/Phoenix')).strftime("%B %d, %Y - %I:%M%p").encode('utf8')) def main() -> None: params = Params() @@ -468,7 +468,7 @@ def main() -> None: # Format "InstallDate" to Phoenix time zone if not install_date_set: - params.put_nonblocking("InstallDate", datetime.datetime.now().astimezone(ZoneInfo('America/Phoenix')).strftime("%B %d, %Y - %I:%M%p").encode('utf8')) + params.put("InstallDate", datetime.datetime.now().astimezone(ZoneInfo('America/Phoenix')).strftime("%B %d, %Y - %I:%M%p").encode('utf8')) install_date_set = True if not (frogpilot_toggles.automatic_updates or params_memory.get_bool("ManualUpdateInitiated")):