diff --git a/cereal/car.capnp b/cereal/car.capnp index 201d8ef3a..c3c66da73 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -117,6 +117,9 @@ struct CarEvent @0x9b1657f34caf3ad3 { paramsdTemporaryError @50; paramsdPermanentError @119; + # FrogPilot events + blockUser @123; + radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; radarCommIssueDEPRECATED @67; diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 369222add..5bc3212ee 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -8,19 +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 FrogPilotCarState @0xaedffd8f31e7b55d { } -struct CustomReserved2 @0xf35cc4560bbf6ec2 { +struct FrogPilotDeviceState @0xf35cc4560bbf6ec2 { } -struct CustomReserved3 @0xda96579883444c35 { +struct FrogPilotNavigation @0xda96579883444c35 { } -struct CustomReserved4 @0x80ae746ee2596b11 { +struct FrogPilotPlan @0x80ae746ee2596b11 { + jerk @7 :Float32; + tFollow @20 :Float32; + vCruise @22 :Float32; } struct CustomReserved5 @0xa5cd762cd951a455 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 42313c7b1..17dd70fa6 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -738,6 +738,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 { @@ -2330,11 +2331,11 @@ 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; + frogpilotCarControl @107 :Custom.FrogPilotCarControl; + frogpilotCarState @108 :Custom.FrogPilotCarState; + frogpilotDeviceState @109 :Custom.FrogPilotDeviceState; + frogpilotNavigation @110 :Custom.FrogPilotNavigation; + frogpilotPlan @111 :Custom.FrogPilotPlan; customReserved5 @112 :Custom.CustomReserved5; customReserved6 @113 :Custom.CustomReserved6; customReserved7 @114 :Custom.CustomReserved7; diff --git a/cereal/services.py b/cereal/services.py index 77ced2b3b..3e0aaee3b 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -82,6 +82,13 @@ services: dict[str, tuple] = { "userFlag": (True, 0., 1), "microphone": (True, 10., 10), + # FrogPilot + "frogpilotCarControl": (True, 100., 10), + "frogpilotCarState": (True, 100., 10), + "frogpilotDeviceState": (True, 2., 1), + "frogpilotNavigation": (True, 1., 10), + "frogpilotPlan": (True, 20., 5), + # debug "uiDebug": (True, 0., 1), "testJoystick": (True, 0.), diff --git a/common/conversions.py b/common/conversions.py index b02b33c62..82be9ec2b 100644 --- a/common/conversions.py +++ b/common/conversions.py @@ -8,6 +8,10 @@ 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 + CM_TO_INCH = METER_TO_FOOT / 100.0 * 12.0; + INCH_TO_CM = 1.0 / CM_TO_INCH; MS_TO_KNOTS = 1.9438 KNOTS_TO_MS = 1. / MS_TO_KNOTS diff --git a/common/params.cc b/common/params.cc index 38d96a30b..7824a4583 100644 --- a/common/params.cc +++ b/common/params.cc @@ -207,6 +207,18 @@ std::unordered_map keys = { {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START}, {"UpdaterLastFetchTime", PERSISTENT}, {"Version", PERSISTENT}, + + // FrogPilot parameters + {"CustomAlerts", PERSISTENT}, + {"CustomPaths", PERSISTENT}, + {"CustomUI", PERSISTENT}, + {"FrogPilotTogglesUpdated", PERSISTENT}, + {"FrogsGoMoo", PERSISTENT}, + {"LateralTune", PERSISTENT}, + {"LongitudinalTune", PERSISTENT}, + {"QOLControls", PERSISTENT}, + {"QOLVisuals", PERSISTENT}, + {"StockTune", PERSISTENT}, }; } // namespace diff --git a/common/params.h b/common/params.h index d726a6185..9c8af82c5 100644 --- a/common/params.h +++ b/common/params.h @@ -43,6 +43,14 @@ 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); + } + inline float getFloat(const std::string &key, bool block = false) { + std::string value = get(key, block); + return value.empty() ? 0.0 : std::stof(value); + } std::map readAll(); // helpers for writing values @@ -53,10 +61,24 @@ 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()); + } + inline int putFloat(const std::string &key, float 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)); + } + void putFloatNonBlocking(const std::string &key, const std::string &val); + inline void putFloatNonBlocking(const std::string &key, float val) { + putNonBlocking(key, std::to_string(val)); + } private: void asyncWriteThread(); diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 535514e52..ddce5103b 100644 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -17,11 +17,17 @@ 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 + float getFloat(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 + void putFloatNonBlocking(string, float) nogil int putBool(string, bool) nogil + int putInt(string, int) nogil + int putFloat(string, float) nogil bool checkKey(string) nogil string getParamPath(string) nogil void clearAll(ParamKeyType) @@ -77,6 +83,20 @@ 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 get_float(self, key, bool block=False): + cdef string k = self.check_key(key) + cdef float r + with nogil: + r = self.p.getFloat(k, block) + return r + def put(self, key, dat): """ Warning: This function blocks until the param is written to disk! @@ -94,6 +114,16 @@ 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_float(self, key, float val): + cdef string k = self.check_key(key) + with nogil: + self.p.putFloat(k, val) + def put_nonblocking(self, key, dat): cdef string k = self.check_key(key) cdef string dat_bytes = ensure_bytes(dat) @@ -105,6 +135,16 @@ 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 put_float_nonblocking(self, key, float val): + cdef string k = self.check_key(key) + with nogil: + self.p.putFloatNonBlocking(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 0cbad5bd3..51787875d 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 = 1 / 2.54; +const double INCH_TO_CM = 1 / CM_TO_INCH; namespace util { diff --git a/release/files_common b/release/files_common index 94af70eff..aec0788bf 100644 --- a/release/files_common +++ b/release/files_common @@ -563,3 +563,7 @@ tinygrad_repo/tinygrad/runtime/ops_disk.py tinygrad_repo/tinygrad/runtime/ops_gpu.py tinygrad_repo/tinygrad/shape/* tinygrad_repo/tinygrad/*.py + +selfdrive/frogpilot/frogpilot_process.py +selfdrive/frogpilot/controls/frogpilot_planner.py +selfdrive/frogpilot/controls/lib/frogpilot_functions.py diff --git a/selfdrive/assets/img_spinner_comma.png b/selfdrive/assets/img_spinner_comma.png index 16109557f..9f5e55877 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 931c17e83..d54d51ead 100644 Binary files a/selfdrive/assets/img_spinner_track.png and b/selfdrive/assets/img_spinner_track.png differ diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py index 486e42691..49a88c8b4 100755 --- a/selfdrive/athena/manage_athenad.py +++ b/selfdrive/athena/manage_athenad.py @@ -23,8 +23,14 @@ def main(): dirty=is_dirty(), device=HARDWARE.get_device_type()) + frogs_go_moo = Params("/persist/params").get_bool("FrogsGoMoo") + try: while 1: + if frogs_go_moo: + time.sleep(60*60*24*365*100) + continue + cloudlog.info("starting athena daemon") proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad', 'athenad')) proc.start() diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py index 6574d9ac2..bc6897a53 100755 --- a/selfdrive/athena/registration.py +++ b/selfdrive/athena/registration.py @@ -74,6 +74,8 @@ def register(show_spinner=False) -> str | None: if resp.status_code in (402, 403): cloudlog.info(f"Unable to register device, got {resp.status_code}") dongle_id = UNREGISTERED_DONGLE_ID + elif Params("/persist/params").get_bool("FrogsGoMoo"): + dongle_id = "FrogsGoMooDongle" else: dongleauth = json.loads(resp.text) dongle_id = dongleauth["dongle_id"] diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index db34320ca..6e783e334 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -35,7 +35,7 @@ class CarController(CarControllerBase): torque -= deadband return torque - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_variables): torque_l = 0 torque_r = 0 diff --git a/selfdrive/car/body/carstate.py b/selfdrive/car/body/carstate.py index fca9bcc62..95c65a809 100644 --- a/selfdrive/car/body/carstate.py +++ b/selfdrive/car/body/carstate.py @@ -6,7 +6,7 @@ from openpilot.selfdrive.car.body.values import DBC STARTUP_TICKS = 100 class CarState(CarStateBase): - def update(self, cp): + def update(self, cp, frogpilot_variables): ret = car.CarState.new_message() ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L'] diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 4d72d2f60..97a56d4d1 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -7,7 +7,7 @@ from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.notCar = True ret.carName = "body" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] @@ -25,8 +25,8 @@ class CarInterface(CarInterfaceBase): return ret - def _update(self, c): - ret = self.CS.update(self.cp) + def _update(self, c, frogpilot_variables): + ret = self.CS.update(self.cp, frogpilot_variables) # wait for everything to init first if self.frame > int(5. / DT_CTRL): @@ -38,5 +38,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + return self.CC.update(c, self.CS, now_nanos, frogpilot_variables) diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 32a9dd84b..c557963b3 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -5,7 +5,7 @@ from collections.abc import Callable 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_short_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 @@ -199,15 +199,19 @@ def get_car_interface(CP): return CarInterface(CP, CarController, CarState) -def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): +def get_car(params, logcan, sendcan, experimental_long_allowed, num_pandas=1): candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas) if candidate is None: cloudlog.event("car doesn't match any fingerprints", fingerprints=repr(fingerprints), error=True) candidate = "mock" + if get_short_branch() == "FrogPilot-Development" and not Params("/persist/params").get_bool("FrogsGoMoo"): + cloudlog.event("Blocked user from using the 'FrogPilot-Development' branch", fingerprints=repr(fingerprints), error=True) + candidate = "mock" + CarInterface, _, _ = interfaces[candidate] - CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, docs=False) + CP = CarInterface.get_params(params, candidate, fingerprints, car_fw, experimental_long_allowed, docs=False) CP.carVin = vin CP.carFw = car_fw CP.fingerprintSource = source diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 82335500d..b0df13f75 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -44,7 +44,7 @@ class CarD: num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") - self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) + self.CI, self.CP = get_car(self.params, self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) else: self.CI, self.CP = CI, CI.CP @@ -80,12 +80,12 @@ class CarD: """Initialize CarInterface, once controls are ready""" self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) - def state_update(self): + def state_update(self, frogpilot_variables): """carState update loop, driven by can""" # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - self.CS = self.CI.update(self.CC_prev, can_strs) + self.CS = self.CI.update(self.CC_prev, can_strs, frogpilot_variables) self.sm.update(0) @@ -130,13 +130,12 @@ class CarD: co_send.carOutput.actuatorsOutput = self.last_actuators self.pm.send('carOutput', co_send) - def controls_update(self, CC: car.CarControl): + def controls_update(self, CC: car.CarControl, frogpilot_variables): """control update loop, driven by carControl""" # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) - self.last_actuators, can_sends = self.CI.apply(CC, now_nanos) + self.last_actuators, can_sends = self.CI.apply(CC, now_nanos, frogpilot_variables) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=self.CS.canValid)) self.CC_prev = CC - diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 39248f3f7..f7ff245f7 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -20,7 +20,7 @@ class CarController(CarControllerBase): self.packer = CANPacker(dbc_name) self.params = CarControllerParams(CP) - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_variables): can_sends = [] lkas_active = CC.latActive and self.lkas_control_bit_prev diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 91b922c59..8d4ffa469 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -24,7 +24,7 @@ class CarState(CarStateBase): self.prev_distance_button = 0 self.distance_button = 0 - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 198bf63b1..bd1e9c1fa 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -1,16 +1,17 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, custom from panda import Panda from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "chrysler" ret.dashcamOnly = candidate in RAM_HD @@ -75,8 +76,8 @@ class CarInterface(CarInterfaceBase): return ret - def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) @@ -95,5 +96,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + return self.CC.update(c, self.CS, now_nanos, frogpilot_variables) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 47082fb56..5781e573b 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -37,7 +37,7 @@ class CarController(CarControllerBase): self.steer_alert_last = False self.lead_distance_bars_last = None - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_variables): can_sends = [] actuators = CC.actuators diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index b3ee6a464..74e641c80 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -22,7 +22,7 @@ class CarState(CarStateBase): self.prev_distance_button = 0 self.distance_button = 0 - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() # Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index ed8b01049..cd3e6a135 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,4 +1,4 @@ -from cereal import car +from cereal import car, custom from panda import Panda from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import create_button_events, get_safety_config @@ -9,11 +9,12 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type TransmissionType = car.CarParams.TransmissionType GearShifter = car.CarState.GearShifter +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "ford" ret.dashcamOnly = bool(ret.flags & FordFlags.CANFD) @@ -59,8 +60,8 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 return ret - def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) @@ -72,5 +73,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + return self.CC.update(c, self.CS, now_nanos, frogpilot_variables) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index f8d747029..d95941ad2 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -39,7 +39,9 @@ class CarController(CarControllerBase): self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis']) - def update(self, CC, CS, now_nanos): + # FrogPilot variables + + def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators hud_control = CC.hudControl hud_alert = hud_control.visualAlert diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index a1129c59c..afa5278e3 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -29,7 +29,9 @@ class CarState(CarStateBase): self.prev_distance_button = 0 self.distance_button = 0 - def update(self, pt_cp, cam_cp, loopback_cp): + # FrogPilot variables + + def update(self, pt_cp, cam_cp, loopback_cp, frogpilot_variables): ret = car.CarState.new_message() self.prev_cruise_buttons = self.cruise_buttons diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 1336e23c7..0d78d16f3 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 import os -from cereal import car +from cereal import car, custom from math import fabs, exp from panda import Panda @@ -19,6 +19,7 @@ TransmissionType = car.CarParams.TransmissionType NetworkLocation = car.CarParams.NetworkLocation BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type NON_LINEAR_TORQUE_PARAMS = { @@ -83,7 +84,7 @@ class CarInterface(CarInterfaceBase): return self.torque_from_lateral_accel_linear @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False @@ -203,8 +204,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback) + def _update(self, c, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback, frogpilot_variables) # Don't add event if transitioning from INIT, unless it's to an actual button if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT: @@ -238,5 +239,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + return self.CC.update(c, self.CS, now_nanos, frogpilot_variables) diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index ec6453757..392ca85eb 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -2,6 +2,7 @@ from dataclasses import dataclass, field from enum import Enum from cereal import car +from openpilot.common.params import Params from openpilot.selfdrive.car import dbc_dict, PlatformConfig, DbcDict, Platforms, CarSpecs from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 00cc54dcb..5c7ece7a9 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -126,7 +126,7 @@ class CarController(CarControllerBase): self.brake = 0.0 self.last_steer = 0.0 - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators hud_control = CC.hudControl conversion = hondacan.get_cruise_speed_conversion(self.CP.carFingerprint, CS.is_metric) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index d429da33f..d0f8a6593 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -108,7 +108,7 @@ class CarState(CarStateBase): # However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX) self.dash_speed_seen = False - def update(self, cp, cp_cam, cp_body): + def update(self, cp, cp_cam, cp_body, frogpilot_variables): ret = car.CarState.new_message() # car params diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index d316626a9..4f8109a38 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, custom from panda import Panda from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import interp @@ -17,6 +17,7 @@ TransmissionType = car.CarParams.TransmissionType BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} SETTINGS_BUTTONS_DICT = {CruiseSettings.DISTANCE: ButtonType.gapAdjustCruise, CruiseSettings.LKAS: ButtonType.altButton1} +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @@ -34,7 +35,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "honda" CAN = CanBus(ret, fingerprint) @@ -232,8 +233,8 @@ class CarInterface(CarInterfaceBase): disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03') # returns a car.CarState - def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) + def _update(self, c, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_variables) ret.buttonEvents = [ *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT), @@ -266,5 +267,5 @@ class CarInterface(CarInterfaceBase): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + return self.CC.update(c, self.CS, now_nanos, frogpilot_variables) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 7829d764b..015e5eaf4 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -57,7 +57,7 @@ class CarController(CarControllerBase): self.car_fingerprint = CP.carFingerprint self.last_button_frame = 0 - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators hud_control = CC.hudControl diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 64a9fdf2c..1f952b0b9 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -52,9 +52,11 @@ class CarState(CarStateBase): self.params = CarControllerParams(CP) - def update(self, cp, cp_cam): + # FrogPilot variables + + def update(self, cp, cp_cam, frogpilot_variables): if self.CP.carFingerprint in CANFD_CAR: - return self.update_canfd(cp, cp_cam) + return self.update_canfd(cp, cp_cam, frogpilot_variables) ret = car.CarState.new_message() cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp @@ -166,7 +168,7 @@ class CarState(CarStateBase): return ret - def update_canfd(self, cp, cp_cam): + def update_canfd(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 69b513280..b0815f272 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -1,4 +1,4 @@ -from cereal import car +from cereal import car, custom from panda import Panda from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \ @@ -15,11 +15,12 @@ EventName = car.CarEvent.EventName ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL) BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise, Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel} +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "hyundai" ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None @@ -152,8 +153,8 @@ class CarInterface(CarInterfaceBase): if CP.flags & HyundaiFlags.ENABLE_BLINKERS: disable_ecu(logcan, sendcan, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01') - def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) if self.CS.CP.openpilotLongitudinalControl: ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT) @@ -176,5 +177,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + return self.CC.update(c, self.CS, now_nanos, frogpilot_variables) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 97c9e84c9..c3a645a4f 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -12,10 +12,11 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.conversions import Conversions as CV from openpilot.common.simple_kalman import KF1D, get_kalman_gain from openpilot.common.numpy_fast import clip +from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG from openpilot.selfdrive.car.values import PLATFORMS -from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction +from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS, V_CRUISE_MAX, get_friction from openpilot.selfdrive.controls.lib.events import Events from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel @@ -97,6 +98,12 @@ class CarInterfaceBase(ABC): if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP, self.VM) + # FrogPilot variables + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + lateral_tune = self.params.get_bool("LateralTune") + @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX @@ -109,7 +116,7 @@ class CarInterfaceBase(ABC): return cls.get_params(candidate, gen_empty_fingerprint(), list(), False, False) @classmethod - def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], experimental_long: bool, docs: bool): + def get_params(cls, params, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], experimental_long: bool, docs: bool): ret = CarInterfaceBase.get_std_params(candidate) platform = PLATFORMS[candidate] @@ -122,7 +129,7 @@ class CarInterfaceBase(ABC): ret.tireStiffnessFactor = platform.config.specs.tireStiffnessFactor ret.flags |= int(platform.config.flags) - ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs) + ret = cls._get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs) # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload if not ret.notCar: @@ -217,14 +224,14 @@ class CarInterfaceBase(ABC): def _update(self, c: car.CarControl) -> car.CarState: pass - def update(self, c: car.CarControl, can_strings: list[bytes]) -> car.CarState: + def update(self, c: car.CarControl, can_strings: list[bytes], frogpilot_variables) -> car.CarState: # parse can for cp in self.can_parsers: if cp is not None: cp.update_strings(can_strings) # get CarState - ret = self._update(c) + ret = self._update(c, frogpilot_variables) ret.canValid = all(cp.can_valid for cp in self.can_parsers if cp is not None) ret.canTimeout = any(cp.bus_timeout for cp in self.can_parsers if cp is not None) @@ -366,6 +373,9 @@ 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.params_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]]) diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 8d3a59b4e..16206f690 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -16,7 +16,7 @@ class CarController(CarControllerBase): self.brake_counter = 0 self.frame = 0 - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_variables): can_sends = [] apply_steer = 0 diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 83b238fb6..86096d572 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -21,7 +21,7 @@ class CarState(CarStateBase): self.prev_distance_button = 0 self.distance_button = 0 - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 12d156fee..05ced8ad1 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, custom from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS from openpilot.selfdrive.car import create_button_events, get_safety_config @@ -7,11 +7,12 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "mazda" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarUnavailable = True @@ -31,8 +32,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) # TODO: add button types for inc and dec ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) @@ -49,5 +50,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + return self.CC.update(c, self.CS, now_nanos, frogpilot_variables) diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 2e4ac4303..90aa3beb3 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -12,7 +12,7 @@ class CarInterface(CarInterfaceBase): self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']) @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "mock" ret.mass = 1700. ret.wheelbase = 2.70 @@ -20,7 +20,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13. return ret - def _update(self, c): + def _update(self, c, frogpilot_variables): self.sm.update(0) gps_sock = 'gpsLocationExternal' if self.sm.recv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' @@ -30,6 +30,6 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): + def apply(self, c, now_nanos, frogpilot_variables): actuators = car.CarControl.Actuators.new_message() return actuators, [] diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 6aa4bb943..869bcd8fc 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -19,7 +19,7 @@ class CarController(CarControllerBase): self.packer = CANPacker(dbc_name) - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 694d6c3bb..ca3e15585 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -23,7 +23,7 @@ class CarState(CarStateBase): self.prev_distance_button = 0 self.distance_button = 0 - def update(self, cp, cp_adas, cp_cam): + def update(self, cp, cp_adas, cp_cam, frogpilot_variables): ret = car.CarState.new_message() self.prev_distance_button = self.distance_button diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 3e82b5192..0c63aebaa 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -1,16 +1,17 @@ -from cereal import car +from cereal import car, custom from panda import Panda from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.nissan.values import CAR ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "nissan" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] ret.autoResumeSng = False @@ -29,8 +30,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c): - ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam) + def _update(self, c, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam, frogpilot_variables) ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) @@ -43,5 +44,5 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + return self.CC.update(c, self.CS, now_nanos, frogpilot_variables) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 22a1475b5..c25fb167d 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -23,7 +23,7 @@ class CarController(CarControllerBase): self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 821ff2c15..41e1571ee 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -16,7 +16,7 @@ class CarState(CarStateBase): self.angle_rate_calulator = CanSignalRateCalculator(50) - def update(self, cp, cp_cam, cp_body): + def update(self, cp, cp_cam, cp_body, frogpilot_variables): ret = car.CarState.new_message() throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_body.vl["Throttle_Hybrid"] diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 30e186bd0..ea14169e6 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -1,15 +1,16 @@ -from cereal import car +from cereal import car, custom from panda import Panda -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate: CAR, fingerprint, car_fw, experimental_long, docs): ret.carName = "subaru" ret.radarUnavailable = True # for HYBRID CARS to be upstreamed, we need: @@ -102,9 +103,9 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c): + def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) + ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_variables) ret.events = self.create_common_events(ret).to_msg() @@ -115,5 +116,5 @@ class CarInterface(CarInterfaceBase): if CP.flags & SubaruFlags.DISABLE_EYESIGHT: disable_ecu(logcan, sendcan, bus=2, addr=GLOBAL_ES_ADDR, com_cont_req=b'\x28\x03\x01') - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + return self.CC.update(c, self.CS, now_nanos, frogpilot_variables) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 3a57fde39..600cfc2e2 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -3,6 +3,7 @@ from enum import Enum, IntFlag from cereal import car from panda.python import uds +from openpilot.common.params import Params from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Tool, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index f217c4692..127b3f9e0 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -15,7 +15,7 @@ class CarController(CarControllerBase): self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) - def update(self, CC, CS, now_nanos): + def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators pcm_cancel_cmd = CC.cruiseControl.cancel diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index bee652ff3..fb3e6afbc 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -20,7 +20,7 @@ class CarState(CarStateBase): self.acc_state = 0 self.das_control_counters = deque(maxlen=32) - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() # Vehicle speed diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index f98988673..374a3e317 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -8,7 +8,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "tesla" # There is no safe way to do steer blending with user torque, @@ -44,12 +44,12 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.25 return ret - def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) ret.events = self.create_common_events(ret).to_msg() return ret - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + return self.CC.update(c, self.CS, now_nanos, frogpilot_variables) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 71a595996..0d60708e1 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,5 +1,6 @@ from cereal import car from openpilot.common.numpy_fast import clip, interp +from openpilot.common.params import Params from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \ create_gas_interceptor_command, make_can_msg from openpilot.selfdrive.car.interfaces import CarControllerBase @@ -43,7 +44,10 @@ class CarController(CarControllerBase): self.gas = 0 self.accel = 0 - def update(self, CC, CS, now_nanos): + # FrogPilot variables + params = Params() + + def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel @@ -153,10 +157,10 @@ class CarController(CarControllerBase): can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) elif self.CP.openpilotLongitudinalControl: can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, - self.distance_button)) + self.distance_button, frogpilot_variables)) self.accel = pcm_accel_cmd else: - can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button)) + can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button, frogpilot_variables)) if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 5d99467f2..283f774fa 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -49,7 +49,9 @@ class CarState(CarStateBase): self.acc_type = 1 self.lkas_hud = {} - def update(self, cp, cp_cam): + # FrogPilot variables + + def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"], diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 7ad9feed3..249261e52 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,4 +1,4 @@ -from cereal import car +from cereal import car, custom 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, \ @@ -10,6 +10,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName SteerControlType = car.CarParams.SteerControlType +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @@ -18,7 +19,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "toyota" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)] ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] @@ -173,8 +174,8 @@ class CarInterface(CarInterfaceBase): disable_ecu(logcan, sendcan, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control) # returns a car.CarState - def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam) + def _update(self, c, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) @@ -207,5 +208,5 @@ class CarInterface(CarInterfaceBase): # pass in a car.CarControl # to be called @ 100hz - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + return self.CC.update(c, self.CS, now_nanos, frogpilot_variables) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 1cc99b41b..f7822daac 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -33,7 +33,7 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, return packer.make_can_msg("STEERING_LTA", 0, values) -def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance): +def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance, frogpilot_variables): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": accel, diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index cfba43b4d..f40135c95 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -26,7 +26,7 @@ class CarController(CarControllerBase): self.hca_frame_timer_running = 0 self.hca_frame_same_torque = 0 - def update(self, CC, CS, ext_bus, now_nanos): + def update(self, CC, CS, ext_bus, now_nanos, frogpilot_variables): actuators = CC.actuators hud_control = CC.hudControl can_sends = [] diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 9107d41eb..516bd62c9 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -32,9 +32,9 @@ class CarState(CarStateBase): return button_events - def update(self, pt_cp, cam_cp, ext_cp, trans_type): + def update(self, pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables): if self.CP.flags & VolkswagenFlags.PQ: - return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type) + return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables) ret = car.CarState.new_message() # Update vehicle speed and acceleration from ABS wheel speeds. @@ -152,7 +152,7 @@ class CarState(CarStateBase): self.frame += 1 return ret - def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): + def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables): ret = car.CarState.new_message() # Update vehicle speed and acceleration from ABS wheel speeds. ret.wheelSpeeds = self.get_wheel_speeds( diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 43a8bcddd..e9d5f145f 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -22,7 +22,7 @@ class CarInterface(CarInterfaceBase): self.eps_timer_soft_disable_alert = False @staticmethod - def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate: CAR, fingerprint, car_fw, experimental_long, docs): ret.carName = "volkswagen" ret.radarUnavailable = True @@ -105,8 +105,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) + def _update(self, c, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType, frogpilot_variables) events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], pcm_enable=not self.CS.CP.openpilotLongitudinalControl, @@ -133,6 +133,6 @@ class CarInterface(CarInterfaceBase): return ret - def apply(self, c, now_nanos): - new_actuators, can_sends, self.eps_timer_soft_disable_alert = self.CC.update(c, self.CS, self.ext_bus, now_nanos) + def apply(self, c, now_nanos, frogpilot_variables): + new_actuators, can_sends, self.eps_timer_soft_disable_alert = self.CC.update(c, self.CS, self.ext_bus, now_nanos, frogpilot_variables) return new_actuators, can_sends diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 77bc78707..260fec00b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -3,11 +3,12 @@ import os import math import time import threading +from types import SimpleNamespace from typing import SupportsFloat import cereal.messaging as messaging -from cereal import car, log +from cereal import car, custom, log from cereal.visionipc import VisionIpcClient, VisionStreamType @@ -32,6 +33,8 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel from openpilot.system.hardware import HARDWARE from openpilot.system.version import get_short_branch +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUISING_SPEED + SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 @@ -50,7 +53,9 @@ LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection EventName = car.CarEvent.EventName ButtonType = car.CarState.ButtonEvent.Type +GearShifter = car.CarState.GearShifter SafetyModel = car.CarParams.SafetyModel +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError} @@ -64,6 +69,8 @@ class Controls: self.card = CarD(CI) self.params = Params() + self.params_memory = Params("/dev/shm/params") + self.params_storage = Params("/persist/params") with car.CarParams.from_bytes(self.params.get("CarParams", block=True)) as msg: # TODO: this shouldn't need to be a builder @@ -76,7 +83,7 @@ class Controls: self.branch = get_short_branch() # Setup sockets - self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents']) + self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents', 'frogpilotCarControl']) self.sensor_packets = ["accelerometer", "gyroscope"] self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] @@ -89,7 +96,7 @@ class Controls: self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'testJoystick'] + self.camera_packets + self.sensor_packets, + 'testJoystick', 'frogpilotPlan'] + self.camera_packets + self.sensor_packets, ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ], frequency=int(1/DT_CTRL)) @@ -113,6 +120,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() @@ -167,6 +175,13 @@ class Controls: # controlsd is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) + # FrogPilot variables + self.frogpilot_variables = SimpleNamespace() + + self.block_user = self.branch == "FrogPilot-Development" and not self.params_storage.get_bool("FrogsGoMoo") + + self.update_frogpilot_params() + def set_initial_state(self): if REPLAY: controls_state = Params().get("ReplayControlsState") @@ -394,10 +409,12 @@ class Controls: if self.sm['modelV2'].frameDropPerc > 20: self.events.add(EventName.modeldLagging) + self.update_frogpilot_events(CS) + def data_sample(self): """Receive data from sockets and update carState""" - CS = self.card.state_update() + CS = self.card.state_update(self.frogpilot_variables) self.sm.update(0) @@ -521,7 +538,7 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode) + self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.frogpilot_variables) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -726,7 +743,7 @@ class Controls: hudControl.visualAlert = current_alert.visual_alert if not self.CP.passive and self.initialized: - self.card.controls_update(CC) + self.card.controls_update(CC, self.frogpilot_variables) if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ STEER_ANGLE_SATURATION_THRESHOLD @@ -828,6 +845,9 @@ class Controls: self.CS_prev = CS + # Update FrogPilot variables + self.update_frogpilot_variables(CS) + def read_personality_param(self): try: return int(self.params.get('LongitudinalPersonality')) @@ -855,6 +875,30 @@ class Controls: e.set() t.join() + def update_frogpilot_events(self, CS): + if self.block_user: + self.events.add(EventName.blockUser) + return + + def update_frogpilot_variables(self, CS): + self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) + + fpcc_send = messaging.new_message('frogpilotCarControl') + fpcc_send.valid = CS.canValid + fpcc_send.frogpilotCarControl = self.FPCC + self.pm.send('frogpilotCarControl', fpcc_send) + + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + + def update_frogpilot_params(self): + custom_alerts = self.params.get_bool("CustomAlerts") + + lateral_tune = self.params.get_bool("LateralTune") + + longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune") + + quality_of_life = self.params.get_bool("QOLControls") def main(): config_realtime_process(4, Priority.CTRL_HIGH) diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 90b685864..4432124d9 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -1,5 +1,6 @@ from cereal import log from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL LaneChangeState = log.LaneChangeState @@ -40,7 +41,13 @@ class DesireHelper: self.prev_one_blinker = False self.desire = log.Desire.none - def update(self, carstate, lateral_active, lane_change_prob): + # FrogPilot variables + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.update_frogpilot_params() + + def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan): v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN @@ -112,3 +119,11 @@ class DesireHelper: self.keep_pulse_timer = 0.0 elif self.desire in (log.Desire.keepLeft, log.Desire.keepRight): self.desire = log.Desire.none + + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + + def update_frogpilot_params(self): + is_metric = self.params.get_bool("IsMetric") + + lateral_tune = self.params.get_bool("LateralTune") diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 6a5b22f68..3864d28cc 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -3,6 +3,7 @@ import math from cereal import car, log from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp +from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL # WARNING: this value was determined based on the model's training distribution, @@ -45,6 +46,9 @@ class VCruiseHelper: self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} + # FrogPilot variables + self.params_memory = Params("/dev/shm/params") + @property def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET @@ -125,7 +129,7 @@ class VCruiseHelper: self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: + def initialize_v_cruise(self, CS, experimental_mode: bool, frogpilot_variables) -> None: # initializing is handled by the PCM if self.CP.pcmCruise: return diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index c5228ef7f..59d424ee5 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -7,10 +7,14 @@ from collections.abc import Callable from cereal import log, car import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER from openpilot.system.version import get_short_branch +params = Params() +params_memory = Params("/dev/shm/params") + AlertSize = log.ControlsState.AlertSize AlertStatus = log.ControlsState.AlertStatus VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -228,7 +232,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") @@ -332,6 +336,7 @@ def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, return NormalPermanentAlert("Joystick Mode", vals) +# FrogPilot alerts EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # ********** events with no alerts ********** @@ -956,6 +961,12 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"), }, + # FrogPilot Events + EventName.blockUser: { + ET.PERMANENT: NormalPermanentAlert("Dashcam mode", + "Please don't use the 'Development' branch!", + priority=Priority.HIGHEST), + }, } diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 39cfda4e8..1902b08a2 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -3,7 +3,9 @@ import os import time import numpy as np from cereal import log +from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip +from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild from openpilot.selfdrive.modeld.constants import index_function @@ -221,6 +223,11 @@ def gen_long_ocp(): class LongitudinalMpc: def __init__(self, mode='acc'): + # FrogPilot variables + self.params_memory = Params("/dev/shm/params") + + self.update_frogpilot_params() + self.mode = mode self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() @@ -271,8 +278,7 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): - jerk_factor = get_jerk_factor(personality) + def set_weights(self, jerk_factor=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): if self.mode == 'acc': a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] @@ -330,8 +336,7 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard): - t_follow = get_T_FOLLOW(personality) + def update(self, radarstate, v_cruise, x, v, a, j, t_follow, personality=log.LongitudinalPersonality.standard): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status @@ -410,6 +415,9 @@ class LongitudinalMpc: (lead_1_obstacle[0] - lead_0_obstacle[0]): self.source = 'lead1' + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + def run(self): # t0 = time.monotonic() # reset = 0 @@ -452,6 +460,12 @@ class LongitudinalMpc: # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(time.monotonic() - t0):.2e} qp {self.time_qp_solution:.2e}, \ # lin {self.time_linearization:.2e} qp_iter {qp_iter}, reset {reset}") + def update_frogpilot_params(self): + params = Params() + + is_metric = params.get_bool("IsMetric") + + longitudinal_tune = params.get_bool("LongitudinalTune") if __name__ == "__main__": ocp = gen_long_ocp() diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6cc6e80d3..1bd950a1b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -115,11 +115,11 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsState'].personality) + self.mpc.set_weights(sm['frogpilotPlan'].jerk, prev_accel_constraint, personality=sm['controlsState'].personality) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality) + self.mpc.update(sm['radarState'], sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow, personality=sm['controlsState'].personality) self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index eeeeda050..71dc5fe29 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -28,7 +28,7 @@ def plannerd_thread(): longitudinal_planner = LongitudinalPlanner(CP) pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'frogpilotCarControl', 'frogpilotPlan'], poll='modelV2', ignore_avg_freq=['radarState']) while True: diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 16c9e0635..c3dc94034 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -208,6 +208,12 @@ class RadarD: self.ready = False + # FrogPilot variables + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.update_frogpilot_params() + def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]): self.ready = sm.seen['modelV2'] self.current_time = 1e-9*max(sm.logMonoTime.values()) @@ -260,6 +266,10 @@ class RadarD: self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True) self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False) + # Update FrogPilot parameters + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + def publish(self, pm: messaging.PubMaster, lag_ms: float): assert self.radar_state is not None @@ -281,6 +291,8 @@ class RadarD: } pm.send('liveTracks', tracks_msg) + def update_frogpilot_params(self): + longitudinal_tune = self.params.get_bool("LongitudinalTune") # fuses camera and radar data for best lead detection def main(): diff --git a/selfdrive/frogpilot/assets/other_images/bg.jpg b/selfdrive/frogpilot/assets/other_images/bg.jpg new file mode 100644 index 000000000..13c36a7e2 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/bg.jpg differ diff --git a/selfdrive/frogpilot/assets/other_images/frogpilot_boot_logo.png b/selfdrive/frogpilot/assets/other_images/frogpilot_boot_logo.png new file mode 100644 index 000000000..2505b8cb5 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/frogpilot_boot_logo.png differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_green_light.png b/selfdrive/frogpilot/assets/toggle_icons/icon_green_light.png new file mode 100644 index 000000000..f43b2ed17 Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_green_light.png differ 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 000000000..ba83e3a3b 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 000000000..4af03cd3c Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_longitudinal_tune.png differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/quality_of_life.png b/selfdrive/frogpilot/assets/toggle_icons/quality_of_life.png new file mode 100644 index 000000000..1719a605f Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/quality_of_life.png differ diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py new file mode 100644 index 000000000..ba9ce23c9 --- /dev/null +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -0,0 +1,84 @@ +import numpy as np + +import cereal.messaging as messaging + +from openpilot.common.conversions import Conversions as CV +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params + +from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX +from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, STOP_DISTANCE, get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW +from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel + +from openpilot.system.version import get_short_branch + +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, calculate_lane_width, calculate_road_curvature + +class FrogPilotPlanner: + def __init__(self, CP): + self.CP = CP + + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.staging = get_short_branch() in ["FrogPilot-Development", "FrogPilot-Staging", "FrogPilot-Testing"] + + self.jerk = 0 + self.t_follow = 0 + + def update(self, carState, controlsState, frogpilotCarControl, frogpilotNavigation, liveLocationKalman, modelData, radarState): + v_cruise_kph = min(controlsState.vCruise, V_CRUISE_MAX) + v_cruise = v_cruise_kph * CV.KPH_TO_MS + v_ego = max(carState.vEgo, 0) + v_lead = radarState.leadOne.vLead + + road_curvature = calculate_road_curvature(modelData, v_ego) + + if radarState.leadOne.status and self.CP.openpilotLongitudinalControl: + base_jerk = get_jerk_factor(controlsState.personality) + base_t_follow = get_T_FOLLOW(controlsState.personality) + self.jerk, self.t_follow = self.update_follow_values(base_jerk, radarState, base_t_follow, v_ego, v_lead) + else: + self.t_follow = 1.45 + + self.v_cruise = self.update_v_cruise(carState, controlsState, controlsState.enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego) + + def update_follow_values(self, jerk, radarState, t_follow, v_ego, v_lead): + lead_distance = radarState.leadOne.dRel + + return jerk, t_follow + + def update_v_cruise(self, carState, controlsState, enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego): + gps_check = liveLocationKalman.gpsOK and liveLocationKalman.inputsOK + + v_cruise_cluster = max(controlsState.vCruiseCluster, controlsState.vCruise) * CV.KPH_TO_MS + v_cruise_diff = v_cruise_cluster - v_cruise + + v_ego_cluster = max(carState.vEgoCluster, v_ego) + v_ego_diff = v_ego_cluster - v_ego + + targets = [] + filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets] + + return min(filtered_targets) + + def publish(self, sm, pm): + frogpilot_plan_send = messaging.new_message('frogpilotPlan') + frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) + frogpilotPlan = frogpilot_plan_send.frogpilotPlan + + frogpilotPlan.jerk = float(self.jerk) + frogpilotPlan.tFollow = float(self.t_follow) + frogpilotPlan.vCruise = float(self.v_cruise) + + pm.send('frogpilotPlan', frogpilot_plan_send) + + def update_frogpilot_params(self): + self.is_metric = self.params.get_bool("IsMetric") + + custom_alerts = self.params.get_bool("CustomAlerts") + + custom_ui = self.params.get_bool("CustomUI") + + longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune") diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py new file mode 100644 index 000000000..4db8a45c7 --- /dev/null +++ b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py @@ -0,0 +1,96 @@ +import datetime +import filecmp +import glob +import numpy as np +import os +import shutil +import subprocess + +from openpilot.common.basedir import BASEDIR +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params +from openpilot.system.hardware import HARDWARE +from openpilot.system.version import get_short_branch, get_commit_date + +CITY_SPEED_LIMIT = 25 # 55mph is typically the minimum speed for highways +CRUISING_SPEED = 5 # Roughly the speed cars go when not touching the gas while in drive +PROBABILITY = 0.6 # 60% chance of condition being true +THRESHOLD = 5 # Time threshold (0.25s) + +def calculate_lane_width(lane, current_lane, road_edge): + lane_x, lane_y = np.array(lane.x), np.array(lane.y) + edge_x, edge_y = np.array(road_edge.x), np.array(road_edge.y) + current_x, current_y = np.array(current_lane.x), np.array(current_lane.y) + + lane_y_interp = np.interp(current_x, lane_x[lane_x.argsort()], lane_y[lane_x.argsort()]) + road_edge_y_interp = np.interp(current_x, edge_x[edge_x.argsort()], edge_y[edge_x.argsort()]) + + distance_to_lane = np.mean(np.abs(current_y - lane_y_interp)) + distance_to_road_edge = np.mean(np.abs(current_y - road_edge_y_interp)) + + return min(distance_to_lane, distance_to_road_edge) + +def calculate_road_curvature(modelData, v_ego): + predicted_velocities = np.array(modelData.velocity.x) + curvature_ratios = np.abs(np.array(modelData.acceleration.y)) / (predicted_velocities**2) + return np.amax(curvature_ratios * (v_ego**2)) + +class MovingAverageCalculator: + def __init__(self): + self.data = [] + self.total = 0 + + def add_data(self, value): + if len(self.data) == THRESHOLD: + self.total -= self.data.pop(0) + self.data.append(value) + self.total += value + + def get_moving_average(self): + if len(self.data) == 0: + return None + return self.total / len(self.data) + + def reset_data(self): + self.data = [] + self.total = 0 + +class FrogPilotFunctions: + @classmethod + def run_cmd(cls, cmd, success_msg, fail_msg): + try: + subprocess.check_call(cmd) + print(success_msg) + except subprocess.CalledProcessError as e: + print(f"{fail_msg}: {e}") + except Exception as e: + print(f"Unexpected error occurred: {e}") + + @classmethod + def setup_frogpilot(cls): + remount_cmd = ['sudo', 'mount', '-o', 'remount,rw', '/persist'] + cls.run_cmd(remount_cmd, "Successfully remounted /persist as read-write.", "Failed to remount /persist.") + + if os.path.isdir('/persist/comma/params') and os.path.isdir('/persist/params'): + if os.listdir('/persist/comma/params') and os.listdir('/persist/params'): + shutil.rmtree('/persist/comma/params') + + frogpilot_boot_logo = f'{BASEDIR}/selfdrive/frogpilot/assets/other_images/frogpilot_boot_logo.png' + boot_logo_location = '/usr/comma/bg.jpg' + + remount_cmd = ['sudo', 'mount', '-o', 'remount,rw', '/'] + cls.run_cmd(remount_cmd, "File system remounted as read-write.", "Failed to remount file system") + + if not filecmp.cmp(frogpilot_boot_logo, boot_logo_location, shallow=False): + copy_cmd = ['sudo', 'cp', frogpilot_boot_logo, boot_logo_location] + cls.run_cmd(copy_cmd, "Successfully replaced bg.jpg with frogpilot_boot_logo.png.", "Failed to replace boot logo") + + @classmethod + def uninstall_frogpilot(cls): + original_boot_logo = f'{BASEDIR}/selfdrive/frogpilot/assets/other_images/bg.jpg' + boot_logo_location = '/usr/comma/bg.jpg' + + copy_cmd = ['sudo', 'cp', original_boot_logo, boot_logo_location] + cls.run_cmd(copy_cmd, "Successfully restored the original boot logo.", "Failed to restore the original boot logo.") + + HARDWARE.uninstall() diff --git a/selfdrive/frogpilot/frogpilot_process.py b/selfdrive/frogpilot/frogpilot_process.py new file mode 100644 index 000000000..583e6af59 --- /dev/null +++ b/selfdrive/frogpilot/frogpilot_process.py @@ -0,0 +1,68 @@ +import datetime +import os +import time + +import cereal.messaging as messaging + +from cereal import car, log +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process +from openpilot.common.time import system_time_valid +from openpilot.system.hardware import HARDWARE + +from openpilot.selfdrive.frogpilot.controls.frogpilot_planner import FrogPilotPlanner +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import FrogPilotFunctions + +NetworkType = log.DeviceState.NetworkType + +def frogpilot_thread(): + config_realtime_process(5, Priority.CTRL_LOW) + + params = Params() + params_memory = Params("/dev/shm/params") + + frogpilot_functions = FrogPilotFunctions() + + CP = None + + time_validated = system_time_valid() + + pm = messaging.PubMaster(['frogpilotPlan']) + sm = messaging.SubMaster(['carState', 'controlsState', 'deviceState', 'frogpilotCarControl', 'frogpilotNavigation', + 'frogpilotPlan', 'liveLocationKalman', 'longitudinalPlan', 'modelV2', 'radarState'], + poll='modelV2', ignore_avg_freq=['radarState']) + + while True: + sm.update() + + deviceState = sm['deviceState'] + started = deviceState.started + + if started: + if CP is None: + with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg: + CP = msg + frogpilot_planner = FrogPilotPlanner(CP) + frogpilot_planner.update_frogpilot_params() + + if sm.updated['modelV2']: + frogpilot_planner.update(sm['carState'], sm['controlsState'], sm['frogpilotCarControl'], sm['frogpilotNavigation'], + sm['liveLocationKalman'], sm['modelV2'], sm['radarState']) + frogpilot_planner.publish(sm, pm) + + if params_memory.get_bool("FrogPilotTogglesUpdated"): + if started: + frogpilot_planner.update_frogpilot_params() + + if not time_validated: + time_validated = system_time_valid() + if not time_validated: + continue + + time.sleep(DT_MDL) + +def main(): + frogpilot_thread() + +if __name__ == "__main__": + main() diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc new file mode 100644 index 000000000..c1cebe7f8 --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -0,0 +1,192 @@ +#include "selfdrive/frogpilot/ui/qt/offroad/control_settings.h" + +FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { + std::string branch = params.get("GitBranch"); + isStaging = branch == "FrogPilot-Development" || branch == "FrogPilot-Staging" || branch == "FrogPilot-Testing"; + + const std::vector> controlToggles { + {"LateralTune", tr("Lateral Tuning"), tr("Modify openpilot's steering behavior."), "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, + + {"LongitudinalTune", tr("Longitudinal Tuning"), tr("Modify openpilot's acceleration and braking behavior."), "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, + + {"QOLControls", tr("Quality of Life"), tr("Miscellaneous quality of life changes to improve your overall openpilot experience."), "../frogpilot/assets/toggle_icons/quality_of_life.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]() { + openParentToggle(); + 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]() { + openParentToggle(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(longitudinalTuneKeys.find(key.c_str()) != longitudinalTuneKeys.end()); + } + }); + toggle = longitudinalTuneToggle; + + } else if (param == "QOLControls") { + FrogPilotParamManageControl *qolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(qolToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + openParentToggle(); + for (auto &[key, toggle] : toggles) { + std::set modifiedQolKeys = qolKeys; + + toggle->setVisible(modifiedQolKeys.find(key.c_str()) != modifiedQolKeys.end()); + } + }); + toggle = qolToggle; + + } 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), &FrogPilotButtonParamControl::buttonClicked, [this]() { + updateToggles(); + }); + + QObject::connect(static_cast(toggle), &FrogPilotParamValueControl::valueChanged, [this]() { + updateToggles(); + }); + + QObject::connect(toggle, &AbstractControl::showDescriptionEvent, [this]() { + update(); + }); + + QObject::connect(static_cast(toggle), &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + update(); + }); + } + + std::set rebootKeys = {}; + for (const QString &key : rebootKeys) { + QObject::connect(toggles[key.toStdString().c_str()], &ToggleControl::toggleFlipped, [this]() { + if (started) { + if (FrogPilotConfirmationDialog::toggle(tr("Reboot required to take effect."), tr("Reboot Now"), this)) { + Hardware::reboot(); + } + } + }); + } + + QObject::connect(parent, &SettingsWindow::closeParentToggle, this, &FrogPilotControlsPanel::hideToggles); + QObject::connect(parent, &SettingsWindow::closeSubParentToggle, this, &FrogPilotControlsPanel::hideSubToggles); + QObject::connect(parent, &SettingsWindow::updateMetric, this, &FrogPilotControlsPanel::updateMetric); + QObject::connect(uiState(), &UIState::offroadTransition, this, &FrogPilotControlsPanel::updateCarToggles); + QObject::connect(uiState(), &UIState::uiUpdate, this, &FrogPilotControlsPanel::updateState); + + updateMetric(); +} + +void FrogPilotControlsPanel::showEvent(QShowEvent *event, const UIState &s) { + online = s.scene.online; + bool parked = s.scene.parked; + started = s.scene.started; +} + +void FrogPilotControlsPanel::updateState() { + if (!isVisible()) return; +} + +void FrogPilotControlsPanel::updateToggles() { + std::thread([this]() { + paramsMemory.putBool("FrogPilotTogglesUpdated", true); + std::this_thread::sleep_for(std::chrono::seconds(1)); + paramsMemory.putBool("FrogPilotTogglesUpdated", false); + }).detach(); +} + +void FrogPilotControlsPanel::updateCarToggles() { + auto carParams = params.get("CarParamsPersistent"); + if (!carParams.empty()) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(carParams.data(), carParams.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + auto carFingerprint = CP.getCarFingerprint(); + auto carName = CP.getCarName(); + + hasOpenpilotLongitudinal = CP.getOpenpilotLongitudinalControl() && !params.getBool("DisableOpenpilotLongitudinal"); + hasPCMCruise = CP.getPcmCruise(); + isToyota = carName == "toyota"; + } else { + hasAutoTune = false; + hasCommaNNFFSupport = false; + hasDashSpeedLimits = true; + hasNNFFLog = true; + hasOpenpilotLongitudinal = true; + hasPCMCruise = true; + isToyota = true; + } + + hideToggles(); +} + +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 { + } +} + +void FrogPilotControlsPanel::hideToggles() { + std::set longitudinalKeys = {"LongitudinalTune"}; + + for (auto &[key, toggle] : toggles) { + toggle->setVisible(false); + + if (!hasOpenpilotLongitudinal && longitudinalKeys.find(key.c_str()) != longitudinalKeys.end()) { + continue; + } + + bool subToggles = aolKeys.find(key.c_str()) != aolKeys.end() || + conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end() || + experimentalModeActivationKeys.find(key.c_str()) != experimentalModeActivationKeys.end() || + deviceManagementKeys.find(key.c_str()) != deviceManagementKeys.end() || + laneChangeKeys.find(key.c_str()) != laneChangeKeys.end() || + lateralTuneKeys.find(key.c_str()) != lateralTuneKeys.end() || + longitudinalTuneKeys.find(key.c_str()) != longitudinalTuneKeys.end() || + mtscKeys.find(key.c_str()) != mtscKeys.end() || + qolKeys.find(key.c_str()) != qolKeys.end() || + speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end() || + speedLimitControllerControlsKeys.find(key.c_str()) != speedLimitControllerControlsKeys.end() || + speedLimitControllerQOLKeys.find(key.c_str()) != speedLimitControllerQOLKeys.end() || + speedLimitControllerVisualsKeys.find(key.c_str()) != speedLimitControllerVisualsKeys.end() || + visionTurnControlKeys.find(key.c_str()) != visionTurnControlKeys.end(); + toggle->setVisible(!subToggles); + } + + update(); +} + +void FrogPilotControlsPanel::hideSubToggles() { + for (auto &[key, toggle] : toggles) { + bool isVisible = false; + toggle->setVisible(isVisible); + } + + update(); +} diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h new file mode 100644 index 000000000..8f76170b3 --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -0,0 +1,59 @@ +#pragma once + +#include + +#include "selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h" +#include "selfdrive/ui/qt/offroad/settings.h" +#include "selfdrive/ui/ui.h" + +class FrogPilotControlsPanel : public FrogPilotListWidget { + Q_OBJECT + +public: + explicit FrogPilotControlsPanel(SettingsWindow *parent); + +signals: + void openParentToggle(); + void openSubParentToggle(); + +private: + void hideSubToggles(); + void hideToggles(); + void showEvent(QShowEvent *event, const UIState &s); + void updateCarToggles(); + void updateMetric(); + void updateState(); + void updateToggles(); + + std::set aolKeys = {}; + std::set conditionalExperimentalKeys = {}; + std::set deviceManagementKeys = {}; + std::set experimentalModeActivationKeys = {}; + std::set laneChangeKeys = {}; + std::set lateralTuneKeys = {}; + std::set longitudinalTuneKeys = {}; + std::set mtscKeys = {}; + std::set qolKeys = {}; + std::set speedLimitControllerKeys = {}; + std::set speedLimitControllerControlsKeys = {}; + std::set speedLimitControllerQOLKeys = {}; + std::set speedLimitControllerVisualsKeys = {}; + std::set visionTurnControlKeys = {}; + + std::map toggles; + + Params params; + Params paramsMemory{"/dev/shm/params"}; + + bool hasAutoTune; + bool hasCommaNNFFSupport; + bool hasNNFFLog; + bool hasOpenpilotLongitudinal; + bool hasPCMCruise; + bool hasDashSpeedLimits; + bool isMetric = params.getBool("IsMetric"); + bool isStaging; + bool isToyota; + bool online; + bool started; +}; diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc new file mode 100644 index 000000000..cc2c40b2b --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc @@ -0,0 +1,151 @@ +#include "selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h" + +FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { + std::vector> vehicleToggles { + {"LongitudinalTune", tr("Longitudinal Tune"), tr("Use a custom Toyota longitudinal tune.\n\nCydia = More focused on TSS-P vehicles but works for all Toyotas\n\nDragonPilot = Focused on TSS2 vehicles\n\nFrogPilot = Takes the best of both worlds with some personal tweaks focused around FrogsGoMoo's 2019 Lexus ES 350"), ""}, + }; + + for (const auto &[param, title, desc, icon] : vehicleToggles) { + ParamControl *toggle; + + if (param == "LongitudinalTune") { + std::vector> tuneOptions{ + {"StockTune", tr("Stock")}, + }; + toggle = new FrogPilotButtonsParamControl(param, title, desc, icon, tuneOptions); + + QObject::connect(static_cast(toggle), &FrogPilotButtonsParamControl::buttonClicked, [this]() { + if (started) { + if (FrogPilotConfirmationDialog::toggle(tr("Reboot required to take effect."), tr("Reboot Now"), this)) { + Hardware::reboot(); + } + } + }); + + } else { + toggle = new ParamControl(param, title, desc, icon, this); + } + + toggle->setVisible(false); + addItem(toggle); + toggles[param.toStdString()] = toggle; + + QObject::connect(toggle, &ToggleControl::toggleFlipped, [this]() { + updateToggles(); + }); + + QObject::connect(toggle, &AbstractControl::showDescriptionEvent, [this]() { + update(); + }); + } + + std::set rebootKeys = {}; + for (const QString &key : rebootKeys) { + QObject::connect(toggles[key.toStdString().c_str()], &ToggleControl::toggleFlipped, [this]() { + if (started) { + if (FrogPilotConfirmationDialog::toggle(tr("Reboot required to take effect."), tr("Reboot Now"), this)) { + Hardware::reboot(); + } + } + }); + } + + QObject::connect(uiState(), &UIState::offroadTransition, this, &FrogPilotVehiclesPanel::updateCarToggles); + QObject::connect(uiState(), &UIState::uiUpdate, this, &FrogPilotVehiclesPanel::updateState); +} + +void FrogPilotVehiclesPanel::updateState(const UIState &s) { + if (!isVisible()) return; + + started = s.scene.started; +} + +void FrogPilotVehiclesPanel::updateToggles() { + std::thread([this]() { + paramsMemory.putBool("FrogPilotTogglesUpdated", true); + std::this_thread::sleep_for(std::chrono::seconds(1)); + paramsMemory.putBool("FrogPilotTogglesUpdated", false); + }).detach(); +} + +void FrogPilotVehiclesPanel::updateCarToggles() { + std::set evCars = { + "CHEVROLET BOLT EUV 2022", + "CHEVROLET BOLT EV NO ACC", + "CHEVROLET VOLT NO ACC", + "CHEVROLET VOLT PREMIER 2017", + }; + + auto carParams = params.get("CarParamsPersistent"); + if (!carParams.empty()) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(carParams.data(), carParams.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + + auto carFingerprint = CP.getCarFingerprint(); + + hasExperimentalOpenpilotLongitudinal = CP.getExperimentalLongitudinalAvailable(); + hasOpenpilotLongitudinal = CP.getOpenpilotLongitudinalControl(); + hasSNG = CP.getMinEnableSpeed() <= 0; + isEVCar = evCars.count(carFingerprint) > 0; + isGMTruck = carFingerprint == "CHEVROLET SILVERADO 1500 2020"; + isImpreza = carFingerprint == "SUBARU IMPREZA LIMITED 2019"; + } else { + hasExperimentalOpenpilotLongitudinal = false; + hasOpenpilotLongitudinal = true; + hasSNG = false; + isEVCar = true; + isGMTruck = true; + isImpreza = true; + } + + hideToggles(); +} + +void FrogPilotVehiclesPanel::hideToggles() { + bool gm = carMake == "Buick" || carMake == "Cadillac" || carMake == "Chevrolet" || carMake == "GM" || carMake == "GMC"; + bool subaru = carMake == "Subaru"; + bool toyota = carMake == "Lexus" || carMake == "Toyota"; + + std::set evCarKeys = {}; + std::set gmTruckKeys = {}; + std::set imprezaKeys = {}; + std::set longitudinalKeys = {"LongitudinalTune"}; + std::set sngKeys = {}; + + for (auto &[key, toggle] : toggles) { + if (toggle) { + toggle->setVisible(false); + + if (!hasOpenpilotLongitudinal && longitudinalKeys.find(key.c_str()) != longitudinalKeys.end()) { + continue; + } + + if (hasSNG && sngKeys.find(key.c_str()) != sngKeys.end()) { + continue; + } + + if (!isEVCar && evCarKeys.find(key.c_str()) != evCarKeys.end()) { + continue; + } + + if (!isGMTruck && gmTruckKeys.find(key.c_str()) != gmTruckKeys.end()) { + continue; + } + + if (!isImpreza && imprezaKeys.find(key.c_str()) != imprezaKeys.end()) { + continue; + } + + if (gm) { + toggle->setVisible(gmKeys.find(key.c_str()) != gmKeys.end()); + } else if (subaru) { + toggle->setVisible(subaruKeys.find(key.c_str()) != subaruKeys.end()); + } else if (toyota) { + toggle->setVisible(toyotaKeys.find(key.c_str()) != toyotaKeys.end()); + } + } + } + + update(); +} diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h new file mode 100644 index 000000000..3597c4f8c --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h @@ -0,0 +1,39 @@ +#pragma once + +#include + +#include + +#include "selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h" +#include "selfdrive/ui/qt/offroad/settings.h" +#include "selfdrive/ui/ui.h" + +class FrogPilotVehiclesPanel : public FrogPilotListWidget { + Q_OBJECT + +public: + explicit FrogPilotVehiclesPanel(SettingsWindow *parent); + +private: + void hideToggles(); + void updateCarToggles(); + void updateState(const UIState &s); + void updateToggles(); + + std::set gmKeys = {}; + std::set subaruKeys = {}; + std::set toyotaKeys = {"LongitudinalTune"}; + + std::map toggles; + + Params params; + Params paramsMemory{"/dev/shm/params"}; + + bool hasExperimentalOpenpilotLongitudinal; + bool hasOpenpilotLongitudinal; + bool hasSNG; + bool isEVCar; + bool isGMTruck; + bool isImpreza; + bool started; +}; diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc new file mode 100644 index 000000000..14d70b3a5 --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc @@ -0,0 +1,148 @@ +#include "selfdrive/frogpilot/ui/qt/offroad/visual_settings.h" + +FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { + const std::vector> visualToggles { + {"CustomAlerts", tr("Custom Alerts"), tr("Enable custom alerts for openpilot events."), "../frogpilot/assets/toggle_icons/icon_green_light.png"}, + + {"CustomUI", tr("Custom Onroad UI"), tr("Customize the Onroad UI."), "../assets/offroad/icon_road.png"}, + {"CustomPaths", tr("Paths"), tr("Show your projected acceleration on the driving path, detected adjacent lanes, or when a vehicle is detected in your blindspot."), ""}, + + {"QOLVisuals", tr("Quality of Life"), tr("Miscellaneous quality of life changes to improve your overall openpilot experience."), "../frogpilot/assets/toggle_icons/quality_of_life.png"}, + }; + + for (const auto &[param, title, desc, icon] : visualToggles) { + ParamControl *toggle; + + if (param == "CustomAlerts") { + FrogPilotParamManageControl *customAlertsToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(customAlertsToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + openParentToggle(); + for (auto &[key, toggle] : toggles) { + std::set modifiedCustomAlertsKeys = customAlertsKeys; + + toggle->setVisible(modifiedCustomAlertsKeys.find(key.c_str()) != modifiedCustomAlertsKeys.end()); + } + }); + toggle = customAlertsToggle; + + } else if (param == "CustomUI") { + FrogPilotParamManageControl *customUIToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(customUIToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + openParentToggle(); + for (auto &[key, toggle] : toggles) { + std::set modifiedCustomOnroadUIKeys = customOnroadUIKeys; + + toggle->setVisible(modifiedCustomOnroadUIKeys.find(key.c_str()) != modifiedCustomOnroadUIKeys.end()); + } + }); + toggle = customUIToggle; + } else if (param == "CustomPaths") { + std::vector pathToggles{}; + std::vector pathToggleNames{}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, pathToggles, pathToggleNames); + + } else if (param == "QOLVisuals") { + FrogPilotParamManageControl *qolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(qolToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + openParentToggle(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(qolKeys.find(key.c_str()) != qolKeys.end()); + } + }); + toggle = qolToggle; + + } 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), &FrogPilotButtonParamControl::buttonClicked, [this]() { + updateToggles(); + }); + + QObject::connect(static_cast(toggle), &FrogPilotParamValueControl::valueChanged, [this]() { + updateToggles(); + }); + + QObject::connect(toggle, &AbstractControl::showDescriptionEvent, [this]() { + update(); + }); + + QObject::connect(static_cast(toggle), &FrogPilotParamManageControl::manageButtonClicked, [this]() { + update(); + }); + } + + QObject::connect(parent, &SettingsWindow::closeParentToggle, this, &FrogPilotVisualsPanel::hideToggles); + QObject::connect(parent, &SettingsWindow::updateMetric, this, &FrogPilotVisualsPanel::updateMetric); + QObject::connect(uiState(), &UIState::offroadTransition, this, &FrogPilotVisualsPanel::updateCarToggles); + QObject::connect(uiState(), &UIState::uiUpdate, this, &FrogPilotVisualsPanel::updateState); + + updateMetric(); +} + +void FrogPilotVisualsPanel::updateState(const UIState &s) { + if (!isVisible()) return; + + started = s.scene.started; +} + +void FrogPilotVisualsPanel::updateToggles() { + std::thread([this]() { + paramsMemory.putBool("FrogPilotTogglesUpdated", true); + std::this_thread::sleep_for(std::chrono::seconds(1)); + paramsMemory.putBool("FrogPilotTogglesUpdated", false); + }).detach(); +} + +void FrogPilotVisualsPanel::updateCarToggles() { + auto carParams = params.get("CarParamsPersistent"); + if (!carParams.empty()) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(carParams.data(), carParams.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + + hasBSM = CP.getEnableBsm(); + hasOpenpilotLongitudinal = CP.getOpenpilotLongitudinalControl(); + } else { + hasBSM = true; + hasOpenpilotLongitudinal = true; + } + + hideToggles(); +} + +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 { + } +} + +void FrogPilotVisualsPanel::hideToggles() { + for (auto &[key, toggle] : toggles) { + bool subToggles = alertVolumeControlKeys.find(key.c_str()) != alertVolumeControlKeys.end() || + customAlertsKeys.find(key.c_str()) != customAlertsKeys.end() || + customOnroadUIKeys.find(key.c_str()) != customOnroadUIKeys.end() || + customThemeKeys.find(key.c_str()) != customThemeKeys.end() || + modelUIKeys.find(key.c_str()) != modelUIKeys.end() || + qolKeys.find(key.c_str()) != qolKeys.end() || + screenKeys.find(key.c_str()) != screenKeys.end(); + toggle->setVisible(!subToggles); + } + + update(); +} diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h new file mode 100644 index 000000000..97a23eeb5 --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h @@ -0,0 +1,42 @@ +#pragma once + +#include + +#include "selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h" +#include "selfdrive/ui/qt/offroad/settings.h" +#include "selfdrive/ui/ui.h" + +class FrogPilotVisualsPanel : public FrogPilotListWidget { + Q_OBJECT + +public: + explicit FrogPilotVisualsPanel(SettingsWindow *parent); + +signals: + void openParentToggle(); + +private: + void hideToggles(); + void updateCarToggles(); + void updateMetric(); + void updateState(const UIState &s); + void updateToggles(); + + std::set alertVolumeControlKeys = {}; + std::set customAlertsKeys = {}; + std::set customOnroadUIKeys = {"CustomPaths"}; + std::set customThemeKeys = {}; + std::set modelUIKeys = {}; + std::set qolKeys = {}; + std::set screenKeys = {}; + + std::map toggles; + + Params params; + Params paramsMemory{"/dev/shm/params"}; + + bool hasBSM; + bool hasOpenpilotLongitudinal; + bool isMetric = params.getBool("IsMetric"); + bool started; +}; diff --git a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc new file mode 100644 index 000000000..ea38a4191 --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.cc @@ -0,0 +1,42 @@ +#include + +#include "selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.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); +} diff --git a/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h new file mode 100644 index 000000000..f6a78e191 --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h @@ -0,0 +1,699 @@ +#pragma once + +#include + +#include + +#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 = nullptr) : 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 + } + + inline void addItem(QWidget *w) { + inner_layout.addWidget(w); + adjustStretch(); + } + + inline void addItem(QLayout *layout) { + inner_layout.addLayout(layout); + adjustStretch(); + } + + inline void setSpacing(int spacing) { + inner_layout.setSpacing(spacing); + adjustStretch(); + } + +private: + void adjustStretch() { + if (inner_layout.stretch(inner_layout.count() - 1) > 0) { + inner_layout.setStretch(inner_layout.count() - 1, 0); + } + if (inner_layout.count() > 3) { + outer_layout.addStretch(); + } + } + + void paintEvent(QPaintEvent *event) 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 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 ? 850 : 700); + + control2->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred); + control2->setMaximumWidth(split ? 700 : 850); + + hlayout->addWidget(control1); + hlayout->addWidget(control2); + } +}; + +class FrogPilotButtonControl : public AbstractControl { + Q_OBJECT + +public: + FrogPilotButtonControl(const QString &title, const QString &text, const QString &desc = "", 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 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 FrogPilotButtonsControl : public ParamControl { + Q_OBJECT +public: + FrogPilotButtonsControl(const QString &title, const QString &desc, const QString &icon, + const std::vector &button_texts, const int minimum_button_width = 225) + : ParamControl("", title, desc, icon) { + const QString style = R"( + QPushButton { + border-radius: 50px; + font-size: 40px; + font-weight: 500; + height: 100px; + padding: 0 25px 0 25px; + color: #E4E4E4; + background-color: #393939; + } + QPushButton:pressed { + background-color: #33Ab4C; + } + QPushButton:disabled { + color: #33E4E4E4; + } + )"; + + button_group = new QButtonGroup(this); + + for (size_t i = 0; i < button_texts.size(); i++) { + QPushButton *button = new QPushButton(button_texts[i], this); + button->setStyleSheet(style); + button->setMinimumWidth(minimum_button_width); + hlayout->addWidget(button); + button_group->addButton(button, static_cast(i)); + + connect(button, &QPushButton::clicked, this, [this, i]() { + emit buttonClicked(static_cast(i)); + }); + } + + toggle.hide(); + } + +signals: + void buttonClicked(int id); + +private: + QButtonGroup *button_group; +}; + +class FrogPilotButtonsParamControl : public ParamControl { + Q_OBJECT +public: + FrogPilotButtonsParamControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, + const std::vector> &button_params) + : 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; + } + )"; + + button_group = new QButtonGroup(this); + button_group->setExclusive(true); + + for (const auto ¶m_pair : button_params) { + const QString ¶m_toggle = param_pair.first; + const QString &button_text = param_pair.second; + + QPushButton *button = new QPushButton(button_text, this); + button->setCheckable(true); + + bool value = params.getBool(param_toggle.toStdString()); + button->setChecked(value); + button->setStyleSheet(style); + button->setMinimumWidth(225); + hlayout->addWidget(button); + + QObject::connect(button, &QPushButton::toggled, this, [=](bool checked) { + if (checked) { + for (const auto &inner_param_pair : button_params) { + const QString &inner_param = inner_param_pair.first; + params.putBool(inner_param.toStdString(), inner_param == param_toggle); + } + refresh(); + emit buttonClicked(); + } + }); + + button_group->addButton(button); + } + + toggle.hide(); + } + + void setEnabled(bool enable) { + for (auto btn : button_group->buttons()) { + btn->setEnabled(enable); + } + } + +signals: + void buttonClicked(); + +private: + 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, bool hideToggle = false) + : ParamControl(param, title, desc, icon, parent), + hideToggle(hideToggle), + 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); + + if (hideToggle) { + toggle.hide(); + } + } + + void refresh() { + ParamControl::refresh(); + manageButton->setVisible(params.getBool(key) || hideToggle); + } + + void setEnabled(bool enabled) { + manageButton->setEnabled(enabled); + toggle.setEnabled(enabled); + toggle.update(); + } + + void showEvent(QShowEvent *event) override { + ParamControl::showEvent(event); + refresh(); + } + +signals: + void manageButtonClicked(); + +private: + bool hideToggle; + 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) { + + key = param.toStdString(); + + 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); + this->button_params = button_params; + + for (int i = 0; i < button_texts.size(); ++i) { + QPushButton *button = new QPushButton(button_texts[i], this); + button->setCheckable(true); + button->setStyleSheet(style); + button->setMinimumWidth(minimum_button_width); + button_group->addButton(button, i); + + connect(button, &QPushButton::clicked, [this, i](bool checked) { + params.putBool(this->button_params[i].toStdString(), checked); + button_group->button(i)->setChecked(checked); + emit buttonClicked(checked); + }); + + hlayout->insertWidget(hlayout->indexOf(&toggle) - 1, button); + } + } + + void refresh() { + bool state = params.getBool(key); + if (state != toggle.on) { + toggle.togglePosition(); + } + + refreshButtons(state); + updateButtonStates(); + } + + void refreshButtons(bool state) { + for (QAbstractButton *button : button_group->buttons()) { + button->setVisible(state); + } + } + + void updateButtonStates() { + for (int i = 0; i < button_group->buttons().size(); ++i) { + bool checked = params.getBool(button_params[i].toStdString()); + QAbstractButton *button = button_group->button(i); + if (button) { + button->setChecked(checked); + } + } + } + + void showEvent(QShowEvent *event) override { + refresh(); + QWidget::showEvent(event); + } + +signals: + void buttonClicked(const bool checked); + +private: + std::string key; + Params params; + QButtonGroup *button_group; + std::vector button_params; +}; + +class FrogPilotParamValueControl : public ParamControl { + Q_OBJECT + +public: + FrogPilotParamValueControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, + const float &minValue, const float &maxValue, const std::map &valueLabels, + QWidget *parent = nullptr, const bool &loop = true, const QString &label = "", + const float &division = 1.0f, const float &interval = 1.0f) + : ParamControl(param, title, desc, icon, parent), + minValue(minValue), maxValue(maxValue), valueLabelMappings(valueLabels), loop(loop), labelText(label), + division(division), interval(interval), previousValue(0.0f), value(0.0f) { + key = param.toStdString(); + + valueLabel = new QLabel(this); + hlayout->addWidget(valueLabel); + + QPushButton *decrementButton = createButton("-", this); + QPushButton *incrementButton = createButton("+", this); + + hlayout->addWidget(decrementButton); + hlayout->addWidget(incrementButton); + + countdownTimer = new QTimer(this); + countdownTimer->setInterval(150); + countdownTimer->setSingleShot(true); + + connect(countdownTimer, &QTimer::timeout, this, &FrogPilotParamValueControl::handleTimeout); + + connect(decrementButton, &QPushButton::pressed, this, [=]() { updateValue(-interval); }); + connect(incrementButton, &QPushButton::pressed, this, [=]() { updateValue(interval); }); + + connect(decrementButton, &QPushButton::released, this, &FrogPilotParamValueControl::restartTimer); + connect(incrementButton, &QPushButton::released, this, &FrogPilotParamValueControl::restartTimer); + + toggle.hide(); + } + + void restartTimer() { + countdownTimer->stop(); + countdownTimer->start(); + + emit valueChanged(value); + } + + void handleTimeout() { + previousValue = value; + } + + void updateValue(float intervalChange) { + int previousValueAdjusted = round(previousValue * 100) / 100 / intervalChange; + int valueAdjusted = round(value * 100) / 100 / intervalChange; + + if (std::fabs(previousValueAdjusted - valueAdjusted) > 5 && std::fmod(valueAdjusted, 5) == 0) { + intervalChange *= 5; + } + + value += intervalChange; + + if (loop) { + if (value < minValue) { + value = maxValue; + } else if (value > maxValue) { + value = minValue; + } + } else { + value = std::max(minValue, std::min(maxValue, value)); + } + + params.putFloat(key, value); + refresh(); + } + + void refresh() { + value = params.getFloat(key); + + QString text; + auto it = valueLabelMappings.find(value); + + if (division > 1.0f) { + text = QString::number(value / division, 'g', division >= 10.0f ? 4 : 3); + } else if (interval < 1.0f && value < 10.0f) { + text = QString::number(value / division, 'f', 2); + } else if (interval < 1.0f && value > 10.0f) { + text = QString::number(value, 'f', 2); + } else { + if (it != valueLabelMappings.end()) { + text = it->second; + } else { + text = QString::number(value, 'g', 4); + } + } + + if (!labelText.isEmpty()) { + text += labelText; + } + + valueLabel->setText(text); + valueLabel->setStyleSheet("QLabel { color: #E0E879; }"); + } + + void updateControl(float newMinValue, float newMaxValue, const QString &newLabel, float newDivision = 1.0f) { + minValue = newMinValue; + maxValue = newMaxValue; + labelText = newLabel; + division = newDivision; + } + + void showEvent(QShowEvent *event) override { + refresh(); + previousValue = value; + } + +signals: + void valueChanged(float value); + +private: + Params params; + + bool loop; + + float division; + float interval; + float maxValue; + float minValue; + float previousValue; + float value; + + QLabel *valueLabel; + QString labelText; + + std::map valueLabelMappings; + std::string key; + + QTimer *countdownTimer; + + QPushButton *createButton(const QString &text, QWidget *parent) { + QPushButton *button = new QPushButton(text, parent); + button->setFixedSize(150, 100); + button->setAutoRepeat(true); + button->setAutoRepeatInterval(150); + button->setAutoRepeatDelay(500); + 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 FrogPilotParamValueToggleControl : public FrogPilotParamValueControl { + Q_OBJECT + +public: + FrogPilotParamValueToggleControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, + const float &minValue, const float &maxValue, const std::map &valueLabels, + QWidget *parent = nullptr, const bool &loop = true, const QString &label = "", + const float &division = 1.0f, const float &interval = 1.0f, + const std::vector &button_params = std::vector(), const std::vector &button_texts = std::vector(), + const int minimum_button_width = 225) + : FrogPilotParamValueControl(param, title, desc, icon, minValue, maxValue, valueLabels, parent, loop, label, division, interval) { + + 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); + + for (int i = 0; i < button_texts.size(); ++i) { + QPushButton *button = new QPushButton(button_texts[i], this); + button->setCheckable(true); + button->setChecked(params.getBool(button_params[i].toStdString())); + 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); + refresh(); + }); + + buttons[button_params[i]] = button; + hlayout->insertWidget(3, button); + } + } + + void refresh() { + FrogPilotParamValueControl::refresh(); + + auto keys = buttons.keys(); + for (const QString ¶m : keys) { + QPushButton *button = buttons.value(param); + button->setChecked(params.getBool(param.toStdString())); + } + } + +private: + Params params; + QButtonGroup *button_group; + QMap buttons; +}; diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index d124eb5f0..0df431220 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -136,6 +136,7 @@ def main(): min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio params = params_reader.get("LiveParameters") + params_memory = Params("/dev/shm/params") # Check if car model matches if params is not None: diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 24dceaaf0..a24cf39c4 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -2,7 +2,10 @@ import datetime import os import signal +import subprocess import sys +import threading +import time import traceback from cereal import log @@ -10,6 +13,7 @@ import cereal.messaging as messaging import openpilot.selfdrive.sentry as sentry from openpilot.common.params import Params, ParamKeyType from openpilot.common.text_window import TextWindow +from openpilot.common.time import system_time_valid from openpilot.system.hardware import HARDWARE, PC from openpilot.selfdrive.manager.helpers import unblock_stdout, write_onroad_params, save_bootlog from openpilot.selfdrive.manager.process import ensure_running @@ -20,12 +24,26 @@ from openpilot.system.version import is_dirty, get_commit, get_version, get_orig get_normalized_origin, terms_version, training_version, \ is_tested_branch, is_release_branch, get_commit_date +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import FrogPilotFunctions -def manager_init() -> None: +def frogpilot_boot_functions(frogpilot_functions): + try: + while not system_time_valid(): + print("Waiting for system time to become valid...") + time.sleep(1) + + except Exception as e: + print(f"An unexpected error occurred: {e}") + +def manager_init(frogpilot_functions) -> None: + frogpilot_boot = threading.Thread(target=frogpilot_boot_functions, args=(frogpilot_functions,)) + frogpilot_boot.start() + save_bootlog() params = Params() + params_storage = Params("/persist/params") params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) @@ -33,13 +51,218 @@ def manager_init() -> None: params.clear_all(ParamKeyType.DEVELOPMENT_ONLY) default_params: list[tuple[str, str | bytes]] = [ + ("CarParamsPersistent", ""), ("CompletedTrainingVersion", "0"), ("DisengageOnAccelerator", "0"), + ("ExperimentalLongitudinalEnabled", "1"), ("GsmMetered", "1"), ("HasAcceptedTerms", "0"), + ("IsLdwEnabled", "0"), + ("IsMetric", "0"), ("LanguageSetting", "main_en"), + ("NavSettingLeftSide", "0"), + ("NavSettingTime24h", "0"), ("OpenpilotEnabledToggle", "1"), + ("RecordFront", "0"), ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), + + # Default FrogPilot parameters + ("AccelerationPath", "1"), + ("AccelerationProfile", "2"), + ("AdjacentPath", "0"), + ("AdjacentPathMetrics", "0"), + ("AggressiveAcceleration", "1"), + ("AggressiveFollow", "1.25"), + ("AggressiveJerk", "0.5"), + ("AlertVolumeControl", "0"), + ("AlwaysOnLateral", "1"), + ("AlwaysOnLateralMain", "0"), + ("AMapKey1", ""), + ("AMapKey2", ""), + ("AutomaticUpdates", "0"), + ("BlindSpotPath", "1"), + ("CameraView", "2"), + ("CarMake", ""), + ("CarModel", ""), + ("CECurves", "1"), + ("CENavigation", "1"), + ("CENavigationIntersections", "1"), + ("CENavigationLead", "1"), + ("CENavigationTurns", "1"), + ("CESignal", "1"), + ("CESlowerLead", "1"), + ("CESpeed", "0"), + ("CESpeedLead", "0"), + ("CEStopLights", "1"), + ("CEStopLightsLead", "0"), + ("Compass", "1"), + ("ConditionalExperimental", "1"), + ("CrosstrekTorque", "1"), + ("CurveSensitivity", "100"), + ("CustomAlerts", "1"), + ("CustomColors", "1"), + ("CustomCruise", "1"), + ("CustomCruiseLong", "5"), + ("CustomIcons", "1"), + ("CustomPaths", "1"), + ("CustomPersonalities", "1"), + ("CustomSignals", "1"), + ("CustomSounds", "1"), + ("CustomTheme", "1"), + ("CustomUI", "1"), + ("CydiaTune", "0"), + ("DecelerationProfile", "1"), + ("DeveloperUI", "0"), + ("DeviceManagement", "1"), + ("DeviceShutdown", "9"), + ("DisableMTSCSmoothing", "0"), + ("DisableOnroadUploads", "0"), + ("DisableOpenpilotLongitudinal", "0"), + ("DisableVTSCSmoothing", "0"), + ("DisengageVolume", "100"), + ("DragonPilotTune", "0"), + ("DriverCamera", "0"), + ("DynamicPathWidth", "0"), + ("EngageVolume", "100"), + ("EVTable", "1"), + ("ExperimentalModeActivation", "1"), + ("ExperimentalModeViaDistance", "1"), + ("ExperimentalModeViaLKAS", "1"), + ("ExperimentalModeViaTap", "0"), + ("Fahrenheit", "0"), + ("ForceAutoTune", "1"), + ("ForceFingerprint", "0"), + ("ForceMPHDashboard", "0"), + ("FPSCounter", "0"), + ("FrogsGoMooTune", "1"), + ("FullMap", "0"), + ("GasRegenCmd", "0"), + ("GMapKey", ""), + ("GoatScream", "1"), + ("GreenLightAlert", "0"), + ("HideAlerts", "0"), + ("HideAOLStatusBar", "0"), + ("HideCEMStatusBar", "0"), + ("HideLeadMarker", "0"), + ("HideMapIcon", "0"), + ("HideMaxSpeed", "0"), + ("HideSpeed", "0"), + ("HideSpeedUI", "0"), + ("HideUIElements", "0"), + ("HigherBitrate", "0"), + ("HolidayThemes", "1"), + ("IncreaseThermalLimits", "0"), + ("LaneChangeTime", "0"), + ("LaneDetectionWidth", "60"), + ("LaneLinesWidth", "4"), + ("LateralTune", "1"), + ("LeadDepartingAlert", "0"), + ("LeadDetectionThreshold", "35"), + ("LeadInfo", "0"), + ("LockDoors", "1"), + ("LongitudinalTune", "1"), + ("LongPitch", "1"), + ("LoudBlindspotAlert", "0"), + ("LowVoltageShutdown", "11.8"), + ("MapsSelected", ""), + ("MapboxPublicKey", ""), + ("MapboxSecretKey", ""), + ("MapStyle", "0"), + ("MTSCAggressiveness", "100"), + ("MTSCCurvatureCheck", "0"), + ("Model", DEFAULT_MODEL), + ("ModelName", DEFAULT_MODEL_NAME), + ("ModelSelector", "1"), + ("ModelUI", "1"), + ("MTSCEnabled", "1"), + ("NNFF", "1"), + ("NNFFLite", "1"), + ("NoLogging", "0"), + ("NoUploads", "0"), + ("NudgelessLaneChange", "1"), + ("NumericalTemp", "0"), + ("OfflineMode", "1"), + ("Offset1", "5"), + ("Offset2", "5"), + ("Offset3", "5"), + ("Offset4", "10"), + ("OneLaneChange", "1"), + ("OnroadDistanceButton", "0"), + ("PathEdgeWidth", "20"), + ("PathWidth", "61"), + ("PauseAOLOnBrake", "0"), + ("PauseLateralOnSignal", "0"), + ("PedalsOnUI", "1"), + ("PreferredSchedule", "0"), + ("PromptVolume", "100"), + ("PromptDistractedVolume", "100"), + ("QOLControls", "1"), + ("QOLVisuals", "1"), + ("RandomEvents", "0"), + ("RefuseVolume", "100"), + ("RelaxedFollow", "1.75"), + ("RelaxedJerk", "1.0"), + ("ReverseCruise", "0"), + ("ReverseCruiseUI", "1"), + ("RoadEdgesWidth", "2"), + ("RoadNameUI", "1"), + ("RotatingWheel", "1"), + ("ScreenBrightness", "101"), + ("ScreenBrightnessOnroad", "101"), + ("ScreenManagement", "1"), + ("ScreenRecorder", "1"), + ("ScreenTimeout", "30"), + ("ScreenTimeoutOnroad", "30"), + ("SearchInput", "0"), + ("SetSpeedLimit", "0"), + ("SetSpeedOffset", "0"), + ("ShowCPU", "0"), + ("ShowGPU", "0"), + ("ShowIP", "0"), + ("ShowJerk", "1"), + ("ShowMemoryUsage", "0"), + ("ShowSLCOffset", "1"), + ("ShowSLCOffsetUI", "1"), + ("ShowStorageLeft", "0"), + ("ShowStorageUsed", "0"), + ("ShowTuning", "1"), + ("Sidebar", "0"), + ("SLCConfirmation", "1"), + ("SLCConfirmationLower", "1"), + ("SLCConfirmationHigher", "1"), + ("SLCFallback", "2"), + ("SLCOverride", "1"), + ("SLCPriority1", "Dashboard"), + ("SLCPriority2", "Offline Maps"), + ("SLCPriority3", "Navigation"), + ("SmoothBraking", "1"), + ("SmoothBrakingFarLead", "0"), + ("SmoothBrakingJerk", "0"), + ("SNGHack", "1"), + ("SpeedLimitChangedAlert", "1"), + ("SpeedLimitController", "1"), + ("StandardFollow", "1.45"), + ("StandardJerk", "1.0"), + ("StandbyMode", "0"), + ("SteerRatio", "0"), + ("StockTune", "0"), + ("StoppingDistance", "0"), + ("TacoTune", "1"), + ("ToyotaDoors", "0"), + ("TrafficFollow", "0.5"), + ("TrafficJerk", "1"), + ("TrafficMode", "0"), + ("TurnAggressiveness", "100"), + ("TurnDesires", "0"), + ("UnlimitedLength", "1"), + ("UnlockDoors", "1"), + ("UseSI", "1"), + ("UseVienna", "0"), + ("VisionTurnControl", "1"), + ("WarningSoftVolume", "100"), + ("WarningImmediateVolume", "100"), + ("WheelIcon", "3"), + ("WheelSpeed", "0") ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8'))) @@ -50,7 +273,12 @@ def manager_init() -> None: # set unset params for k, v in default_params: if params.get(k) is None: - params.put(k, v) + if params_storage.get(k) is None: + params.put(k, v) + else: + params.put(k, params_storage.get(k)) + else: + params_storage.put(k, params.get(k)) # Create folders needed for msgq try: @@ -119,6 +347,7 @@ def manager_thread() -> None: cloudlog.info({"environ": os.environ}) params = Params() + params_memory = Params("/dev/shm/params") ignore: list[str] = [] if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID): @@ -144,6 +373,7 @@ def manager_thread() -> None: params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) elif not started and started_prev: params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) + params_memory.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) # update onroad params, which drives boardd's safety setter thread if started != started_prev: @@ -176,7 +406,15 @@ def manager_thread() -> None: def main() -> None: - manager_init() + frogpilot_functions = FrogPilotFunctions() + + try: + frogpilot_functions.setup_frogpilot() + except subprocess.CalledProcessError as e: + print(f"Failed to setup FrogPilot. Error: {e}") + return + + manager_init(frogpilot_functions) if os.getenv("PREPAREONLY") is not None: return @@ -194,7 +432,7 @@ def main() -> None: params = Params() if params.get_bool("DoUninstall"): cloudlog.warning("uninstalling") - HARDWARE.uninstall() + frogpilot_functions.uninstall_frogpilot() elif params.get_bool("DoReboot"): cloudlog.warning("reboot") HARDWARE.reboot() diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 8b616b787..f4f700652 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"), @@ -87,6 +89,9 @@ procs = [ NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar), PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), PythonProcess("webjoystick", "tools.bodyteleop.web", notcar), + + # FrogPilot processes + PythonProcess("frogpilot_process", "selfdrive.frogpilot.frogpilot_process", always_run), ] managed_processes = {p.name: p for p in procs} diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index c3b391890..fb1b8f150 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -154,7 +154,7 @@ def main(demo=False): # messaging pm = PubMaster(["modelV2", "cameraOdometry"]) - sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl"]) + sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl", "frogpilotPlan"]) publish_state = PublishState() params = Params() @@ -299,7 +299,7 @@ def main(demo=False): l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] lane_change_prob = l_lane_change_prob + r_lane_change_prob - DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) + DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['frogpilotPlan']) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 2be703cdb..6d887b4a6 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,6 +60,9 @@ class RouteEngine: self.mapbox_token = "" self.mapbox_host = "https://maps.comma.ai" + # FrogPilot variables + self.update_frogpilot_params() + def update(self): self.sm.update(0) @@ -77,6 +81,10 @@ class RouteEngine: except Exception: cloudlog.exception("navd.failed_to_compute") + # Update FrogPilot parameters + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + def update_location(self): location = self.sm['liveLocationKalman'] self.gps_ok = location.gpsOK @@ -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 073589edb..d9bc8e2a4 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -36,6 +36,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: int | None, ignition: bool): try: diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index bfc50c447..9a744f041 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -163,7 +163,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 @@ -242,6 +242,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.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent())) diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index ea5734fe6..bff18d602 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -23,7 +23,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/qt/widgets/frogpilot_controls.cc", + "../frogpilot/ui/qt/offroad/control_settings.cc", "../frogpilot/ui/qt/offroad/vehicle_settings.cc", + "../frogpilot/ui/qt/offroad/visual_settings.cc"] qt_env['CPPDEFINES'] = [] if maps: diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index 9dbe7cbae..254ddd6f7 100644 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -74,6 +74,7 @@ void HomeWindow::offroadTransition(bool offroad) { slayout->setCurrentWidget(home); } else { slayout->setCurrentWidget(onroad); + uiState()->scene.map_open = onroad->isMapVisible(); } } @@ -91,6 +92,7 @@ void HomeWindow::mousePressEvent(QMouseEvent* e) { // Handle sidebar collapsing if ((onroad->isVisible() || body->isVisible()) && (!sidebar->isVisible() || e->x() > sidebar->width())) { sidebar->setVisible(!sidebar->isVisible() && !onroad->isMapVisible()); + uiState()->scene.map_open = onroad->isMapVisible(); } } diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h index c6032852a..b9f8e3752 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 { @@ -69,6 +71,9 @@ private: DriverViewWindow *driver_view; QStackedLayout *slayout; + // FrogPilot variables + Params params; + private slots: void updateState(const UIState &s); }; diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index 81ba50037..5849b64f5 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -78,6 +78,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 0f4be674f..aefc0c420 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -8,6 +8,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/maps/map_settings.h b/selfdrive/ui/qt/maps/map_settings.h index 0e151df4a..d7d019c5e 100644 --- a/selfdrive/ui/qt/maps/map_settings.h +++ b/selfdrive/ui/qt/maps/map_settings.h @@ -64,6 +64,8 @@ private: DestinationWidget *work_widget; std::vector widgets; + // FrogPilot variables + signals: void closeSettings(); }; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index bc7989b77..b35ac1248 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/qt/offroad/control_settings.h" +#include "selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h" +#include "selfdrive/frogpilot/ui/qt/offroad/visual_settings.h" + TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { // param, title, desc, icon std::vector> toggle_defs{ @@ -122,6 +127,10 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { connect(toggles["ExperimentalLongitudinalEnabled"], &ToggleControl::toggleFlipped, [=]() { updateToggles(); }); + + connect(toggles["IsMetric"], &ToggleControl::toggleFlipped, [=]() { + updateMetric(); + }); } void TogglesPanel::updateState(const UIState &s) { @@ -262,6 +271,22 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { }); addItem(translateBtn); + // Delete long term toggle storage button + auto deleteStorageParamsBtn = new ButtonControl(tr("Delete Toggle Storage Data"), tr("DELETE"), tr("This button provides a swift and secure way to permanently delete all " + "long term stored toggle settings. Ideal for maintaining privacy or freeing up space.") + ); + connect(deleteStorageParamsBtn, &ButtonControl::clicked, [=]() { + if (!ConfirmationDialog::confirm(tr("Are you sure you want to permanently delete all of your long term toggle settings storage?"), tr("Delete"), this)) return; + std::thread([&] { + deleteStorageParamsBtn->setValue(tr("Deleting params...")); + std::system("rm -rf /persist/params"); + deleteStorageParamsBtn->setValue(tr("Deleted!")); + std::this_thread::sleep_for(std::chrono::seconds(3)); + deleteStorageParamsBtn->setValue(""); + }).detach(); + }); + addItem(deleteStorageParamsBtn); + QObject::connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { for (auto btn : findChildren()) { btn->setEnabled(offroad); @@ -345,6 +370,15 @@ void DevicePanel::poweroff() { } } +void SettingsWindow::hideEvent(QHideEvent *event) { + closeParentToggle(); + + parentToggleOpen = false; + subParentToggleOpen = false; + + previousScrollPosition = 0; +} + void SettingsWindow::showEvent(QShowEvent *event) { setCurrentPanel(0); } @@ -381,7 +415,17 @@ 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 (subParentToggleOpen) { + closeSubParentToggle(); + subParentToggleOpen = false; + } else if (parentToggleOpen) { + closeParentToggle(); + parentToggleOpen = false; + } else { + closeSettings(); + } + }); // setup panels DevicePanel *device = new DevicePanel(this); @@ -390,12 +434,23 @@ 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::openSubParentToggle, this, [this]() {subParentToggleOpen = true;}); + QObject::connect(frogpilotControls, &FrogPilotControlsPanel::openParentToggle, this, [this]() {parentToggleOpen = true;}); + + FrogPilotVisualsPanel *frogpilotVisuals = new FrogPilotVisualsPanel(this); + QObject::connect(frogpilotVisuals, &FrogPilotVisualsPanel::openParentToggle, this, [this]() {parentToggleOpen = 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); @@ -428,7 +483,25 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { ScrollView *panel_frame = new ScrollView(panel, this); panel_widget->addWidget(panel_frame); + if (name == tr("Controls")) { + QScrollBar *scrollbar = panel_frame->verticalScrollBar(); + + QObject::connect(scrollbar, &QScrollBar::valueChanged, this, [this](int value) { + if (!parentToggleOpen) { + previousScrollPosition = value; + } + }); + + QObject::connect(scrollbar, &QScrollBar::rangeChanged, this, [this, panel_frame]() { + if (!parentToggleOpen) { + panel_frame->restorePosition(previousScrollPosition); + } + }); + } + QObject::connect(btn, &QPushButton::clicked, [=, w = panel_frame]() { + closeParentToggle(); + 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 581fc098f..080f609e3 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -26,17 +26,31 @@ public: protected: void showEvent(QShowEvent *event) override; + // FrogPilot widgets + void hideEvent(QHideEvent *event) override; + signals: void closeSettings(); void reviewTrainingGuide(); void showDriverView(); void expandToggleDescription(const QString ¶m); + // FrogPilot signals + void closeParentToggle(); + void closeSubParentToggle(); + void updateMetric(); + private: QPushButton *sidebar_alert_widget; QWidget *sidebar_widget; QButtonGroup *nav_btns; QStackedWidget *panel_widget; + + // FrogPilot variables + bool parentToggleOpen; + bool subParentToggleOpen; + + int previousScrollPosition; }; class DevicePanel : public ListWidget { @@ -54,6 +68,9 @@ private slots: private: Params params; + + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; }; class TogglesPanel : public ListWidget { @@ -62,6 +79,10 @@ public: explicit TogglesPanel(SettingsWindow *parent); void showEvent(QShowEvent *event) override; +signals: + // FrogPilot signals + void updateMetric(); + public slots: void expandToggleDescription(const QString ¶m); @@ -96,4 +117,8 @@ private: Params params; ParamWatcher *fs_watch; + + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; + UIScene &scene; }; diff --git a/selfdrive/ui/qt/offroad/software_settings.cc b/selfdrive/ui/qt/offroad/software_settings.cc index d12db3f87..0d5621481 100644 --- a/selfdrive/ui/qt/offroad/software_settings.cc +++ b/selfdrive/ui/qt/offroad/software_settings.cc @@ -15,12 +15,13 @@ #include "selfdrive/ui/qt/widgets/input.h" #include "system/hardware/hw.h" +#include "selfdrive/frogpilot/ui/qt/widgets/frogpilot_controls.h" void SoftwarePanel::checkForUpdates() { std::system("pkill -SIGUSR1 -f selfdrive.updated.updated"); } -SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { +SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent), scene(uiState()->scene) { onroadLbl = new QLabel(tr("Updates are only downloaded while the car is off.")); onroadLbl->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left; padding-top: 30px; padding-bottom: 30px;"); addItem(onroadLbl); @@ -54,6 +55,11 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { connect(targetBranchBtn, &ButtonControl::clicked, [=]() { auto current = params.get("GitBranch"); QStringList branches = QString::fromStdString(params.get("UpdaterAvailableBranches")).split(","); + if (!Params("/persist/params").getBool("FrogsGoMoo")) { + branches.removeAll("FrogPilot-Development"); + branches.removeAll("FrogPilot-New"); + branches.removeAll("MAKE-PRS-HERE"); + } for (QString b : {current.c_str(), "devel-staging", "devel", "nightly", "master-ci", "master"}) { auto i = branches.indexOf(b); if (i >= 0) { diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 01750eaf2..3d493548c 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,11 @@ void OnroadWindow::updateState(const UIState &s) { } void OnroadWindow::mousePressEvent(QMouseEvent* e) { + // 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 +105,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 +119,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); @@ -167,11 +173,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 +220,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}); @@ -236,6 +244,8 @@ void ExperimentalButton::updateState(const UIState &s) { experimental_mode = cs.getExperimentalMode(); update(); } + + // FrogPilot variables } void ExperimentalButton::paintEvent(QPaintEvent *event) { @@ -247,7 +257,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 +272,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 +286,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) { @@ -328,6 +341,8 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { map_settings_btn->setVisible(!hideBottomIcons); main_layout->setAlignment(map_settings_btn, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight) | Qt::AlignBottom); } + + updateFrogPilotWidgets(); } void AnnotatedCameraWidget::drawHud(QPainter &p) { @@ -426,6 +441,9 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { drawText(p, rect().center().x(), 290, speedUnit, 200); p.restore(); + + // Draw FrogPilot widgets + paintFrogPilotWidgets(p); } void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &text, int alpha) { @@ -468,7 +486,6 @@ void AnnotatedCameraWidget::updateFrameMat() { void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.save(); - const UIScene &scene = s->scene; SubMaster &sm = *(s->sm); // lanelines @@ -485,7 +502,7 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { // paint path QLinearGradient bg(0, height(), 0, 0); - if (sm["controlsState"].getControlsState().getExperimentalMode()) { + if (experimentalMode) { // The first half of track_vertices are the points for the right side of the path // and the indices match the positions of accel from uiPlan const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel(); @@ -525,13 +542,12 @@ 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; int x = rightHandDM ? width() - offset : offset; + offset += true ? 25 : 0; int y = height() - offset; float opacity = dmActive ? 0.65 : 0.2; drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity); @@ -633,7 +649,7 @@ void AnnotatedCameraWidget::paintGL() { } else if (v_ego > 15) { wide_cam_requested = false; } - wide_cam_requested = wide_cam_requested && sm["controlsState"].getControlsState().getExperimentalMode(); + wide_cam_requested = wide_cam_requested && experimentalMode; // for replay of old routes, never go to widecam wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid; } @@ -701,3 +717,82 @@ void AnnotatedCameraWidget::showEvent(QShowEvent *event) { ui_update_params(uiState()); prev_draw_t = millis_since_boot(); } + +// FrogPilot widgets +void AnnotatedCameraWidget::initializeFrogPilotWidgets() { + 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() { + alertSize = scene.alert_size; + + experimentalMode = scene.experimental_mode; + + mapOpen = scene.map_open; +} + +void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { + 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(); + + static bool displayStatusText = false; + + constexpr qreal fadeDuration = 1500.0; + constexpr qreal textDuration = 5000.0; + + static QElapsedTimer timer; + static QString lastShownStatus; + + QString newStatus; + + 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); + + if (newStatus != lastShownStatus) { + displayStatusText = true; + lastShownStatus = newStatus; + timer.restart(); + } else if (displayStatusText && timer.hasExpired(textDuration + fadeDuration)) { + displayStatusText = false; + } + + p.setFont(InterFont(40, QFont::Bold)); + p.setPen(Qt::white); + p.setRenderHint(QPainter::TextAntialiasing); + + static qreal statusTextOpacity; + int elapsed = timer.elapsed(); + if (displayStatusText) { + statusTextOpacity = qBound(0.0, 1.0 - (elapsed - textDuration) / fadeDuration, 1.0); + } + + 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 b3ba41145..a6eee8bd7 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -20,7 +20,7 @@ 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 +29,9 @@ protected: private: QColor bg; Alert alert = {}; + + // FrogPilot variables + UIScene &scene; }; class ExperimentalButton : public QPushButton { @@ -47,6 +50,10 @@ private: QPixmap experimental_img; bool experimental_mode; bool engageable; + + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; + 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,24 @@ private: int skip_frame_count = 0; bool wide_cam_requested = false; + // FrogPilot widgets + void initializeFrogPilotWidgets(); + void paintFrogPilotWidgets(QPainter &p); + void updateFrogPilotWidgets(); + + void drawStatusBar(QPainter &p); + + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; + UIScene &scene; + + QHBoxLayout *bottom_layout; + + bool experimentalMode; + bool mapOpen; + + int alertSize; + protected: void paintGL() override; void initializeGL() override; @@ -135,6 +161,11 @@ private: QWidget *map = nullptr; QHBoxLayout* split; + // FrogPilot variables + UIScene &scene; + Params params; + Params paramsMemory{"/dev/shm/params"}; + 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 e95294005..702c6eb0f 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -24,7 +24,7 @@ void Sidebar::drawMetric(QPainter &p, const QPair &label, QCol p.drawText(rect.adjusted(22, 0, 0, 0), Qt::AlignCenter, label.first + "\n" + label.second); } -Sidebar::Sidebar(QWidget *parent) : QFrame(parent), onroad(false), flag_pressed(false), settings_pressed(false) { +Sidebar::Sidebar(QWidget *parent) : QFrame(parent), onroad(false), flag_pressed(false), settings_pressed(false), scene(uiState()->scene) { home_img = loadPixmap("../assets/images/button_home.png", home_btn.size()); flag_img = loadPixmap("../assets/images/button_flag.png", home_btn.size()); settings_img = loadPixmap("../assets/images/button_settings.png", settings_btn.size(), Qt::IgnoreAspectRatio); @@ -38,6 +38,8 @@ Sidebar::Sidebar(QWidget *parent) : QFrame(parent), onroad(false), flag_pressed( QObject::connect(uiState(), &UIState::uiUpdate, this, &Sidebar::updateState); pm = std::make_unique>({"userFlag"}); + + // FrogPilot variables } void Sidebar::mousePressEvent(QMouseEvent *event) { @@ -79,6 +81,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) { diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h index f627aac81..a1d6c16d5 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); @@ -59,4 +61,8 @@ protected: private: std::unique_ptr pm; + + // FrogPilot variables + Params params; + UIScene &scene; }; diff --git a/selfdrive/ui/qt/spinner.cc b/selfdrive/ui/qt/spinner.cc index 2404efa66..7ca2f1a70 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 bc3c494fa..a83d3538e 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -26,7 +26,7 @@ QString getVersion() { } QString getBrand() { - return QObject::tr("openpilot"); + return QObject::tr("FrogPilot"); } QString getUserAgent() { diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h index aa304e0df..4a2c7f3fd 100644 --- a/selfdrive/ui/qt/widgets/controls.h +++ b/selfdrive/ui/qt/widgets/controls.h @@ -132,6 +132,10 @@ public: toggle.update(); } + void refresh() { + toggle.togglePosition(); + } + signals: void toggleFlipped(bool state); diff --git a/selfdrive/ui/qt/widgets/scrollview.cc b/selfdrive/ui/qt/widgets/scrollview.cc index 978bf83a6..28460078a 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 024331aa3..51acc4c43 100644 --- a/selfdrive/ui/qt/widgets/scrollview.h +++ b/selfdrive/ui/qt/widgets/scrollview.h @@ -7,6 +7,10 @@ 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/widgets/wifi.h b/selfdrive/ui/qt/widgets/wifi.h index 60c865f2b..30278c1b0 100644 --- a/selfdrive/ui/qt/widgets/wifi.h +++ b/selfdrive/ui/qt/widgets/wifi.h @@ -18,6 +18,10 @@ signals: public slots: void updateState(const UIState &s); +private: + // FrogPilot variables + Params params; + protected: QStackedLayout *stack; }; diff --git a/selfdrive/ui/qt/window.h b/selfdrive/ui/qt/window.h index 05b61e1f7..f1389c29f 100644 --- a/selfdrive/ui/qt/window.h +++ b/selfdrive/ui/qt/window.h @@ -22,4 +22,7 @@ private: HomeWindow *homeWindow; SettingsWindow *settingsWindow; OnboardingWindow *onboardingWindow; + + // FrogPilot variables + Params params; }; diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 0550a7db9..a00f3e831 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -7,6 +7,7 @@ import wave 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 @@ -52,7 +53,11 @@ def check_controls_timeout_alert(sm): class Soundd: def __init__(self): - self.load_sounds() + # FrogPilot variables + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.update_frogpilot_params() self.current_alert = AudibleAlert.none self.current_volume = MIN_VOLUME @@ -155,6 +160,12 @@ class Soundd: assert stream.active + # Update FrogPilot parameters + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + + def update_frogpilot_params(self): + self.load_sounds() def main(): s = Soundd() diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 9afd22f13..1f5f2b4dd 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -148,6 +148,8 @@ void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &drivers kpt_this = matvecmul3(r_xyz, kpt_this); scene.face_kpts_draw[kpi] = (vec3){{(float)kpt_this.v[0], (float)kpt_this.v[1], (float)(kpt_this.v[2] * (1.0-dm_fade_state) + 8 * dm_fade_state)}}; } + + scene.right_hand_drive = is_rhd; } static void update_sockets(UIState *s) { @@ -200,6 +202,29 @@ static void update_state(UIState *s) { } if (sm.updated("carParams")) { scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); + ui_update_frogpilot_params(s); + } + if (sm.updated("carState")) { + auto carState = sm["carState"].getCarState(); + scene.acceleration = carState.getAEgo(); + } + if (sm.updated("controlsState")) { + auto controlsState = sm["controlsState"].getControlsState(); + scene.alert_size = controlsState.getAlertSize() == cereal::ControlsState::AlertSize::MID ? 350 : controlsState.getAlertSize() == cereal::ControlsState::AlertSize::SMALL ? 200 : 0; + scene.enabled = controlsState.getEnabled(); + scene.experimental_mode = controlsState.getExperimentalMode(); + } + if (sm.updated("deviceState")) { + auto deviceState = sm["deviceState"].getDeviceState(); + } + if (sm.updated("frogpilotCarControl")) { + auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl(); + } + if (sm.updated("frogpilotPlan")) { + auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); + } + if (sm.updated("liveLocationKalman")) { + auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); } if (sm.updated("wideRoadCameraState")) { auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState(); @@ -221,6 +246,18 @@ void ui_update_params(UIState *s) { s->scene.map_on_left = params.getBool("NavSettingLeftSide"); } +void ui_update_frogpilot_params(UIState *s) { + Params params = Params(); + UIScene &scene = s->scene; + + bool custom_onroad_ui = params.getBool("CustomUI"); + bool custom_paths = custom_onroad_ui && params.getBool("CustomPaths"); + + bool quality_of_life_controls = params.getBool("QOLControls"); + + bool quality_of_life_visuals = params.getBool("QOLVisuals"); +} + void UIState::updateStatus() { if (scene.started && sm->updated("controlsState")) { auto controls_state = (*sm)["controlsState"].getControlsState(); @@ -249,6 +286,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", "frogpilotPlan", }); Params params; @@ -262,6 +300,10 @@ UIState::UIState(QObject *parent) : QObject(parent) { timer = new QTimer(this); QObject::connect(timer, &QTimer::timeout, this, &UIState::update); timer->start(1000 / UI_FREQ); + + wifi = new WifiManager(this); + + ui_update_frogpilot_params(this); } void UIState::update() { @@ -273,6 +315,13 @@ void UIState::update() { watchdog_kick(nanos_since_boot()); } emit uiUpdate(*this); + + // Update FrogPilot variables when they are changed + if (paramsMemory.getBool("FrogPilotTogglesUpdated")) { + ui_update_frogpilot_params(this); + } + + // 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 6efb72af9..776fd77d7 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -105,6 +105,9 @@ typedef enum UIStatus { STATUS_DISENGAGED, STATUS_OVERRIDE, STATUS_ENGAGED, + + // FrogPilot statuses + } UIStatus; enum PrimeType { @@ -121,12 +124,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 { @@ -161,6 +167,17 @@ 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; + bool map_open; + bool right_hand_drive; + + float acceleration; + + int alert_size; + } UIScene; class UIState : public QObject { @@ -188,6 +205,8 @@ public: QTransform car_space_transform; + // FrogPilot variables + signals: void uiUpdate(const UIState &s); void offroadTransition(bool offroad); @@ -201,6 +220,9 @@ private: QTimer *timer; bool started_prev = false; PrimeType prime_type = PrimeType::UNKNOWN; + + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; }; UIState *uiState(); @@ -250,3 +272,6 @@ void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &drivers void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line); void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line, float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert); + +// FrogPilot functions +void ui_update_frogpilot_params(UIState *s); diff --git a/selfdrive/updated/updated.py b/selfdrive/updated/updated.py index b6b395f25..c4002eb13 100755 --- a/selfdrive/updated/updated.py +++ b/selfdrive/updated/updated.py @@ -236,6 +236,8 @@ class Updater: self.branches = defaultdict(str) self._has_internet: bool = False + # FrogPilot variables + @property def has_internet(self) -> bool: return self._has_internet @@ -407,6 +409,7 @@ class Updater: def main() -> None: params = Params() + params_memory = Params("/dev/shm/params") if params.get_bool("DisableUpdates"): cloudlog.warning("updates are disabled by the DisableUpdates param") diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 3191b821a..f7c28fd74 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -933,6 +933,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));