From 178129602d5abbc563f3bee51c5b6d336907d943 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 19 Jun 2024 19:27:15 -0700 Subject: [PATCH] FrogPilot setup - Setup toggles --- common/params.cc | 285 +++++++++++++++++- 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 | 18 +- selfdrive/controls/radard.py | 13 + .../frogpilot/controls/frogpilot_planner.py | 22 +- .../controls/lib/frogpilot_variables.py | 227 ++++++++++++++ selfdrive/frogpilot/frogpilot_process.py | 17 +- .../ui/qt/widgets/frogpilot_controls.cc | 11 + .../ui/qt/widgets/frogpilot_controls.h | 3 + selfdrive/locationd/paramsd.py | 1 + selfdrive/modeld/modeld.py | 17 +- 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/onroad/annotated_camera.cc | 1 + 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 | 28 ++ selfdrive/ui/ui.h | 8 + system/camerad/cameras/camera_qcom2.cc | 3 + system/hardware/hardwared.py | 17 +- system/hardware/power_monitoring.py | 2 +- system/manager/manager.py | 2 + system/updated/updated.py | 3 + 76 files changed, 848 insertions(+), 117 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 233016017..8108dbbe7 100644 --- a/common/params.cc +++ b/common/params.cc @@ -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,281 @@ 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}, + {"AlwaysOnLateralMain", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AlwaysOnLateralLKAS", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"AlwaysOnLateralSet", CLEAR_ON_ONROAD_TRANSITION}, + {"AMapKey1", PERSISTENT}, + {"AMapKey2", PERSISTENT}, + {"ApiCache_DriveStats", PERSISTENT}, + {"AutomaticUpdates", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER}, + {"AvailableModels", PERSISTENT}, + {"AvailableModelsNames", PERSISTENT}, + {"BigMap", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"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}, + {"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}, + {"CESignal", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CESlowerLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CEStoppedLead", 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}, + {"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}, + {"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}, + {"DragonPilotTune", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"DriverCamera", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"DrivingPersonalities", 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}, + {"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}, + {"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}, + {"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}, + {"ModelName", 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}, + {"NextMapSpeedLimit", PERSISTENT}, + {"NNFF", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NNFFLite", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"NNFFModelName", PERSISTENT}, + {"NoLogging", 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}, + {"RandomEvents", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"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}, + {"ResetSteerRatio", PERSISTENT}, + {"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}, + {"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}, + {"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}, + {"SLCPriority", 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}, + {"WheelIcon", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, + {"WheelSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, }; } // namespace diff --git a/common/params.h b/common/params.h index 9c8af82c5..c52551b9f 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_VEHICLES = 0x100, + FROGPILOT_VISUALS = 0x200, + FROGPILOT_OTHER = 0x400, + FROGPILOT_STORAGE = 0x800, + FROGPILOT_TRACKING = 0x1000, ALL = 0xFFFFFFFF }; diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index ddce5103b..b94441399 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_VEHICLES + FROGPILOT_VISUALS + FROGPILOT_OTHER + FROGPILOT_STORAGE + FROGPILOT_TRACKING 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 259126c41..ba73044d9 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 e184276a2..08ee92299 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 87da16a26..8d14bd6af 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, params, candidate, fingerprint, car_fw, experimental_long, docs): 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 5433606f0..0cadf124f 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -195,7 +195,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(params, logcan, sendcan, experimental_long_allowed, num_pandas=1): candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas) if candidate is None: @@ -211,7 +211,7 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): fingerprint_log.start() CarInterface, _, _ = interfaces[candidate] - CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, docs=False) + CP = CarInterface.get_params(params, candidate, fingerprints, car_fw, experimental_long_allowed, 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 9f89f39e4..eb0af0ebd 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.params, self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, 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 85f53f68e..124b92f50 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 7a94b0e6e..05d6c7802 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 1ffaa026f..94065b2ae 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, params, candidate, fingerprint, car_fw, experimental_long, docs): 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 7be3b2ebe..ff13f45a2 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 bf22533b3..ab79d091f 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 8c22a6d80..2b6807bdd 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, params, candidate, fingerprint, car_fw, experimental_long, docs): 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 9ece11b05..c323a278c 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 481615a40..f2de55f59 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 2230d21ae..996ebe09a 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, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False @@ -305,8 +305,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 fe023ea17..809b9d1c4 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 563b94268..3e15682dd 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 96374b2c6..2c7384e30 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, params, candidate, fingerprint, car_fw, experimental_long, docs): 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 4038ddcca..22927e544 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 33a7e267b..3c817db8a 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 cb513cd75..17466801f 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, params, candidate, fingerprint, car_fw, experimental_long, docs): 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 9aab3d0b5..0fe39fdc9 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, params: Params, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], experimental_long: bool, 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, params, candidate, fingerprint, car_fw, experimental_long, docs) # 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 3d4163487..b38ea72e4 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 c055152c8..bf0065501 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 cde0d7968..47c1947ba 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, params, candidate, fingerprint, car_fw, experimental_long, docs): 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 77f77f2be..e80f5d3dd 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, params, candidate, fingerprint, car_fw, experimental_long, docs): 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 c7bd23139..2c3cd3faa 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 67d0e664a..5144765e9 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 86100eb3c..60d676644 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, params, candidate, fingerprint, car_fw, experimental_long, docs): 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 d89ae8c63..6b935cbc8 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 d969616cb..fe438d170 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 f9bbb6c4c..525a5cd6d 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, params, candidate: CAR, fingerprint, car_fw, experimental_long, docs): 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 dcbea1979..b138b7f83 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 e460111e3..3ad189f36 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 26e36e5e8..315dcd594 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 192a70b01..dd40cfeb4 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, params, candidate, fingerprint, car_fw, experimental_long, docs): 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 f9b7a478e..2b457b691 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 d474a67e8..3478c0cff 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 4d98d0e49..581577b02 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, params, candidate, fingerprint, car_fw, experimental_long, docs): 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 1cc99b41b..f84dd9949 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 a4e0c8946..045498fb6 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 4a8603fc9..a51a4d8f8 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 11dd1364a..aa4bae815 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, params, candidate: CAR, fingerprint, car_fw, experimental_long, docs): 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 08ec20403..5a9d84e72 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 5bb537ea1..ae343a5a2 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 cfc6374a1..ef1f2d437 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 db3ecb6dc..8ba56d005 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 d6a4901ed..cadbf68bc 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 146340d8b..dfbf0e1af 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 b14410abd..87a341185 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']) @@ -17,7 +19,7 @@ def publish_ui_plan(sm, pm, longitudinal_planner): uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist() pm.send('uiPlan', ui_send) -def plannerd_thread(): +def plannerd_thread(frogpilot_toggles): config_realtime_process(5, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") @@ -31,15 +33,25 @@ def plannerd_thread(): sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'frogpilotCarControl', 'frogpilotCarState', 'frogpilotPlan'], poll='modelV2', ignore_avg_freq=['radarState']) + # FrogPilot variables + 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() + plannerd_thread(FrogPilotVariables.toggles) if __name__ == "__main__": diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 7420f666f..7426e4730 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 fce904527..2c28ca764 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -28,7 +28,7 @@ class FrogPilotPlanner: self.speed_jerk = 0 self.v_cruise = 0 - 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 @@ -45,12 +45,12 @@ class FrogPilotPlanner: else: self.tracking_lead &= self.lead_one.status - self.set_acceleration(controlsState, frogpilotCarState, v_cruise, v_ego) - self.set_follow_values(controlsState, frogpilotCarState, v_ego, v_lead) - self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead) - self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego) + self.set_acceleration(controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles) + self.set_follow_values(controlsState, frogpilotCarState, v_ego, v_lead, frogpilot_toggles) + self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles) + 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: @@ -61,20 +61,20 @@ class FrogPilotPlanner: else: self.min_accel = A_CRUISE_MIN - def set_follow_values(self, controlsState, frogpilotCarState, v_ego, v_lead): + def set_follow_values(self, controlsState, frogpilotCarState, 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 self.speed_jerk = self.base_speed_jerk - 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 @@ -84,7 +84,7 @@ class FrogPilotPlanner: targets = [] self.v_cruise = 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 1187d6024..f5949267d 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py @@ -1,5 +1,232 @@ +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.selfdrive.modeld.constants import ModelConstants +from openpilot.system.version import get_build_metadata + +from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS 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 TRAJECTORY_SIZE = ModelConstants.IDX_N # Minimum path length + +class FrogPilotVariables: + def __init__(self): + self.frogpilot_toggles = SimpleNamespace() + + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.release = get_build_metadata().channel == "FrogPilot" + + 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=started and openpilot_installed) + + if msg_bytes is None: + always_on_lateral_set = False + car_make = "mock" + openpilot_longitudinal = False + pcm_cruise = False + else: + with car.CarParams.from_bytes(msg_bytes) as CP: + always_on_lateral_set = self.params.get_bool("AlwaysOnLateralSet") + car_make = CP.carName + openpilot_longitudinal = CP.openpilotLongitudinalControl + pcm_cruise = CP.pcmCruise + + 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 + + if not started: + toggle.model = self.params.get("Model", block=openpilot_installed, encoding='utf-8') + toggle.navigationless_model = toggle.model not in NAVIGATION_MODELS + toggle.radarless_model = not self.release and toggle.model in RADARLESS_MODELS + toggle.secretgoodopenpilot_model = not self.release and toggle.model == "secret-good-openpilot" + + 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 = 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.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 aggressive_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 aggressive_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_make == "gm" 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.)) + + toggle.model_selector = self.params.get_bool("ModelSelector") + + 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 = quality_of_life_controls 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 = quality_of_life_controls 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_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 ca9003096..508afcaa8 100644 --- a/selfdrive/frogpilot/frogpilot_process.py +++ b/selfdrive/frogpilot/frogpilot_process.py @@ -11,6 +11,7 @@ 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 FrogPilotFunctions +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables WIFI = log.DeviceState.NetworkType.wifi @@ -25,7 +26,7 @@ def time_checks(deviceState, now, params, params_memory): screen_off = deviceState.screenBrightnessPercent == 0 wifi_connection = deviceState.networkType == WIFI -def frogpilot_thread(): +def frogpilot_thread(frogpilot_toggles): config_realtime_process(5, Priority.CTRL_LOW) params = Params() @@ -35,6 +36,7 @@ def frogpilot_thread(): frogpilot_planner = FrogPilotPlanner() time_validated = system_time_valid() + update_toggles = False pm = messaging.PubMaster(['frogpilotPlan']) sm = messaging.SubMaster(['carState', 'controlsState', 'deviceState', 'frogpilotCarControl', 'frogpilotCarState', 'frogpilotNavigation', @@ -50,8 +52,15 @@ def frogpilot_thread(): if started and sm.updated['modelV2']: frogpilot_planner.update(sm['carState'], sm['controlsState'], sm['frogpilotCarControl'], sm['frogpilotCarState'], - sm['frogpilotNavigation'], sm['modelV2'], sm['radarState']) - frogpilot_planner.publish(sm, pm) + 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: + FrogPilotVariables.update_frogpilot_params(started) + + update_toggles = False if now.second == 0 or not time_validated: if not started: @@ -64,7 +73,7 @@ def frogpilot_thread(): continue def main(): - frogpilot_thread() + frogpilot_thread(FrogPilotVariables.toggles) if __name__ == "__main__": main() 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 000000000..771e6c8c6 --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc @@ -0,0 +1,11 @@ +#include "selfdrive/ui/ui.h" + +Params paramsMemory{"/dev/shm/params"}; + +void updateFrogPilotToggles() { + std::thread([]() { + paramsMemory.putBool("FrogPilotTogglesUpdated", true); + std::this_thread::sleep_for(std::chrono::seconds(1)); + 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 000000000..336455150 --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h @@ -0,0 +1,3 @@ +#pragma once + +void updateFrogPilotToggles(); diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index d124eb5f0..0df431220 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -136,6 +136,7 @@ def main(): min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio params = params_reader.get("LiveParameters") + params_memory = Params("/dev/shm/params") # Check if car model matches if params is not None: diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 3f94ceb9e..dd26cfc8c 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -24,6 +24,8 @@ 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 + PROCESS_NAME = "selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -111,7 +113,7 @@ class ModelState: return outputs -def main(demo=False): +def main(demo=False, frogpilot_toggles=None): cloudlog.warning("modeld init") sentry.set_tag("daemon", PROCESS_NAME) @@ -181,6 +183,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 +272,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 +282,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: @@ -284,7 +295,7 @@ if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.') args = parser.parse_args() - main(demo=args.demo) + main(demo=args.demo, frogpilot_toggles=FrogPilotVariables.toggles) except KeyboardInterrupt: cloudlog.warning(f"child {PROCESS_NAME} got SIGINT") except Exception: diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index a1c770e61..f15937a20 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 f60b80b21..7a3f4b4bb 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 31a44f27b..88220d0b1 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 35a044bdd..8b1ae8cee 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/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index ae721e1ef..395cea5d0 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -454,6 +454,7 @@ void AnnotatedCameraWidget::showEvent(QShowEvent *event) { CameraWidget::showEvent(event); ui_update_params(uiState()); + ui_update_frogpilot_params(uiState()); prev_draw_t = millis_since_boot(); } diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h index f627aac81..e9fec784b 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 60c865f2b..30278c1b0 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 05b61e1f7..f1389c29f 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 0550a7db9..450bd8a78 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 1f6c5213e..4804e4441 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -254,6 +254,19 @@ void ui_update_params(UIState *s) { s->scene.map_on_left = params.getBool("NavSettingLeftSide"); } +void ui_update_frogpilot_params(UIState *s) { + UIScene &scene = s->scene; + Params params = Params(); + + auto carParams = params.get("CarParamsPersistent"); + if (!carParams.empty() && !scene.started) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(carParams.data(), carParams.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + scene.longitudinal_control = CP.getOpenpilotLongitudinalControl() && !params.getBool("DisableOpenpilotLongitudinal"); + } +} + void UIState::updateStatus() { if (scene.started && sm->updated("controlsState")) { auto controls_state = (*sm)["controlsState"].getControlsState(); @@ -297,6 +310,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_frogpilot_params(this); } void UIState::update() { @@ -308,6 +324,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_frogpilot_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 647ec9094..ce90f3044 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -16,6 +16,8 @@ #include "common/timing.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 +160,9 @@ private: QTimer *timer; bool started_prev = false; PrimeType prime_type = PrimeType::UNKNOWN; + + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; }; UIState *uiState(); @@ -207,3 +212,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); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 096e288cc..ab7548f73 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 83885f290..17eaf74e6 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 @@ -162,7 +164,7 @@ def hw_state_thread(end_event, hw_queue): time.sleep(DT_HW) -def hardware_thread(end_event, hw_queue) -> None: +def hardware_thread(end_event, hw_queue, frogpilot_toggles) -> None: pm = messaging.PubMaster(['deviceState', 'frogpilotDeviceState']) sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll="pandaStates") @@ -204,6 +206,9 @@ def hardware_thread(end_event, hw_queue) -> None: fan_controller = None + # FrogPilot variables + update_toggles = False + while not end_event.is_set(): sm.update(PANDA_STATES_TIMEOUT) @@ -387,7 +392,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 +454,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) @@ -456,7 +467,7 @@ def main(): threads = [ threading.Thread(target=hw_state_thread, args=(end_event, hw_queue)), - threading.Thread(target=hardware_thread, args=(end_event, hw_queue)), + threading.Thread(target=hardware_thread, args=(end_event, hw_queue, FrogPilotVariables.toggles)), ] for t in threads: diff --git a/system/hardware/power_monitoring.py b/system/hardware/power_monitoring.py index 5a94625b4..3b82848c2 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 b66fc2c67..1fa83f3ad 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -366,6 +366,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): @@ -391,6 +392,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 6ae05c311..01a827d81 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")