diff --git a/cereal/car.capnp b/cereal/car.capnp index 3d7d737..ea7d096 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -117,6 +117,8 @@ struct CarEvent @0x9b1657f34caf3ad3 { paramsdTemporaryError @50; paramsdPermanentError @119; + # FrogPilot events + radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; radarCommIssueDEPRECATED @67; diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 369222a..fcca5e3 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -8,16 +8,16 @@ $Cxx.namespace("cereal"); # cereal, so use these if you want custom events in your fork. # you can rename the struct, but don't change the identifier -struct CustomReserved0 @0x81c2f05a394cf4af { +struct FrogPilotCarControl @0x81c2f05a394cf4af { } -struct CustomReserved1 @0xaedffd8f31e7b55d { +struct FrogPilotDeviceState @0xaedffd8f31e7b55d { } -struct CustomReserved2 @0xf35cc4560bbf6ec2 { +struct FrogPilotNavigation @0xf35cc4560bbf6ec2 { } -struct CustomReserved3 @0xda96579883444c35 { +struct FrogPilotPlan @0xda96579883444c35 { } struct CustomReserved4 @0x80ae746ee2596b11 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 9f0ca64..7ff2293 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -731,6 +731,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 { @@ -2322,10 +2323,10 @@ 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; + frogpilotCarControl @107 :Custom.FrogPilotCarControl; + frogpilotDeviceState @108 :Custom.FrogPilotDeviceState; + frogpilotNavigation @109 :Custom.FrogPilotNavigation; + frogpilotPlan @110 :Custom.FrogPilotPlan; customReserved4 @111 :Custom.CustomReserved4; customReserved5 @112 :Custom.CustomReserved5; customReserved6 @113 :Custom.CustomReserved6; diff --git a/cereal/services.py b/cereal/services.py index e79f2e0..0892885 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -81,6 +81,12 @@ services: dict[str, tuple] = { "userFlag": (True, 0., 1), "microphone": (True, 10., 10), + # FrogPilot + "frogpilotCarControl": (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 b02b33c..ea23a5f 100644 --- a/common/conversions.py +++ b/common/conversions.py @@ -8,6 +8,8 @@ class Conversions: KPH_TO_MS = 1. / MS_TO_KPH MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS + METER_TO_FOOT = 3.28084 + FOOT_TO_METER = 1 / METER_TO_FOOT MS_TO_KNOTS = 1.9438 KNOTS_TO_MS = 1. / MS_TO_KNOTS diff --git a/common/params.cc b/common/params.cc index aef2cbd..7b90e6b 100644 --- a/common/params.cc +++ b/common/params.cc @@ -207,6 +207,18 @@ std::unordered_map keys = { {"UpdaterLastFetchTime", PERSISTENT}, {"Version", PERSISTENT}, {"VisionRadarToggle", PERSISTENT}, + + // FrogPilot parameters + {"CustomAlerts", PERSISTENT}, + {"CustomUI", PERSISTENT}, + {"FrogPilotTogglesUpdated", PERSISTENT}, + {"FrogsGoMoo", PERSISTENT}, + {"LateralTune", PERSISTENT}, + {"LongitudinalTune", PERSISTENT}, + {"QOLControls", PERSISTENT}, + {"QOLVisuals", PERSISTENT}, + {"SilentMode", PERSISTENT}, + {"StockTune", PERSISTENT}, }; } // namespace diff --git a/common/params.h b/common/params.h index d726a61..9c8af82 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 535514e..ddce510 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 0cbad5b..6f29a36 100644 --- a/common/util.h +++ b/common/util.h @@ -37,6 +37,9 @@ const double MS_TO_KPH = 3.6; const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE; const double METER_TO_MILE = KM_TO_MILE / 1000.0; const double METER_TO_FOOT = 3.28084; +const double FOOT_TO_METER = 1 / METER_TO_FOOT; +const double CM_TO_INCH = METER_TO_FOOT / 100.0 * 12.0; +const double INCH_TO_CM = 1.0 / CM_TO_INCH; namespace util { diff --git a/release/files_common b/release/files_common index 1158d2c..dc05f7f 100644 --- a/release/files_common +++ b/release/files_common @@ -557,3 +557,6 @@ tinygrad_repo/tinygrad/runtime/ops_disk.py tinygrad_repo/tinygrad/runtime/ops_gpu.py tinygrad_repo/tinygrad/shape/* tinygrad_repo/tinygrad/*.py + +selfdrive/frogpilot/functions/frogpilot_functions.py +selfdrive/frogpilot/functions/frogpilot_planner.py diff --git a/selfdrive/assets/img_spinner_comma.png b/selfdrive/assets/img_spinner_comma.png index 1610955..9f5e558 100644 Binary files a/selfdrive/assets/img_spinner_comma.png and b/selfdrive/assets/img_spinner_comma.png differ diff --git a/selfdrive/assets/img_spinner_track.png b/selfdrive/assets/img_spinner_track.png index 931c17e..d54d51e 100644 Binary files a/selfdrive/assets/img_spinner_track.png and b/selfdrive/assets/img_spinner_track.png differ diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index d15f11d..abb3f56 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -34,7 +34,7 @@ class CarController: 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 fca9bcc..95c65a8 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 12a2d5f..25b39de 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)] @@ -29,8 +29,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): @@ -42,5 +42,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 95028e1..de3d870 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -5,7 +5,7 @@ from typing import Callable, Dict, List, Optional, Tuple from cereal import car from openpilot.common.params import Params from openpilot.common.basedir import BASEDIR -from openpilot.system.version import is_comma_remote, is_tested_branch +from openpilot.system.version import get_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 @@ -193,14 +193,20 @@ def fingerprint(logcan, sendcan, num_pandas): def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): + params = Params() + dongle_id = params.get("DongleId", encoding='utf-8') + candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas) if candidate is None: cloudlog.event("car doesn't match any fingerprints", fingerprints=repr(fingerprints), error=True) candidate = "mock" + if get_short_branch() == "FrogPilot-Development" and not Params("/persist/comma/params").get_bool("FrogsGoMoo"): + candidate = "mock" + CarInterface, CarController, CarState = 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/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 050eb41..dd3c841 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -19,7 +19,7 @@ class CarController: 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 eb1cf7e..56a4dbc 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -21,7 +21,7 @@ class CarState(CarStateBase): else: self.shifter_values = can_define.dv["GEAR"]["PRNDL"] - 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 32a4f5d..8278e55 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/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 = "chrysler" ret.dashcamOnly = candidate in RAM_HD @@ -88,11 +88,11 @@ 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) # events - events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low]) + events = self.create_common_events(ret, frogpilot_variables, extra_gears=[car.CarState.GearShifter.low]) # Low speed steer alert hysteresis logic if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 0.5): @@ -106,5 +106,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 45516d6..d8b2a70 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -35,7 +35,7 @@ class CarController: self.lkas_enabled_last = False self.steer_alert_last = False - 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 ef56d23..da8f635 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -20,7 +20,7 @@ class CarState(CarStateBase): self.vehicle_sensors_valid = False self.unsupported_platform = False - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() # Ford Q3 hybrid variants experience a bug where a message from the PCM sends invalid checksums, diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index cc013fb..320ebb8 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -12,7 +12,7 @@ GearShifter = car.CarState.GearShifter class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "ford" ret.dashcamOnly = candidate in CANFD_CAR @@ -103,10 +103,10 @@ 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) - events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic]) + events = self.create_common_events(ret, frogpilot_variables, extra_gears=[GearShifter.manumatic]) if not self.CS.vehicle_sensors_valid: events.add(car.CarEvent.EventName.vehicleSensorsInvalid) if self.CS.unsupported_platform: @@ -116,5 +116,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 6c7e800..ec5077b 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -38,7 +38,9 @@ class CarController: 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 c3d061d..dd0198b 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -26,7 +26,9 @@ class CarState(CarStateBase): self.cam_lka_steering_cmd_counter = 0 self.buttons_counter = 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 05caa28..47682c1 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -83,7 +83,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 @@ -271,8 +271,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: @@ -280,7 +280,7 @@ class CarInterface(CarInterfaceBase): unpressed_btn=CruiseButtons.UNPRESS) # The ECM allows enabling on falling edge of set, but only rising edge of resume - events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, + events = self.create_common_events(ret, frogpilot_variables, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.eco, GearShifter.manumatic], pcm_enable=self.CP.pcmCruise, enable_buttons=(ButtonType.decelCruise,)) if not self.CP.pcmCruise: @@ -302,5 +302,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 8188ad4..745ac43 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -4,6 +4,7 @@ from enum import Enum, StrEnum from typing import Dict, List, Union from cereal import car +from openpilot.common.params import Params from openpilot.selfdrive.car import dbc_dict from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, 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 056b47c..b34b8b5 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -124,7 +124,7 @@ class CarController: 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 9025f72..0d6f945 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 153fa1e..5577f9f 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -33,7 +33,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" if candidate in HONDA_BOSCH: @@ -310,8 +310,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), @@ -319,7 +319,7 @@ class CarInterface(CarInterfaceBase): ] # events - events = self.create_common_events(ret, pcm_enable=False) + events = self.create_common_events(ret, frogpilot_variables, pcm_enable=False) if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed: events.add(EventName.belowEngageSpeed) @@ -344,5 +344,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 0b5f724..4bb0c46 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -56,7 +56,7 @@ class CarController: 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 64a9fdf..1f952b0 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 049a633..0df2163 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -20,7 +20,7 @@ BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: Bu class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "hyundai" ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None @@ -343,8 +343,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) @@ -353,7 +353,7 @@ class CarInterface(CarInterfaceBase): # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons # Main button also can trigger an engagement on these cars allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons) - events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable) + events = self.create_common_events(ret, frogpilot_variables, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable) # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: @@ -367,5 +367,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 9767752..2e0b3a1 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -11,12 +11,15 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.conversions import Conversions as CV from openpilot.common.simple_kalman import KF1D, get_kalman_gain from openpilot.common.numpy_fast import clip +from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction from openpilot.selfdrive.controls.lib.events import Events from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel +from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import FrogPilotFunctions + ButtonType = car.CarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter EventName = car.CarEvent.EventName @@ -95,6 +98,9 @@ class CarInterfaceBase(ABC): if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP, self.VM) + # FrogPilot variables + params = Params() + @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX @@ -107,9 +113,9 @@ 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) - 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: @@ -204,14 +210,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) @@ -241,7 +247,7 @@ class CarInterfaceBase(ABC): def apply(self, c: car.CarControl, now_nanos: int) -> Tuple[car.CarControl.Actuators, List[bytes]]: pass - def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True, + def create_common_events(self, cs_out, frogpilot_variables, extra_gears=None, pcm_enable=True, allow_enable=True, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)): events = Events() @@ -354,6 +360,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.fpf = FrogPilotFunctions() + 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 320ad19..4952989 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -15,7 +15,7 @@ class CarController: 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 c081959..6b930e6 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -18,7 +18,7 @@ class CarState(CarStateBase): self.lkas_allowed_speed = False self.lkas_disabled = False - def update(self, cp, cp_cam): + def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() ret.wheelSpeeds = self.get_wheel_speeds( diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 7ac93d9..e2215b7 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -11,7 +11,7 @@ EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "mazda" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarUnavailable = True @@ -49,11 +49,11 @@ 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) # events - events = self.create_common_events(ret) + events = self.create_common_events(ret, frogpilot_variables) if self.CS.lkas_disabled: events.add(EventName.lkasDisabled) @@ -64,5 +64,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 2e4ac43..90aa3be 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 2da518b..beb5d3c 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -18,7 +18,7 @@ class CarController: 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 b2ba9ce..1d0052b 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -20,7 +20,7 @@ class CarState(CarStateBase): self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES) self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"] - def update(self, cp, cp_adas, cp_cam): + def update(self, cp, cp_adas, cp_cam, frogpilot_variables): ret = car.CarState.new_message() if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index aedcaa1..cfc10aa 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -8,7 +8,7 @@ from openpilot.selfdrive.car.nissan.values import CAR 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 @@ -39,15 +39,15 @@ 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) buttonEvents = [] be = car.CarState.ButtonEvent.new_message() be.type = car.CarState.ButtonEvent.Type.accelCruise buttonEvents.append(be) - events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.brake]) + events = self.create_common_events(ret, frogpilot_variables, extra_gears=[car.CarState.GearShifter.brake]) if self.CS.lkas_enabled: events.add(car.CarEvent.EventName.invalidLkasSetting) @@ -56,5 +56,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 cc8ce4f..bcfc515 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -23,7 +23,7 @@ class CarController: 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 c8a6dfe..bda8342 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 self.car_fingerprint not in HYBRID_CARS else cp_body.vl["Throttle_Hybrid"] diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 1296aea..d501965 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -9,7 +9,7 @@ from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, LKAS_ANGL 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 = "subaru" ret.radarUnavailable = True # for HYBRID CARS to be upstreamed, we need: @@ -136,11 +136,11 @@ 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() + ret.events = self.create_common_events(ret, frogpilot_variables).to_msg() return ret @@ -149,5 +149,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 2f7350b..02b0498 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -4,6 +4,7 @@ from typing import Dict, List, Union from cereal import car from panda.python import uds +from openpilot.common.params import Params from openpilot.selfdrive.car import dbc_dict from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, 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 95a248a..9d5617b 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -14,7 +14,7 @@ class CarController: 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 2cb4f09..ed59693 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 e061397..72e2b0d 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, @@ -51,12 +51,12 @@ 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.events = self.create_common_events(ret).to_msg() + ret.events = self.create_common_events(ret, frogpilot_variables).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/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 02a8d60..6122577 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -9,6 +9,7 @@ from parameterized import parameterized from cereal import car, messaging from openpilot.common.realtime import DT_CTRL +from openpilot.common.params import Params from openpilot.selfdrive.car import gen_empty_fingerprint from openpilot.selfdrive.car.car_helpers import interfaces from openpilot.selfdrive.car.fingerprints import all_known_cars @@ -18,6 +19,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl +from openpilot.selfdrive.controls.controlsd import Controls from openpilot.selfdrive.test.fuzzy_generation import DrawType, FuzzyGenerator ALL_ECUS = list({ecu for ecus in FW_VERSIONS.values() for ecu in ecus.keys()}) @@ -44,7 +46,6 @@ def get_fuzzy_car_interface_args(draw: DrawType) -> dict: params['car_fw'] = [car.CarParams.CarFw(ecu=fw[0], address=fw[1], subAddress=fw[2] or 0) for fw in params['car_fw']] return params - class TestCarInterfaces(unittest.TestCase): # FIXME: Due to the lists used in carParams, Phase.target is very slow and will cause # many generated examples to overrun when max_examples > ~20, don't use it @@ -56,9 +57,11 @@ class TestCarInterfaces(unittest.TestCase): CarInterface, CarController, CarState = interfaces[car_name] args = get_fuzzy_car_interface_args(data.draw) + params = Params() - car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'], + car_params = CarInterface.get_params(params, car_name, args['fingerprints'], args['car_fw'], experimental_long=args['experimental_long'], docs=False) + car_interface = CarInterface(car_params, CarController, CarState) assert car_params assert car_interface @@ -94,18 +97,19 @@ class TestCarInterfaces(unittest.TestCase): # Run car interface now_nanos = 0 CC = car.CarControl.new_message(**cc_msg) + controls = Controls(CI=car_interface) for _ in range(10): - car_interface.update(CC, []) - car_interface.apply(CC, now_nanos) - car_interface.apply(CC, now_nanos) + car_interface.update(CC, [], controls.frogpilot_variables) + car_interface.apply(CC, now_nanos, controls.frogpilot_variables) + car_interface.apply(CC, now_nanos, controls.frogpilot_variables) now_nanos += DT_CTRL * 1e9 # 10 ms CC = car.CarControl.new_message(**cc_msg) CC.enabled = True for _ in range(10): - car_interface.update(CC, []) - car_interface.apply(CC, now_nanos) - car_interface.apply(CC, now_nanos) + car_interface.update(CC, [], controls.frogpilot_variables) + car_interface.apply(CC, now_nanos, controls.frogpilot_variables) + car_interface.apply(CC, now_nanos, controls.frogpilot_variables) now_nanos += DT_CTRL * 1e9 # 10ms # Test controller initialization diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 343b1d3..af80d4e 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.toyota import toyotacan @@ -41,7 +42,10 @@ class CarController: 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 @@ -142,10 +146,12 @@ class CarController: if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) elif self.CP.openpilotLongitudinalControl: - can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert)) + can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, + 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)) + can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, + 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 e4ea0d3..18c8067 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -44,7 +44,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 988b1b4..3f0b2f4 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -18,7 +18,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "toyota" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)] ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] @@ -289,11 +289,11 @@ 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) # events - events = self.create_common_events(ret) + events = self.create_common_events(ret, frogpilot_variables) # Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until # the more accurate angle sensor signal is initialized @@ -320,5 +320,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 e14e3e5..3cc08dd 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): +def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, 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 0cee2c7..48f6d42 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -25,7 +25,7 @@ class CarController: 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 25c5bc0..853a19f 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -30,9 +30,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.carFingerprint in PQ_CARS: - 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. @@ -153,7 +153,7 @@ class CarState(CarStateBase): 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 710e779..b2f5db7 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -23,7 +23,7 @@ class CarInterface(CarInterfaceBase): self.eps_timer_soft_disable_alert = False @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 = "volkswagen" ret.radarUnavailable = True @@ -225,10 +225,10 @@ 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], + events = self.create_common_events(ret, frogpilot_variables, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], pcm_enable=not self.CS.CP.openpilotLongitudinalControl, enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise)) @@ -253,6 +253,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 bf2b14c..a1fc02b 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, log, custom from cereal.visionipc import VisionIpcClient, VisionStreamType from panda import ALTERNATIVE_EXPERIENCE @@ -53,6 +54,7 @@ LaneChangeDirection = log.LaneChangeDirection EventName = car.CarEvent.EventName ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel +GearShifter = car.CarState.GearShifter IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError} @@ -90,14 +92,14 @@ class CarD: """Initialize CarInterface, once controls are ready""" self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) - def state_update(self, CC: car.CarControl): + def state_update(self, CC: car.CarControl, frogpilot_variables): """carState update loop, driven by can""" # TODO: This should not depend on carControl # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - self.CS = self.CI.update(CC, can_strs) + self.CS = self.CI.update(CC, can_strs, frogpilot_variables) self.sm.update(0) @@ -136,12 +138,12 @@ class CarD: cp_send.carParams = self.CP self.pm.send('carParams', cp_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) - actuators_output, can_sends = self.CI.apply(CC, now_nanos) + actuators_output, 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)) return actuators_output @@ -160,21 +162,28 @@ 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"] self.log_sock = messaging.sub_sock('androidLog') + # FrogPilot variables self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.frogpilot_variables = SimpleNamespace() + + self.driving_gear = False + ignore = self.sensor_packets + ['testJoystick'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', '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)) @@ -191,6 +200,8 @@ class Controls: self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") + self.update_frogpilot_params() + # detect sound card presence and ensure successful init sounds_available = HARDWARE.get_sound_card_online() @@ -222,6 +233,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() @@ -291,6 +303,8 @@ class Controls: self.events.clear() + frogpilot_plan = self.sm['frogpilotPlan'] + # Add joystick event, static on cars, dynamic on nonCars if self.joystick_mode: self.events.add(EventName.joystickDebug) @@ -506,7 +520,7 @@ class Controls: def data_sample(self): """Receive data from sockets and update carState""" - CS = self.card.state_update(self.CC) + CS = self.card.state_update(self.CC, self.frogpilot_variables) self.sm.update(0) @@ -630,7 +644,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 @@ -660,6 +674,12 @@ class Controls: CC = car.CarControl.new_message() CC.enabled = self.enabled + # FrogPilot functions + frogpilot_plan = self.sm['frogpilotPlan'] + + # Gear Check + self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) + # Check which actuators can be enabled standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ @@ -826,7 +846,7 @@ class Controls: hudControl.visualAlert = current_alert.visual_alert if not self.CP.passive and self.initialized: - self.last_actuators = self.card.controls_update(CC) + self.last_actuators = self.card.controls_update(CC, self.frogpilot_variables) CC.actuatorsOutput = self.last_actuators if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.steer_limited = abs(CC.actuators.steeringAngleDeg - CC.actuatorsOutput.steeringAngleDeg) > \ @@ -910,6 +930,12 @@ class Controls: # copy CarControl to pass to CarInterface on the next iteration self.CC = CC + # Publish FrogPilot variables + fpcs_send = messaging.new_message('frogpilotCarControl') + fpcs_send.valid = CS.canValid + fpcs_send.frogpilotCarControl = self.FPCC + self.pm.send('frogpilotCarControl', fpcs_send) + def step(self): start_time = time.monotonic() @@ -940,6 +966,10 @@ class Controls: self.joystick_mode = self.params.get_bool("JoystickDebugMode") time.sleep(0.1) + # Update FrogPilot parameters + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + def controlsd_thread(self): e = threading.Event() t = threading.Thread(target=self.params_thread, args=(e, )) @@ -952,6 +982,14 @@ class Controls: e.set() t.join() + def update_frogpilot_params(self): + custom_alerts = self.params.get_bool("CustomAlerts") + + lateral_tune = self.params.get_bool("LateralTune") + + longitudinal_tune = self.params.get_bool("LongitudinalTune") + + quality_of_life = self.params.get_bool("QOLControls") def main(): controls = Controls() diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 90b6858..17de459 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,9 @@ 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") diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 6a5b22f..37fcb62 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,8 @@ 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 + @property def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET @@ -125,7 +128,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 d68c95b..1266fa1 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -228,7 +228,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 +332,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, Union[Alert, AlertCallbackType]]] = { # ********** events with no alerts ********** @@ -956,6 +957,8 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"), }, + # FrogPilot Events + } diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 39cfda4..9b2b7d5 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -221,6 +221,8 @@ def gen_long_ocp(): class LongitudinalMpc: def __init__(self, mode='acc'): + # FrogPilot variables + self.mode = mode self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() @@ -330,7 +332,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): + def update(self, radarstate, v_cruise, x, v, a, j, frogpilot_planner, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(personality) v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 018768f..d740b22 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -88,7 +88,7 @@ class LongitudinalPlanner: j = np.zeros(len(T_IDXS_MPC)) return x, v, a, j - def update(self, sm): + def update(self, sm, frogpilot_planner, params_memory): if self.param_read_counter % 50 == 0: self.read_param() self.param_read_counter += 1 @@ -134,7 +134,7 @@ class LongitudinalPlanner: 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=self.personality) + self.mpc.update(sm['radarState'], frogpilot_planner.v_cruise, x, v, a, j, frogpilot_planner, personality=self.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) @@ -152,7 +152,9 @@ class LongitudinalPlanner: self.a_desired = float(interp(self.dt, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 - def publish(self, sm, pm): + frogpilot_planner.update(sm['carState'], sm['controlsState'], sm['modelV2'], self.mpc, sm, v_cruise, v_ego) + + def publish(self, sm, pm, frogpilot_planner): plan_send = messaging.new_message('longitudinalPlan') plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) @@ -173,3 +175,5 @@ class LongitudinalPlanner: longitudinalPlan.personality = self.personality pm.send('longitudinalPlan', plan_send) + + frogpilot_planner.publish(sm, pm, self.mpc) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index eeeeda0..6e6a20a 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -6,6 +6,8 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner import cereal.messaging as messaging +from openpilot.selfdrive.frogpilot.functions.frogpilot_planner import FrogPilotPlanner + def publish_ui_plan(sm, pm, longitudinal_planner): ui_send = messaging.new_message('uiPlan') ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) @@ -22,22 +24,27 @@ def plannerd_thread(): cloudlog.info("plannerd is waiting for CarParams") params = Params() + params_memory = Params("/dev/shm/params") with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg: CP = msg cloudlog.info("plannerd got CarParams: %s", CP.carName) + frogpilot_planner = FrogPilotPlanner(CP, params, params_memory) longitudinal_planner = LongitudinalPlanner(CP) - pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], + pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan', 'frogpilotPlan']) + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'frogpilotNavigation'], poll='modelV2', ignore_avg_freq=['radarState']) while True: sm.update() if sm.updated['modelV2']: - longitudinal_planner.update(sm) - longitudinal_planner.publish(sm, pm) + longitudinal_planner.update(sm, frogpilot_planner, params_memory) + longitudinal_planner.publish(sm, pm, frogpilot_planner) publish_ui_plan(sm, pm, longitudinal_planner) + if params_memory.get_bool("FrogPilotTogglesUpdated"): + frogpilot_planner.update_frogpilot_params(params) + def main(): plannerd_thread() diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_lateral_tune.png b/selfdrive/frogpilot/assets/toggle_icons/icon_lateral_tune.png new file mode 100644 index 0000000..ba83e3a Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_lateral_tune.png differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_longitudinal_tune.png b/selfdrive/frogpilot/assets/toggle_icons/icon_longitudinal_tune.png new file mode 100644 index 0000000..4af03cd Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_longitudinal_tune.png differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/quality_of_life.png b/selfdrive/frogpilot/assets/toggle_icons/quality_of_life.png new file mode 100644 index 0000000..1719a60 Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/quality_of_life.png differ diff --git a/selfdrive/frogpilot/functions/frogpilot_functions.py b/selfdrive/frogpilot/functions/frogpilot_functions.py new file mode 100644 index 0000000..5bbd4aa --- /dev/null +++ b/selfdrive/frogpilot/functions/frogpilot_functions.py @@ -0,0 +1,17 @@ +import numpy as np + +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params +from openpilot.system.hardware import HARDWARE + + +params_memory = Params("/dev/shm/params") + +CITY_SPEED_LIMIT = 25 +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) + +class FrogPilotFunctions: + def __init__(self) -> None: + self.params = Params() diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py new file mode 100644 index 0000000..e894c89 --- /dev/null +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -0,0 +1,49 @@ +import cereal.messaging as messaging + +from openpilot.common.conversions import Conversions as CV + +from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions + +class FrogPilotPlanner: + def __init__(self, CP, params, params_memory): + self.CP = CP + self.params_memory = params_memory + + self.fpf = FrogPilotFunctions() + + self.v_cruise = 0 + + self.update_frogpilot_params(params) + + def update(self, carState, controlsState, modelData, mpc, sm, v_cruise, v_ego): + enabled = controlsState.enabled + + # Update the max allowed speed + self.v_cruise = self.update_v_cruise(carState, controlsState, enabled, modelData, v_cruise, v_ego) + + def update_v_cruise(self, carState, controlsState, enabled, modelData, v_cruise, v_ego): + # Offsets to adjust the max speed to match the cluster + v_ego_cluster = max(carState.vEgoCluster, v_ego) + v_ego_diff = v_ego_cluster - v_ego + + v_cruise_cluster = max(controlsState.vCruiseCluster, controlsState.vCruise) * CV.KPH_TO_MS + v_cruise_diff = v_cruise_cluster - v_cruise + + targets = [] + filtered_targets = [target for target in targets if target > CRUISING_SPEED] + + return min(filtered_targets) if filtered_targets else v_cruise + + def publish(self, sm, pm, mpc): + frogpilot_plan_send = messaging.new_message('frogpilotPlan') + frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) + frogpilotPlan = frogpilot_plan_send.frogpilotPlan + + pm.send('frogpilotPlan', frogpilot_plan_send) + + def update_frogpilot_params(self, params): + self.is_metric = params.get_bool("IsMetric") + + custom_ui = params.get_bool("CustomUI") + + longitudinal_tune = params.get_bool("LongitudinalTune") diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc new file mode 100644 index 0000000..1f40f49 --- /dev/null +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -0,0 +1,163 @@ +#include "selfdrive/frogpilot/ui/control_settings.h" + +FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { + const std::vector> controlToggles { + {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, + + {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, + + {"QOLControls", "Quality of Life", "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]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(lateralTuneKeys.find(key.c_str()) != lateralTuneKeys.end()); + } + }); + toggle = lateralTuneToggle; + + } else if (param == "LongitudinalTune") { + FrogPilotParamManageControl *longitudinalTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(longitudinalTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(longitudinalTuneKeys.find(key.c_str()) != longitudinalTuneKeys.end()); + } + }); + toggle = longitudinalTuneToggle; + + } else if (param == "QOLControls") { + FrogPilotParamManageControl *qolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(qolToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + 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, [this]() { + update(); + }); + } + + std::set rebootKeys = {}; + for (const std::string &key : rebootKeys) { + QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this, key]() { + if (started) { + if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) { + Hardware::soft_reboot(); + } + } + }); + } + + QObject::connect(device(), &Device::interactiveTimeout, this, &FrogPilotControlsPanel::hideSubToggles); + QObject::connect(parent, &SettingsWindow::closeParentToggle, this, &FrogPilotControlsPanel::hideSubToggles); + QObject::connect(parent, &SettingsWindow::closeSubParentToggle, this, &FrogPilotControlsPanel::hideSubSubToggles); + QObject::connect(parent, &SettingsWindow::updateMetric, this, &FrogPilotControlsPanel::updateMetric); + QObject::connect(uiState(), &UIState::offroadTransition, this, &FrogPilotControlsPanel::updateCarToggles); + QObject::connect(uiState(), &UIState::uiUpdate, this, &FrogPilotControlsPanel::updateState); + + hideSubToggles(); + updateMetric(); +} + +void FrogPilotControlsPanel::updateState(const UIState &s) { + started = s.scene.started; +} + +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() { +} + +void FrogPilotControlsPanel::updateMetric() { + bool previousIsMetric = isMetric; + isMetric = params.getBool("IsMetric"); + + if (isMetric != previousIsMetric) { + double distanceConversion = isMetric ? FOOT_TO_METER : METER_TO_FOOT; + double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE; + } + + if (isMetric) { + } else { + } + + previousIsMetric = isMetric; +} + +void FrogPilotControlsPanel::parentToggleClicked() { + openParentToggle(); +} + +void FrogPilotControlsPanel::subParentToggleClicked() { + openSubParentToggle(); +} + +void FrogPilotControlsPanel::hideSubToggles() { + for (auto &[key, toggle] : toggles) { + bool subToggles = conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end() || + fireTheBabysitterKeys.find(key.c_str()) != fireTheBabysitterKeys.end() || + laneChangeKeys.find(key.c_str()) != laneChangeKeys.end() || + lateralTuneKeys.find(key.c_str()) != lateralTuneKeys.end() || + longitudinalTuneKeys.find(key.c_str()) != longitudinalTuneKeys.end() || + mtscKeys.find(key.c_str()) != mtscKeys.end() || + qolKeys.find(key.c_str()) != qolKeys.end() || + speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end() || + visionTurnControlKeys.find(key.c_str()) != visionTurnControlKeys.end(); + toggle->setVisible(!subToggles); + } + + closeParentToggle(); +} + +void FrogPilotControlsPanel::hideSubSubToggles() { + for (auto &[key, toggle] : toggles) { + bool isVisible = false; + toggle->setVisible(isVisible); + } + + closeSubParentToggle(); + update(); +} + +void FrogPilotControlsPanel::hideEvent(QHideEvent *event) { + hideSubToggles(); +} diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h new file mode 100644 index 0000000..addea26 --- /dev/null +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -0,0 +1,52 @@ +#pragma once + +#include + +#include "selfdrive/frogpilot/ui/frogpilot_ui_functions.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 closeParentToggle(); + void closeSubParentToggle(); + void openParentToggle(); + void openSubParentToggle(); + +private: + void hideEvent(QHideEvent *event); + void hideSubSubToggles(); + void hideSubToggles(); + void parentToggleClicked(); + void subParentToggleClicked(); + void updateCarToggles(); + void updateMetric(); + void updateState(const UIState &s); + void updateToggles(); + + std::set conditionalExperimentalKeys = {}; + std::set fireTheBabysitterKeys = {}; + 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 isMetric = params.getBool("IsMetric"); + bool started = false; +}; diff --git a/selfdrive/frogpilot/ui/frogpilot_ui_functions.cc b/selfdrive/frogpilot/ui/frogpilot_ui_functions.cc new file mode 100644 index 0000000..7bd6c3b --- /dev/null +++ b/selfdrive/frogpilot/ui/frogpilot_ui_functions.cc @@ -0,0 +1,42 @@ +#include + +#include "selfdrive/frogpilot/ui/frogpilot_ui_functions.h" +#include "selfdrive/ui/ui.h" + +bool FrogPilotConfirmationDialog::toggle(const QString &prompt_text, const QString &confirm_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, confirm_text, tr("Reboot Later"), false, parent); + return d.exec(); +} + +bool FrogPilotConfirmationDialog::toggleAlert(const QString &prompt_text, const QString &button_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, button_text, "", false, parent); + return d.exec(); +} + +bool FrogPilotConfirmationDialog::yesorno(const QString &prompt_text, QWidget *parent) { + ConfirmationDialog d = ConfirmationDialog(prompt_text, tr("Yes"), tr("No"), false, parent); + return d.exec(); +} + +FrogPilotButtonIconControl::FrogPilotButtonIconControl(const QString &title, const QString &text, const QString &desc, const QString &icon, QWidget *parent) : AbstractControl(title, desc, icon, parent) { + btn.setText(text); + btn.setStyleSheet(R"( + QPushButton { + padding: 0; + border-radius: 50px; + font-size: 35px; + font-weight: 500; + color: #E4E4E4; + background-color: #393939; + } + QPushButton:pressed { + background-color: #4a4a4a; + } + QPushButton:disabled { + color: #33E4E4E4; + } + )"); + btn.setFixedSize(250, 100); + QObject::connect(&btn, &QPushButton::clicked, this, &FrogPilotButtonIconControl::clicked); + hlayout->addWidget(&btn); +} diff --git a/selfdrive/frogpilot/ui/frogpilot_ui_functions.h b/selfdrive/frogpilot/ui/frogpilot_ui_functions.h new file mode 100644 index 0000000..3785b1c --- /dev/null +++ b/selfdrive/frogpilot/ui/frogpilot_ui_functions.h @@ -0,0 +1,745 @@ +#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 = 0) : QWidget(parent), outer_layout(this) { + outer_layout.setMargin(0); + outer_layout.setSpacing(0); + outer_layout.addLayout(&inner_layout); + inner_layout.setMargin(0); + inner_layout.setSpacing(25); // default spacing is 25 + outer_layout.addStretch(); + } + inline void addItem(QWidget *w) { inner_layout.addWidget(w); } + inline void addItem(QLayout *layout) { inner_layout.addLayout(layout); } + inline void setSpacing(int spacing) { inner_layout.setSpacing(spacing); } + +private: + void paintEvent(QPaintEvent *) override { + QPainter p(this); + p.setPen(Qt::gray); + + int visibleWidgetCount = 0; + std::vector visibleRects; + + for (int i = 0; i < inner_layout.count(); ++i) { + QWidget *widget = inner_layout.itemAt(i)->widget(); + if (widget && widget->isVisible()) { + visibleWidgetCount++; + visibleRects.push_back(inner_layout.itemAt(i)->geometry()); + } + } + + for (int i = 0; i < visibleWidgetCount - 1; ++i) { + int bottom = visibleRects[i].bottom() + inner_layout.spacing() / 2; + p.drawLine(visibleRects[i].left() + 40, bottom, visibleRects[i].right() - 40, bottom); + } + } + QVBoxLayout outer_layout; + QVBoxLayout inner_layout; +}; + +class 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 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 &intervals = 1.0f) + : ParamControl(param, title, desc, icon, parent), + minValue(minValue), maxValue(maxValue), valueLabelMappings(valueLabels), loop(loop), labelText(label), + division(division), 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(-intervals); }); + connect(incrementButton, &QPushButton::pressed, this, [=]() { updateValue(intervals); }); + + connect(decrementButton, &QPushButton::released, this, &FrogPilotParamValueControl::restartTimer); + connect(incrementButton, &QPushButton::released, this, &FrogPilotParamValueControl::restartTimer); + + toggle.hide(); + } + + void restartTimer() { + countdownTimer->stop(); + countdownTimer->start(); + } + + void handleTimeout() { + previousValue = value; + } + + void updateValue(float increment) { + if (std::fabs(previousValue - value) > 5.0f && std::fmod(value, 5.0f) == 0.0f) { + increment *= 5; + } + value += increment; + + if (loop) { + if (value < minValue) value = maxValue; + else if (value > maxValue) value = minValue; + } else { + value = std::max(minValue, std::min(maxValue, value)); + } + + params.putFloat(key, value); + refresh(); + emit valueChanged(value); + } + + 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 (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: + bool loop; + float division; + float maxValue; + float minValue; + float previousValue; + float value; + QLabel *valueLabel; + QString labelText; + std::map valueLabelMappings; + std::string key; + Params params; + 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 ParamControl { + Q_OBJECT + +public: + FrogPilotParamValueToggleControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, + const int &minValue, const int &maxValue, const std::map &valueLabels, + QWidget *parent = nullptr, const bool &loop = true, const QString &label = "", const int &division = 1, + const std::vector &button_params = std::vector(), const std::vector &button_texts = std::vector(), + const int minimum_button_width = 225) + : ParamControl(param, title, desc, icon, parent), + minValue(minValue), maxValue(maxValue), valueLabelMappings(valueLabels), loop(loop), labelText(label), division(division) { + key = param.toStdString(); + + const QString style = R"( + QPushButton { + border-radius: 50px; + font-size: 40px; + font-weight: 500; + height:100px; + padding: 0 25 0 25; + color: #E4E4E4; + background-color: #393939; + } + QPushButton:pressed { + background-color: #4a4a4a; + } + QPushButton:checked:enabled { + background-color: #33Ab4C; + } + QPushButton:disabled { + color: #33E4E4E4; + } + )"; + + button_group = new QButtonGroup(this); + button_group->setExclusive(false); + + std::map paramState; + for (const QString &button_param : button_params) { + paramState[button_param] = params.getBool(button_param.toStdString()); + } + + for (int i = 0; i < button_texts.size(); ++i) { + QPushButton *button = new QPushButton(button_texts[i], this); + button->setCheckable(true); + button->setChecked(paramState[button_params[i]]); + button->setStyleSheet(style); + button->setMinimumWidth(minimum_button_width); + button_group->addButton(button, i); + + connect(button, &QPushButton::clicked, [this, button_params, i](bool checked) { + params.putBool(button_params[i].toStdString(), checked); + button_group->button(i)->setChecked(checked); + }); + + hlayout->addWidget(button); + } + + valueLabel = new QLabel(this); + hlayout->addWidget(valueLabel); + + QPushButton *decrementButton = createButton("-", this); + QPushButton *incrementButton = createButton("+", this); + + hlayout->addWidget(decrementButton); + hlayout->addWidget(incrementButton); + + connect(decrementButton, &QPushButton::clicked, this, [=]() { + updateValue(-1); + }); + + connect(incrementButton, &QPushButton::clicked, this, [=]() { + updateValue(1); + }); + + toggle.hide(); + } + + void updateValue(int increment) { + value = value + increment; + + if (loop) { + if (value < minValue) value = maxValue; + else if (value > maxValue) value = minValue; + } else { + value = std::max(minValue, std::min(maxValue, value)); + } + + params.putInt(key, value); + refresh(); + emit valueChanged(value); + } + + void refresh() { + value = params.getInt(key); + + QString text; + auto it = valueLabelMappings.find(value); + if (division > 1) { + text = QString::number(value / (division * 1.0), 'g'); + } else { + text = it != valueLabelMappings.end() ? it->second : QString::number(value); + } + if (!labelText.isEmpty()) { + text += labelText; + } + valueLabel->setText(text); + valueLabel->setStyleSheet("QLabel { color: #E0E879; }"); + } + + void updateControl(int newMinValue, int newMaxValue, const QString &newLabel, int newDivision) { + minValue = newMinValue; + maxValue = newMaxValue; + labelText = newLabel; + division = newDivision; + } + + void showEvent(QShowEvent *event) override { + refresh(); + } + +signals: + void valueChanged(int value); + +private: + bool loop; + int division; + int maxValue; + int minValue; + int value; + QButtonGroup *button_group; + QLabel *valueLabel; + QString labelText; + std::map valueLabelMappings; + std::string key; + Params params; + + QPushButton *createButton(const QString &text, QWidget *parent) { + QPushButton *button = new QPushButton(text, parent); + button->setFixedSize(150, 100); + button->setAutoRepeat(true); + button->setAutoRepeatInterval(150); + button->setAutoRepeatDelay(350); + 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; + } +}; diff --git a/selfdrive/frogpilot/ui/vehicle_settings.cc b/selfdrive/frogpilot/ui/vehicle_settings.cc new file mode 100644 index 0000000..ed27626 --- /dev/null +++ b/selfdrive/frogpilot/ui/vehicle_settings.cc @@ -0,0 +1,88 @@ +#include "selfdrive/frogpilot/ui/vehicle_settings.h" + +FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { + std::vector> vehicleToggles { + {"LongitudinalTune", "Longitudinal Tune", "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 my 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("Reboot required to take effect.", "Reboot Now", this)) { + Hardware::soft_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 std::string &key : rebootKeys) { + QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this]() { + if (started) { + if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) { + Hardware::soft_reboot(); + } + } + }); + } + + QObject::connect(uiState(), &UIState::uiUpdate, this, &FrogPilotVehiclesPanel::updateState); +} + +void FrogPilotVehiclesPanel::updateState(const UIState &s) { + 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::setToggles() { + bool gm = false; + bool subaru false; + bool toyota = false; + + for (auto &[key, toggle] : toggles) { + if (toggle) { + toggle->setVisible(false); + + 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/vehicle_settings.h b/selfdrive/frogpilot/ui/vehicle_settings.h new file mode 100644 index 0000000..e8bc788 --- /dev/null +++ b/selfdrive/frogpilot/ui/vehicle_settings.h @@ -0,0 +1,32 @@ +#pragma once + +#include + +#include + +#include "selfdrive/frogpilot/ui/frogpilot_ui_functions.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 setToggles(); + void updateState(const UIState &s); + void updateToggles(); + + std::map toggles; + + std::set gmKeys = {}; + std::set subaruKeys = {}; + std::set toyotaKeys = {"LongitudinalTune"}; + + Params params; + Params paramsMemory{"/dev/shm/params"}; + + bool started = false; +}; diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc new file mode 100644 index 0000000..5488eb6 --- /dev/null +++ b/selfdrive/frogpilot/ui/visual_settings.cc @@ -0,0 +1,156 @@ +#include "selfdrive/frogpilot/ui/visual_settings.h" + +FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { + const std::vector> visualToggles { + {"CustomAlerts", "Custom Alerts", "Enable custom alerts for various logic or situational changes.", "../frogpilot/assets/toggle_icons/icon_green_light.png"}, + + {"CustomUI", "Custom Onroad UI", "Customize the Onroad UI with some additional visual functions.", "../assets/offroad/icon_road.png"}, + + {"QOLVisuals", "Quality of Life", "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]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(customAlertsKeys .find(key.c_str()) != customAlertsKeys .end()); + } + }); + toggle = customAlertsToggle; + + } else if (param == "CustomUI") { + FrogPilotParamManageControl *customUIToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(customUIToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(customOnroadUIKeys.find(key.c_str()) != customOnroadUIKeys.end()); + } + }); + toggle = customUIToggle; + + } else if (param == "QOLVisuals") { + FrogPilotParamManageControl *qolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(qolToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + 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(); + }); + } + + std::set rebootKeys = {""}; + for (const std::string &key : rebootKeys) { + QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this, key]() { + if (started) { + if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) { + Hardware::soft_reboot(); + } + } + }); + } + + QObject::connect(device(), &Device::interactiveTimeout, this, &FrogPilotVisualsPanel::hideSubToggles); + QObject::connect(parent, &SettingsWindow::closeParentToggle, this, &FrogPilotVisualsPanel::hideSubToggles); + QObject::connect(parent, &SettingsWindow::closeSubParentToggle, this, &FrogPilotVisualsPanel::hideSubSubToggles); + QObject::connect(parent, &SettingsWindow::updateMetric, this, &FrogPilotVisualsPanel::updateMetric); + QObject::connect(uiState(), &UIState::uiUpdate, this, &FrogPilotVisualsPanel::updateState); + + hideSubToggles(); + updateMetric(); +} + +void FrogPilotVisualsPanel::updateState(const UIState &s) { + 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::updateMetric() { + bool previousIsMetric = isMetric; + isMetric = params.getBool("IsMetric"); + + if (isMetric != previousIsMetric) { + double distanceConversion = isMetric ? INCH_TO_CM : CM_TO_INCH; + double speedConversion = isMetric ? FOOT_TO_METER : METER_TO_FOOT; + } + + if (isMetric) { + } else { + } + + previousIsMetric = isMetric; +} + +void FrogPilotVisualsPanel::parentToggleClicked() { + openParentToggle(); +} + +void FrogPilotVisualsPanel::subParentToggleClicked() { + openSubParentToggle(); +} + +void FrogPilotVisualsPanel::hideSubToggles() { + 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(); + toggle->setVisible(!subToggles); + } + + closeParentToggle(); +} + +void FrogPilotVisualsPanel::hideSubSubToggles() { + for (auto &[key, toggle] : toggles) { + bool isVisible = false; + toggle->setVisible(isVisible); + } + + closeSubParentToggle(); + update(); +} + +void FrogPilotVisualsPanel::hideEvent(QHideEvent *event) { + hideSubToggles(); +} diff --git a/selfdrive/frogpilot/ui/visual_settings.h b/selfdrive/frogpilot/ui/visual_settings.h new file mode 100644 index 0000000..ce69c60 --- /dev/null +++ b/selfdrive/frogpilot/ui/visual_settings.h @@ -0,0 +1,45 @@ +#pragma once + +#include + +#include "selfdrive/frogpilot/ui/frogpilot_ui_functions.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 closeParentToggle(); + void closeSubParentToggle(); + void openParentToggle(); + void openSubParentToggle(); + +private: + void hideEvent(QHideEvent *event); + void hideSubToggles(); + void hideSubSubToggles(); + void parentToggleClicked(); + void subParentToggleClicked(); + void updateMetric(); + void updateState(const UIState &s); + void updateToggles(); + + std::set alertVolumeControlKeys = {}; + std::set customAlertsKeys = {}; + std::set customOnroadUIKeys = {}; + std::set customThemeKeys = {}; + std::set modelUIKeys = {}; + std::set qolKeys = {}; + + std::map toggles; + + Params params; + Params paramsMemory{"/dev/shm/params"}; + + bool isMetric = params.getBool("IsMetric"); + bool started = false; +}; diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index bf1eeb8..cc6cc8d 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -2,6 +2,7 @@ import datetime import os import signal +import subprocess import sys import traceback from typing import List, Tuple, Union @@ -27,12 +28,38 @@ def manager_init() -> None: save_bootlog() params = Params() + params_storage = Params("/persist/comma/params") params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) if is_release_branch(): params.clear_all(ParamKeyType.DEVELOPMENT_ONLY) + ############### Remove this after the April 26th update ############### + + previous_speed_limit = params.get_float("PreviousSpeedLimit") + if previous_speed_limit >= 50: + params.put_float("PreviousSpeedLimit", previous_speed_limit / 100) + + for priority_key in ["SLCPriority", "SLCPriority1", "SLCPriority2", "SLCPriority3"]: + priority_value = params.get(priority_key) + if isinstance(priority_value, int): + params.remove(priority_key) + + attributes = ["AggressiveFollow", "StandardFollow", "RelaxedFollow", "AggressiveJerk", "StandardJerk", "RelaxedJerk"] + values = {attr: params.get_float(attr) for attr in attributes} + if any(value > 5 for value in values.values()): + for attr, value in values.items(): + params.put_float(attr, value / 10) + + if params.get_bool("SilentMode"): + attributes = ["DisengageVolume", "EngageVolume", "PromptVolume", "PromptDistractedVolume", "RefuseVolume", "WarningSoftVolume", "WarningImmediateVolume"] + for attr in attributes: + params.put_float(attr, 0) + params.put_bool("SilentMode", False) + + ####################################################################### + default_params: List[Tuple[str, Union[str, bytes]]] = [ ("CompletedTrainingVersion", "0"), ("DisengageOnAccelerator", "0"), @@ -40,7 +67,192 @@ def manager_init() -> None: ("HasAcceptedTerms", "0"), ("LanguageSetting", "main_en"), ("OpenpilotEnabledToggle", "1"), + ("UpdaterAvailableBranches", ""), ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), + + # Default FrogPilot parameters + ("AccelerationPath", "1"), + ("AccelerationProfile", "2"), + ("AdjacentPath", "0"), + ("AdjacentPathMetrics", "0"), + ("AdjustablePersonalities", "1"), + ("AggressiveAcceleration", "1"), + ("AggressiveFollow", "1.25"), + ("AggressiveJerk", "0.5"), + ("AlertVolumeControl", "0"), + ("AlwaysOnLateral", "1"), + ("AlwaysOnLateralMain", "0"), + ("BlindSpotPath", "1"), + ("CameraView", "1"), + ("CarMake", ""), + ("CarModel", ""), + ("CECurves", "1"), + ("CENavigation", "1"), + ("CENavigationIntersections", "1"), + ("CENavigationLead", "1"), + ("CENavigationTurns", "1"), + ("CESignal", "1"), + ("CESlowerLead", "0"), + ("CESpeed", "0"), + ("CESpeedLead", "0"), + ("CEStopLights", "1"), + ("CEStopLightsLead", "0"), + ("Compass", "0"), + ("ConditionalExperimental", "1"), + ("CrosstrekTorque", "1"), + ("CurveSensitivity", "100"), + ("CustomAlerts", "1"), + ("CustomColors", "1"), + ("CustomIcons", "1"), + ("CustomPersonalities", "1"), + ("CustomSignals", "1"), + ("CustomSounds", "1"), + ("CustomTheme", "1"), + ("CustomUI", "1"), + ("CydiaTune", "0"), + ("DecelerationProfile", "1"), + ("DeviceShutdown", "9"), + ("DisableMTSCSmoothing", "0"), + ("DisableOnroadUploads", "0"), + ("DisableOpenpilotLongitudinal", "0"), + ("DisableVTSCSmoothing", "0"), + ("DisengageVolume", "100"), + ("DragonPilotTune", "0"), + ("DriverCamera", "0"), + ("DriveStats", "1"), + ("DynamicPathWidth", "0"), + ("EngageVolume", "100"), + ("EVTable", "1"), + ("ExperimentalModeActivation", "1"), + ("ExperimentalModeViaDistance", "0"), + ("ExperimentalModeViaLKAS", "0"), + ("ExperimentalModeViaScreen", "1"), + ("Fahrenheit", "0"), + ("FireTheBabysitter", "0"), + ("ForceAutoTune", "0"), + ("ForceFingerprint", "0"), + ("ForceMPHDashboard", "0"), + ("FPSCounter", "0"), + ("FrogPilotDrives", "0"), + ("FrogPilotKilometers", "0"), + ("FrogPilotMinutes", "0"), + ("FrogsGoMooTune", "1"), + ("FullMap", "0"), + ("GasRegenCmd", "0"), + ("GoatScream", "1"), + ("GreenLightAlert", "0"), + ("HideAlerts", "0"), + ("HideAOLStatusBar", "0"), + ("HideCEMStatusBar", "0"), + ("HideLeadMarker", "0"), + ("HideMapIcon", "0"), + ("HideMaxSpeed", "0"), + ("HideSpeed", "0"), + ("HideSpeedUI", "0"), + ("HideUIElements", "0"), + ("HigherBitrate", "0"), + ("LaneChangeTime", "0"), + ("LaneDetection", "1"), + ("LaneDetectionWidth", "60"), + ("LaneLinesWidth", "4"), + ("LateralTune", "1"), + ("LeadDepartingAlert", "0"), + ("LeadDetectionThreshold", "25"), + ("LeadInfo", "0"), + ("LockDoors", "0"), + ("LongitudinalTune", "1"), + ("LongPitch", "1"), + ("LoudBlindspotAlert", "0"), + ("LowerVolt", "1"), + ("MapsSelected", ""), + ("MapStyle", "0"), + ("MTSCAggressiveness", "100"), + ("MTSCCurvatureCheck", "0"), + ("MTSCLimit", "0"), + ("Model", DEFAULT_MODEL), + ("ModelUI", "1"), + ("MTSCEnabled", "1"), + ("MuteOverheated", "0"), + ("NavChill", "0"), + ("NNFF", "1"), + ("NoLogging", "0"), + ("NoUploads", "0"), + ("NudgelessLaneChange", "1"), + ("NumericalTemp", "0"), + ("OfflineMode", "0"), + ("Offset1", "5"), + ("Offset2", "5"), + ("Offset3", "5"), + ("Offset4", "10"), + ("OneLaneChange", "1"), + ("PathEdgeWidth", "20"), + ("PathWidth", "61"), + ("PauseLateralOnSignal", "0"), + ("PedalsOnUI", "1"), + ("PersonalitiesViaScreen", "1"), + ("PersonalitiesViaWheel", "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"), + ("ShowMemoryUsage", "0"), + ("ShowSLCOffset", "0"), + ("ShowSLCOffsetUI", "1"), + ("ShowStorageLeft", "0"), + ("ShowStorageUsed", "0"), + ("Sidebar", "0"), + ("SLCConfirmation", "1"), + ("SLCConfirmationLower", "1"), + ("SLCConfirmationHigher", "1"), + ("SLCFallback", "2"), + ("SLCOverride", "1"), + ("SLCPriority1", "Dashboard"), + ("SLCPriority2", "Offline Maps"), + ("SLCPriority3", "Navigation"), + ("SmoothBraking", "1"), + ("SNGHack", "1"), + ("SpeedLimitChangedAlert", "1"), + ("SpeedLimitController", "1"), + ("StandardFollow", "1.45"), + ("StandardJerk", "1.0"), + ("StandbyMode", "0"), + ("SteerRatio", "0"), + ("StockTune", "0"), + ("StoppingDistance", "0"), + ("TurnAggressiveness", "100"), + ("TurnDesires", "0"), + ("UnlimitedLength", "1"), + ("UpdateSchedule", "0"), + ("UseLateralJerk", "0"), + ("UseSI", "0"), + ("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'))) @@ -51,7 +263,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: @@ -114,12 +331,21 @@ def manager_cleanup() -> None: cloudlog.info("everything is dead") +def update_frogpilot_params(params, params_memory): + keys = [] + for key in keys: + params_memory.put_bool(key, params.get_bool(key)) + def manager_thread() -> None: cloudlog.bind(daemon="manager") cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) params = Params() + params_memory = Params("/dev/shm/params") + params_storage = Params("/persist/comma/params") + + update_frogpilot_params(params, params_memory) ignore: List[str] = [] if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID): @@ -132,7 +358,7 @@ def manager_thread() -> None: pm = messaging.PubMaster(['managerState']) write_onroad_params(False, params) - ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore) + ensure_running(managed_processes.values(), False, params=params, params_memory=params_memory, CP=sm['carParams'], not_run=ignore) started_prev = False @@ -152,7 +378,7 @@ def manager_thread() -> None: started_prev = started - ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore) + ensure_running(managed_processes.values(), started, params=params, params_memory=params_memory, CP=sm['carParams'], not_run=ignore) running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) for p in managed_processes.values() if p.proc) @@ -175,8 +401,18 @@ def manager_thread() -> None: if shutdown: break + if params_memory.get_bool("FrogPilotTogglesUpdated"): + update_frogpilot_params(params, params_memory) def main() -> None: + # Create the long term param storage folder + try: + # Attempt to remount /persist as read-write + subprocess.run(['sudo', 'mount', '-o', 'remount,rw', '/persist'], check=True) + print("Successfully remounted /persist as read-write.") + except subprocess.CalledProcessError as e: + print(f"Failed to remount /persist. Error: {e}") + manager_init() if os.getenv("PREPAREONLY") is not None: return diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index ac1b4ac..e122e58 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -238,7 +238,7 @@ class DaemonProcess(ManagerProcess): self.params = None @staticmethod - def should_run(started, params, CP): + def should_run(started, params, params_memory, CP): return True def prepare(self) -> None: @@ -273,14 +273,14 @@ class DaemonProcess(ManagerProcess): pass -def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None, CP: car.CarParams=None, +def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None, params_memory=None, CP: car.CarParams=None, not_run: Optional[List[str]]=None) -> List[ManagerProcess]: if not_run is None: not_run = [] running = [] for p in procs: - if p.enabled and p.name not in not_run and p.should_run(started, params, CP): + if p.enabled and p.name not in not_run and p.should_run(started, params, params_memory, CP): running.append(p) else: p.stop(block=False) diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index cb6dd88..7ab2a5a 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -7,40 +7,42 @@ from openpilot.selfdrive.manager.process import PythonProcess, NativeProcess, Da WEBCAM = os.getenv("USE_WEBCAM") is not None -def driverview(started: bool, params: Params, CP: car.CarParams) -> bool: +def driverview(started: bool, params: Params, params_memory: Params, CP: car.CarParams) -> bool: return started or params.get_bool("IsDriverViewEnabled") -def notcar(started: bool, params: Params, CP: car.CarParams) -> bool: +def notcar(started: bool, params: Params, params_memory: Params, CP: car.CarParams) -> bool: return started and CP.notCar -def iscar(started: bool, params: Params, CP: car.CarParams) -> bool: +def iscar(started: bool, params: Params, params_memory: Params, CP: car.CarParams) -> bool: return started and not CP.notCar -def logging(started, params, CP: car.CarParams) -> bool: +def logging(started, params, params_memory, CP: car.CarParams) -> bool: run = (not CP.notCar) or not params.get_bool("DisableLogging") return started and run def ublox_available() -> bool: return os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps') -def ublox(started, params, CP: car.CarParams) -> bool: +def ublox(started, params, params_memory, CP: car.CarParams) -> bool: use_ublox = ublox_available() if use_ublox != params.get_bool("UbloxAvailable"): params.put_bool("UbloxAvailable", use_ublox) return started and use_ublox -def qcomgps(started, params, CP: car.CarParams) -> bool: +def qcomgps(started, params, params_memory, CP: car.CarParams) -> bool: return started and not ublox_available() -def always_run(started, params, CP: car.CarParams) -> bool: +def always_run(started, params, params_memory, CP: car.CarParams) -> bool: return True -def only_onroad(started: bool, params, CP: car.CarParams) -> bool: +def only_onroad(started: bool, params, params_memory, CP: car.CarParams) -> bool: return started -def only_offroad(started, params, CP: car.CarParams) -> bool: +def only_offroad(started, params, params_memory, CP: car.CarParams) -> bool: return not started +# FrogPilot functions + procs = [ DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), @@ -86,6 +88,8 @@ procs = [ NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar), PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), PythonProcess("webjoystick", "tools.bodyteleop.web", notcar), + + # FrogPilot processes ] managed_processes = {p.name: p for p in procs} diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index f2842d9..8a12fb9 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(["carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl"]) + sm = SubMaster(["carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl", "frogpilotPlan"]) publish_state = PublishState() params = Params() @@ -298,7 +298,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 2be703c..863e251 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -28,6 +28,7 @@ class RouteEngine: self.pm = pm self.params = Params() + self.params_memory = Params("/dev/shm/params") # Get last gps position from params self.last_position = coordinate_from_param("LastGPSPosition", self.params) @@ -59,7 +60,14 @@ class RouteEngine: self.mapbox_token = "" self.mapbox_host = "https://maps.comma.ai" + # FrogPilot variables + self.update_frogpilot_params() + def update(self): + # Update FrogPilot parameters + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + self.sm.update(0) if self.sm.updated["managerState"]: @@ -301,6 +309,11 @@ class RouteEngine: self.params.remove("NavDestination") self.clear_route() + frogpilot_plan_send = messaging.new_message('frogpilotNavigation') + frogpilotNavigation = frogpilot_plan_send.frogpilotNavigation + + self.pm.send('frogpilotNavigation', frogpilot_plan_send) + def send_route(self): coords = [] @@ -349,9 +362,10 @@ class RouteEngine: return self.reroute_counter > REROUTE_COUNTER_MIN # TODO: Check for going wrong way in segment + def update_frogpilot_params(self): def main(): - pm = messaging.PubMaster(['navInstruction', 'navRoute']) + pm = messaging.PubMaster(['navInstruction', 'navRoute', 'frogpilotNavigation']) sm = messaging.SubMaster(['liveLocationKalman', 'managerState']) rk = Ratekeeper(1.0) diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index f74118b..62245ba 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -37,6 +37,8 @@ class PowerMonitoring: # Reset capacity if it's low self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh)) + # FrogPilot variables + # Calculation tick def calculate(self, voltage: Optional[int], ignition: bool): try: diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index e868f2f..6a02499 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -164,7 +164,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 ea5734f..bc55373 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/frogpilot_ui_functions.cc", + "../frogpilot/ui/control_settings.cc", "../frogpilot/ui/vehicle_settings.cc", + "../frogpilot/ui/visual_settings.cc"] qt_env['CPPDEFINES'] = [] if maps: diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h index c603285..b9f8e37 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 81ba500..5849b64 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 0f4be67..aefc0c4 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 0e151df..d7d019c 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 85393ca..c0c5bde 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -7,6 +7,7 @@ #include #include +#include #include "selfdrive/ui/qt/network/networking.h" @@ -23,6 +24,10 @@ #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/frogpilot/ui/control_settings.h" +#include "selfdrive/frogpilot/ui/vehicle_settings.h" +#include "selfdrive/frogpilot/ui/visual_settings.h" + TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { // param, title, desc, icon std::vector> toggle_defs{ @@ -117,6 +122,10 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { connect(toggles["ExperimentalLongitudinalEnabled"], &ToggleControl::toggleFlipped, [=]() { updateToggles(); }); + + connect(toggles["IsMetric"], &ToggleControl::toggleFlipped, [=]() { + updateMetric(); + }); } void TogglesPanel::expandToggleDescription(const QString ¶m) { @@ -245,6 +254,20 @@ 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("Deleting params..."); + std::system("rm -rf /persist/comma/params"); + deleteStorageParamsBtn->setValue(""); + }).detach(); + }); + addItem(deleteStorageParamsBtn); + QObject::connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { for (auto btn : findChildren()) { btn->setEnabled(offroad); @@ -364,7 +387,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 (parentToggleOpen) { + if (subParentToggleOpen) { + closeSubParentToggle(); + } else { + closeParentToggle(); + } + } else { + closeSettings(); + } + }); // setup panels DevicePanel *device = new DevicePanel(this); @@ -373,12 +406,28 @@ 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::closeSubParentToggle, this, [this]() {subParentToggleOpen = false;}); + QObject::connect(frogpilotControls, &FrogPilotControlsPanel::openSubParentToggle, this, [this]() {subParentToggleOpen = true;}); + QObject::connect(frogpilotControls, &FrogPilotControlsPanel::closeParentToggle, this, [this]() {parentToggleOpen = false;}); + QObject::connect(frogpilotControls, &FrogPilotControlsPanel::openParentToggle, this, [this]() {parentToggleOpen = true;}); + + FrogPilotVisualsPanel *frogpilotVisuals = new FrogPilotVisualsPanel(this); + QObject::connect(frogpilotVisuals, &FrogPilotVisualsPanel::closeSubParentToggle, this, [this]() {subParentToggleOpen = false;}); + QObject::connect(frogpilotVisuals, &FrogPilotVisualsPanel::openSubParentToggle, this, [this]() {subParentToggleOpen = true;}); + QObject::connect(frogpilotVisuals, &FrogPilotVisualsPanel::closeParentToggle, this, [this]() {parentToggleOpen = false;}); + 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); @@ -411,7 +460,22 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { ScrollView *panel_frame = new ScrollView(panel, this); panel_widget->addWidget(panel_frame); + if (name == tr("Controls") || name == tr("Visuals")) { + QScrollBar *scrollbar = panel_frame->verticalScrollBar(); + + QObject::connect(scrollbar, &QScrollBar::valueChanged, this, [this](int value) { + if (!parentToggleOpen) { + previousScrollPosition = value; + } + }); + + QObject::connect(scrollbar, &QScrollBar::rangeChanged, this, [this, panel_frame]() { + panel_frame->restorePosition(previousScrollPosition); + }); + } + QObject::connect(btn, &QPushButton::clicked, [=, w = panel_frame]() { + previousScrollPosition = 0; btn->setChecked(true); panel_widget->setCurrentWidget(w); }); diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index a5dd25b..53cc735 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -31,11 +31,21 @@ signals: 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 { @@ -53,6 +63,9 @@ private slots: private: Params params; + + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; }; class TogglesPanel : public ListWidget { @@ -61,6 +74,10 @@ public: explicit TogglesPanel(SettingsWindow *parent); void showEvent(QShowEvent *event) override; +signals: + // FrogPilot signals + void updateMetric(); + public slots: void expandToggleDescription(const QString ¶m); diff --git a/selfdrive/ui/qt/offroad/software_settings.cc b/selfdrive/ui/qt/offroad/software_settings.cc index 15c022d..b44dc48 100644 --- a/selfdrive/ui/qt/offroad/software_settings.cc +++ b/selfdrive/ui/qt/offroad/software_settings.cc @@ -15,6 +15,7 @@ #include "selfdrive/ui/qt/widgets/input.h" #include "system/hardware/hw.h" +#include "selfdrive/frogpilot/ui/frogpilot_ui_functions.h" void SoftwarePanel::checkForUpdates() { std::system("pkill -SIGUSR1 -f selfdrive.updated"); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 01750ea..775a89e 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,13 @@ void OnroadWindow::updateState(const UIState &s) { } void OnroadWindow::mousePressEvent(QMouseEvent* e) { + Params params = Params(); + + // FrogPilot clickable widgets + bool widgetClicked = false; + #ifdef ENABLE_MAPS - if (map != nullptr) { + if (map != nullptr && !widgetClicked) { // Switch between map and sidebar when using navigate on openpilot bool sidebarVisible = geometry().x() > 0; bool show_map = uiState()->scene.navigate_on_openpilot ? sidebarVisible : !sidebarVisible; @@ -102,7 +107,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 +121,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 +175,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 +222,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { } // ExperimentalButton -ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent) { +ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent), scene(uiState()->scene) { setFixedSize(btn_size, btn_size); engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); @@ -221,6 +231,8 @@ ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(fals } void ExperimentalButton::changeMode() { + Params paramsMemory = Params("/dev/shm/params"); + const auto cp = (*uiState()->sm)["carParams"].getCarParams(); bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed"); if (can_change) { @@ -236,6 +248,8 @@ void ExperimentalButton::updateState(const UIState &s) { experimental_mode = cs.getExperimentalMode(); update(); } + + // FrogPilot variables } void ExperimentalButton::paintEvent(QPaintEvent *event) { @@ -247,7 +261,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 +276,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 +290,9 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* par main_layout->addWidget(map_settings_btn, 0, Qt::AlignBottom | Qt::AlignRight); dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5}); + + // Initialize FrogPilot widgets + initializeFrogPilotWidgets(); } void AnnotatedCameraWidget::updateState(const UIState &s) { @@ -468,7 +485,6 @@ void AnnotatedCameraWidget::updateFrameMat() { void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.save(); - const UIScene &scene = s->scene; SubMaster &sm = *(s->sm); // lanelines @@ -525,12 +541,11 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { } void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) { - const UIScene &scene = s->scene; - painter.save(); // base icon int offset = UI_BORDER_SIZE + btn_size / 2; + offset += true ? 25 : 0; int x = rightHandDM ? width() - offset : offset; int y = height() - offset; float opacity = dmActive ? 0.65 : 0.2; @@ -693,6 +708,9 @@ void AnnotatedCameraWidget::paintGL() { auto m = msg.initEvent().initUiDebug(); m.setDrawTimeMillis(cur_draw_t - start_draw_t); pm->send("uiDebug", msg); + + // Update FrogPilot widgets + updateFrogPilotWidgets(painter); } void AnnotatedCameraWidget::showEvent(QShowEvent *event) { @@ -701,3 +719,81 @@ 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(QPainter &p) { + experimentalMode = scene.experimental_mode; + + if (true) { + drawStatusBar(p); + } + + map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled()); + if (map_settings_btn_bottom->isEnabled()) { + map_settings_btn_bottom->setVisible(!hideBottomIcons); + bottom_layout->setAlignment(map_settings_btn_bottom, rightHandDM ? Qt::AlignLeft : Qt::AlignRight); + } +} + +void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { + p.save(); + + // Variable declarations + static QElapsedTimer timer; + static QString lastShownStatus; + + QString newStatus; + + static bool displayStatusText = false; + + constexpr qreal fadeDuration = 1500.0; + constexpr qreal textDuration = 5000.0; + + // Draw status bar + QRect currentRect = rect(); + QRect statusBarRect(currentRect.left() - 1, currentRect.bottom() - 50, currentRect.width() + 2, 100); + p.setBrush(QColor(0, 0, 0, 150)); + p.setOpacity(1.0); + p.drawRoundedRect(statusBarRect, 30, 30); + + // Check if status has changed + if (newStatus != lastShownStatus) { + displayStatusText = true; + lastShownStatus = newStatus; + timer.restart(); + } else if (displayStatusText && timer.hasExpired(textDuration + fadeDuration)) { + displayStatusText = false; + } + + // Configure the text + p.setFont(InterFont(40, QFont::Bold)); + p.setPen(Qt::white); + p.setRenderHint(QPainter::TextAntialiasing); + + // Calculate text opacity + static qreal statusTextOpacity; + int elapsed = timer.elapsed(); + if (displayStatusText) { + statusTextOpacity = qBound(0.0, 1.0 - (elapsed - textDuration) / fadeDuration, 1.0); + } + + // Draw the status text + p.setOpacity(statusTextOpacity); + QRect textRect = p.fontMetrics().boundingRect(statusBarRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus); + textRect.moveBottom(statusBarRect.bottom() - 50); + p.drawText(textRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus); + + p.restore(); +} diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index b3ba411..962a148 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -14,13 +14,14 @@ const int btn_size = 192; const int img_size = (btn_size / 4) * 3; +// FrogPilot global variables // ***** onroad widgets ***** class OnroadAlerts : public QWidget { Q_OBJECT public: - OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {} + OnroadAlerts(QWidget *parent = 0) : QWidget(parent), scene(uiState()->scene) {} void updateAlert(const Alert &a); protected: @@ -29,6 +30,9 @@ protected: private: QColor bg; Alert alert = {}; + + // FrogPilot variables + UIScene &scene; }; class ExperimentalButton : public QPushButton { @@ -47,6 +51,9 @@ private: QPixmap experimental_img; bool experimental_mode; bool engageable; + + // FrogPilot variables + UIScene &scene; }; @@ -71,6 +78,7 @@ public: void updateState(const UIState &s); MapSettingsButton *map_settings_btn; + MapSettingsButton *map_settings_btn_bottom; private: void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); @@ -97,6 +105,21 @@ private: int skip_frame_count = 0; bool wide_cam_requested = false; + // FrogPilot widgets + void initializeFrogPilotWidgets(); + void updateFrogPilotWidgets(QPainter &p); + + void drawStatusBar(QPainter &p); + + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; + + UIScene &scene; + + QHBoxLayout *bottom_layout; + + bool experimentalMode; + protected: void paintGL() override; void initializeGL() override; @@ -135,6 +158,10 @@ private: QWidget *map = nullptr; QHBoxLayout* split; + // FrogPilot variables + UIScene &scene; + 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 e952940..702c6eb 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 f627aac..a1d6c16 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 2404efa..7ca2f1a 100644 --- a/selfdrive/ui/qt/spinner.cc +++ b/selfdrive/ui/qt/spinner.cc @@ -88,7 +88,7 @@ Spinner::Spinner(QWidget *parent) : QWidget(parent) { } QProgressBar::chunk { border-radius: 10px; - background-color: white; + background-color: rgba(23, 134, 68, 255); } )"); diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index bc3c494..a83d353 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 e4630c6..8c4f06f 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 978bf83..2846007 100644 --- a/selfdrive/ui/qt/widgets/scrollview.cc +++ b/selfdrive/ui/qt/widgets/scrollview.cc @@ -44,6 +44,10 @@ ScrollView::ScrollView(QWidget *w, QWidget *parent) : QScrollArea(parent) { scroller->setScrollerProperties(sp); } +void ScrollView::restorePosition(int previousScrollPosition) { + verticalScrollBar()->setValue(previousScrollPosition); +} + void ScrollView::hideEvent(QHideEvent *e) { verticalScrollBar()->setValue(0); } diff --git a/selfdrive/ui/qt/widgets/scrollview.h b/selfdrive/ui/qt/widgets/scrollview.h index 024331a..1e67bbe 100644 --- a/selfdrive/ui/qt/widgets/scrollview.h +++ b/selfdrive/ui/qt/widgets/scrollview.h @@ -7,6 +7,9 @@ class ScrollView : public QScrollArea { public: explicit ScrollView(QWidget *w = nullptr, QWidget *parent = nullptr); + + // FrogPilot functions + void restorePosition(int previousScrollPosition); protected: void hideEvent(QHideEvent *e) override; }; diff --git a/selfdrive/ui/qt/widgets/wifi.h b/selfdrive/ui/qt/widgets/wifi.h index 60c865f..e15c4b0 100644 --- a/selfdrive/ui/qt/widgets/wifi.h +++ b/selfdrive/ui/qt/widgets/wifi.h @@ -12,6 +12,10 @@ class WiFiPromptWidget : public QFrame { public: explicit WiFiPromptWidget(QWidget* parent = 0); +private: + // FrogPilot variables + Params params; + signals: void openSettings(int index = 0, const QString ¶m = ""); diff --git a/selfdrive/ui/qt/window.h b/selfdrive/ui/qt/window.h index 05b61e1..f1389c2 100644 --- a/selfdrive/ui/qt/window.h +++ b/selfdrive/ui/qt/window.h @@ -22,4 +22,7 @@ private: HomeWindow *homeWindow; SettingsWindow *settingsWindow; OnboardingWindow *onboardingWindow; + + // FrogPilot variables + Params params; }; diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 11caf80..2546a15 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -8,6 +8,7 @@ from typing import Dict, Optional, Tuple from cereal import car, messaging from openpilot.common.basedir import BASEDIR from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper from openpilot.common.retry import retry from openpilot.common.swaglog import cloudlog @@ -53,6 +54,12 @@ def check_controls_timeout_alert(sm): class Soundd: def __init__(self): + # FrogPilot variables + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.update_frogpilot_params() + self.load_sounds() self.current_alert = AudibleAlert.none @@ -156,6 +163,11 @@ class Soundd: assert stream.active + # Update FrogPilot parameters + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + + def update_frogpilot_params(self): def main(): s = Soundd() diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 9afd22f..140f879 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -12,7 +12,6 @@ #include "common/util.h" #include "common/watchdog.h" #include "system/hardware/hw.h" - #define BACKLIGHT_DT 0.05 #define BACKLIGHT_TS 10.00 @@ -201,6 +200,23 @@ static void update_state(UIState *s) { if (sm.updated("carParams")) { scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); } + if (sm.updated("carState")) { + auto carState = sm["carState"].getCarState(); + } + if (sm.updated("controlsState")) { + auto controlsState = sm["controlsState"].getControlsState(); + scene.enabled = controlsState.getEnabled(); + scene.experimental_mode = controlsState.getExperimentalMode(); + } + if (sm.updated("frogpilotCarControl")) { + auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl(); + } + if (sm.updated("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(); float scale = (cam_state.getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f; @@ -221,6 +237,19 @@ void ui_update_params(UIState *s) { s->scene.map_on_left = params.getBool("NavSettingLeftSide"); } +void ui_update_frogpilot_params(UIState *s) { + ui_update_params(s); + + Params params = Params(); + UIScene &scene = s->scene; + + bool custom_onroad_ui = params.getBool("CustomUI"); + + 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 +278,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 +292,8 @@ UIState::UIState(QObject *parent) : QObject(parent) { timer = new QTimer(this); QObject::connect(timer, &QTimer::timeout, this, &UIState::update); timer->start(1000 / UI_FREQ); + + ui_update_frogpilot_params(this); } void UIState::update() { @@ -273,6 +305,15 @@ void UIState::update() { watchdog_kick(nanos_since_boot()); } emit uiUpdate(*this); + + // Update FrogPilot variables when they are changed + Params paramsMemory{"/dev/shm/params"}; + if (paramsMemory.getBool("FrogPilotTogglesUpdated")) { + std::thread updateFrogPilotParams(ui_update_frogpilot_params, this); + updateFrogPilotParams.detach(); + } + + // FrogPilot live variables that need to be constantly checked } void UIState::setPrimeType(PrimeType type) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 86cd70f..8ac8034 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -105,6 +105,8 @@ typedef enum UIStatus { STATUS_DISENGAGED, STATUS_OVERRIDE, STATUS_ENGAGED, + + // FrogPilot statuses } UIStatus; enum PrimeType { @@ -121,12 +123,15 @@ const QColor bg_colors [] = { [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1), [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), + + // FrogPilot colors }; static std::map alert_colors = { {cereal::ControlsState::AlertStatus::NORMAL, QColor(0x15, 0x15, 0x15, 0xf1)}, {cereal::ControlsState::AlertStatus::USER_PROMPT, QColor(0xDA, 0x6F, 0x25, 0xf1)}, {cereal::ControlsState::AlertStatus::CRITICAL, QColor(0xC9, 0x22, 0x31, 0xf1)}, + {cereal::ControlsState::AlertStatus::FROGPILOT, QColor(0x17, 0x86, 0x44, 0xf1)}, }; typedef struct UIScene { @@ -160,6 +165,11 @@ typedef struct UIScene { bool started, ignition, is_metric, map_on_left, longitudinal_control; bool world_objects_visible = false; uint64_t started_frame; + + // FrogPilot variables + bool enabled; + bool experimental_mode; + } UIScene; class UIState : public QObject { @@ -249,3 +259,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.py b/selfdrive/updated.py index 3e4a849..5ed8e0b 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -237,6 +237,8 @@ class Updater: self.branches = defaultdict(lambda: '') self._has_internet: bool = False + # FrogPilot variables + @property def has_internet(self) -> bool: return self._has_internet diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 3191b82..f7c28fd 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));