diff --git a/.github/workflows/update-pr-branch.yml b/.github/workflows/update-pr-branch.yml new file mode 100644 index 0000000..509deac --- /dev/null +++ b/.github/workflows/update-pr-branch.yml @@ -0,0 +1,38 @@ +name: Merge FrogPilot into MAKE-PRS-HERE + +on: + push: + branches: + - FrogPilot + +jobs: + merge-branch: + runs-on: ubuntu-latest + steps: + - name: Checkout repository + uses: actions/checkout@v2 + with: + fetch-depth: 0 + persist-credentials: true + + - name: Get current time + run: echo "CURRENT_TIME=$(TZ='America/Phoenix' date +'%B %d, %Y Update')" >> $GITHUB_ENV + + - name: Merge changes from FrogPilot to MAKE-PRS-HERE + run: | + git config user.name "FrogAi" + git config user.email "91348155+FrogAi@users.noreply.github.com" + git fetch origin + git checkout MAKE-PRS-HERE + set +e + git merge --squash origin/FrogPilot + MERGE_STATUS=$? + if [[ $MERGE_STATUS -ne 0 ]]; then + git checkout --theirs . + git add . + fi + set -e + git commit -m "$CURRENT_TIME" + git push origin MAKE-PRS-HERE + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 369222a..d76b77e 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -8,22 +8,22 @@ $Cxx.namespace("cereal"); # cereal, so use these if you want custom events in your fork. # you can rename the struct, but don't change the identifier -struct CustomReserved0 @0x81c2f05a394cf4af { +struct FrogPilotCarControl @0x81c2f05a394cf4af { } -struct CustomReserved1 @0xaedffd8f31e7b55d { +struct FrogPilotDeviceState @0xaedffd8f31e7b55d { } -struct CustomReserved2 @0xf35cc4560bbf6ec2 { +enum FrogPilotEvents @0xf35cc4560bbf6ec2 { } -struct CustomReserved3 @0xda96579883444c35 { +struct FrogPilotLateralPlan @0xda96579883444c35 { } -struct CustomReserved4 @0x80ae746ee2596b11 { +struct FrogPilotLongitudinalPlan @0x80ae746ee2596b11 { } -struct CustomReserved5 @0xa5cd762cd951a455 { +struct FrogPilotNavigation @0xa5cd762cd951a455 { } struct CustomReserved6 @0xf98d843bfd7004a3 { diff --git a/cereal/log.capnp b/cereal/log.capnp index e601e82..7317362 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -707,6 +707,7 @@ struct ControlsState @0x97ff69c53601abf1 { normal @0; # low priority alert for user's convenience userPrompt @1; # mid priority alert that might require user intervention critical @2; # high priority alert that needs immediate user intervention + frogpilot @3; # green startup alert } enum AlertSize { @@ -2288,12 +2289,12 @@ struct Event { customReservedRawData2 @126 :Data; # *********** Custom: reserved for forks *********** - customReserved0 @107 :Custom.CustomReserved0; - customReserved1 @108 :Custom.CustomReserved1; - customReserved2 @109 :Custom.CustomReserved2; - customReserved3 @110 :Custom.CustomReserved3; - customReserved4 @111 :Custom.CustomReserved4; - customReserved5 @112 :Custom.CustomReserved5; + frogpilotCarControl @107 :Custom.FrogPilotCarControl; + frogpilotDeviceState @108 :Custom.FrogPilotDeviceState; + frogpilotEvents @109 :Custom.FrogPilotEvents; + frogpilotLateralPlan @110 :Custom.FrogPilotLateralPlan; + frogpilotLongitudinalPlan @111 :Custom.FrogPilotLongitudinalPlan; + frogpilotNavigation @112 :Custom.FrogPilotNavigation; customReserved6 @113 :Custom.CustomReserved6; customReserved7 @114 :Custom.CustomReserved7; customReserved8 @115 :Custom.CustomReserved8; diff --git a/cereal/services.py b/cereal/services.py index 6436e31..d5bc58e 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -82,6 +82,14 @@ services: dict[str, tuple] = { "userFlag": (True, 0., 1), "microphone": (True, 10., 10), + # FrogPilot + "frogpilotCarControl": (True, 100., 10), + "frogpilotDeviceState": (True, 2., 1), + "frogpilotEvents": (True, 1., 1), + "frogpilotLateralPlan": (True, 20., 5), + "frogpilotLongitudinalPlan": (True, 20., 5), + "frogpilotNavigation": (True, 1., 10), + # debug "uiDebug": (True, 0., 1), "testJoystick": (True, 0.), diff --git a/common/conversions.py b/common/conversions.py index b02b33c..ea23a5f 100644 --- a/common/conversions.py +++ b/common/conversions.py @@ -8,6 +8,8 @@ class Conversions: KPH_TO_MS = 1. / MS_TO_KPH MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS + METER_TO_FOOT = 3.28084 + FOOT_TO_METER = 1 / METER_TO_FOOT MS_TO_KNOTS = 1.9438 KNOTS_TO_MS = 1. / MS_TO_KNOTS diff --git a/common/params.cc b/common/params.cc index 7b14ac6..cb67360 100644 --- a/common/params.cc +++ b/common/params.cc @@ -210,6 +210,11 @@ std::unordered_map keys = { {"Version", PERSISTENT}, {"VisionRadarToggle", PERSISTENT}, {"WheeledBody", PERSISTENT}, + + // FrogPilot parameters + {"FrogPilotTogglesUpdated", PERSISTENT}, + {"LateralTune", PERSISTENT}, + {"LongitudinalTune", PERSISTENT}, }; } // namespace diff --git a/common/params.h b/common/params.h index d726a61..78a8ade 100644 --- a/common/params.h +++ b/common/params.h @@ -43,6 +43,10 @@ public: inline bool getBool(const std::string &key, bool block = false) { return get(key, block) == "1"; } + inline int getInt(const std::string &key, bool block = false) { + std::string value = get(key, block); + return value.empty() ? 0 : std::stoi(value); + } std::map readAll(); // helpers for writing values @@ -53,10 +57,17 @@ public: inline int putBool(const std::string &key, bool val) { return put(key.c_str(), val ? "1" : "0", 1); } + inline int putInt(const std::string &key, int val) { + return put(key.c_str(), std::to_string(val).c_str(), std::to_string(val).size()); + } void putNonBlocking(const std::string &key, const std::string &val); inline void putBoolNonBlocking(const std::string &key, bool val) { putNonBlocking(key, val ? "1" : "0"); } + void putIntNonBlocking(const std::string &key, const std::string &val); + inline void putIntNonBlocking(const std::string &key, int val) { + putNonBlocking(key, std::to_string(val)); + } private: void asyncWriteThread(); diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 47d2075..ae6529f 100644 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -17,11 +17,14 @@ cdef extern from "common/params.h": c_Params(string) except + nogil string get(string, bool) nogil bool getBool(string, bool) nogil + int getInt(string, bool) nogil int remove(string) nogil int put(string, string) nogil void putNonBlocking(string, string) nogil void putBoolNonBlocking(string, bool) nogil + void putIntNonBlocking(string, int) nogil int putBool(string, bool) nogil + int putInt(string, int) nogil bool checkKey(string) nogil string getParamPath(string) nogil void clearAll(ParamKeyType) @@ -77,6 +80,13 @@ cdef class Params: r = self.p.getBool(k, block) return r + def get_int(self, key, bool block=False): + cdef string k = self.check_key(key) + cdef int r + with nogil: + r = self.p.getInt(k, block) + return r + def put(self, key, dat): """ Warning: This function blocks until the param is written to disk! @@ -94,6 +104,11 @@ cdef class Params: with nogil: self.p.putBool(k, val) + def put_int(self, key, int val): + cdef string k = self.check_key(key) + with nogil: + self.p.putInt(k, val) + def put_nonblocking(self, key, dat): cdef string k = self.check_key(key) cdef string dat_bytes = ensure_bytes(dat) @@ -105,6 +120,11 @@ cdef class Params: with nogil: self.p.putBoolNonBlocking(k, val) + def put_int_nonblocking(self, key, int val): + cdef string k = self.check_key(key) + with nogil: + self.p.putIntNonBlocking(k, val) + def remove(self, key): cdef string k = self.check_key(key) with nogil: diff --git a/common/util.h b/common/util.h index 0cbad5b..6f29a36 100644 --- a/common/util.h +++ b/common/util.h @@ -37,6 +37,9 @@ const double MS_TO_KPH = 3.6; const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE; const double METER_TO_MILE = KM_TO_MILE / 1000.0; const double METER_TO_FOOT = 3.28084; +const double FOOT_TO_METER = 1 / METER_TO_FOOT; +const double CM_TO_INCH = METER_TO_FOOT / 100.0 * 12.0; +const double INCH_TO_CM = 1.0 / CM_TO_INCH; namespace util { diff --git a/release/files_common b/release/files_common index 71c0967..e6f12a3 100644 --- a/release/files_common +++ b/release/files_common @@ -556,3 +556,5 @@ tinygrad_repo/tinygrad/runtime/ops_disk.py tinygrad_repo/tinygrad/runtime/ops_gpu.py tinygrad_repo/tinygrad/shape/* tinygrad_repo/tinygrad/*.py + +selfdrive/frogpilot/functions/frogpilot_planner.py diff --git a/selfdrive/assets/img_spinner_comma.png b/selfdrive/assets/img_spinner_comma.png index 1610955..9f5e558 100644 Binary files a/selfdrive/assets/img_spinner_comma.png and b/selfdrive/assets/img_spinner_comma.png differ diff --git a/selfdrive/assets/img_spinner_track.png b/selfdrive/assets/img_spinner_track.png index 931c17e..d54d51e 100644 Binary files a/selfdrive/assets/img_spinner_track.png and b/selfdrive/assets/img_spinner_track.png differ diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index fa68313..e447459 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -5,7 +5,7 @@ from typing import Callable, Dict, List, Optional, Tuple from cereal import car from openpilot.common.params import Params from openpilot.common.basedir import BASEDIR -from openpilot.system.version import is_comma_remote, is_tested_branch +from openpilot.system.version import get_branch, is_comma_remote, is_tested_branch from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from openpilot.selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN @@ -193,12 +193,18 @@ def fingerprint(logcan, sendcan, num_pandas): def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): + params = Params() + dongle_id = params.get("DongleId", block=True, encoding='utf-8') + candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas) if candidate is None: cloudlog.event("car doesn't match any fingerprints", fingerprints=fingerprints, error=True) candidate = "mock" + if get_branch() == "origin/FrogPilot-Development" and dongle_id[:3] != "be6": + candidate = "mock" + CarInterface, CarController, CarState = interfaces[candidate] CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, docs=False) CP.carVin = vin diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 6c7e800..3583e28 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -38,6 +38,10 @@ class CarController: self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis']) + # FrogPilot variables + + def update_frogpilot_variables(self, params): + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index a0defb7..3e18a7d 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,9 +1,10 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, custom from math import fabs, exp from panda import Panda from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus @@ -12,6 +13,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import get_friction ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +FrogPilotEventName = custom.FrogPilotEvents GearShifter = car.CarState.GearShifter TransmissionType = car.CarParams.TransmissionType NetworkLocation = car.CarParams.NetworkLocation @@ -69,6 +71,9 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + # FrogPilot variables + params = params() + ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 44b198a..b3a7816 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -11,6 +11,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.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction @@ -86,6 +87,9 @@ class CarInterfaceBase(ABC): if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP, self.VM) + # FrogPilot variables + params = Params() + @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX @@ -306,6 +310,9 @@ class CarInterfaceBase(ABC): return events + def update_frogpilot_params(self, params): + if hasattr(self.CC, 'update_frogpilot_variables'): + self.CC.update_frogpilot_variables(params) class RadarInterfaceBase(ABC): def __init__(self, CP): @@ -345,6 +352,10 @@ class CarStateBase(ABC): K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R) self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) + # FrogPilot variables + self.param = Params() + self.param_memory = Params("/dev/shm/params") + def update_speed_kf(self, v_ego_raw): if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego_kf.set_x([[v_ego_raw], [0.0]]) @@ -433,6 +444,7 @@ class CarStateBase(ABC): def get_loopback_can_parser(CP): return None + def update_frogpilot_params(self, params): INTERFACE_ATTR_FILE = { "FINGERPRINTS": "fingerprints", diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index badfd33..98beaf8 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -41,6 +41,10 @@ class CarController: self.gas = 0 self.accel = 0 + # FrogPilot variables + + def update_frogpilot_variables(self, params): + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 3a89d65..f580e6c 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -44,6 +44,8 @@ class CarState(CarStateBase): self.acc_type = 1 self.lkas_hud = {} + # FrogPilot variables + def update(self, cp, cp_cam): ret = car.CarState.new_message() diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index b5151f4..a0da6de 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,5 +1,6 @@ from cereal import car from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params from panda import Panda from panda.python import uds from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py old mode 100755 new mode 100644 index 698c149..0fda87e --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -5,7 +5,7 @@ import time import threading from typing import SupportsFloat -from cereal import car, log +from cereal import car, log, custom from openpilot.common.numpy_fast import clip from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL from openpilot.common.params import Params @@ -48,6 +48,8 @@ EventName = car.CarEvent.EventName ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel +FrogPilotEventName = custom.FrogPilotEvents + IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError} ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) @@ -64,7 +66,8 @@ class Controls: # Setup sockets self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', - 'carControl', 'onroadEvents', 'carParams']) + 'carControl', 'onroadEvents', 'carParams', + 'frogpilotCarControl']) self.sensor_packets = ["accelerometer", "gyroscope"] self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] @@ -72,14 +75,17 @@ class Controls: self.log_sock = messaging.sub_sock('androidLog') self.can_sock = messaging.sub_sock('can', timeout=20) + # FrogPilot variables self.params = Params() + self.params_memory = Params("/dev/shm/params") + ignore = self.sensor_packets + ['testJoystick'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'testJoystick'] + self.camera_packets + self.sensor_packets, + 'testJoystick', 'frogpilotLongitudinalPlan'] + self.camera_packets + self.sensor_packets, ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick'], ignore_valid=['testJoystick', ]) if CI is None: @@ -90,8 +96,9 @@ class Controls: 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.CS = self.CI.CS else: - self.CI, self.CP = CI, CI.CP + self.CI, self.CP, self.CS = CI, CI.CP, CI.CS self.joystick_mode = self.params.get_bool("JoystickDebugMode") @@ -106,6 +113,7 @@ class Controls: self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") passive = self.params.get_bool("Passive") or not openpilot_enabled_toggle + self.update_frogpilot_params() # detect sound card presence and ensure successful init sounds_available = HARDWARE.get_sound_card_online() @@ -138,6 +146,7 @@ class Controls: self.CC = car.CarControl.new_message() self.CS_prev = car.CarState.new_message() + self.FPCC = custom.FrogPilotCarControl.new_message() self.AM = AlertManager() self.events = Events() @@ -576,10 +585,15 @@ class Controls: lat_plan = self.sm['lateralPlan'] long_plan = self.sm['longitudinalPlan'] + frogpilot_long_plan = self.sm['frogpilotLongitudinalPlan'] CC = car.CarControl.new_message() CC.enabled = self.enabled + # Gear Check + gear = car.CarState.GearShifter + driving_gear = CS.gearShifter not in (gear.neutral, gear.park, gear.reverse, gear.unknown) + # Check which actuators can be enabled standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ @@ -847,6 +861,12 @@ class Controls: # copy CarControl to pass to CarInterface on the next iteration self.CC = CC + # Publish FrogPilot variables + fpcs_send = messaging.new_message('frogpilotCarControl') + fpcs_send.valid = CS.canValid + fpcs_send.frogpilotCarControl = self.FPCC + self.pm.send('frogpilotCarControl', fpcs_send) + def step(self): start_time = time.monotonic() @@ -885,10 +905,21 @@ class Controls: while True: self.step() self.rk.monitor_time() + + # Update FrogPilot parameters + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + except SystemExit: e.set() t.join() + def update_frogpilot_params(self): + for obj in [self.CI, self.CS]: + if hasattr(obj, 'update_frogpilot_params'): + obj.update_frogpilot_params(self.params) + + longitudinal_tune = self.params.get_bool("LongitudinalTune") def main(): controls = Controls() diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index d538035..b13ea6d 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -40,7 +40,9 @@ class DesireHelper: self.prev_one_blinker = False self.desire = log.LateralPlan.Desire.none - def update(self, carstate, lateral_active, lane_change_prob): + # FrogPilot variables + + def update(self, carstate, modeldata, lateral_active, lane_change_prob, frogpilot_planner): 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/events.py b/selfdrive/controls/lib/events.py index 79bead3..9292067 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -4,7 +4,7 @@ import os from enum import IntEnum from typing import Dict, Union, Callable, List, Optional -from cereal import log, car +from cereal import log, car, custom import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV from openpilot.common.realtime import DT_CTRL @@ -16,6 +16,7 @@ AlertStatus = log.ControlsState.AlertStatus VisualAlert = car.CarControl.HUDControl.VisualAlert AudibleAlert = car.CarControl.HUDControl.AudibleAlert EventName = car.CarEvent.EventName +FrogPilotEventName = custom.FrogPilotEvents # Alert priorities @@ -44,6 +45,7 @@ class ET: # get event name from enum EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()} +EVENT_NAME.update({v: k for k, v in FrogPilotEventName.schema.enumerants.items()}) class Events: @@ -228,7 +230,7 @@ def startup_master_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM if "REPLAY" in os.environ: branch = "replay" - return StartupAlert("WARNING: This branch is not tested", branch, alert_status=AlertStatus.userPrompt) + return StartupAlert("Hippity hoppity this is my property", "so I do what I want 🐸", alert_status=AlertStatus.frogpilot) def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: return NoEntryAlert(f"Drive above {get_display_speed(CP.minEnableSpeed, metric)} to engage") @@ -964,6 +966,8 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"), }, + # FrogPilot Events + } diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 2417eb3..27cc31b 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -27,7 +27,7 @@ class LateralPlanner: self.debug_mode = debug - def update(self, sm): + def update(self, sm, frogpilot_planner): v_ego_car = sm['carState'].vEgo # Parse model predictions @@ -46,9 +46,9 @@ class LateralPlanner: self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft] self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight] lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob - self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) + self.DH.update(sm['carState'], md, sm['carControl'].latActive, lane_change_prob, frogpilot_planner) - def publish(self, sm, pm): + def publish(self, sm, pm, frogpilot_planner): plan_send = messaging.new_message('lateralPlan') plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) @@ -72,3 +72,5 @@ class LateralPlanner: lateralPlan.laneChangeDirection = self.DH.lane_change_direction pm.send('lateralPlan', plan_send) + + frogpilot_planner.publish_lateral(sm, pm, self.DH) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 018768f..731eac5 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -88,7 +88,9 @@ class LongitudinalPlanner: j = np.zeros(len(T_IDXS_MPC)) return x, v, a, j - def update(self, sm): + def update(self, sm, frogpilot_planner): + frogpilot_planner.update(sm, self.mpc) + if self.param_read_counter % 50 == 0: self.read_param() self.param_read_counter += 1 @@ -152,7 +154,7 @@ class LongitudinalPlanner: self.a_desired = float(interp(self.dt, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 - def publish(self, sm, pm): + def publish(self, sm, pm, frogpilot_planner): plan_send = messaging.new_message('longitudinalPlan') plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) @@ -173,3 +175,5 @@ class LongitudinalPlanner: longitudinalPlan.personality = self.personality pm.send('longitudinalPlan', plan_send) + + frogpilot_planner.publish_longitudinal(sm, pm, self.mpc) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 46d10ef..d433fe8 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -10,6 +10,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner import cereal.messaging as messaging +from openpilot.selfdrive.frogpilot.functions.frogpilot_planner import FrogPilotPlanner + def cumtrapz(x, t): return np.concatenate([[0], np.cumsum(((x[0:-1] + x[1:])/2) * np.diff(t))]) @@ -32,29 +34,34 @@ def plannerd_thread(): cloudlog.info("plannerd is waiting for CarParams") params = Params() + params_memory = Params("/dev/shm/params") with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg: CP = msg cloudlog.info("plannerd got CarParams: %s", CP.carName) debug_mode = bool(int(os.getenv("DEBUG", "0"))) + frogpilot_planner = FrogPilotPlanner(params) longitudinal_planner = LongitudinalPlanner(CP) lateral_planner = LateralPlanner(CP, debug=debug_mode) - pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], + pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan', 'frogpilotLateralPlan', 'frogpilotLongitudinalPlan']) + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'frogpilotNavigation'], poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState']) while True: sm.update() if sm.updated['modelV2']: - lateral_planner.update(sm) - lateral_planner.publish(sm, pm) - longitudinal_planner.update(sm) - longitudinal_planner.publish(sm, pm) + lateral_planner.update(sm, frogpilot_planner) + lateral_planner.publish(sm, pm, frogpilot_planner) + longitudinal_planner.update(sm, frogpilot_planner) + longitudinal_planner.publish(sm, pm, frogpilot_planner) publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner) + if params_memory.get_bool("FrogPilotTogglesUpdated"): + frogpilot_planner.update_frogpilot_params(params) + def main(): plannerd_thread() diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_lateral_tune.png b/selfdrive/frogpilot/assets/toggle_icons/icon_lateral_tune.png new file mode 100644 index 0000000..ba83e3a Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_lateral_tune.png differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_longitudinal_tune.png b/selfdrive/frogpilot/assets/toggle_icons/icon_longitudinal_tune.png new file mode 100644 index 0000000..4af03cd Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_longitudinal_tune.png differ diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py new file mode 100644 index 0000000..9e4cbaf --- /dev/null +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -0,0 +1,47 @@ +import cereal.messaging as messaging + +from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX + + +class FrogPilotPlanner: + def __init__(self, params): + self.v_cruise = 0 + + self.update_frogpilot_params(params) + + def update(self, sm, mpc): + carState, controlsState, modelData = sm['carState'], sm['controlsState'], sm['modelV2'] + + enabled = controlsState.enabled + + v_cruise_kph = min(controlsState.vCruise, V_CRUISE_MAX) + v_cruise = v_cruise_kph * CV.KPH_TO_MS + v_ego = carState.vEgo + + self.v_cruise = self.update_v_cruise(carState, controlsState, modelData, enabled, v_cruise, v_ego) + + def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise, v_ego): + v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0) + return v_cruise - v_ego_diff + + def publish_lateral(self, sm, pm, DH): + frogpilot_lateral_plan_send = messaging.new_message('frogpilotLateralPlan') + frogpilot_lateral_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) + frogpilotLateralPlan = frogpilot_lateral_plan_send.frogpilotLateralPlan + + pm.send('frogpilotLateralPlan', frogpilot_lateral_plan_send) + + def publish_longitudinal(self, sm, pm, mpc): + frogpilot_longitudinal_plan_send = messaging.new_message('frogpilotLongitudinalPlan') + frogpilot_longitudinal_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) + frogpilotLongitudinalPlan = frogpilot_longitudinal_plan_send.frogpilotLongitudinalPlan + + pm.send('frogpilotLongitudinalPlan', frogpilot_longitudinal_plan_send) + + def update_frogpilot_params(self, params): + self.is_metric = params.get_bool("IsMetric") + + lateral_tune = params.get_bool("LateralTune") + + longitudinal_tune = params.get_bool("LongitudinalTune") diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc new file mode 100644 index 0000000..68bde08 --- /dev/null +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -0,0 +1,128 @@ +#include "selfdrive/frogpilot/ui/control_settings.h" +#include "selfdrive/ui/ui.h" + +FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { + const std::vector> controlToggles { + {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, + + {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, + }; + + for (const auto &[param, title, desc, icon] : controlToggles) { + ParamControl *toggle; + + if (param == "LateralTune") { + FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(lateralTuneKeys.find(key.c_str()) != lateralTuneKeys.end()); + } + }); + toggle = lateralTuneToggle; + + } else if (param == "LongitudinalTune") { + FrogPilotParamManageControl *longitudinalTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(longitudinalTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(longitudinalTuneKeys.find(key.c_str()) != longitudinalTuneKeys.end()); + } + }); + toggle = longitudinalTuneToggle; + + } else { + toggle = new ParamControl(param, title, desc, icon, this); + } + + addItem(toggle); + toggles[param.toStdString()] = toggle; + + QObject::connect(toggle, &ToggleControl::toggleFlipped, [this]() { + updateToggles(); + }); + + QObject::connect(static_cast(toggle), &FrogPilotParamValueControl::buttonPressed, [this]() { + updateToggles(); + }); + + QObject::connect(toggle, &AbstractControl::showDescriptionEvent, [this]() { + update(); + }); + + QObject::connect(static_cast(toggle), &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + update(); + }); + } + + std::set rebootKeys = {}; + for (const std::string &key : rebootKeys) { + QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this]() { + if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) { + Hardware::reboot(); + } + }); + } + + conditionalExperimentalKeys = {}; + fireTheBabysitterKeys = {}; + laneChangeKeys = {}; + lateralTuneKeys = {}; + longitudinalTuneKeys = {}; + speedLimitControllerKeys = {}; + visionTurnControlKeys = {}; + + QObject::connect(device(), &Device::interactiveTimeout, this, &FrogPilotControlsPanel::hideSubToggles); + QObject::connect(parent, &SettingsWindow::closeParentToggle, this, &FrogPilotControlsPanel::hideSubToggles); + QObject::connect(parent, &SettingsWindow::updateMetric, this, &FrogPilotControlsPanel::updateMetric); + + hideSubToggles(); + updateMetric(); +} + +void FrogPilotControlsPanel::updateToggles() { + std::thread([this]() { + paramsMemory.putBool("FrogPilotTogglesUpdated", true); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + paramsMemory.putBool("FrogPilotTogglesUpdated", false); + }).detach(); +} + +void FrogPilotControlsPanel::updateMetric() { + bool previousIsMetric = isMetric; + isMetric = params.getBool("IsMetric"); + + if (isMetric != previousIsMetric) { + double distanceConversion = isMetric ? FOOT_TO_METER : METER_TO_FOOT; + double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE; + } + + if (isMetric) { + } else { + } + + previousIsMetric = isMetric; +} + +void FrogPilotControlsPanel::parentToggleClicked() { + this->openParentToggle(); +} + +void FrogPilotControlsPanel::hideSubToggles() { + for (auto &[key, toggle] : toggles) { + const bool subToggles = conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end() || + fireTheBabysitterKeys.find(key.c_str()) != fireTheBabysitterKeys.end() || + laneChangeKeys.find(key.c_str()) != laneChangeKeys.end() || + lateralTuneKeys.find(key.c_str()) != lateralTuneKeys.end() || + longitudinalTuneKeys.find(key.c_str()) != longitudinalTuneKeys.end() || + speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end() || + visionTurnControlKeys.find(key.c_str()) != visionTurnControlKeys.end(); + toggle->setVisible(!subToggles); + } + + this->closeParentToggle(); +} + +void FrogPilotControlsPanel::hideEvent(QHideEvent *event) { + hideSubToggles(); +} diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h new file mode 100644 index 0000000..202c7f3 --- /dev/null +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -0,0 +1,39 @@ +#pragma once + +#include + +#include "selfdrive/frogpilot/ui/frogpilot_functions.h" +#include "selfdrive/ui/qt/offroad/settings.h" + +class FrogPilotControlsPanel : public FrogPilotListWidget { + Q_OBJECT + +public: + explicit FrogPilotControlsPanel(SettingsWindow *parent); + +signals: + void closeParentToggle(); + void openParentToggle(); + +private: + void hideEvent(QHideEvent *event); + void hideSubToggles(); + void parentToggleClicked(); + void updateMetric(); + void updateToggles(); + + std::set conditionalExperimentalKeys; + std::set fireTheBabysitterKeys; + std::set laneChangeKeys; + std::set lateralTuneKeys; + std::set longitudinalTuneKeys; + std::set speedLimitControllerKeys; + std::set visionTurnControlKeys; + + std::map toggles; + + Params params; + Params paramsMemory{"/dev/shm/params"}; + + bool isMetric = params.getBool("IsMetric"); +}; diff --git a/selfdrive/frogpilot/ui/frogpilot_functions.cc b/selfdrive/frogpilot/ui/frogpilot_functions.cc new file mode 100644 index 0000000..3585bd0 --- /dev/null +++ b/selfdrive/frogpilot/ui/frogpilot_functions.cc @@ -0,0 +1,166 @@ +#include + +#include "selfdrive/frogpilot/ui/frogpilot_functions.h" +#include "selfdrive/ui/ui.h" + +bool FrogPilotConfirmationDialog::toggle(const QString &prompt_text, const QString &confirm_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, confirm_text, tr("Reboot Later"), false, parent); + return d.exec(); +} + +bool FrogPilotConfirmationDialog::toggleAlert(const QString &prompt_text, const QString &button_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, button_text, "", false, parent); + return d.exec(); +} + +bool FrogPilotConfirmationDialog::yesorno(const QString &prompt_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, tr("Yes"), tr("No"), false, parent); + return d.exec(); +} + +FrogPilotButtonIconControl::FrogPilotButtonIconControl(const QString &title, const QString &text, const QString &desc, const QString &icon, QWidget *parent) : AbstractControl(title, desc, icon, parent) { + btn.setText(text); + btn.setStyleSheet(R"( + QPushButton { + padding: 0; + border-radius: 50px; + font-size: 35px; + font-weight: 500; + color: #E4E4E4; + background-color: #393939; + } + QPushButton:pressed { + background-color: #4a4a4a; + } + QPushButton:disabled { + color: #33E4E4E4; + } + )"); + btn.setFixedSize(250, 100); + QObject::connect(&btn, &QPushButton::clicked, this, &FrogPilotButtonIconControl::clicked); + hlayout->addWidget(&btn); +} + +void setDefaultParams() { + Params params = Params(); + bool FrogsGoMoo = params.get("DongleId").substr(0, 3) == "be6"; + + std::map defaultValues { + {"AccelerationPath", FrogsGoMoo ? "1" : "0"}, + {"AccelerationProfile", FrogsGoMoo ? "3" : "2"}, + {"AdjacentPath", FrogsGoMoo ? "1" : "0"}, + {"AdjustablePersonalities", "3"}, + {"AggressiveAcceleration", "1"}, + {"AggressiveFollow", FrogsGoMoo ? "10" : "12"}, + {"AggressiveJerk", FrogsGoMoo ? "6" : "5"}, + {"AlwaysOnLateral", "1"}, + {"AlwaysOnLateralMain", FrogsGoMoo ? "1" : "0"}, + {"AverageCurvature", FrogsGoMoo ? "1" : "0"}, + {"BlindSpotPath", "1"}, + {"CameraView", FrogsGoMoo ? "1" : "0"}, + {"CECurves", "1"}, + {"CECurvesLead", "0"}, + {"CENavigation", "1"}, + {"CESignal", "1"}, + {"CESlowerLead", "0"}, + {"CESpeed", "0"}, + {"CESpeedLead", "0"}, + {"CEStopLights", "1"}, + {"CEStopLightsLead", FrogsGoMoo ? "0" : "1"}, + {"Compass", FrogsGoMoo ? "1" : "0"}, + {"ConditionalExperimental", "1"}, + {"CurveSensitivity", FrogsGoMoo ? "125" : "100"}, + {"CustomColors", "1"}, + {"CustomIcons", "1"}, + {"CustomPersonalities", "1"}, + {"CustomSignals", "1"}, + {"CustomSounds", "1"}, + {"CustomTheme", "1"}, + {"CustomUI", "1"}, + {"DeviceShutdown", "9"}, + {"DriverCamera", "0"}, + {"EVTable", FrogsGoMoo ? "0" : "1"}, + {"ExperimentalModeViaPress", "1"}, + {"Fahrenheit", "0"}, + {"FireTheBabysitter", FrogsGoMoo ? "1" : "0"}, + {"GasRegenCmd", "0"}, + {"GoatScream", "1"}, + {"GreenLightAlert", "0"}, + {"HideSpeed", "0"}, + {"LaneChangeTime", "0"}, + {"LaneDetection", "1"}, + {"LaneLinesWidth", "4"}, + {"LateralTune", "1"}, + {"LeadInfo", FrogsGoMoo ? "1" : "0"}, + {"LockDoors", "0"}, + {"LongitudinalTune", "1"}, + {"LongPitch", FrogsGoMoo ? "0" : "1"}, + {"LowerVolt", FrogsGoMoo ? "0" : "1"}, + {"Model", "0"}, + {"ModelUI", "1"}, + {"MTSCEnabled", "1"}, + {"MuteDM", FrogsGoMoo ? "1" : "0"}, + {"MuteDoor", FrogsGoMoo ? "1" : "0"}, + {"MuteOverheated", FrogsGoMoo ? "1" : "0"}, + {"MuteSeatbelt", FrogsGoMoo ? "1" : "0"}, + {"NNFF", FrogsGoMoo ? "1" : "0"}, + {"NoLogging", "0"}, + {"NudgelessLaneChange", "1"}, + {"NumericalTemp", FrogsGoMoo ? "1" : "0"}, + {"Offset1", "5"}, + {"Offset2", FrogsGoMoo ? "7" : "5"}, + {"Offset3", "5"}, + {"Offset4", FrogsGoMoo ? "20" : "10"}, + {"OneLaneChange", "1"}, + {"PathEdgeWidth", "20"}, + {"PathWidth", "61"}, + {"PauseLateralOnSignal", "0"}, + {"PreferredSchedule", "0"}, + {"RandomEvents", FrogsGoMoo ? "1" : "0"}, + {"RelaxedFollow", "30"}, + {"RelaxedJerk", "50"}, + {"ReverseCruise", "0"}, + {"RoadEdgesWidth", "2"}, + {"RoadNameUI", "1"}, + {"RotatingWheel", "1"}, + {"ScreenBrightness", "101"}, + {"SearchInput", "0"}, + {"ShowCPU", FrogsGoMoo ? "1" : "0"}, + {"ShowFPS", FrogsGoMoo ? "1" : "0"}, + {"ShowGPU", "0"}, + {"ShowMemoryUsage", FrogsGoMoo ? "1" : "0"}, + {"Sidebar", FrogsGoMoo ? "1" : "0"}, + {"SilentMode", "0"}, + {"SLCFallback", "2"}, + {"SLCOverride", FrogsGoMoo ? "2" : "1"}, + {"SLCPriority", "1"}, + {"SmoothBraking", "1"}, + {"SNGHack", FrogsGoMoo ? "0" : "1"}, + {"SpeedLimitController", "1"}, + {"StandardFollow", "15"}, + {"StandardJerk", "10"}, + {"StoppingDistance", FrogsGoMoo ? "6" : "0"}, + {"TSS2Tune", "1"}, + {"TurnAggressiveness", FrogsGoMoo ? "150" : "100"}, + {"TurnDesires", "1"}, + {"UnlimitedLength", "1"}, + {"UseSI", FrogsGoMoo ? "1" : "0"}, + {"VisionTurnControl", "1"}, + {"WheelIcon", FrogsGoMoo ? "1" : "0"} + }; + + bool rebootRequired = false; + for (const auto &[key, value] : defaultValues) { + if (params.get(key).empty()) { + params.put(key, value); + rebootRequired = true; + } + } + + if (rebootRequired) { + while (!std::filesystem::exists("/data/openpilot/prebuilt")) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + Hardware::reboot(); + } +} diff --git a/selfdrive/frogpilot/ui/frogpilot_functions.h b/selfdrive/frogpilot/ui/frogpilot_functions.h new file mode 100644 index 0000000..06d1b49 --- /dev/null +++ b/selfdrive/frogpilot/ui/frogpilot_functions.h @@ -0,0 +1,545 @@ +#pragma once + +#include "selfdrive/ui/qt/widgets/controls.h" + +class FrogPilotConfirmationDialog : public ConfirmationDialog { + Q_OBJECT + +public: + explicit FrogPilotConfirmationDialog(const QString &prompt_text, const QString &confirm_text, + const QString &cancel_text, const bool rich, QWidget* parent); + static bool toggle(const QString &prompt_text, const QString &confirm_text, QWidget *parent); + static bool toggleAlert(const QString &prompt_text, const QString &button_text, QWidget *parent); + static bool yesorno(const QString &prompt_text, QWidget *parent); +}; + +class FrogPilotListWidget : public QWidget { + Q_OBJECT +public: + explicit FrogPilotListWidget(QWidget *parent = 0) : QWidget(parent), outer_layout(this) { + outer_layout.setMargin(0); + outer_layout.setSpacing(0); + outer_layout.addLayout(&inner_layout); + inner_layout.setMargin(0); + inner_layout.setSpacing(25); // default spacing is 25 + outer_layout.addStretch(); + } + inline void addItem(QWidget *w) { inner_layout.addWidget(w); } + inline void addItem(QLayout *layout) { inner_layout.addLayout(layout); } + inline void setSpacing(int spacing) { inner_layout.setSpacing(spacing); } + +private: + void paintEvent(QPaintEvent *) override { + QPainter p(this); + p.setPen(Qt::gray); + + int visibleWidgetCount = 0; + std::vector visibleRects; + + for (int i = 0; i < inner_layout.count(); ++i) { + QWidget *widget = inner_layout.itemAt(i)->widget(); + if (widget && widget->isVisible()) { + visibleWidgetCount++; + visibleRects.push_back(inner_layout.itemAt(i)->geometry()); + } + } + + for (int i = 0; i < visibleWidgetCount - 1; ++i) { + int bottom = visibleRects[i].bottom() + inner_layout.spacing() / 2; + p.drawLine(visibleRects[i].left() + 40, bottom, visibleRects[i].right() - 40, bottom); + } + } + QVBoxLayout outer_layout; + QVBoxLayout inner_layout; +}; + +class FrogPilotButtonIconControl : public AbstractControl { + Q_OBJECT + +public: + FrogPilotButtonIconControl(const QString &title, const QString &text, const QString &desc = "", const QString &icon = "", QWidget *parent = nullptr); + inline void setText(const QString &text) { btn.setText(text); } + inline QString text() const { return btn.text(); } + +signals: + void clicked(); + +public slots: + void setEnabled(bool enabled) { btn.setEnabled(enabled); } + +private: + QPushButton btn; +}; + +class FrogPilotButtonParamControl : public ParamControl { + Q_OBJECT +public: + FrogPilotButtonParamControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, + const std::vector &button_texts, const int minimum_button_width = 225) + : ParamControl(param, title, desc, icon) { + const QString style = R"( + QPushButton { + border-radius: 50px; + font-size: 40px; + font-weight: 500; + height:100px; + padding: 0 25 0 25; + color: #E4E4E4; + background-color: #393939; + } + QPushButton:pressed { + background-color: #4a4a4a; + } + QPushButton:checked:enabled { + background-color: #33Ab4C; + } + QPushButton:disabled { + color: #33E4E4E4; + } + )"; + + key = param.toStdString(); + int value = atoi(params.get(key).c_str()); + + button_group = new QButtonGroup(this); + button_group->setExclusive(true); + for (size_t i = 0; i < button_texts.size(); i++) { + QPushButton *button = new QPushButton(button_texts[i], this); + button->setCheckable(true); + button->setChecked(i == value); + button->setStyleSheet(style); + button->setMinimumWidth(minimum_button_width); + hlayout->addWidget(button); + button_group->addButton(button, i); + } + + QObject::connect(button_group, QOverload::of(&QButtonGroup::buttonToggled), [=](int id, bool checked) { + if (checked) { + params.put(key, std::to_string(id)); + refresh(); + emit buttonClicked(id); + } + }); + + toggle.hide(); + } + + void setEnabled(bool enable) { + for (auto btn : button_group->buttons()) { + btn->setEnabled(enable); + } + } + +signals: + void buttonClicked(int id); + +private: + std::string key; + Params params; + QButtonGroup *button_group; +}; + +class FrogPilotParamManageControl : public ParamControl { + Q_OBJECT + +public: + FrogPilotParamManageControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr) + : ParamControl(param, title, desc, icon, parent), + key(param.toStdString()), + manageButton(new ButtonControl(tr(""), tr("MANAGE"), tr(""))) { + hlayout->insertWidget(hlayout->indexOf(&toggle) - 1, manageButton); + + connect(this, &ToggleControl::toggleFlipped, this, [this](bool state) { + refresh(); + }); + + connect(manageButton, &ButtonControl::clicked, this, &FrogPilotParamManageControl::manageButtonClicked); + } + + void refresh() { + ParamControl::refresh(); + manageButton->setVisible(params.getBool(key)); + } + + void showEvent(QShowEvent *event) override { + ParamControl::showEvent(event); + refresh(); + } + +signals: + void manageButtonClicked(); + +private: + std::string key; + Params params; + ButtonControl *manageButton; +}; + +class FrogPilotParamToggleControl : public ParamControl { + Q_OBJECT +public: + FrogPilotParamToggleControl(const QString ¶m, const QString &title, const QString &desc, + const QString &icon, const std::vector &button_params, + const std::vector &button_texts, QWidget *parent = nullptr, + const int minimum_button_width = 225) + : ParamControl(param, title, desc, icon, parent) { + + connect(this, &ToggleControl::toggleFlipped, this, [this](bool state) { + refreshButtons(state); + }); + + const QString style = R"( + QPushButton { + border-radius: 50px; + font-size: 40px; + font-weight: 500; + height:100px; + padding: 0 25 0 25; + color: #E4E4E4; + background-color: #393939; + } + QPushButton:pressed { + background-color: #4a4a4a; + } + QPushButton:checked:enabled { + background-color: #33Ab4C; + } + QPushButton:disabled { + color: #33E4E4E4; + } + )"; + + button_group = new QButtonGroup(this); + button_group->setExclusive(false); + + std::map paramState; + for (const QString &button_param : button_params) { + paramState[button_param] = params.getBool(button_param.toStdString()); + } + + for (int i = 0; i < button_texts.size(); ++i) { + QPushButton *button = new QPushButton(button_texts[i], this); + button->setCheckable(true); + button->setChecked(paramState[button_params[i]]); + button->setStyleSheet(style); + button->setMinimumWidth(minimum_button_width); + button_group->addButton(button, i); + + connect(button, &QPushButton::clicked, [this, button_params, i](bool checked) { + params.putBool(button_params[i].toStdString(), checked); + button_group->button(i)->setChecked(checked); + emit buttonClicked(checked); + }); + + hlayout->insertWidget(hlayout->indexOf(&toggle), button); + } + } + + void refreshButtons(bool state) { + for (QAbstractButton *button : button_group->buttons()) { + button->setVisible(state); + } + } + +signals: + void buttonClicked(const bool checked); + +private: + Params params; + QButtonGroup *button_group; +}; + +class FrogPilotParamValueControl : public ParamControl { + Q_OBJECT + +public: + FrogPilotParamValueControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, + const int &minValue, const int &maxValue, const std::map &valueLabels, + QWidget *parent = nullptr, const bool &loop = true, const QString &label = "", const int &division = 1) + : ParamControl(param, title, desc, icon, parent), + minValue(minValue), maxValue(maxValue), valueLabelMappings(valueLabels), loop(loop), labelText(label), division(division) { + key = param.toStdString(); + + valueLabel = new QLabel(this); + hlayout->addWidget(valueLabel); + + QPushButton *decrementButton = createButton("-", this); + QPushButton *incrementButton = createButton("+", this); + + hlayout->addWidget(decrementButton); + hlayout->addWidget(incrementButton); + + connect(decrementButton, &QPushButton::clicked, this, [=]() { + updateValue(-1); + }); + + connect(incrementButton, &QPushButton::clicked, this, [=]() { + updateValue(1); + }); + + toggle.hide(); + } + + void updateValue(int increment) { + value = value + increment; + + if (loop) { + if (value < minValue) value = maxValue; + else if (value > maxValue) value = minValue; + } else { + value = std::max(minValue, std::min(maxValue, value)); + } + + params.putInt(key, value); + refresh(); + emit buttonPressed(); + emit valueChanged(value); + } + + void refresh() { + value = params.getInt(key); + + QString text; + auto it = valueLabelMappings.find(value); + if (division > 1) { + text = QString::number(value / (division * 1.0), 'g'); + } else { + text = it != valueLabelMappings.end() ? it->second : QString::number(value); + } + if (!labelText.isEmpty()) { + text += labelText; + } + valueLabel->setText(text); + valueLabel->setStyleSheet("QLabel { color: #E0E879; }"); + } + + void updateControl(int newMinValue, int newMaxValue, const QString &newLabel) { + minValue = newMinValue; + maxValue = newMaxValue; + labelText = newLabel; + } + + void showEvent(QShowEvent *event) override { + refresh(); + } + +signals: + void buttonPressed(); + void valueChanged(int value); + +private: + bool loop; + int division; + int maxValue; + int minValue; + int value; + QLabel *valueLabel; + QString labelText; + std::map valueLabelMappings; + std::string key; + Params params; + + QPushButton *createButton(const QString &text, QWidget *parent) { + QPushButton *button = new QPushButton(text, parent); + button->setFixedSize(150, 100); + button->setAutoRepeat(true); + button->setAutoRepeatInterval(150); + button->setStyleSheet(R"( + QPushButton { + border-radius: 50px; + font-size: 50px; + font-weight: 500; + height: 100px; + padding: 0 25 0 25; + color: #E4E4E4; + background-color: #393939; + } + QPushButton:pressed { + background-color: #4a4a4a; + } + )"); + return button; + } +}; + +class FrogPilotDualParamControl : public QFrame { + Q_OBJECT + +public: + FrogPilotDualParamControl(ParamControl *control1, ParamControl *control2, QWidget *parent = nullptr, bool split=false) + : QFrame(parent) { + QHBoxLayout *hlayout = new QHBoxLayout(this); + + control1->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Preferred); + control1->setMaximumWidth(split ? 800 : 700); + + control2->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred); + control2->setMaximumWidth(800); + + hlayout->addWidget(control1); + hlayout->addWidget(control2); + } +}; + +class FrogPilotParamValueToggleControl : public ParamControl { + Q_OBJECT + +public: + FrogPilotParamValueToggleControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, + const int &minValue, const int &maxValue, const std::map &valueLabels, + QWidget *parent = nullptr, const bool &loop = true, const QString &label = "", const int &division = 1, + const std::vector &button_params = std::vector(), const std::vector &button_texts = std::vector(), + const int minimum_button_width = 225) + : ParamControl(param, title, desc, icon, parent), + minValue(minValue), maxValue(maxValue), valueLabelMappings(valueLabels), loop(loop), labelText(label), division(division) { + key = param.toStdString(); + + const QString style = R"( + QPushButton { + border-radius: 50px; + font-size: 40px; + font-weight: 500; + height:100px; + padding: 0 25 0 25; + color: #E4E4E4; + background-color: #393939; + } + QPushButton:pressed { + background-color: #4a4a4a; + } + QPushButton:checked:enabled { + background-color: #33Ab4C; + } + QPushButton:disabled { + color: #33E4E4E4; + } + )"; + + button_group = new QButtonGroup(this); + button_group->setExclusive(false); + + std::map paramState; + for (const QString &button_param : button_params) { + paramState[button_param] = params.getBool(button_param.toStdString()); + } + + for (int i = 0; i < button_texts.size(); ++i) { + QPushButton *button = new QPushButton(button_texts[i], this); + button->setCheckable(true); + button->setChecked(paramState[button_params[i]]); + button->setStyleSheet(style); + button->setMinimumWidth(minimum_button_width); + button_group->addButton(button, i); + + connect(button, &QPushButton::clicked, [this, button_params, i](bool checked) { + params.putBool(button_params[i].toStdString(), checked); + button_group->button(i)->setChecked(checked); + }); + + hlayout->addWidget(button); + } + + valueLabel = new QLabel(this); + hlayout->addWidget(valueLabel); + + QPushButton *decrementButton = createButton("-", this); + QPushButton *incrementButton = createButton("+", this); + + hlayout->addWidget(decrementButton); + hlayout->addWidget(incrementButton); + + connect(decrementButton, &QPushButton::clicked, this, [=]() { + updateValue(-1); + }); + + connect(incrementButton, &QPushButton::clicked, this, [=]() { + updateValue(1); + }); + + toggle.hide(); + } + + void updateValue(int increment) { + value = value + increment; + + if (loop) { + if (value < minValue) value = maxValue; + else if (value > maxValue) value = minValue; + } else { + value = std::max(minValue, std::min(maxValue, value)); + } + + params.putInt(key, value); + refresh(); + emit buttonPressed(); + emit valueChanged(value); + } + + void refresh() { + value = params.getInt(key); + + QString text; + auto it = valueLabelMappings.find(value); + if (division > 1) { + text = QString::number(value / (division * 1.0), 'g'); + } else { + text = it != valueLabelMappings.end() ? it->second : QString::number(value); + } + if (!labelText.isEmpty()) { + text += labelText; + } + valueLabel->setText(text); + valueLabel->setStyleSheet("QLabel { color: #E0E879; }"); + } + + void updateControl(int newMinValue, int newMaxValue, const QString &newLabel) { + minValue = newMinValue; + maxValue = newMaxValue; + labelText = newLabel; + } + + void showEvent(QShowEvent *event) override { + refresh(); + } + +signals: + void buttonPressed(); + void valueChanged(int value); + +private: + bool loop; + int division; + int maxValue; + int minValue; + int value; + QButtonGroup *button_group; + QLabel *valueLabel; + QString labelText; + std::map valueLabelMappings; + std::string key; + Params params; + + QPushButton *createButton(const QString &text, QWidget *parent) { + QPushButton *button = new QPushButton(text, parent); + button->setFixedSize(150, 100); + button->setAutoRepeat(true); + button->setAutoRepeatInterval(150); + button->setStyleSheet(R"( + QPushButton { + border-radius: 50px; + font-size: 50px; + font-weight: 500; + height: 100px; + padding: 0 25 0 25; + color: #E4E4E4; + background-color: #393939; + } + QPushButton:pressed { + background-color: #4a4a4a; + } + )"); + return button; + } +}; + +void setDefaultParams(); diff --git a/selfdrive/frogpilot/ui/vehicle_settings.cc b/selfdrive/frogpilot/ui/vehicle_settings.cc new file mode 100644 index 0000000..e093b4a --- /dev/null +++ b/selfdrive/frogpilot/ui/vehicle_settings.cc @@ -0,0 +1,56 @@ +#include "selfdrive/frogpilot/ui/vehicle_settings.h" +#include "selfdrive/ui/ui.h" + +FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { + std::vector> vehicleToggles { + }; + + for (auto &[param, title, desc, icon] : vehicleToggles) { + ParamControl *toggle = new ParamControl(param, title, desc, icon, this); + + addItem(toggle); + toggle->setVisible(false); + toggles[param.toStdString()] = toggle; + + QObject::connect(toggle, &ToggleControl::toggleFlipped, [this]() { + updateToggles(); + }); + } + + gmKeys = {}; + toyotaKeys = {}; + + std::set rebootKeys = {}; + for (const std::string &key : rebootKeys) { + QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this]() { + if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) { + Hardware::reboot(); + } + }); + } +} + +void FrogPilotVehiclesPanel::updateToggles() { + std::thread([this]() { + paramsMemory.putBool("FrogPilotTogglesUpdated", true); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + paramsMemory.putBool("FrogPilotTogglesUpdated", false); + }).detach(); +} + +void FrogPilotVehiclesPanel::setToggles() { + bool gm = false; + bool toyota = false; + + for (auto &[key, toggle] : toggles) { + toggle->setVisible(false); + + if (gm) { + toggle->setVisible(gmKeys.find(key.c_str()) != gmKeys.end()); + } else if (toyota) { + toggle->setVisible(toyotaKeys.find(key.c_str()) != toyotaKeys.end()); + } + } + + update(); +} diff --git a/selfdrive/frogpilot/ui/vehicle_settings.h b/selfdrive/frogpilot/ui/vehicle_settings.h new file mode 100644 index 0000000..3986826 --- /dev/null +++ b/selfdrive/frogpilot/ui/vehicle_settings.h @@ -0,0 +1,25 @@ +#pragma once + +#include + +#include "selfdrive/frogpilot/ui/frogpilot_functions.h" +#include "selfdrive/ui/qt/offroad/settings.h" + +class FrogPilotVehiclesPanel : public FrogPilotListWidget { + Q_OBJECT + +public: + explicit FrogPilotVehiclesPanel(SettingsWindow *parent); + +private: + void setToggles(); + void updateToggles(); + + std::map toggles; + + std::set gmKeys; + std::set toyotaKeys; + + Params params; + Params paramsMemory{"/dev/shm/params"}; +}; diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc new file mode 100644 index 0000000..5fea03c --- /dev/null +++ b/selfdrive/frogpilot/ui/visual_settings.cc @@ -0,0 +1,86 @@ +#include "selfdrive/frogpilot/ui/visual_settings.h" +#include "selfdrive/ui/ui.h" + +FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { + const std::vector> visualToggles { + }; + + for (const auto &[param, title, desc, icon] : visualToggles) { + ParamControl *toggle; + + toggle = new ParamControl(param, title, desc, icon, this); + + addItem(toggle); + toggles[param.toStdString()] = toggle; + + QObject::connect(toggle, &ToggleControl::toggleFlipped, [this]() { + updateToggles(); + }); + + QObject::connect(static_cast(toggle), &FrogPilotParamValueControl::buttonPressed, [this]() { + updateToggles(); + }); + + QObject::connect(toggle, &AbstractControl::showDescriptionEvent, [this]() { + update(); + }); + + QObject::connect(static_cast(toggle), &FrogPilotParamManageControl::manageButtonClicked, [this]() { + update(); + }); + } + + customOnroadUIKeys = {}; + customThemeKeys = {}; + modelUIKeys = {}; + + QObject::connect(device(), &Device::interactiveTimeout, this, &FrogPilotVisualsPanel::hideSubToggles); + QObject::connect(parent, &SettingsWindow::closeParentToggle, this, &FrogPilotVisualsPanel::hideSubToggles); + QObject::connect(parent, &SettingsWindow::updateMetric, this, &FrogPilotVisualsPanel::updateMetric); + + hideSubToggles(); + updateMetric(); +} + +void FrogPilotVisualsPanel::updateToggles() { + std::thread([this]() { + paramsMemory.putBool("FrogPilotTogglesUpdated", true); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + paramsMemory.putBool("FrogPilotTogglesUpdated", false); + }).detach(); +} + +void FrogPilotVisualsPanel::updateMetric() { + bool previousIsMetric = isMetric; + isMetric = params.getBool("IsMetric"); + + if (isMetric != previousIsMetric) { + double distanceConversion = isMetric ? INCH_TO_CM : CM_TO_INCH; + double speedConversion = isMetric ? FOOT_TO_METER : METER_TO_FOOT; + } + + if (isMetric) { + } else { + } + + previousIsMetric = isMetric; +} + +void FrogPilotVisualsPanel::parentToggleClicked() { + this->openParentToggle(); +} + +void FrogPilotVisualsPanel::hideSubToggles() { + for (auto &[key, toggle] : toggles) { + bool subToggles = modelUIKeys.find(key.c_str()) != modelUIKeys.end() || + customOnroadUIKeys.find(key.c_str()) != customOnroadUIKeys.end() || + customThemeKeys.find(key.c_str()) != customThemeKeys.end(); + toggle->setVisible(!subToggles); + } + + this->closeParentToggle(); +} + +void FrogPilotVisualsPanel::hideEvent(QHideEvent *event) { + hideSubToggles(); +} diff --git a/selfdrive/frogpilot/ui/visual_settings.h b/selfdrive/frogpilot/ui/visual_settings.h new file mode 100644 index 0000000..55602cb --- /dev/null +++ b/selfdrive/frogpilot/ui/visual_settings.h @@ -0,0 +1,35 @@ +#pragma once + +#include + +#include "selfdrive/frogpilot/ui/frogpilot_functions.h" +#include "selfdrive/ui/qt/offroad/settings.h" + +class FrogPilotVisualsPanel : public FrogPilotListWidget { + Q_OBJECT + +public: + explicit FrogPilotVisualsPanel(SettingsWindow *parent); + +signals: + void closeParentToggle(); + void openParentToggle(); + +private: + void hideEvent(QHideEvent *event); + void hideSubToggles(); + void parentToggleClicked(); + void updateMetric(); + void updateToggles(); + + std::set customOnroadUIKeys; + std::set customThemeKeys; + std::set modelUIKeys; + + std::map toggles; + + Params params; + Params paramsMemory{"/dev/shm/params"}; + + bool isMetric = params.getBool("IsMetric"); +}; diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 36d69b0..41a59cf 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -41,6 +41,8 @@ def only_onroad(started: bool, params, CP: car.CarParams) -> bool: def only_offroad(started, params, CP: car.CarParams) -> bool: return not started +# FrogPilot functions + procs = [ DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), @@ -86,6 +88,8 @@ procs = [ NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar), PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), PythonProcess("webjoystick", "tools.bodyteleop.web", notcar), + + # FrogPilot processes ] managed_processes = {p.name: p for p in procs} diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 2be703c..863e251 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -28,6 +28,7 @@ class RouteEngine: self.pm = pm self.params = Params() + self.params_memory = Params("/dev/shm/params") # Get last gps position from params self.last_position = coordinate_from_param("LastGPSPosition", self.params) @@ -59,7 +60,14 @@ class RouteEngine: self.mapbox_token = "" self.mapbox_host = "https://maps.comma.ai" + # FrogPilot variables + self.update_frogpilot_params() + def update(self): + # Update FrogPilot parameters + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + self.sm.update(0) if self.sm.updated["managerState"]: @@ -301,6 +309,11 @@ class RouteEngine: self.params.remove("NavDestination") self.clear_route() + frogpilot_plan_send = messaging.new_message('frogpilotNavigation') + frogpilotNavigation = frogpilot_plan_send.frogpilotNavigation + + self.pm.send('frogpilotNavigation', frogpilot_plan_send) + def send_route(self): coords = [] @@ -349,9 +362,10 @@ class RouteEngine: return self.reroute_counter > REROUTE_COUNTER_MIN # TODO: Check for going wrong way in segment + def update_frogpilot_params(self): def main(): - pm = messaging.PubMaster(['navInstruction', 'navRoute']) + pm = messaging.PubMaster(['navInstruction', 'navRoute', 'frogpilotNavigation']) sm = messaging.SubMaster(['liveLocationKalman', 'managerState']) rk = Ratekeeper(1.0) diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 8802a82..5f0a4fe 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -38,6 +38,8 @@ class PowerMonitoring: # Reset capacity if it's low self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh)) + # FrogPilot variables + # Calculation tick def calculate(self, voltage: Optional[int], ignition: bool): try: diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 7011ff0..9280225 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -167,7 +167,7 @@ def hw_state_thread(end_event, hw_queue): def thermald_thread(end_event, hw_queue) -> None: - pm = messaging.PubMaster(['deviceState']) + pm = messaging.PubMaster(['deviceState', 'frogpilotDeviceState']) sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll=["pandaStates"]) count = 0 @@ -245,6 +245,10 @@ def thermald_thread(end_event, hw_queue) -> None: except queue.Empty: pass + fpmsg = messaging.new_message('frogpilotDeviceState') + + pm.send("frogpilotDeviceState", fpmsg) + msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent)) msg.deviceState.cpuUsagePercent = [int(round(n)) for n in psutil.cpu_percent(percpu=True)] diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 7b570b9..eed1a52 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -24,7 +24,10 @@ widgets_src = ["ui.cc", "qt/widgets/input.cc", "qt/widgets/wifi.cc", "qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc", "qt/widgets/offroad_alerts.cc", "qt/widgets/prime.cc", "qt/widgets/keyboard.cc", "qt/widgets/scrollview.cc", "qt/widgets/cameraview.cc", "#third_party/qrcode/QrCode.cc", - "qt/request_repeater.cc", "qt/qt_window.cc", "qt/network/networking.cc", "qt/network/wifi_manager.cc"] + "qt/request_repeater.cc", "qt/qt_window.cc", "qt/network/networking.cc", "qt/network/wifi_manager.cc", + "../frogpilot/ui/frogpilot_functions.cc", + "../frogpilot/ui/control_settings.cc", "../frogpilot/ui/vehicle_settings.cc", + "../frogpilot/ui/visual_settings.cc"] qt_env['CPPDEFINES'] = [] if maps: diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h index c603285..b9ecfa5 100644 --- a/selfdrive/ui/qt/home.h +++ b/selfdrive/ui/qt/home.h @@ -39,6 +39,8 @@ private: OffroadAlert* alerts_widget; QPushButton* alert_notif; QPushButton* update_notif; + + // FrogPilot variables }; class HomeWindow : public QWidget { diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index 5fe79f8..243ab96 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -77,6 +77,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/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index 4d6e9b5..3d045cb 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -7,6 +7,7 @@ #include #include +#include "common/params.h" #include "common/util.h" #include "common/transformations/coordinates.hpp" #include "common/transformations/orientation.hpp" diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 669de5e..a64174c 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -7,6 +7,7 @@ #include #include +#include #include "selfdrive/ui/qt/network/networking.h" @@ -23,6 +24,10 @@ #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/frogpilot/ui/control_settings.h" +#include "selfdrive/frogpilot/ui/vehicle_settings.h" +#include "selfdrive/frogpilot/ui/visual_settings.h" + TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { // param, title, desc, icon std::vector> toggle_defs{ @@ -117,6 +122,10 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { connect(toggles["ExperimentalLongitudinalEnabled"], &ToggleControl::toggleFlipped, [=]() { updateToggles(); }); + + connect(toggles["IsMetric"], &ToggleControl::toggleFlipped, [=]() { + updateMetric(); + }); } void TogglesPanel::expandToggleDescription(const QString ¶m) { @@ -368,7 +377,14 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { close_btn->setFixedSize(200, 200); sidebar_layout->addSpacing(45); sidebar_layout->addWidget(close_btn, 0, Qt::AlignCenter); - QObject::connect(close_btn, &QPushButton::clicked, this, &SettingsWindow::closeSettings); + QObject::connect(close_btn, &QPushButton::clicked, [this]() { + if (frogPilotTogglesOpen) { + frogPilotTogglesOpen = false; + this->closeParentToggle(); + } else { + this->closeSettings(); + } + }); // setup panels DevicePanel *device = new DevicePanel(this); @@ -377,12 +393,24 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { TogglesPanel *toggles = new TogglesPanel(this); QObject::connect(this, &SettingsWindow::expandToggleDescription, toggles, &TogglesPanel::expandToggleDescription); + QObject::connect(toggles, &TogglesPanel::updateMetric, this, &SettingsWindow::updateMetric); + + FrogPilotControlsPanel *frogpilotControls = new FrogPilotControlsPanel(this); + QObject::connect(frogpilotControls, &FrogPilotControlsPanel::closeParentToggle, this, [this]() {frogPilotTogglesOpen = false;}); + QObject::connect(frogpilotControls, &FrogPilotControlsPanel::openParentToggle, this, [this]() {frogPilotTogglesOpen = true;}); + + FrogPilotVisualsPanel *frogpilotVisuals = new FrogPilotVisualsPanel(this); + QObject::connect(frogpilotVisuals, &FrogPilotVisualsPanel::closeParentToggle, this, [this]() {frogPilotTogglesOpen = false;}); + QObject::connect(frogpilotVisuals, &FrogPilotVisualsPanel::openParentToggle, this, [this]() {frogPilotTogglesOpen = true;}); QList> panels = { {tr("Device"), device}, {tr("Network"), new Networking(this)}, {tr("Toggles"), toggles}, {tr("Software"), new SoftwarePanel(this)}, + {tr("Controls"), frogpilotControls}, + {tr("Vehicles"), new FrogPilotVehiclesPanel(this)}, + {tr("Visuals"), frogpilotVisuals}, }; nav_btns = new QButtonGroup(this); @@ -415,7 +443,22 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { ScrollView *panel_frame = new ScrollView(panel, this); panel_widget->addWidget(panel_frame); + if (name == tr("Controls") || name == tr("Visuals")) { + QScrollBar *scrollbar = panel_frame->verticalScrollBar(); + + QObject::connect(scrollbar, &QScrollBar::valueChanged, this, [this](int value) { + if (!frogPilotTogglesOpen) { + previousScrollPosition = value; + } + }); + + QObject::connect(scrollbar, &QScrollBar::rangeChanged, this, [this, panel_frame]() { + panel_frame->restorePosition(previousScrollPosition); + }); + } + QObject::connect(btn, &QPushButton::clicked, [=, w = panel_frame]() { + previousScrollPosition = 0; btn->setChecked(true); panel_widget->setCurrentWidget(w); }); diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index a5dd25b..c5291ee 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -31,11 +31,18 @@ signals: void showDriverView(); void expandToggleDescription(const QString ¶m); + // FrogPilot signals + void closeParentToggle(); + void updateMetric(); private: QPushButton *sidebar_alert_widget; QWidget *sidebar_widget; QButtonGroup *nav_btns; QStackedWidget *panel_widget; + + // FrogPilot variables + bool frogPilotTogglesOpen; + int previousScrollPosition; }; class DevicePanel : public ListWidget { @@ -61,6 +68,10 @@ public: explicit TogglesPanel(SettingsWindow *parent); void showEvent(QShowEvent *event) override; +signals: + // FrogPilot signals + void updateMetric(); + public slots: void expandToggleDescription(const QString ¶m); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 4df0ed8..ba07a76 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -28,7 +28,7 @@ static void drawIcon(QPainter &p, const QPoint ¢er, const QPixmap &img, cons p.setOpacity(1.0); } -OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { +OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent), scene(uiState()->scene) { QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->setMargin(UI_BORDER_SIZE); QStackedLayout *stacked_layout = new QStackedLayout; @@ -93,8 +93,14 @@ void OnroadWindow::updateState(const UIState &s) { } void OnroadWindow::mousePressEvent(QMouseEvent* e) { + Params params = Params(); + Params paramsMemory = Params("/dev/shm/params"); + + // FrogPilot clickable widgets + bool widgetClicked = false; + #ifdef ENABLE_MAPS - if (map != nullptr) { + if (map != nullptr && !widgetClicked) { // Switch between map and sidebar when using navigate on openpilot bool sidebarVisible = geometry().x() > 0; bool show_map = uiState()->scene.navigate_on_openpilot ? sidebarVisible : !sidebarVisible; @@ -102,7 +108,9 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { } #endif // propagation event to parent(HomeWindow) - QWidget::mousePressEvent(e); + if (!widgetClicked) { + QWidget::mousePressEvent(e); + } } void OnroadWindow::offroadTransition(bool offroad) { @@ -114,6 +122,7 @@ void OnroadWindow::offroadTransition(bool offroad) { QObject::connect(m, &MapPanel::mapPanelRequested, this, &OnroadWindow::mapPanelRequested); QObject::connect(nvg->map_settings_btn, &MapSettingsButton::clicked, m, &MapPanel::toggleMapSettings); + QObject::connect(nvg->map_settings_btn_bottom, &MapSettingsButton::clicked, m, &MapPanel::toggleMapSettings); nvg->map_settings_btn->setEnabled(true); m->setFixedWidth(topWidget(this)->width() / 2 - UI_BORDER_SIZE); @@ -140,6 +149,8 @@ void OnroadWindow::primeChanged(bool prime) { } void OnroadWindow::paintEvent(QPaintEvent *event) { + Params paramsMemory = Params("/dev/shm/params"); + QPainter p(this); p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255)); } @@ -167,11 +178,13 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { int margin = 40; int radius = 30; + int offset = true ? 25 : 0; if (alert.size == cereal::ControlsState::AlertSize::FULL) { margin = 0; radius = 0; + offset = 0; } - QRect r = QRect(0 + margin, height() - h + margin, width() - margin*2, h - margin*2); + QRect r = QRect(0 + margin, height() - h + margin - offset, width() - margin*2, h - margin*2); QPainter p(this); @@ -212,7 +225,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { } // ExperimentalButton -ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent) { +ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent), scene(uiState()->scene) { setFixedSize(btn_size, btn_size); engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); @@ -221,6 +234,8 @@ ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(fals } void ExperimentalButton::changeMode() { + Params paramsMemory = Params("/dev/shm/params"); + const auto cp = (*uiState()->sm)["carParams"].getCarParams(); bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed"); if (can_change) { @@ -236,6 +251,8 @@ void ExperimentalButton::updateState(const UIState &s) { experimental_mode = cs.getExperimentalMode(); update(); } + + // FrogPilot variables } void ExperimentalButton::paintEvent(QPaintEvent *event) { @@ -247,7 +264,7 @@ void ExperimentalButton::paintEvent(QPaintEvent *event) { // MapSettingsButton MapSettingsButton::MapSettingsButton(QWidget *parent) : QPushButton(parent) { - setFixedSize(btn_size, btn_size); + setFixedSize(btn_size, btn_size + 20); settings_img = loadPixmap("../assets/navigation/icon_directions_outlined.svg", {img_size, img_size}); // hidden by default, made visible if map is created (has prime or mapbox token) @@ -262,7 +279,7 @@ void MapSettingsButton::paintEvent(QPaintEvent *event) { // Window that shows camera view and variety of info drawn on top -AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent) { +AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent), scene(uiState()->scene) { pm = std::make_unique>({"uiDebug"}); main_layout = new QVBoxLayout(this); @@ -276,6 +293,9 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* par main_layout->addWidget(map_settings_btn, 0, Qt::AlignBottom | Qt::AlignRight); dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5}); + + // Initialize FrogPilot widgets + initializeFrogPilotWidgets(); } void AnnotatedCameraWidget::updateState(const UIState &s) { @@ -468,7 +488,6 @@ void AnnotatedCameraWidget::updateFrameMat() { void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.save(); - const UIScene &scene = s->scene; SubMaster &sm = *(s->sm); // lanelines @@ -525,12 +544,11 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { } void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) { - const UIScene &scene = s->scene; - painter.save(); // base icon int offset = UI_BORDER_SIZE + btn_size / 2; + offset += true ? 25 : 0; int x = rightHandDM ? width() - offset : offset; int y = height() - offset; float opacity = dmActive ? 0.65 : 0.2; @@ -693,6 +711,9 @@ void AnnotatedCameraWidget::paintGL() { auto m = msg.initEvent().initUiDebug(); m.setDrawTimeMillis(cur_draw_t - start_draw_t); pm->send("uiDebug", msg); + + // Update FrogPilot widgets + updateFrogPilotWidgets(painter); } void AnnotatedCameraWidget::showEvent(QShowEvent *event) { @@ -701,3 +722,82 @@ void AnnotatedCameraWidget::showEvent(QShowEvent *event) { ui_update_params(uiState()); prev_draw_t = millis_since_boot(); } + +// FrogPilot widgets +void AnnotatedCameraWidget::initializeFrogPilotWidgets() { + Params params = Params(); + + bottom_layout = new QHBoxLayout(); + + QSpacerItem *spacer = new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Minimum); + bottom_layout->addItem(spacer); + + map_settings_btn_bottom = new MapSettingsButton(this); + bottom_layout->addWidget(map_settings_btn_bottom); + + main_layout->addLayout(bottom_layout); +} + +void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { + experimentalMode = scene.experimental_mode; + + if (true) { + drawStatusBar(p); + } + + map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled()); + if (map_settings_btn_bottom->isEnabled()) { + map_settings_btn_bottom->setVisible(!hideBottomIcons); + bottom_layout->setAlignment(map_settings_btn_bottom, rightHandDM ? Qt::AlignLeft : Qt::AlignRight); + } +} + +void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { + p.save(); + + // Variable declarations + QElapsedTimer timer; + static QString lastShownStatus; + static QString newStatus; + + static bool displayStatusText = false; + + constexpr qreal fadeDuration = 1500.0; + constexpr qreal textDuration = 5000.0; + + // Draw status bar + QRect currentRect = rect(); + QRect statusBarRect(currentRect.left() - 1, currentRect.bottom() - 50, currentRect.width() + 2, 100); + p.setBrush(QColor(0, 0, 0, 150)); + p.setOpacity(1.0); + p.drawRoundedRect(statusBarRect, 30, 30); + + // Check if status has changed + if (newStatus != lastShownStatus) { + displayStatusText = true; + lastShownStatus = newStatus; + timer.restart(); + } else if (displayStatusText && timer.hasExpired(textDuration + fadeDuration)) { + displayStatusText = false; + } + + // Configure the text + p.setFont(InterFont(40, QFont::Bold)); + p.setPen(Qt::white); + p.setRenderHint(QPainter::TextAntialiasing); + + // Calculate text opacity + static qreal statusTextOpacity; + int elapsed = timer.elapsed(); + if (displayStatusText) { + statusTextOpacity = qBound(0.0, 1.0 - (elapsed - textDuration) / fadeDuration, 1.0); + } + + // Draw the status text + p.setOpacity(statusTextOpacity); + QRect textRect = p.fontMetrics().boundingRect(statusBarRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus); + textRect.moveBottom(statusBarRect.bottom() - 50); + p.drawText(textRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus); + + p.restore(); +} diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index b3ba411..ef8be08 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -14,13 +14,14 @@ const int btn_size = 192; const int img_size = (btn_size / 4) * 3; +// FrogPilot global variables // ***** onroad widgets ***** class OnroadAlerts : public QWidget { Q_OBJECT public: - OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {} + OnroadAlerts(QWidget *parent = 0) : QWidget(parent), scene(uiState()->scene) {} void updateAlert(const Alert &a); protected: @@ -29,6 +30,9 @@ protected: private: QColor bg; Alert alert = {}; + + // FrogPilot variables + UIScene &scene; }; class ExperimentalButton : public QPushButton { @@ -47,6 +51,9 @@ private: QPixmap experimental_img; bool experimental_mode; bool engageable; + + // FrogPilot variables + UIScene &scene; }; @@ -71,6 +78,7 @@ public: void updateState(const UIState &s); MapSettingsButton *map_settings_btn; + MapSettingsButton *map_settings_btn_bottom; private: void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); @@ -97,6 +105,20 @@ private: int skip_frame_count = 0; bool wide_cam_requested = false; + // FrogPilot widgets + void drawStatusBar(QPainter &p); + void initializeFrogPilotWidgets(); + void updateFrogPilotWidgets(QPainter &p); + + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; + + UIScene &scene; + + QHBoxLayout *bottom_layout; + + bool experimentalMode; + protected: void paintGL() override; void initializeGL() override; @@ -135,6 +157,9 @@ private: QWidget *map = nullptr; QHBoxLayout* split; + // FrogPilot variables + UIScene &scene; + private slots: void offroadTransition(bool offroad); void primeChanged(bool prime); diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index e952940..05fd3eb 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -36,8 +36,12 @@ Sidebar::Sidebar(QWidget *parent) : QFrame(parent), onroad(false), flag_pressed( setFixedWidth(300); QObject::connect(uiState(), &UIState::uiUpdate, this, &Sidebar::updateState); + QObject::connect(uiState(), &UIState::uiUpdateFrogPilotParams, this, &Sidebar::updateFrogPilotParams); pm = std::make_unique>({"userFlag"}); + + // FrogPilot variables + updateFrogPilotParams(); } void Sidebar::mousePressEvent(QMouseEvent *event) { @@ -79,6 +83,9 @@ void Sidebar::updateState(const UIState &s) { int strength = (int)deviceState.getNetworkStrength(); setProperty("netStrength", strength > 0 ? strength + 1 : 0); + // FrogPilot properties + auto frogpilotDeviceState = sm["frogpilotDeviceState"].getFrogpilotDeviceState(); + ItemStatus connectStatus; auto last_ping = deviceState.getLastAthenaPingTime(); if (last_ping == 0) { @@ -108,6 +115,10 @@ void Sidebar::updateState(const UIState &s) { setProperty("pandaStatus", QVariant::fromValue(pandaStatus)); } +void Sidebar::updateFrogPilotParams() { + // Update FrogPilot parameters upon toggle change +} + void Sidebar::paintEvent(QPaintEvent *event) { QPainter p(this); p.setPen(Qt::NoPen); diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h index f627aac..ea0d51d 100644 --- a/selfdrive/ui/qt/sidebar.h +++ b/selfdrive/ui/qt/sidebar.h @@ -18,6 +18,8 @@ class Sidebar : public QFrame { Q_PROPERTY(QString netType MEMBER net_type NOTIFY valueChanged); Q_PROPERTY(int netStrength MEMBER net_strength NOTIFY valueChanged); + // FrogPilot properties + public: explicit Sidebar(QWidget* parent = 0); @@ -29,6 +31,9 @@ public slots: void offroadTransition(bool offroad); void updateState(const UIState &s); + // FrogPilot slots + void updateFrogPilotParams(); + protected: void paintEvent(QPaintEvent *event) override; void mousePressEvent(QMouseEvent *event) override; @@ -59,4 +64,7 @@ protected: private: std::unique_ptr pm; + + // FrogPilot variables + Params params; }; diff --git a/selfdrive/ui/qt/spinner.cc b/selfdrive/ui/qt/spinner.cc index 2404efa..7ca2f1a 100644 --- a/selfdrive/ui/qt/spinner.cc +++ b/selfdrive/ui/qt/spinner.cc @@ -88,7 +88,7 @@ Spinner::Spinner(QWidget *parent) : QWidget(parent) { } QProgressBar::chunk { border-radius: 10px; - background-color: white; + background-color: rgba(23, 134, 68, 255); } )"); diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index c5222b8..d3981ae 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -25,7 +25,7 @@ QString getVersion() { } QString getBrand() { - return Params().getBool("Passive") ? QObject::tr("dashcam") : QObject::tr("openpilot"); + return Params().getBool("Passive") ? QObject::tr("dashcam") : QObject::tr("FrogPilot"); } QString getUserAgent() { diff --git a/selfdrive/ui/qt/widgets/scrollview.cc b/selfdrive/ui/qt/widgets/scrollview.cc index 978bf83..2846007 100644 --- a/selfdrive/ui/qt/widgets/scrollview.cc +++ b/selfdrive/ui/qt/widgets/scrollview.cc @@ -44,6 +44,10 @@ ScrollView::ScrollView(QWidget *w, QWidget *parent) : QScrollArea(parent) { scroller->setScrollerProperties(sp); } +void ScrollView::restorePosition(int previousScrollPosition) { + verticalScrollBar()->setValue(previousScrollPosition); +} + void ScrollView::hideEvent(QHideEvent *e) { verticalScrollBar()->setValue(0); } diff --git a/selfdrive/ui/qt/widgets/scrollview.h b/selfdrive/ui/qt/widgets/scrollview.h index 024331a..1e67bbe 100644 --- a/selfdrive/ui/qt/widgets/scrollview.h +++ b/selfdrive/ui/qt/widgets/scrollview.h @@ -7,6 +7,9 @@ class ScrollView : public QScrollArea { public: explicit ScrollView(QWidget *w = nullptr, QWidget *parent = nullptr); + + // FrogPilot functions + void restorePosition(int previousScrollPosition); protected: void hideEvent(QHideEvent *e) override; }; diff --git a/selfdrive/ui/qt/window.h b/selfdrive/ui/qt/window.h index 05b61e1..f1389c2 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 01148ec..0962c1e 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -8,6 +8,7 @@ from typing import Dict, Optional, Tuple from cereal import car, messaging from openpilot.common.basedir import BASEDIR from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper from openpilot.common.retry import retry from openpilot.common.swaglog import cloudlog @@ -53,6 +54,12 @@ def check_controls_timeout_alert(sm): class Soundd: def __init__(self): + # FrogPilot variables + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.update_frogpilot_params() + self.load_sounds() self.current_alert = AudibleAlert.none @@ -156,6 +163,11 @@ class Soundd: assert stream.active + # Update FrogPilot parameters + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + + def update_frogpilot_params(self): def main(): s = Soundd() diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 9afd22f..2caad9b 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -13,6 +13,8 @@ #include "common/watchdog.h" #include "system/hardware/hw.h" +#include "selfdrive/frogpilot/ui/frogpilot_functions.h" + #define BACKLIGHT_DT 0.05 #define BACKLIGHT_TS 10.00 @@ -201,6 +203,26 @@ static void update_state(UIState *s) { if (sm.updated("carParams")) { scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); } + if (sm.updated("carState")) { + auto carState = sm["carState"].getCarState(); + } + if (sm.updated("controlsState")) { + auto controlsState = sm["controlsState"].getControlsState(); + scene.enabled = controlsState.getEnabled(); + scene.experimental_mode = controlsState.getExperimentalMode(); + } + if (sm.updated("frogpilotCarControl")) { + auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl(); + } + if (sm.updated("frogpilotLateralPlan")) { + auto frogpilotLateralPlan = sm["frogpilotLateralPlan"].getFrogpilotLateralPlan(); + } + if (sm.updated("frogpilotLongitudinalPlan")) { + auto frogpilotLongitudinalPlan = sm["frogpilotLongitudinalPlan"].getFrogpilotLongitudinalPlan(); + } + if (sm.updated("liveLocationKalman")) { + auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); + } if (sm.updated("wideRoadCameraState")) { auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState(); float scale = (cam_state.getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f; @@ -219,6 +241,9 @@ void ui_update_params(UIState *s) { auto params = Params(); s->scene.is_metric = params.getBool("IsMetric"); s->scene.map_on_left = params.getBool("NavSettingLeftSide"); + + // FrogPilot variables + UIScene &scene = s->scene; } void UIState::updateStatus() { @@ -249,6 +274,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", + "frogpilotCarControl", "frogpilotDeviceState", "frogpilotLateralPlan", "frogpilotLongitudinalPlan", }); Params params; @@ -262,6 +288,8 @@ UIState::UIState(QObject *parent) : QObject(parent) { timer = new QTimer(this); QObject::connect(timer, &QTimer::timeout, this, &UIState::update); timer->start(1000 / UI_FREQ); + + setDefaultParams(); } void UIState::update() { @@ -273,6 +301,15 @@ void UIState::update() { watchdog_kick(nanos_since_boot()); } emit uiUpdate(*this); + + // Update FrogPilot variables when they are changed + Params paramsMemory{"/dev/shm/params"}; + if (paramsMemory.getBool("FrogPilotTogglesUpdated")) { + ui_update_params(this); + emit uiUpdateFrogPilotParams(); + } + + // FrogPilot live variables that need to be constantly checked } void UIState::setPrimeType(PrimeType type) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 86cd70f..b6f3bef 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -105,6 +105,8 @@ typedef enum UIStatus { STATUS_DISENGAGED, STATUS_OVERRIDE, STATUS_ENGAGED, + + // FrogPilot statuses } UIStatus; enum PrimeType { @@ -121,12 +123,15 @@ const QColor bg_colors [] = { [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1), [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), + + // FrogPilot colors }; static std::map alert_colors = { {cereal::ControlsState::AlertStatus::NORMAL, QColor(0x15, 0x15, 0x15, 0xf1)}, {cereal::ControlsState::AlertStatus::USER_PROMPT, QColor(0xDA, 0x6F, 0x25, 0xf1)}, {cereal::ControlsState::AlertStatus::CRITICAL, QColor(0xC9, 0x22, 0x31, 0xf1)}, + {cereal::ControlsState::AlertStatus::FROGPILOT, QColor(0x17, 0x86, 0x44, 0xf1)}, }; typedef struct UIScene { @@ -160,6 +165,11 @@ typedef struct UIScene { bool started, ignition, is_metric, map_on_left, longitudinal_control; bool world_objects_visible = false; uint64_t started_frame; + + // FrogPilot variables + bool enabled; + bool experimental_mode; + } UIScene; class UIState : public QObject { @@ -193,6 +203,9 @@ signals: void primeChanged(bool prime); void primeTypeChanged(PrimeType prime_type); + // FrogPilot signals + void uiUpdateFrogPilotParams(); + private slots: void update(); diff --git a/selfdrive/updated.py b/selfdrive/updated.py index a623aae..1639431 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -190,6 +190,8 @@ def finalize_update() -> None: set_consistent_flag(True) cloudlog.info("done finalizing overlay") + # FrogPilot update functions + params = Params() def handle_agnos_update() -> None: from openpilot.system.hardware.tici.agnos import flash_agnos_update, get_target_slot_number @@ -221,6 +223,8 @@ class Updater: self.branches = defaultdict(lambda: '') self._has_internet: bool = False + # FrogPilot variables + @property def has_internet(self) -> bool: return self._has_internet diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index e17203a..e011618 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -932,6 +932,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));