From c3b2e5d9adb8923f2ae45bd7081a642c0bc10590 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 24 Jul 2024 22:27:01 -0700 Subject: [PATCH] FrogPilot setup - Setup toggles --- common/params.cc | 341 +++++++++++++++++- common/params.h | 6 + common/params_pyx.pyx | 11 + selfdrive/car/body/carcontroller.py | 2 +- selfdrive/car/body/carstate.py | 2 +- selfdrive/car/body/interface.py | 6 +- selfdrive/car/car_helpers.py | 4 +- selfdrive/car/card.py | 19 +- selfdrive/car/chrysler/carcontroller.py | 2 +- selfdrive/car/chrysler/carstate.py | 2 +- selfdrive/car/chrysler/interface.py | 6 +- selfdrive/car/ford/carcontroller.py | 2 +- selfdrive/car/ford/carstate.py | 2 +- selfdrive/car/ford/interface.py | 6 +- selfdrive/car/gm/carcontroller.py | 2 +- selfdrive/car/gm/carstate.py | 2 +- selfdrive/car/gm/interface.py | 6 +- selfdrive/car/honda/carcontroller.py | 2 +- selfdrive/car/honda/carstate.py | 2 +- selfdrive/car/honda/interface.py | 6 +- selfdrive/car/hyundai/carcontroller.py | 2 +- selfdrive/car/hyundai/carstate.py | 6 +- selfdrive/car/hyundai/interface.py | 6 +- selfdrive/car/interfaces.py | 17 +- selfdrive/car/mazda/carcontroller.py | 2 +- selfdrive/car/mazda/carstate.py | 2 +- selfdrive/car/mazda/interface.py | 6 +- selfdrive/car/mock/interface.py | 4 +- selfdrive/car/nissan/carcontroller.py | 2 +- selfdrive/car/nissan/carstate.py | 2 +- selfdrive/car/nissan/interface.py | 6 +- selfdrive/car/subaru/carcontroller.py | 2 +- selfdrive/car/subaru/carstate.py | 2 +- selfdrive/car/subaru/interface.py | 6 +- selfdrive/car/subaru/values.py | 1 + selfdrive/car/tesla/carcontroller.py | 2 +- selfdrive/car/tesla/carstate.py | 2 +- selfdrive/car/tesla/interface.py | 6 +- selfdrive/car/toyota/carcontroller.py | 10 +- selfdrive/car/toyota/carstate.py | 2 +- selfdrive/car/toyota/interface.py | 6 +- selfdrive/car/toyota/toyotacan.py | 2 +- selfdrive/car/volkswagen/carcontroller.py | 2 +- selfdrive/car/volkswagen/carstate.py | 6 +- selfdrive/car/volkswagen/interface.py | 7 +- selfdrive/controls/controlsd.py | 17 +- selfdrive/controls/lib/desire_helper.py | 2 +- selfdrive/controls/lib/drive_helpers.py | 6 +- selfdrive/controls/lib/events.py | 4 + .../lib/longitudinal_mpc_lib/long_mpc.py | 2 +- .../controls/lib/longitudinal_planner.py | 4 +- selfdrive/controls/plannerd.py | 16 +- selfdrive/controls/radard.py | 13 + .../frogpilot/controls/frogpilot_planner.py | 20 +- .../controls/lib/frogpilot_variables.py | 223 ++++++++++++ selfdrive/frogpilot/frogpilot_process.py | 12 + .../ui/qt/widgets/frogpilot_controls.cc | 16 + .../ui/qt/widgets/frogpilot_controls.h | 3 + selfdrive/locationd/calibrationd.py | 14 + selfdrive/locationd/paramsd.py | 1 + selfdrive/locationd/torqued.py | 17 + selfdrive/modeld/modeld.py | 15 +- selfdrive/navd/navd.py | 14 + selfdrive/ui/qt/home.h | 3 + selfdrive/ui/qt/maps/map.h | 3 + selfdrive/ui/qt/offroad/settings.h | 3 + selfdrive/ui/qt/sidebar.h | 3 + selfdrive/ui/qt/widgets/wifi.h | 4 + selfdrive/ui/qt/window.h | 3 + selfdrive/ui/soundd.py | 18 + selfdrive/ui/ui.cc | 29 ++ selfdrive/ui/ui.h | 9 + system/camerad/cameras/camera_qcom2.cc | 3 + system/hardware/hardwared.py | 15 +- system/hardware/power_monitoring.py | 2 +- system/manager/manager.py | 2 + system/updated/updated.py | 3 + 77 files changed, 934 insertions(+), 107 deletions(-) create mode 100644 selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc create mode 100644 selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h diff --git a/common/params.cc b/common/params.cc index 23301601..5779b6af 100644 --- a/common/params.cc +++ b/common/params.cc @@ -89,7 +89,7 @@ private: std::unordered_map keys = { {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG}, - {"AlwaysOnDM", PERSISTENT}, + {"AlwaysOnDM", PERSISTENT | FROGPILOT_STORAGE}, {"ApiCache_Device", PERSISTENT}, {"ApiCache_NavDestinations", PERSISTENT}, {"AssistNowToken", PERSISTENT}, @@ -112,14 +112,14 @@ std::unordered_map keys = { {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"DisablePowerDown", PERSISTENT}, - {"DisableUpdates", PERSISTENT}, - {"DisengageOnAccelerator", PERSISTENT}, + {"DisableUpdates", PERSISTENT | FROGPILOT_STORAGE}, + {"DisengageOnAccelerator", PERSISTENT | FROGPILOT_STORAGE}, {"DmModelInitialized", CLEAR_ON_ONROAD_TRANSITION}, {"DongleId", PERSISTENT}, {"DoReboot", CLEAR_ON_MANAGER_START}, {"DoShutdown", CLEAR_ON_MANAGER_START}, {"DoUninstall", CLEAR_ON_MANAGER_START}, - {"ExperimentalLongitudinalEnabled", PERSISTENT | DEVELOPMENT_ONLY}, + {"ExperimentalLongitudinalEnabled", PERSISTENT | DEVELOPMENT_ONLY | FROGPILOT_STORAGE}, {"ExperimentalMode", PERSISTENT}, {"ExperimentalModeConfirmed", PERSISTENT}, {"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, @@ -140,8 +140,8 @@ std::unordered_map keys = { {"InstallDate", PERSISTENT}, {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START}, {"IsEngaged", PERSISTENT}, - {"IsLdwEnabled", PERSISTENT}, - {"IsMetric", PERSISTENT}, + {"IsLdwEnabled", PERSISTENT | FROGPILOT_STORAGE}, + {"IsMetric", PERSISTENT | FROGPILOT_STORAGE}, {"IsOffroad", CLEAR_ON_MANAGER_START}, {"IsOnroad", PERSISTENT}, {"IsRhdDetected", PERSISTENT}, @@ -207,6 +207,335 @@ std::unordered_map keys = { {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START}, {"UpdaterLastFetchTime", PERSISTENT}, {"Version", PERSISTENT}, + + // FrogPilot parameters + {"AccelerationPath", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"AccelerationProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AdjacentPath", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"AdjacentPathMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"AggressiveAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AggressiveFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AggressiveJerkAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AggressiveJerkDanger", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AggressiveJerkSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AggressivePersonalityProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AlertVolumeControl", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"AlwaysOnLateral", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AlwaysOnLateralLKAS", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AlwaysOnLateralMain", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AlwaysOnLateralSet", CLEAR_ON_ONROAD_TRANSITION}, + {"AMapKey1", PERSISTENT}, + {"AMapKey2", PERSISTENT}, + {"ApiCache_DriveStats", PERSISTENT}, + {"AutomaticallyUpdateModels", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AutomaticUpdates", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, + {"AvailableModels", PERSISTENT}, + {"AvailableModelsNames", PERSISTENT}, + {"BigMap", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"BlacklistedModels", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"BlindSpotMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"BlindSpotPath", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"BorderMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"CameraFPS", PERSISTENT}, + {"CameraView", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"CancelModelDownload", PERSISTENT}, + {"CarMake", PERSISTENT}, + {"CarModel", PERSISTENT}, + {"CarModelName", PERSISTENT}, + {"CECurves", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CECurvesLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CELead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CENavigation", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CENavigationIntersections", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CENavigationLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CENavigationTurns", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CertifiedHerbalistCalibrationParams", PERSISTENT}, + {"CertifiedHerbalistDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CertifiedHerbalistLiveTorqueParameters", PERSISTENT}, + {"CertifiedHerbalistScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CESignal", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CESlowerLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CESpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CESpeedLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CEStatus", PERSISTENT}, + {"CEStopLights", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CEStopLightsLessSensitive", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CEStoppedLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ClusterOffset", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"Compass", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ConditionalExperimental", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CrosstrekTorque", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"CurrentHolidayTheme", PERSISTENT}, + {"CurrentModel", PERSISTENT | CLEAR_ON_OFFROAD_TRANSITION}, + {"CurrentModelName", PERSISTENT | CLEAR_ON_OFFROAD_TRANSITION}, + {"CurrentRandomEvent", PERSISTENT}, + {"CurveSensitivity", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CustomAlerts", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"CustomColors", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"CustomCruise", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CustomCruiseLong", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CustomIcons", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"CustomPaths", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"CustomPersonalities", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CustomSignals", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"CustomSounds", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"CustomTheme", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"CustomUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"CydiaTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"DecelerationProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DeveloperUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"DeviceManagement", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DeviceShutdown", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DisableMTSCSmoothing", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DisableOnroadUploads", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DisableOpenpilotLongitudinal", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"DisableVTSCSmoothing", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DisengageVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"DoToggleReset", PERSISTENT}, + {"DownloadAllModels", PERSISTENT}, + {"DragonPilotTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"DriveRated", CLEAR_ON_ONROAD_TRANSITION}, + {"DriverCamera", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"DrivingPersonalities", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DuckAmigoCalibrationParams", PERSISTENT}, + {"DuckAmigoDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DuckAmigoLiveTorqueParameters", PERSISTENT}, + {"DuckAmigoScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"DynamicPathWidth", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"DynamicPedalsOnUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"EngageVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ExperimentalModeActivation", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ExperimentalModels", PERSISTENT}, + {"ExperimentalModeViaDistance", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ExperimentalModeViaLKAS", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ExperimentalModeViaTap", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"Fahrenheit", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"FingerprintLogged", CLEAR_ON_MANAGER_START}, + {"ForceAutoTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ForceFingerprint", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"ForceMPHDashboard", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ForceOffroad", PERSISTENT}, + {"ForceOnroad", PERSISTENT}, + {"ForceStandstill", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ForceStops", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"FPSCounter", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"FrogPilotDrives", PERSISTENT | FROGPILOT_TRACKING}, + {"FrogPilotKilometers", PERSISTENT | FROGPILOT_TRACKING}, + {"FrogPilotMinutes", PERSISTENT | FROGPILOT_TRACKING}, + {"FrogPilotTogglesUpdated", PERSISTENT}, + {"FrogsGoMoo", PERSISTENT}, + {"FrogsGoMooTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"FullMap", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"GasRegenCmd", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"GMapKey", PERSISTENT}, + {"GoatScream", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"GreenLightAlert", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"HideAlerts", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"HideAOLStatusBar", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"HideCEMStatusBar", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"HideLeadMarker", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"HideMapIcon", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"HideMaxSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"HideSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"HideSpeedUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"HideUIElements", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"HolidayThemes", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"IncreaseThermalLimits", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"JerkInfo", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"KaofuiIcons", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LaneChangeCustomizations", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LaneChangeTime", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LaneDetectionWidth", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LaneLinesWidth", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"LastMapsUpdate", PERSISTENT | FROGPILOT_OTHER}, + {"LateralMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"LateralTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LeadDepartingAlert", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"LeadDetectionThreshold", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LeadInfo", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"LockDoors", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"LongitudinalMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"LongitudinalTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LongPitch", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"LosAngelesCalibrationParams", PERSISTENT}, + {"LosAngelesDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LosAngelesLiveTorqueParameters", PERSISTENT}, + {"LosAngelesScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"LoudBlindspotAlert", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"LowVoltageShutdown", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ManualUpdateInitiated", PERSISTENT}, + {"MapAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"MapboxPublicKey", PERSISTENT}, + {"MapboxSecretKey", PERSISTENT}, + {"MapDeceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"MapGears", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"MapsSelected", PERSISTENT | FROGPILOT_OTHER}, + {"MapSpeedLimit", PERSISTENT}, + {"MapStyle", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"MapTargetLatA", PERSISTENT}, + {"MapTargetVelocities", PERSISTENT}, + {"MinimumLaneChangeSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"Model", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ModelDownloadProgress", PERSISTENT}, + {"ModelManagement", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ModelName", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ModelRandomizer", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ModelsDownloaded", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ModelSelector", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ModelToDownload", PERSISTENT}, + {"ModelUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"MTSCAggressiveness", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"MTSCCurvatureCheck", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"MTSCEnabled", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NavigationModels", PERSISTENT}, + {"NextMapSpeedLimit", PERSISTENT}, + {"NNFF", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NNFFLite", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NNFFModelName", PERSISTENT}, + {"NoLogging", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NorthDakotaCalibrationParams", PERSISTENT}, + {"NorthDakotaDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NorthDakotaLiveTorqueParameters", PERSISTENT}, + {"NorthDakotaScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NorthDakotaV2CalibrationParams", PERSISTENT}, + {"NorthDakotaV2Drives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NorthDakotaV2LiveTorqueParameters", PERSISTENT}, + {"NorthDakotaV2Score", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NotreDameCalibrationParams", PERSISTENT}, + {"NotreDameDrives", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NotreDameLiveTorqueParameters", PERSISTENT}, + {"NotreDameScore", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NoUploads", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NudgelessLaneChange", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NumericalTemp", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"OfflineMode", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"Offset1", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"Offset2", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"Offset3", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"Offset4", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"OneLaneChange", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"OnroadDistanceButton", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"OnroadDistanceButtonPressed", PERSISTENT}, + {"OSMDownloadBounds", PERSISTENT}, + {"OSMDownloadLocations", PERSISTENT}, + {"OSMDownloadProgress", CLEAR_ON_MANAGER_START}, + {"ParamConversionVersion", PERSISTENT}, + {"PathEdgeWidth", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"PathWidth", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"PauseAOLOnBrake", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"PauseLateralOnSignal", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"PauseLateralSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"PedalsOnUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"PreferredSchedule", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, + {"PreviousSpeedLimit", PERSISTENT}, + {"PromptDistractedVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"PromptVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"QOLControls", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"QOLVisuals", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"RadarlessModels", PERSISTENT}, + {"RadicalTurtleCalibrationParams", PERSISTENT}, + {"RadicalTurtleDrives", PERSISTENT | FROGPILOT_CONTROLS}, + {"RadicalTurtleLiveTorqueParameters", PERSISTENT}, + {"RadicalTurtleScore", PERSISTENT | FROGPILOT_CONTROLS}, + {"RandomEvents", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"RecertifiedHerbalistCalibrationParams", PERSISTENT}, + {"RecertifiedHerbalistDrives", PERSISTENT | FROGPILOT_CONTROLS}, + {"RecertifiedHerbalistLiveTorqueParameters", PERSISTENT}, + {"RecertifiedHerbalistScore", PERSISTENT | FROGPILOT_CONTROLS}, + {"RefuseVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"RelaxedFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"RelaxedJerkAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"RelaxedJerkDanger", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"RelaxedJerkSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"RelaxedPersonalityProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ReverseCruise", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ReverseCruiseUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"RoadEdgesWidth", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"RoadName", PERSISTENT}, + {"RoadNameUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"RotatingWheel", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ScreenBrightness", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ScreenBrightnessOnroad", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ScreenManagement", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ScreenRecorder", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ScreenTimeout", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ScreenTimeoutOnroad", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"SearchInput", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, + {"SecretGoodOpenpilotCalibrationParams", PERSISTENT}, + {"SecretGoodOpenpilotDrives", PERSISTENT | FROGPILOT_CONTROLS}, + {"SecretGoodOpenpilotLiveTorqueParameters", PERSISTENT}, + {"SecretGoodOpenpilotScore", PERSISTENT | FROGPILOT_CONTROLS}, + {"SetSpeedLimit", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SetSpeedOffset", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ShowCPU", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ShowGPU", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ShowIP", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ShowMemoryUsage", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ShowSLCOffset", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ShowSLCOffsetUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"ShowSteering", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ShowStoppingPoint", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ShowStoppingPointMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ShowStorageLeft", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"ShowStorageUsed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"Sidebar", PERSISTENT | FROGPILOT_OTHER}, + {"SidebarMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"SignalMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"SLCConfirmation", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"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}, + {"SLCOverride", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SLCPriority1", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SLCPriority2", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SLCPriority3", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SmoothBraking", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SNGHack", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"SpeedLimitChangedAlert", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SpeedLimitController", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"StandardFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"StandardJerkAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"StandardJerkDanger", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"StandardJerkSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"StandardPersonalityProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"StandbyMode", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"StaticPedalsOnUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"SteerRatio", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"SteerRatioStock", PERSISTENT}, + {"StockTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"StoppedTimer", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"StoppingDistance", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TacoTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TetheringEnabled", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, + {"ToyotaDoors", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"TrafficFollow", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TrafficJerkAcceleration", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TrafficJerkDanger", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TrafficJerkSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TrafficMode", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TrafficPersonalityProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TuningInfo", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"TurnAggressiveness", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"TurnDesires", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"UnlimitedLength", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"UnlockDoors", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"Updated", PERSISTENT}, + {"UseSI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"UseVienna", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"VisionTurnControl", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"VoltSNG", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"WarningImmediateVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"WarningSoftVolume", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"WD40CalibrationParams", PERSISTENT}, + {"WD40Drives", PERSISTENT | FROGPILOT_CONTROLS}, + {"WD40LiveTorqueParameters", PERSISTENT}, + {"WD40Score", PERSISTENT | FROGPILOT_CONTROLS}, + {"WheelIcon", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"WheelSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, }; } // namespace diff --git a/common/params.h b/common/params.h index 9c8af82c..e7ad3e1c 100644 --- a/common/params.h +++ b/common/params.h @@ -16,6 +16,12 @@ enum ParamKeyType { CLEAR_ON_OFFROAD_TRANSITION = 0x10, DONT_LOG = 0x20, DEVELOPMENT_ONLY = 0x40, + FROGPILOT_CONTROLS = 0x80, + FROGPILOT_OTHER = 0x400, + FROGPILOT_STORAGE = 0x800, + FROGPILOT_TRACKING = 0x1000, + FROGPILOT_VEHICLES = 0x100, + FROGPILOT_VISUALS = 0x200, ALL = 0xFFFFFFFF }; diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index ddce5103..e56eadf5 100644 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -11,6 +11,12 @@ cdef extern from "common/params.h": CLEAR_ON_ONROAD_TRANSITION CLEAR_ON_OFFROAD_TRANSITION DEVELOPMENT_ONLY + FROGPILOT_CONTROLS + FROGPILOT_OTHER + FROGPILOT_STORAGE + FROGPILOT_TRACKING + FROGPILOT_VEHICLES + FROGPILOT_VISUALS ALL cdef cppclass c_Params "Params": @@ -32,6 +38,7 @@ cdef extern from "common/params.h": string getParamPath(string) nogil void clearAll(ParamKeyType) vector[string] allKeys() + ParamKeyType getKeyType(string) nogil def ensure_bytes(v): @@ -156,3 +163,7 @@ cdef class Params: def all_keys(self): return self.p.allKeys() + + def get_key_type(self, key): + cdef string k = self.check_key(key) + return self.p.getKeyType(k) diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 259126c4..ba73044d 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -35,7 +35,7 @@ class CarController(CarControllerBase): torque -= deadband return torque - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_toggles): torque_l = 0 torque_r = 0 diff --git a/selfdrive/car/body/carstate.py b/selfdrive/car/body/carstate.py index e184276a..08ee9229 100644 --- a/selfdrive/car/body/carstate.py +++ b/selfdrive/car/body/carstate.py @@ -6,7 +6,7 @@ from openpilot.selfdrive.car.body.values import DBC STARTUP_TICKS = 100 class CarState(CarStateBase): - def update(self, cp): + def update(self, cp, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 87da16a2..4026799a 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -7,7 +7,7 @@ from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params): ret.notCar = True ret.carName = "body" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] @@ -25,8 +25,8 @@ class CarInterface(CarInterfaceBase): return ret - def _update(self, c): - ret, fp_ret = self.CS.update(self.cp) + def _update(self, c, frogpilot_toggles): + ret, fp_ret = self.CS.update(self.cp, frogpilot_toggles) # wait for everything to init first if self.frame > int(5. / DT_CTRL): diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 8cd4942f..3d19f489 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -191,7 +191,7 @@ def get_car_interface(CP): return CarInterface(CP, CarController, CarState) -def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): +def get_car(logcan, sendcan, experimental_long_allowed, params, num_pandas=1): candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas) if candidate is None: @@ -205,7 +205,7 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): threading.Thread(target=sentry.capture_fingerprint, args=(candidate, params,)).start() CarInterface, _, _ = interfaces[candidate] - CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, docs=False) + CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, params, docs=False) CP.carVin = vin CP.carFw = car_fw CP.fingerprintSource = source diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 9f89f39e..c5e656e1 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -16,6 +16,8 @@ from openpilot.selfdrive.car.car_helpers import get_car, get_one_can from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.controls.lib.events import Events +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + REPLAY = "REPLAY" in os.environ EventName = car.CarEvent.EventName @@ -46,7 +48,7 @@ class Car: num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") - self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, self.params, num_pandas) else: self.CI, self.CP = CI, CI.CP @@ -82,12 +84,17 @@ class Car: # card is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) + # FrogPilot variables + self.frogpilot_toggles = FrogPilotVariables.toggles + + self.update_toggles = False + def state_update(self) -> car.CarState: """carState update loop, driven by can""" # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - CS, FPCS = self.CI.update(self.CC_prev, can_strs) + CS, FPCS = self.CI.update(self.CC_prev, can_strs, self.frogpilot_toggles) self.sm.update(0) @@ -158,7 +165,7 @@ class Car: if self.sm.all_alive(['carControl']): # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) - self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos) + self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos, self.frogpilot_toggles) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.CC_prev = CC @@ -183,6 +190,12 @@ class Car: self.step() self.rk.monitor_time() + # Update FrogPilot parameters + if FrogPilotVariables.toggles_updated: + self.update_toggles = True + elif self.update_toggles: + FrogPilotVariables.update_frogpilot_params() + self.update_toggles = False def main(): config_realtime_process(4, Priority.CTRL_HIGH) diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 85f53f68..124b92f5 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -20,7 +20,7 @@ class CarController(CarControllerBase): self.packer = CANPacker(dbc_name) self.params = CarControllerParams(CP) - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_toggles): can_sends = [] lkas_active = CC.latActive and self.lkas_control_bit_prev diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 7a94b0e6..05d6c780 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -24,7 +24,7 @@ class CarState(CarStateBase): self.prev_distance_button = 0 self.distance_button = 0 - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 1ffaa026..41c1ab4c 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -10,7 +10,7 @@ ButtonType = car.CarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "chrysler" ret.dashcamOnly = candidate in RAM_HD @@ -76,8 +76,8 @@ class CarInterface(CarInterfaceBase): return ret - def _update(self, c): - ret, fp_ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_toggles): + ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 7be3b2eb..ff13f45a 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -37,7 +37,7 @@ class CarController(CarControllerBase): self.steer_alert_last = False self.lead_distance_bars_last = None - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_toggles): can_sends = [] actuators = CC.actuators diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index bf22533b..ab79d091 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -22,7 +22,7 @@ class CarState(CarStateBase): self.prev_distance_button = 0 self.distance_button = 0 - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 8c22a6d8..a291f278 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -13,7 +13,7 @@ GearShifter = car.CarState.GearShifter class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "ford" ret.dashcamOnly = bool(ret.flags & FordFlags.CANFD) @@ -67,8 +67,8 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 return ret - def _update(self, c): - ret, fp_ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_toggles): + ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 9ece11b0..c323a278 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -60,7 +60,7 @@ class CarController(CarControllerBase): return pedal_gas - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators hud_control = CC.hudControl hud_alert = hud_control.visualAlert diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 481615a4..f2de55f5 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -33,7 +33,7 @@ class CarState(CarStateBase): self.single_pedal_mode = False self.pedal_steady = 0. - def update(self, pt_cp, cam_cp, loopback_cp): + def update(self, pt_cp, cam_cp, loopback_cp, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 4826a620..f166c033 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -93,7 +93,7 @@ class CarInterface(CarInterfaceBase): return self.torque_from_lateral_accel_linear @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False @@ -302,8 +302,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c): - ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback) + def _update(self, c, frogpilot_toggles): + ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback, frogpilot_toggles) # Don't add event if transitioning from INIT, unless it's to an actual button if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT: diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index fe023ea1..809b9d1c 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -125,7 +125,7 @@ class CarController(CarControllerBase): self.brake = 0.0 self.last_steer = 0.0 - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators hud_control = CC.hudControl conversion = hondacan.get_cruise_speed_conversion(self.CP.carFingerprint, CS.is_metric) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 563b9426..3e15682d 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -104,7 +104,7 @@ class CarState(CarStateBase): # However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX) self.dash_speed_seen = False - def update(self, cp, cp_cam, cp_body): + def update(self, cp, cp_cam, cp_body, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 96374b2c..5178b51c 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -32,7 +32,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "honda" CAN = CanBus(ret, fingerprint) @@ -222,8 +222,8 @@ class CarInterface(CarInterfaceBase): disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03') # returns a car.CarState - def _update(self, c): - ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) + def _update(self, c, frogpilot_toggles): + ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_toggles) ret.buttonEvents = [ *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT), diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 4038ddcc..22927e54 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -57,7 +57,7 @@ class CarController(CarControllerBase): self.car_fingerprint = CP.carFingerprint self.last_button_frame = 0 - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators hud_control = CC.hudControl diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 33a7e267..3c817db8 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -52,9 +52,9 @@ class CarState(CarStateBase): self.params = CarControllerParams(CP) - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_toggles): if self.CP.carFingerprint in CANFD_CAR: - return self.update_canfd(cp, cp_cam) + return self.update_canfd(cp, cp_cam, frogpilot_toggles) ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() @@ -168,7 +168,7 @@ class CarState(CarStateBase): return ret, fp_ret - def update_canfd(self, cp, cp_cam): + def update_canfd(self, cp, cp_cam, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index cb513cd7..7e4e4c79 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -19,7 +19,7 @@ BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: Bu class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "hyundai" ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None @@ -150,8 +150,8 @@ class CarInterface(CarInterfaceBase): if CP.flags & HyundaiFlags.ENABLE_BLINKERS: disable_ecu(logcan, sendcan, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01') - def _update(self, c): - ret, fp_ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_toggles): + ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) if self.CS.CP.openpilotLongitudinalControl: ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 9aab3d0b..b5ca203e 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -13,6 +13,7 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.conversions import Conversions as CV from openpilot.common.simple_kalman import KF1D, get_kalman_gain from openpilot.common.numpy_fast import clip +from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG from openpilot.selfdrive.car.values import PLATFORMS @@ -110,8 +111,12 @@ class CarInterfaceBase(ABC): dbc_name = "" if self.cp is None else self.cp.dbc_name self.CC: CarControllerBase = CarController(dbc_name, CP, self.VM) - def apply(self, c: car.CarControl, now_nanos: int) -> tuple[car.CarControl.Actuators, list[tuple[int, int, bytes, int]]]: - return self.CC.update(c, self.CS, now_nanos) + # FrogPilot variables + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + def apply(self, c: car.CarControl, now_nanos: int, frogpilot_toggles) -> tuple[car.CarControl.Actuators, list[tuple[int, int, bytes, int]]]: + return self.CC.update(c, self.CS, now_nanos, frogpilot_toggles) @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): @@ -125,7 +130,7 @@ class CarInterfaceBase(ABC): return cls.get_params(candidate, gen_empty_fingerprint(), list(), False, False) @classmethod - def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], experimental_long: bool, docs: bool): + def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], experimental_long: bool, params: Params, docs: bool): ret = CarInterfaceBase.get_std_params(candidate) platform = PLATFORMS[candidate] @@ -138,7 +143,7 @@ class CarInterfaceBase(ABC): ret.tireStiffnessFactor = platform.config.specs.tireStiffnessFactor ret.flags |= int(platform.config.flags) - ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs) + ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params) # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload if not ret.notCar: @@ -230,14 +235,14 @@ class CarInterfaceBase(ABC): def _update(self, c: car.CarControl) -> car.CarState: pass - def update(self, c: car.CarControl, can_strings: list[bytes]) -> car.CarState: + def update(self, c: car.CarControl, can_strings: list[bytes], frogpilot_toggles) -> car.CarState: # parse can for cp in self.can_parsers: if cp is not None: cp.update_strings(can_strings) # get CarState - ret, fp_ret = self._update(c) + ret, fp_ret = self._update(c, frogpilot_toggles) ret.canValid = all(cp.can_valid for cp in self.can_parsers if cp is not None) ret.canTimeout = any(cp.bus_timeout for cp in self.can_parsers if cp is not None) diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 3d416348..b38ea72e 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -16,7 +16,7 @@ class CarController(CarControllerBase): self.brake_counter = 0 self.frame = 0 - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_toggles): can_sends = [] apply_steer = 0 diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index c055152c..bf006550 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -21,7 +21,7 @@ class CarState(CarStateBase): self.prev_distance_button = 0 self.distance_button = 0 - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index cde0d796..8ea21c54 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -11,7 +11,7 @@ EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "mazda" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarUnavailable = True @@ -31,8 +31,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c): - ret, fp_ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_toggles): + ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) # TODO: add button types for inc and dec ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 77f77f2b..4bfe73ce 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -12,7 +12,7 @@ class CarInterface(CarInterfaceBase): self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']) @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "mock" ret.mass = 1700. ret.wheelbase = 2.70 @@ -21,7 +21,7 @@ class CarInterface(CarInterfaceBase): ret.dashcamOnly = True return ret - def _update(self, c): + def _update(self, c, frogpilot_toggles): self.sm.update(0) gps_sock = 'gpsLocationExternal' if self.sm.recv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index c7bd2313..2c3cd3fa 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -19,7 +19,7 @@ class CarController(CarControllerBase): self.packer = CANPacker(dbc_name) - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 67d0e664..5144765e 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -23,7 +23,7 @@ class CarState(CarStateBase): self.prev_distance_button = 0 self.distance_button = 0 - def update(self, cp, cp_adas, cp_cam): + def update(self, cp, cp_adas, cp_cam, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 86100eb3..0176c17c 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -10,7 +10,7 @@ ButtonType = car.CarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "nissan" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] ret.autoResumeSng = False @@ -29,8 +29,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c): - ret, fp_ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam) + def _update(self, c, frogpilot_toggles): + ret, fp_ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam, frogpilot_toggles) ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index d89ae8c6..6b935cbc 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -23,7 +23,7 @@ class CarController(CarControllerBase): self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index d969616c..fe438d17 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -16,7 +16,7 @@ class CarState(CarStateBase): self.angle_rate_calulator = CanSignalRateCalculator(50) - def update(self, cp, cp_cam, cp_body): + def update(self, cp, cp_cam, cp_body, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index f9bbb6c4..97ecd3be 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -9,7 +9,7 @@ from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFla class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "subaru" ret.radarUnavailable = True # for HYBRID CARS to be upstreamed, we need: @@ -97,9 +97,9 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c): + def _update(self, c, frogpilot_toggles): - ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) + ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_toggles) ret.events = self.create_common_events(ret).to_msg() diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index dcbea197..b138b7f8 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -3,6 +3,7 @@ from enum import Enum, IntFlag from cereal import car from panda.python import uds +from openpilot.common.params import Params from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Tool, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index e460111e..3ad189f3 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -15,7 +15,7 @@ class CarController(CarControllerBase): self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators pcm_cancel_cmd = CC.cruiseControl.cancel diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 26e36e5e..315dcd59 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -20,7 +20,7 @@ class CarState(CarStateBase): self.acc_state = 0 self.das_control_counters = deque(maxlen=32) - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 192a70b0..6ccd1f0b 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -8,7 +8,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "tesla" # There is no safe way to do steer blending with user torque, @@ -39,8 +39,8 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.25 return ret - def _update(self, c): - ret, fp_ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_toggles): + ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) ret.events = self.create_common_events(ret).to_msg() diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index f9b7a478..2b457b69 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,5 +1,6 @@ from cereal import car from openpilot.common.numpy_fast import clip +from openpilot.common.params import Params from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.toyota import toyotacan @@ -42,7 +43,10 @@ class CarController(CarControllerBase): self.gas = 0 self.accel = 0 - def update(self, CC, CS, now_nanos): + # FrogPilot variables + params = Params() + + def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel @@ -132,10 +136,10 @@ class CarController(CarControllerBase): can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) elif self.CP.openpilotLongitudinalControl: can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, - self.distance_button)) + self.distance_button, frogpilot_toggles)) self.accel = pcm_accel_cmd else: - can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button)) + can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button, frogpilot_toggles)) # *** 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 d474a67e..3478c0cf 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -49,7 +49,7 @@ class CarState(CarStateBase): self.acc_type = 1 self.lkas_hud = {} - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 4d98d0e4..30ca2f02 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -18,7 +18,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "toyota" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)] ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] @@ -162,8 +162,8 @@ class CarInterface(CarInterfaceBase): disable_ecu(logcan, sendcan, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control) # returns a car.CarState - def _update(self, c): - ret, fp_ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_toggles): + ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 1cc99b41..f84dd994 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -33,7 +33,7 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, return packer.make_can_msg("STEERING_LTA", 0, values) -def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance): +def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance, frogpilot_toggles): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": accel, diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index a4e0c894..045498fb 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -27,7 +27,7 @@ class CarController(CarControllerBase): self.hca_frame_timer_running = 0 self.hca_frame_same_torque = 0 - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators hud_control = CC.hudControl can_sends = [] diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 4a8603fc..a51a4d8f 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -32,9 +32,9 @@ class CarState(CarStateBase): return button_events - def update(self, pt_cp, cam_cp, ext_cp, trans_type): + def update(self, pt_cp, cam_cp, ext_cp, trans_type, frogpilot_toggles): if self.CP.flags & VolkswagenFlags.PQ: - return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type) + return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type, frogpilot_toggles) ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() @@ -156,7 +156,7 @@ class CarState(CarStateBase): self.frame += 1 return ret, fp_ret - def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): + def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type, frogpilot_toggles): ret = car.CarState.new_message() fp_ret = custom.FrogPilotCarState.new_message() # Update vehicle speed and acceleration from ABS wheel speeds. diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 11dd1364..a1082b74 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -20,7 +20,7 @@ class CarInterface(CarInterfaceBase): self.cp_ext = self.cp_cam @staticmethod - def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, docs, params): ret.carName = "volkswagen" ret.radarUnavailable = True @@ -101,8 +101,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c): - ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) + def _update(self, c, frogpilot_toggles): + ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType, frogpilot_toggles) events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], pcm_enable=not self.CS.CP.openpilotLongitudinalControl, @@ -128,4 +128,3 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() return ret, fp_ret - diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ca3a8ceb..e336e51f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -31,6 +31,8 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel from openpilot.system.hardware import HARDWARE +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 @@ -61,6 +63,7 @@ ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) class Controls: def __init__(self, CI=None): self.params = Params() + self.params_memory = Params("/dev/shm/params") if CI is None: cloudlog.info("controlsd is waiting for CarParams") @@ -174,6 +177,11 @@ class Controls: # controlsd is driven by carState, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) + # FrogPilot variables + self.frogpilot_toggles = FrogPilotVariables.toggles + + self.update_toggles = False + def set_initial_state(self): if REPLAY: controls_state = self.params.get("ReplayControlsState") @@ -512,7 +520,7 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode) + self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.frogpilot_toggles) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -837,6 +845,13 @@ class Controls: self.joystick_mode = self.params.get_bool("JoystickDebugMode") time.sleep(0.1) + # Update FrogPilot parameters + if FrogPilotVariables.toggles_updated: + self.update_toggles = True + elif self.update_toggles or self.sm.frame * DT_CTRL < 1: # Force updates at first to check the current state of "Always On Lateral" and holiday theme + FrogPilotVariables.update_frogpilot_params() + self.update_toggles = False + def controlsd_thread(self): e = threading.Event() t = threading.Thread(target=self.params_thread, args=(e, )) diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 5bb537ea..ae343a5a 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -40,7 +40,7 @@ class DesireHelper: self.prev_one_blinker = False self.desire = log.Desire.none - def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan): + def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan, frogpilot_toggles): v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index cfc6374a..ef1f2d43 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -3,6 +3,7 @@ import math from cereal import car, log from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp +from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL # WARNING: this value was determined based on the model's training distribution, @@ -45,6 +46,9 @@ class VCruiseHelper: self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} + # FrogPilot variables + self.params_memory = Params("/dev/shm/params") + @property def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET @@ -125,7 +129,7 @@ class VCruiseHelper: self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: + def initialize_v_cruise(self, CS, experimental_mode: bool, frogpilot_toggles) -> None: # initializing is handled by the PCM if self.CP.pcmCruise: return diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index db3ecb6d..8ba56d00 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -9,9 +9,13 @@ from cereal import log, car import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV from openpilot.common.git import get_short_branch +from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER +params = Params() +params_memory = Params("/dev/shm/params") + AlertSize = log.ControlsState.AlertSize AlertStatus = log.ControlsState.AlertStatus VisualAlert = car.CarControl.HUDControl.VisualAlert diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index d6a4901e..cadbf68b 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -331,7 +331,7 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, t_follow, personality=log.LongitudinalPersonality.standard): + def update(self, radarstate, v_cruise, x, v, a, j, t_follow, frogpilot_toggles, personality=log.LongitudinalPersonality.standard): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 146340d8..dfbf0e1a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -95,7 +95,7 @@ class LongitudinalPlanner: j = np.zeros(len(T_IDXS_MPC)) return x, v, a, j - def update(self, sm): + def update(self, sm, frogpilot_toggles): self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo @@ -138,7 +138,7 @@ class LongitudinalPlanner: self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) self.mpc.update(sm['radarState'], sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow, - personality=sm['controlsState'].personality) + frogpilot_toggles, personality=sm['controlsState'].personality) self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index b14410ab..88d09cc5 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -6,6 +6,8 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner import cereal.messaging as messaging +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + def publish_ui_plan(sm, pm, longitudinal_planner): ui_send = messaging.new_message('uiPlan') ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) @@ -31,13 +33,25 @@ def plannerd_thread(): sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'frogpilotCarControl', 'frogpilotCarState', 'frogpilotPlan'], poll='modelV2', ignore_avg_freq=['radarState']) + # FrogPilot variables + frogpilot_toggles = FrogPilotVariables.toggles + + update_toggles = False + while True: sm.update() if sm.updated['modelV2']: - longitudinal_planner.update(sm) + longitudinal_planner.update(sm, frogpilot_toggles) longitudinal_planner.publish(sm, pm) publish_ui_plan(sm, pm, longitudinal_planner) + # Update FrogPilot parameters + if FrogPilotVariables.toggles_updated: + update_toggles = True + elif update_toggles: + FrogPilotVariables.update_frogpilot_params() + update_toggles = False + def main(): plannerd_thread() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 7420f666..7426e473 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -13,6 +13,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.common.simple_kalman import KF1D +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables # Default lead acceleration decay set to 50% at 1s _LEAD_ACCEL_TAU = 1.5 @@ -208,6 +209,11 @@ class RadarD: self.ready = False + # FrogPilot variables + self.frogpilot_toggles = FrogPilotVariables.toggles + + self.update_toggles = False + def update(self, sm: messaging.SubMaster, rr): self.ready = sm.seen['modelV2'] self.current_time = 1e-9*max(sm.logMonoTime.values()) @@ -260,6 +266,13 @@ class RadarD: self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True) self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False) + # Update FrogPilot parameters + if FrogPilotVariables.toggles_updated: + self.update_toggles = True + elif self.update_toggles: + FrogPilotVariables.update_frogpilot_params() + self.update_toggles = False + def publish(self, pm: messaging.PubMaster, lag_ms: float): assert self.radar_state is not None diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 5e64c97a..61c95518 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -31,7 +31,7 @@ class FrogPilotPlanner: self.tracking_lead_mac = MovingAverageCalculator() - def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarState): + def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarState, frogpilot_toggles): self.lead_one = radarState.leadOne v_cruise = min(controlsState.vCruise, V_CRUISE_UNSET) * CV.KPH_TO_MS @@ -43,12 +43,12 @@ class FrogPilotPlanner: lead_distance = self.lead_one.dRel stopping_distance = STOP_DISTANCE - self.set_acceleration(controlsState, frogpilotCarState, v_cruise, v_ego) - self.set_follow_values(controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead) + self.set_acceleration(controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles) + self.set_follow_values(controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles) self.set_lead_status(lead_distance, stopping_distance, v_ego) - self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego) + self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles) - def set_acceleration(self, controlsState, frogpilotCarState, v_cruise, v_ego): + def set_acceleration(self, controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles): if controlsState.experimentalMode: self.max_accel = ACCEL_MAX else: @@ -59,12 +59,12 @@ class FrogPilotPlanner: else: self.min_accel = A_CRUISE_MIN - def set_follow_values(self, controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead): + def set_follow_values(self, controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles): self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor(controlsState.personality) self.t_follow = get_T_FOLLOW(controlsState.personality) if self.tracking_lead: - self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead) + self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles) else: self.acceleration_jerk = self.base_acceleration_jerk self.danger_jerk = self.base_danger_jerk @@ -77,9 +77,9 @@ class FrogPilotPlanner: self.tracking_lead_mac.add_data(following_lead) self.tracking_lead = self.tracking_lead_mac.get_moving_average() >= PROBABILITY - def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead): + def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles): - def update_v_cruise(self, carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego): + def update_v_cruise(self, carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles): v_cruise_cluster = max(controlsState.vCruiseCluster, v_cruise) * CV.KPH_TO_MS v_cruise_diff = v_cruise_cluster - v_cruise @@ -89,7 +89,7 @@ class FrogPilotPlanner: targets = [] self.v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets])) - def publish(self, sm, pm): + def publish(self, sm, pm, frogpilot_toggles): frogpilot_plan_send = messaging.new_message('frogpilotPlan') frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotPlan = frogpilot_plan_send.frogpilotPlan diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py index f24d6035..f5b097c8 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py @@ -1,2 +1,225 @@ +from types import SimpleNamespace + +from cereal import car +from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params +from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN +from openpilot.system.version import get_build_metadata + CITY_SPEED_LIMIT = 25 # 55mph is typically the minimum speed for highways CRUISING_SPEED = 5 # Roughly the speed cars go when not touching the gas while in drive + +class FrogPilotVariables: + def __init__(self): + self.frogpilot_toggles = SimpleNamespace() + + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.has_prime = self.params.get_int("PrimeType") > 0 + self.release = get_build_metadata().release_channel + + self.update_frogpilot_params(False) + + @property + def toggles(self): + return self.frogpilot_toggles + + @property + def toggles_updated(self): + return self.params_memory.get_bool("FrogPilotTogglesUpdated") + + def update_frogpilot_params(self, started=True): + toggle = self.frogpilot_toggles + + openpilot_installed = self.params.get_bool("HasAcceptedTerms") + + key = "CarParams" if started else "CarParamsPersistent" + msg_bytes = self.params.get(key, block=openpilot_installed and started) + + if msg_bytes: + with car.CarParams.from_bytes(msg_bytes) as CP: + always_on_lateral_set = self.params.get_bool("AlwaysOnLateralSet") + car_make = CP.carName + car_model = CP.carFingerprint + openpilot_longitudinal = CP.openpilotLongitudinalControl + pcm_cruise = CP.pcmCruise + else: + always_on_lateral_set = False + car_make = "mock" + car_model = "mock" + openpilot_longitudinal = False + pcm_cruise = False + + toggle.is_metric = self.params.get_bool("IsMetric") + distance_conversion = 1. if toggle.is_metric else CV.FOOT_TO_METER + speed_conversion = CV.KPH_TO_MS if toggle.is_metric else CV.MPH_TO_MS + + toggle.alert_volume_control = self.params.get_bool("AlertVolumeControl") + toggle.disengage_volume = self.params.get_int("DisengageVolume") if toggle.alert_volume_control else 100 + toggle.engage_volume = self.params.get_int("EngageVolume") if toggle.alert_volume_control else 100 + toggle.prompt_volume = self.params.get_int("PromptVolume") if toggle.alert_volume_control else 100 + toggle.promptDistracted_volume = self.params.get_int("PromptDistractedVolume") if toggle.alert_volume_control else 100 + toggle.refuse_volume = self.params.get_int("RefuseVolume") if toggle.alert_volume_control else 100 + toggle.warningSoft_volume = self.params.get_int("WarningSoftVolume") if toggle.alert_volume_control else 100 + toggle.warningImmediate_volume = self.params.get_int("WarningImmediateVolume") if toggle.alert_volume_control else 100 + + toggle.always_on_lateral = always_on_lateral_set and self.params.get_bool("AlwaysOnLateral") + toggle.always_on_lateral_lkas = toggle.always_on_lateral and self.params.get_bool("AlwaysOnLateralLKAS") + toggle.always_on_lateral_main = toggle.always_on_lateral and self.params.get_bool("AlwaysOnLateralMain") + toggle.always_on_lateral_pause_speed = self.params.get_int("PauseAOLOnBrake") if toggle.always_on_lateral else 0 + + toggle.automatic_updates = self.params.get_bool("AutomaticUpdates") + + toggle.cluster_offset = self.params.get_float("ClusterOffset") if car_make == "toyota" else 1 + + toggle.conditional_experimental_mode = openpilot_longitudinal and self.params.get_bool("ConditionalExperimental") + toggle.conditional_curves = toggle.conditional_experimental_mode and self.params.get_bool("CECurves") + toggle.conditional_curves_lead = toggle.conditional_curves and self.params.get_bool("CECurvesLead") + toggle.conditional_lead = toggle.conditional_experimental_mode and self.params.get_bool("CELead") + toggle.conditional_slower_lead = toggle.conditional_lead and self.params.get_bool("CESlowerLead") + toggle.conditional_stopped_lead = toggle.conditional_lead and self.params.get_bool("CEStoppedLead") + toggle.conditional_limit = self.params.get_int("CESpeed") * speed_conversion if toggle.conditional_experimental_mode else 0 + toggle.conditional_limit_lead = self.params.get_int("CESpeedLead") * speed_conversion if toggle.conditional_experimental_mode else 0 + toggle.conditional_navigation = toggle.conditional_experimental_mode and self.params.get_bool("CENavigation") + toggle.conditional_navigation_intersections = toggle.conditional_navigation and self.params.get_bool("CENavigationIntersections") + toggle.conditional_navigation_lead = toggle.conditional_navigation and self.params.get_bool("CENavigationLead") + toggle.conditional_navigation_turns = toggle.conditional_navigation and self.params.get_bool("CENavigationTurns") + toggle.conditional_signal = toggle.conditional_experimental_mode and self.params.get_bool("CESignal") + toggle.conditional_stop_lights = toggle.conditional_experimental_mode and self.params.get_bool("CEStopLights") + toggle.less_sensitive_lights = toggle.conditional_stop_lights and self.params.get_bool("CEStopLightsLessSensitive") + + custom_alerts = self.params.get_bool("CustomAlerts") + toggle.green_light_alert = custom_alerts and self.params.get_bool("GreenLightAlert") + toggle.lead_departing_alert = custom_alerts and self.params.get_bool("LeadDepartingAlert") + toggle.loud_blindspot_alert = custom_alerts and self.params.get_bool("LoudBlindspotAlert") + + custom_themes = self.params.get_bool("CustomTheme") + holiday_themes = custom_themes and self.params.get_bool("HolidayThemes") + toggle.current_holiday_theme = self.params_memory.get_int("CurrentHolidayTheme") if holiday_themes else 0 + toggle.custom_sounds = self.params.get_int("CustomSounds") if custom_themes else 0 + toggle.goat_scream = toggle.current_holiday_theme == 0 and toggle.custom_sounds == 1 and self.params.get_bool("GoatScream") + toggle.random_events = custom_themes and self.params.get_bool("RandomEvents") + + custom_ui = self.params.get_bool("CustomUI") + custom_paths = custom_ui and self.params.get_bool("CustomPaths") + toggle.adjacent_lanes = custom_paths and self.params.get_bool("AdjacentPath") + toggle.blind_spot_path = custom_paths and self.params.get_bool("BlindSpotPath") + toggle.show_stopping_point = custom_ui and self.params.get_bool("ShowStoppingPoint") + + toggle.device_management = self.params.get_bool("DeviceManagement") + device_shutdown_setting = self.params.get_int("DeviceShutdown") if toggle.device_management else 33 + toggle.device_shutdown_time = (device_shutdown_setting - 3) * 3600 if device_shutdown_setting >= 4 else device_shutdown_setting * (60 * 15) + toggle.increase_thermal_limits = toggle.device_management and self.params.get_bool("IncreaseThermalLimits") + toggle.low_voltage_shutdown = self.params.get_float("LowVoltageShutdown") if toggle.device_management else 11.8 + toggle.offline_mode = toggle.device_management and self.params.get_bool("OfflineMode") + + driving_personalities = openpilot_longitudinal and self.params.get_bool("DrivingPersonalities") + toggle.custom_personalities = driving_personalities and self.params.get_bool("CustomPersonalities") + aggressive_profile = toggle.custom_personalities and self.params.get_bool("AggressivePersonalityProfile") + toggle.aggressive_jerk_acceleration = self.params.get_int("AggressiveJerkAcceleration") / 100. if aggressive_profile else 0.5 + toggle.aggressive_jerk_speed = self.params.get_int("AggressiveJerkSpeed") / 100. if aggressive_profile else 0.5 + toggle.aggressive_jerk_danger = self.params.get_int("AggressiveJerkDanger") / 100. if aggressive_profile else 0.5 + toggle.aggressive_follow = self.params.get_float("AggressiveFollow") if aggressive_profile else 1.25 + standard_profile = toggle.custom_personalities and self.params.get_bool("StandardPersonalityProfile") + toggle.standard_jerk_acceleration = self.params.get_int("StandardJerkAcceleration") / 100. if standard_profile else 1.0 + toggle.standard_jerk_danger = self.params.get_int("StandardJerkDanger") / 100. if standard_profile else 0.5 + toggle.standard_jerk_speed = self.params.get_int("StandardJerkSpeed") / 100. if standard_profile else 1.0 + toggle.standard_follow = self.params.get_float("StandardFollow") if standard_profile else 1.45 + relaxed_profile = toggle.custom_personalities and self.params.get_bool("RelaxedPersonalityProfile") + toggle.relaxed_jerk_acceleration = self.params.get_int("RelaxedJerkAcceleration") / 100. if relaxed_profile else 1.0 + toggle.relaxed_jerk_danger = self.params.get_int("RelaxedJerkDanger") / 100. if relaxed_profile else 0.5 + toggle.relaxed_jerk_speed = self.params.get_int("RelaxedJerkSpeed") / 100. if relaxed_profile else 1.0 + toggle.relaxed_follow = self.params.get_float("RelaxedFollow") if relaxed_profile else 1.75 + traffic_profile = toggle.custom_personalities and self.params.get_bool("TrafficPersonalityProfile") + toggle.traffic_mode_jerk_acceleration = [self.params.get_int("TrafficJerkAcceleration") / 100., toggle.aggressive_jerk_acceleration] if traffic_profile else [0.5, 0.5] + toggle.traffic_mode_jerk_danger = [self.params.get_int("TrafficJerkDanger") / 100., toggle.aggressive_jerk_danger] if traffic_profile else [1.0, 1.0] + toggle.traffic_mode_jerk_speed = [self.params.get_int("TrafficJerkSpeed") / 100., toggle.aggressive_jerk_speed] if traffic_profile else [0.5, 0.5] + toggle.traffic_mode_t_follow = [self.params.get_float("TrafficFollow"), toggle.aggressive_follow] if traffic_profile else [0.5, 1.0] + + toggle.experimental_mode_via_press = openpilot_longitudinal and self.params.get_bool("ExperimentalModeActivation") + toggle.experimental_mode_via_distance = toggle.experimental_mode_via_press and self.params.get_bool("ExperimentalModeViaDistance") + toggle.experimental_mode_via_lkas = not toggle.always_on_lateral_lkas and toggle.experimental_mode_via_press and self.params.get_bool("ExperimentalModeViaLKAS") + + lane_change_customizations = self.params.get_bool("LaneChangeCustomizations") + toggle.lane_change_delay = self.params.get_int("LaneChangeTime") if lane_change_customizations else 0 + toggle.lane_detection_width = self.params.get_int("LaneDetectionWidth") * distance_conversion / 10. if lane_change_customizations else 0 + toggle.lane_detection = toggle.lane_detection_width != 0 + toggle.minimum_lane_change_speed = self.params.get_int("MinimumLaneChangeSpeed") * speed_conversion if lane_change_customizations else LANE_CHANGE_SPEED_MIN + toggle.nudgeless = lane_change_customizations and self.params.get_bool("NudgelessLaneChange") + toggle.one_lane_change = lane_change_customizations and self.params.get_bool("OneLaneChange") + + lateral_tune = self.params.get_bool("LateralTune") + toggle.force_auto_tune = lateral_tune and self.params.get_bool("ForceAutoTune") + stock_steer_ratio = self.params.get_float("SteerRatioStock") + toggle.steer_ratio = self.params.get_float("SteerRatio") if lateral_tune else stock_steer_ratio + toggle.use_custom_steer_ratio = toggle.steer_ratio != stock_steer_ratio + toggle.taco_tune = lateral_tune and self.params.get_bool("TacoTune") + toggle.turn_desires = lateral_tune and self.params.get_bool("TurnDesires") + + toggle.long_pitch = openpilot_longitudinal and car_make == "gm" and self.params.get_bool("LongPitch") + toggle.volt_sng = car_model == "CHEVROLET_VOLT" and self.params.get_bool("VoltSNG") + + longitudinal_tune = openpilot_longitudinal and self.params.get_bool("LongitudinalTune") + toggle.acceleration_profile = self.params.get_int("AccelerationProfile") if longitudinal_tune else 0 + toggle.aggressive_acceleration = longitudinal_tune and self.params.get_bool("AggressiveAcceleration") + toggle.deceleration_profile = self.params.get_int("DecelerationProfile") if longitudinal_tune else 0 + toggle.increased_stopping_distance = self.params.get_int("StoppingDistance") * distance_conversion if longitudinal_tune else 0 + toggle.lead_detection_threshold = self.params.get_int("LeadDetectionThreshold") / 100. if longitudinal_tune else 0.5 + toggle.smoother_braking = longitudinal_tune and self.params.get_bool("SmoothBraking") + toggle.sport_plus = longitudinal_tune and toggle.acceleration_profile == 3 + toggle.traffic_mode = longitudinal_tune and self.params.get_bool("TrafficMode") + + toggle.map_turn_speed_controller = openpilot_longitudinal and self.params.get_bool("MTSCEnabled") + toggle.mtsc_curvature_check = toggle.map_turn_speed_controller and self.params.get_bool("MTSCCurvatureCheck") + self.params_memory.put_float("MapTargetLatA", 2 * (self.params.get_int("MTSCAggressiveness") / 100.)) + + quality_of_life_controls = self.params.get_bool("QOLControls") + toggle.custom_cruise_increase = self.params.get_int("CustomCruise") if quality_of_life_controls and not pcm_cruise else 1 + toggle.custom_cruise_increase_long = self.params.get_int("CustomCruiseLong") if quality_of_life_controls and not pcm_cruise else 5 + toggle.force_standstill = quality_of_life_controls and self.params.get_bool("ForceStandstill") + toggle.force_stops = toggle.force_standstill and self.params.get_bool("ForceStops") + map_gears = quality_of_life_controls and self.params.get_bool("MapGears") + toggle.map_acceleration = map_gears and self.params.get_bool("MapAcceleration") + toggle.map_deceleration = map_gears and self.params.get_bool("MapDeceleration") + toggle.pause_lateral_below_speed = self.params.get_int("PauseLateralSpeed") * speed_conversion if quality_of_life_controls else 0 + toggle.pause_lateral_below_signal = toggle.pause_lateral_below_speed != 0 and self.params.get_bool("PauseLateralOnSignal") + toggle.reverse_cruise_increase = quality_of_life_controls and pcm_cruise and self.params.get_bool("ReverseCruise") + toggle.set_speed_offset = self.params.get_int("SetSpeedOffset") * (1. if toggle.is_metric else CV.MPH_TO_KPH) if quality_of_life_controls and not pcm_cruise else 0 + + toggle.sng_hack = openpilot_longitudinal and car_make == "toyota" and self.params.get_bool("SNGHack") + + toggle.speed_limit_controller = openpilot_longitudinal and self.params.get_bool("SpeedLimitController") + toggle.force_mph_dashboard = toggle.speed_limit_controller and self.params.get_bool("ForceMPHDashboard") + toggle.map_speed_lookahead_higher = self.params.get_int("SLCLookaheadHigher") if toggle.speed_limit_controller else 0 + toggle.map_speed_lookahead_lower = self.params.get_int("SLCLookaheadLower") if toggle.speed_limit_controller else 0 + toggle.offset1 = self.params.get_int("Offset1") * speed_conversion if toggle.speed_limit_controller else 0 + toggle.offset2 = self.params.get_int("Offset2") * speed_conversion if toggle.speed_limit_controller else 0 + toggle.offset3 = self.params.get_int("Offset3") * speed_conversion if toggle.speed_limit_controller else 0 + toggle.offset4 = self.params.get_int("Offset4") * speed_conversion if toggle.speed_limit_controller else 0 + toggle.set_speed_limit = toggle.speed_limit_controller and self.params.get_bool("SetSpeedLimit") + toggle.speed_limit_alert = toggle.speed_limit_controller and self.params.get_bool("SpeedLimitChangedAlert") + toggle.speed_limit_confirmation = toggle.speed_limit_controller and self.params.get_bool("SLCConfirmation") + toggle.speed_limit_confirmation_higher = toggle.speed_limit_confirmation and self.params.get_bool("SLCConfirmationHigher") + toggle.speed_limit_confirmation_lower = toggle.speed_limit_confirmation and self.params.get_bool("SLCConfirmationLower") + speed_limit_controller_override = self.params.get_int("SLCOverride") if toggle.speed_limit_controller else 0 + toggle.speed_limit_controller_override_manual = speed_limit_controller_override == 1 + toggle.speed_limit_controller_override_set_speed = speed_limit_controller_override == 2 + toggle.use_set_speed = toggle.speed_limit_controller and self.params.get_int("SLCFallback") == 0 + toggle.use_experimental_mode = toggle.speed_limit_controller and self.params.get_int("SLCFallback") == 1 + toggle.use_previous_limit = toggle.speed_limit_controller and self.params.get_int("SLCFallback") == 2 + toggle.speed_limit_priority1 = self.params.get("SLCPriority1", encoding='utf-8') if toggle.speed_limit_controller else None + toggle.speed_limit_priority2 = self.params.get("SLCPriority2", encoding='utf-8') if toggle.speed_limit_controller else None + toggle.speed_limit_priority3 = self.params.get("SLCPriority3", encoding='utf-8') if toggle.speed_limit_controller else None + toggle.speed_limit_priority_highest = toggle.speed_limit_priority1 == "Highest" + toggle.speed_limit_priority_lowest = toggle.speed_limit_priority1 == "Lowest" + + toyota_doors = car_make == "toyota" and self.params.get_bool("ToyotaDoors") + toggle.lock_doors = toyota_doors and self.params.get_bool("LockDoors") + toggle.unlock_doors = toyota_doors and self.params.get_bool("UnlockDoors") + + toggle.vision_turn_controller = openpilot_longitudinal and self.params.get_bool("VisionTurnControl") + toggle.curve_sensitivity = self.params.get_int("CurveSensitivity") / 100. if toggle.vision_turn_controller else 1 + toggle.turn_aggressiveness = self.params.get_int("TurnAggressiveness") / 100. if toggle.vision_turn_controller else 1 + +FrogPilotVariables = FrogPilotVariables() diff --git a/selfdrive/frogpilot/frogpilot_process.py b/selfdrive/frogpilot/frogpilot_process.py index 91b91f8d..18852688 100644 --- a/selfdrive/frogpilot/frogpilot_process.py +++ b/selfdrive/frogpilot/frogpilot_process.py @@ -8,11 +8,13 @@ from openpilot.common.time import system_time_valid from openpilot.selfdrive.frogpilot.controls.frogpilot_planner import FrogPilotPlanner from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import is_url_pingable +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables OFFLINE = log.DeviceState.NetworkType.none locks = { "time_checks": threading.Lock(), + "update_frogpilot_params": threading.Lock(), } running_threads = {} @@ -37,6 +39,8 @@ def time_checks(deviceState, now, started, params, params_memory): def frogpilot_thread(): config_realtime_process(5, Priority.CTRL_LOW) + frogpilot_toggles = FrogPilotVariables.toggles + params = Params() params_memory = Params("/dev/shm/params") @@ -45,6 +49,7 @@ def frogpilot_thread(): run_time_checks = False started_previously = False time_validated = system_time_valid() + update_toggles = False pm = messaging.PubMaster(['frogpilotPlan']) sm = messaging.SubMaster(['carState', 'controlsState', 'deviceState', 'frogpilotCarControl', @@ -66,6 +71,13 @@ def frogpilot_thread(): sm['frogpilotNavigation'], sm['modelV2'], sm['radarState'], frogpilot_toggles) frogpilot_planner.publish(sm, pm, frogpilot_toggles) + if FrogPilotVariables.toggles_updated: + update_toggles = True + elif update_toggles: + run_thread_with_lock("update_frogpilot_params", locks["update_frogpilot_params"], FrogPilotVariables.update_frogpilot_params, (started,)) + + update_toggles = False + started_previously = started if now.second == 0: diff --git a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc new file mode 100644 index 00000000..ee2bceed --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc @@ -0,0 +1,16 @@ +#include "selfdrive/ui/ui.h" + +Params paramsMemory{"/dev/shm/params"}; + +std::atomic callCounter(0); + +void updateFrogPilotToggles() { + int currentCall = ++callCounter; + std::thread([currentCall]() { + paramsMemory.putBool("FrogPilotTogglesUpdated", true); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (currentCall == callCounter) { + paramsMemory.putBool("FrogPilotTogglesUpdated", false); + } + }).detach(); +} diff --git a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h new file mode 100644 index 00000000..33645515 --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h @@ -0,0 +1,3 @@ +#pragma once + +void updateFrogPilotToggles(); diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 6e154bf0..aeeddd33 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -20,6 +20,8 @@ from openpilot.common.realtime import set_realtime_priority from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS MAX_VEL_ANGLE_STD = np.radians(0.25) MAX_YAW_RATE_FILTER = np.radians(2) # per second @@ -59,6 +61,11 @@ def moving_avg_with_linear_decay(prev_mean: np.ndarray, new_val: np.ndarray, idx class Calibrator: def __init__(self, param_put: bool = False): + # FrogPilot variables + self.frogpilot_toggles = FrogPilotVariables.toggles + + self.update_toggles = False + self.param_put = param_put self.not_car = False @@ -166,6 +173,13 @@ class Calibrator: if self.param_put and write_this_cycle: self.params.put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes()) + # Update FrogPilot parameters + if FrogPilotVariables.toggles_updated: + self.update_toggles = True + elif self.update_toggles: + FrogPilotVariables.update_frogpilot_params() + self.update_toggles = False + def handle_v_ego(self, v_ego: float) -> None: self.v_ego = v_ego diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index d124eb5f..dc3f91a5 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -127,6 +127,7 @@ def main(): sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll='liveLocationKalman') params_reader = Params() + params_memory = Params("/dev/shm/params") # wait for stats about the car to come in from controls cloudlog.info("paramsd is waiting for CarParams") with car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) as msg: diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 06f90447..6c71a686 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -11,6 +11,8 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + HISTORY = 5 # secs POINTS_PER_BUCKET = 1500 MIN_POINTS_TOTAL = 4000 @@ -51,6 +53,11 @@ class TorqueBuckets(PointBuckets): class TorqueEstimator(ParameterEstimator): def __init__(self, CP, decimated=False): + # FrogPilot variables + self.frogpilot_toggles = FrogPilotVariables.toggles + + self.update_toggles = False + self.hist_len = int(HISTORY / DT_MDL) self.lag = CP.steerActuatorDelay + .2 # from controlsd if decimated: @@ -156,6 +163,13 @@ class TorqueEstimator(ParameterEstimator): self.filtered_params[param].update(value) self.filtered_params[param].update_alpha(self.decay) + # Update FrogPilot parameters + if FrogPilotVariables.toggles_updated: + self.update_toggles = True + elif self.update_toggles: + FrogPilotVariables.update_frogpilot_params() + self.update_toggles = False + def handle_log(self, t, which, msg): if which == "carControl": self.raw_points["carControl_t"].append(t + self.lag) @@ -226,6 +240,9 @@ def main(demo=False): with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP: estimator = TorqueEstimator(CP) + # FrogPilot variables + frogpilot_toggles = FrogPilotVariables.toggles + while True: sm.update() if sm.all_checks(): diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 3f94ceb9..7163e764 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -24,6 +24,10 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + +frogpilot_toggles = FrogPilotVariables.toggles + PROCESS_NAME = "selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -181,6 +185,9 @@ def main(demo=False): DH = DesireHelper() + # FrogPilot variables + update_toggles = False + while True: # Keep receiving frames until we are at least 1 frame ahead of previous extra frame while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: @@ -267,7 +274,7 @@ def main(demo=False): l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] lane_change_prob = l_lane_change_prob + r_lane_change_prob - DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['frogpilotPlan']) + DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['frogpilotPlan'], frogpilot_toggles) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction @@ -277,6 +284,12 @@ def main(demo=False): last_vipc_frame_id = meta_main.frame_id + # Update FrogPilot parameters + if FrogPilotVariables.toggles_updated: + update_toggles = True + elif update_toggles: + FrogPilotVariables.update_frogpilot_params() + update_toggles = False if __name__ == "__main__": try: diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index a1c770e6..f15937a2 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -17,6 +17,8 @@ from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param, parse_banner_instructions) from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + REROUTE_DISTANCE = 25 MANEUVER_TRANSITION_THRESHOLD = 10 REROUTE_COUNTER_MIN = 3 @@ -58,6 +60,11 @@ class RouteEngine: self.api = Api(self.params.get("DongleId", encoding='utf8')) self.mapbox_host = "https://maps.comma.ai" + # FrogPilot variables + self.frogpilot_toggles = FrogPilotVariables.toggles + + self.update_toggles = False + def update(self): self.sm.update(0) @@ -76,6 +83,13 @@ class RouteEngine: except Exception: cloudlog.exception("navd.failed_to_compute") + # Update FrogPilot parameters + if FrogPilotVariables.toggles_updated: + self.update_toggles = True + elif self.update_toggles: + FrogPilotVariables.update_frogpilot_params() + self.update_toggles = False + def update_location(self): location = self.sm['liveLocationKalman'] self.gps_ok = location.gpsOK diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h index f60b80b2..7a3f4b4b 100644 --- a/selfdrive/ui/qt/home.h +++ b/selfdrive/ui/qt/home.h @@ -69,6 +69,9 @@ private: DriverViewWindow *driver_view; QStackedLayout *slayout; + // FrogPilot variables + Params params; + private slots: void updateState(const UIState &s); }; diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index 31a44f27..88220d0b 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -74,6 +74,9 @@ private: void updateDestinationMarker(); uint64_t route_rcv_frame = 0; + // FrogPilot variables + Params params; + private slots: void updateState(const UIState &s); diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index 35a044bd..8b1ae8ce 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -98,4 +98,7 @@ private: Params params; ParamWatcher *fs_watch; + + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; }; diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h index f627aac8..e9fec784 100644 --- a/selfdrive/ui/qt/sidebar.h +++ b/selfdrive/ui/qt/sidebar.h @@ -59,4 +59,7 @@ protected: private: std::unique_ptr pm; + + // FrogPilot variables + Params params; }; diff --git a/selfdrive/ui/qt/widgets/wifi.h b/selfdrive/ui/qt/widgets/wifi.h index 60c865f2..30278c1b 100644 --- a/selfdrive/ui/qt/widgets/wifi.h +++ b/selfdrive/ui/qt/widgets/wifi.h @@ -18,6 +18,10 @@ signals: public slots: void updateState(const UIState &s); +private: + // FrogPilot variables + Params params; + protected: QStackedLayout *stack; }; diff --git a/selfdrive/ui/qt/window.h b/selfdrive/ui/qt/window.h index 05b61e1f..f1389c29 100644 --- a/selfdrive/ui/qt/window.h +++ b/selfdrive/ui/qt/window.h @@ -22,4 +22,7 @@ private: HomeWindow *homeWindow; SettingsWindow *settingsWindow; OnboardingWindow *onboardingWindow; + + // FrogPilot variables + Params params; }; diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 0550a7db..450bd8a7 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -13,6 +13,8 @@ from openpilot.common.swaglog import cloudlog from openpilot.system import micd +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + SAMPLE_RATE = 48000 SAMPLE_BUFFER = 4096 # (approx 100ms) MAX_VOLUME = 1.0 @@ -62,6 +64,13 @@ class Soundd: self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) + # FrogPilot variables + self.frogpilot_toggles = FrogPilotVariables.toggles + + self.update_toggles = False + + self.update_frogpilot_sounds() + def load_sounds(self): self.loaded_sounds: dict[int, np.ndarray] = {} @@ -155,6 +164,15 @@ class Soundd: assert stream.active + # Update FrogPilot parameters + if FrogPilotVariables.toggles_updated: + self.update_toggles = True + elif self.update_toggles: + FrogPilotVariables.update_frogpilot_params() + self.update_frogpilot_sounds() + self.update_toggles = False + + def update_frogpilot_sounds(self): def main(): s = Soundd() diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index df41c239..f37ee0b0 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -252,6 +252,20 @@ void ui_update_params(UIState *s) { auto params = Params(); s->scene.is_metric = params.getBool("IsMetric"); s->scene.map_on_left = params.getBool("NavSettingLeftSide"); + + ui_update_frogpilot_params(s, params); +} + +void ui_update_frogpilot_params(UIState *s, Params ¶ms) { + UIScene &scene = s->scene; + + auto 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(); + scene.longitudinal_control = hasLongitudinalControl(CP); + } } void UIState::updateStatus() { @@ -297,6 +311,9 @@ UIState::UIState(QObject *parent) : QObject(parent) { timer = new QTimer(this); QObject::connect(timer, &QTimer::timeout, this, &UIState::update); timer->start(1000 / UI_FREQ); + + // FrogPilot variables + ui_update_params(this); } void UIState::update() { @@ -308,6 +325,18 @@ void UIState::update() { watchdog_kick(nanos_since_boot()); } emit uiUpdate(*this); + + // Update FrogPilot parameters + static bool update_toggles = false; + + if (paramsMemory.getBool("FrogPilotTogglesUpdated")) { + update_toggles = true; + } else if (update_toggles) { + ui_update_params(this); + update_toggles = false; + } + + // FrogPilot variables that need to be constantly updated } void UIState::setPrimeType(PrimeType type) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 647ec909..38c1ff02 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -14,8 +14,11 @@ #include "common/mat.h" #include "common/params.h" #include "common/timing.h" +#include "selfdrive/ui/qt/util.h" #include "system/hardware/hw.h" +#include "selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h" + const int UI_BORDER_SIZE = 30; const int UI_HEADER_HEIGHT = 420; @@ -158,6 +161,9 @@ private: QTimer *timer; bool started_prev = false; PrimeType prime_type = PrimeType::UNKNOWN; + + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; }; UIState *uiState(); @@ -207,3 +213,6 @@ void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &drivers void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line); void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line, float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert); + +// FrogPilot functions +void ui_update_frogpilot_params(UIState *s, Params ¶ms); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 096e288c..ab7548f7 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -958,6 +958,9 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { } void cameras_run(MultiCameraState *s) { + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; + LOG("-- Starting threads"); std::vector threads; if (s->driver_cam.enabled) threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera)); diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index 83885f29..574a872f 100755 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -25,6 +25,8 @@ from openpilot.system.hardware.power_monitoring import PowerMonitoring from openpilot.system.hardware.fan_controller import TiciFanController from openpilot.system.version import terms_version, training_version +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + ThermalStatus = log.DeviceState.ThermalStatus NetworkType = log.DeviceState.NetworkType NetworkStrength = log.DeviceState.NetworkStrength @@ -204,6 +206,11 @@ def hardware_thread(end_event, hw_queue) -> None: fan_controller = None + # FrogPilot variables + frogpilot_toggles = FrogPilotVariables.toggles + + update_toggles = False + while not end_event.is_set(): sm.update(PANDA_STATES_TIMEOUT) @@ -387,7 +394,7 @@ def hardware_thread(end_event, hw_queue) -> None: msg.deviceState.somPowerDrawW = som_power_draw # Check if we need to shut down - if power_monitor.should_shutdown(onroad_conditions["ignition"], in_car, off_ts, started_seen): + if power_monitor.should_shutdown(onroad_conditions["ignition"], in_car, off_ts, started_seen, frogpilot_toggles): cloudlog.warning(f"shutting device down, offroad since {off_ts}") params.put_bool("DoShutdown", True) @@ -449,6 +456,12 @@ def hardware_thread(end_event, hw_queue) -> None: count += 1 should_start_prev = should_start + # Update FrogPilot parameters + if FrogPilotVariables.toggles_updated: + update_toggles = True + elif update_toggles: + FrogPilotVariables.update_frogpilot_params(started_ts is not None) + update_toggles = False def main(): hw_queue = queue.Queue(maxsize=1) diff --git a/system/hardware/power_monitoring.py b/system/hardware/power_monitoring.py index 5a94625b..3b82848c 100644 --- a/system/hardware/power_monitoring.py +++ b/system/hardware/power_monitoring.py @@ -107,7 +107,7 @@ class PowerMonitoring: return int(self.car_battery_capacity_uWh) # See if we need to shutdown - def should_shutdown(self, ignition: bool, in_car: bool, offroad_timestamp: float | None, started_seen: bool): + def should_shutdown(self, ignition: bool, in_car: bool, offroad_timestamp: float | None, started_seen: bool, frogpilot_toggles): if offroad_timestamp is None: return False diff --git a/system/manager/manager.py b/system/manager/manager.py index 46379e35..09f88b82 100644 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -426,6 +426,7 @@ def manager_thread() -> None: cloudlog.info({"environ": os.environ}) params = Params() + params_memory = Params("/dev/shm/params") ignore: list[str] = [] if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID): @@ -451,6 +452,7 @@ def manager_thread() -> None: params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) elif not started and started_prev: params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) + params_memory.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) # update onroad params, which drives pandad's safety setter thread if started != started_prev: diff --git a/system/updated/updated.py b/system/updated/updated.py index c4dc99f1..1c701fd9 100755 --- a/system/updated/updated.py +++ b/system/updated/updated.py @@ -22,6 +22,8 @@ from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert from openpilot.system.hardware import AGNOS, HARDWARE from openpilot.system.version import get_build_metadata +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + LOCK_FILE = os.getenv("UPDATER_LOCK_FILE", "/tmp/safe_staging_overlay.lock") STAGING_ROOT = os.getenv("UPDATER_STAGING_ROOT", "/data/safe_staging") @@ -411,6 +413,7 @@ class Updater: def main() -> None: params = Params() + params_memory = Params("/dev/shm/params") if params.get_bool("DisableUpdates"): cloudlog.warning("updates are disabled by the DisableUpdates param")