Speed limit controller
Added toggle to control the cruise set speed according to speed limit supplied by OSM, NOO, or the vehicle itself. Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev> Co-Authored-By: Efini <19368997+efini@users.noreply.github.com> Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com> Co-Authored-By: Jason Wen <haibin.wen3@gmail.com>
This commit is contained in:
FrogAi
parent
e20c58fe22
commit
935f66510b
|
|
@ -125,6 +125,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
|||
noLaneAvailable @125;
|
||||
openpilotCrashed @126;
|
||||
pedalInterceptorNoBrake @128;
|
||||
speedLimitChanged @129;
|
||||
torqueNNLoad @130;
|
||||
|
||||
radarCanErrorDEPRECATED @15;
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@ $Cxx.namespace("cereal");
|
|||
# you can rename the struct, but don't change the identifier
|
||||
struct FrogPilotCarControl @0x81c2f05a394cf4af {
|
||||
alwaysOnLateral @0 :Bool;
|
||||
speedLimitChanged @1 :Bool;
|
||||
}
|
||||
|
||||
struct FrogPilotDeviceState @0xaedffd8f31e7b55d {
|
||||
|
|
@ -31,7 +32,12 @@ struct FrogPilotPlan @0xda96579883444c35 {
|
|||
redLight @5 :Bool;
|
||||
safeObstacleDistance @6 :Int16;
|
||||
safeObstacleDistanceStock @7 :Int16;
|
||||
slcOverridden @8 :Bool;
|
||||
slcOverriddenSpeed @9 :Float64;
|
||||
slcSpeedLimit @10 :Float64;
|
||||
slcSpeedLimitOffset @11 :Float32;
|
||||
stoppedEquivalenceFactor @12 :Int16;
|
||||
unconfirmedSlcSpeedLimit @13 :Float64;
|
||||
}
|
||||
|
||||
struct CustomReserved4 @0x80ae746ee2596b11 {
|
||||
|
|
|
|||
|
|
@ -227,6 +227,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||
{"CameraView", PERSISTENT},
|
||||
{"CarMake", PERSISTENT},
|
||||
{"CarModel", PERSISTENT},
|
||||
{"CarSpeedLimit", PERSISTENT},
|
||||
{"CECurves", PERSISTENT},
|
||||
{"CECurvesLead", PERSISTENT},
|
||||
{"CENavigation", PERSISTENT},
|
||||
|
|
@ -269,6 +270,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||
{"FireTheBabysitter", PERSISTENT},
|
||||
{"ForceAutoTune", PERSISTENT},
|
||||
{"ForceFingerprint", PERSISTENT},
|
||||
{"ForceMPHDashboard", PERSISTENT},
|
||||
{"FPSCounter", PERSISTENT},
|
||||
{"FrogPilotTogglesUpdated", PERSISTENT},
|
||||
{"FrogsGoMoo", PERSISTENT},
|
||||
|
|
@ -298,6 +300,8 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||
{"MapboxPublicKey", PERSISTENT},
|
||||
{"MapboxSecretKey", PERSISTENT},
|
||||
{"MapsSelected", PERSISTENT},
|
||||
{"MapSpeedLimit", PERSISTENT},
|
||||
{"MapSpeedLimitControl", PERSISTENT},
|
||||
{"MapTargetLatA", PERSISTENT},
|
||||
{"MapTargetVelocities", PERSISTENT},
|
||||
{"Model", PERSISTENT},
|
||||
|
|
@ -309,6 +313,8 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||
{"MuteOverheated", PERSISTENT},
|
||||
{"NavChill", PERSISTENT},
|
||||
{"NavEnable", PERSISTENT},
|
||||
{"NavSpeedLimit", PERSISTENT},
|
||||
{"NavSpeedLimitControl", PERSISTENT},
|
||||
{"NNFF", PERSISTENT},
|
||||
{"NNFFModelFuzzyMatch", PERSISTENT},
|
||||
{"NNFFModelName", PERSISTENT},
|
||||
|
|
@ -317,6 +323,10 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||
{"NudgelessLaneChange", PERSISTENT},
|
||||
{"NumericalTemp", PERSISTENT},
|
||||
{"OfflineMode", PERSISTENT},
|
||||
{"Offset1", PERSISTENT},
|
||||
{"Offset2", PERSISTENT},
|
||||
{"Offset3", PERSISTENT},
|
||||
{"Offset4", PERSISTENT},
|
||||
{"OneLaneChange", PERSISTENT},
|
||||
{"OSMDownloadLocations", PERSISTENT},
|
||||
{"OSMDownloadProgress", CLEAR_ON_MANAGER_START},
|
||||
|
|
@ -325,6 +335,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||
{"PauseLateralOnSignal", PERSISTENT},
|
||||
{"PedalsOnUI", PERSISTENT},
|
||||
{"PreferredSchedule", PERSISTENT},
|
||||
{"PreviousSpeedLimit", PERSISTENT},
|
||||
{"PromptVolume", PERSISTENT},
|
||||
{"PromptDistractedVolume", PERSISTENT},
|
||||
{"QOLControls", PERSISTENT},
|
||||
|
|
@ -341,16 +352,32 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||
{"SchedulePending", PERSISTENT},
|
||||
{"ScreenBrightness", PERSISTENT},
|
||||
{"SearchInput", PERSISTENT},
|
||||
{"SetSpeedLimit", PERSISTENT},
|
||||
{"SetSpeedOffset", PERSISTENT},
|
||||
{"SilentMode", PERSISTENT},
|
||||
{"ShowCPU", PERSISTENT},
|
||||
{"ShowGPU", PERSISTENT},
|
||||
{"ShowIP", PERSISTENT},
|
||||
{"ShowMemoryUsage", PERSISTENT},
|
||||
{"ShowSLCOffset", PERSISTENT},
|
||||
{"ShowSLCOffsetUI", PERSISTENT},
|
||||
{"ShowStorageLeft", PERSISTENT},
|
||||
{"ShowStorageUsed", PERSISTENT},
|
||||
{"Sidebar", PERSISTENT},
|
||||
{"SLCConfirmation", PERSISTENT},
|
||||
{"SLCConfirmationLower", PERSISTENT},
|
||||
{"SLCConfirmationHigher", PERSISTENT},
|
||||
{"SLCConfirmed", PERSISTENT},
|
||||
{"SLCConfirmedPressed", PERSISTENT},
|
||||
{"SLCFallback", PERSISTENT},
|
||||
{"SLCOverride", PERSISTENT},
|
||||
{"SLCPriority", PERSISTENT},
|
||||
{"SLCPriority1", PERSISTENT},
|
||||
{"SLCPriority2", PERSISTENT},
|
||||
{"SLCPriority3", PERSISTENT},
|
||||
{"SmoothBraking", PERSISTENT},
|
||||
{"SpeedLimitController", PERSISTENT},
|
||||
{"SpeedLimitChangedAlert", PERSISTENT},
|
||||
{"StandardFollow", PERSISTENT},
|
||||
{"StandardJerk", PERSISTENT},
|
||||
{"StockTune", PERSISTENT},
|
||||
|
|
@ -362,6 +389,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||
{"UpdateSchedule", PERSISTENT},
|
||||
{"UpdateTime", PERSISTENT},
|
||||
{"UseSI", PERSISTENT},
|
||||
{"UseVienna", PERSISTENT},
|
||||
{"WarningSoftVolume", PERSISTENT},
|
||||
{"WarningImmediateVolume", PERSISTENT},
|
||||
};
|
||||
|
|
|
|||
|
|
@ -562,3 +562,4 @@ selfdrive/frogpilot/functions/conditional_experimental_mode.py
|
|||
selfdrive/frogpilot/functions/frogpilot_functions.py
|
||||
selfdrive/frogpilot/functions/frogpilot_planner.py
|
||||
selfdrive/frogpilot/functions/map_turn_speed_controller.py
|
||||
selfdrive/frogpilot/functions/speed_limit_controller.py
|
||||
|
|
|
|||
|
|
@ -11,6 +11,8 @@ from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_G
|
|||
CANFD_CAR, Buttons, CarControllerParams
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
||||
|
||||
PREV_BUTTON_SAMPLES = 8
|
||||
CLUSTER_SAMPLE_RATE = 20 # frames
|
||||
STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS
|
||||
|
|
@ -55,6 +57,16 @@ class CarState(CarStateBase):
|
|||
# FrogPilot variables
|
||||
self.main_enabled = False
|
||||
|
||||
def calculate_speed_limit(self, cp, cp_cam):
|
||||
if self.CP.carFingerprint in CANFD_CAR:
|
||||
speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
|
||||
return speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
|
||||
else:
|
||||
if "SpeedLim_Nav_Clu" not in cp.vl["Navi_HU"]:
|
||||
return 0
|
||||
speed_limit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
|
||||
return speed_limit if speed_limit not in (0, 255) else 0
|
||||
|
||||
def update(self, cp, cp_cam, frogpilot_variables):
|
||||
if self.CP.carFingerprint in CANFD_CAR:
|
||||
return self.update_canfd(cp, cp_cam, frogpilot_variables)
|
||||
|
|
@ -177,6 +189,9 @@ class CarState(CarStateBase):
|
|||
self.fpf.lkas_button_function(frogpilot_variables.conditional_experimental_mode)
|
||||
self.lkas_previously_pressed = lkas_pressed
|
||||
|
||||
SpeedLimitController.car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_conv
|
||||
SpeedLimitController.write_car_state()
|
||||
|
||||
return ret
|
||||
|
||||
def update_canfd(self, cp, cp_cam, frogpilot_variables):
|
||||
|
|
@ -269,6 +284,9 @@ class CarState(CarStateBase):
|
|||
self.fpf.lkas_button_function(frogpilot_variables.conditional_experimental_mode)
|
||||
self.lkas_previously_pressed = lkas_pressed
|
||||
|
||||
SpeedLimitController.car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_factor
|
||||
SpeedLimitController.write_car_state()
|
||||
|
||||
return ret
|
||||
|
||||
def get_can_parser(self, CP):
|
||||
|
|
@ -321,6 +339,10 @@ class CarState(CarStateBase):
|
|||
if CP.flags & HyundaiFlags.CAN_LFA_BTN:
|
||||
messages.append(("BCM_PO_11", 50))
|
||||
|
||||
messages += [
|
||||
("Navi_HU", 5),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
|
||||
|
||||
@staticmethod
|
||||
|
|
@ -376,6 +398,9 @@ class CarState(CarStateBase):
|
|||
("SCC_CONTROL", 50),
|
||||
]
|
||||
|
||||
if CP.flags & HyundaiFlags.CANFD_HDA2:
|
||||
messages.append(("CLUSTER_SPEED_LIMIT", 10))
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN)
|
||||
|
||||
@staticmethod
|
||||
|
|
@ -389,4 +414,7 @@ class CarState(CarStateBase):
|
|||
("SCC_CONTROL", 50),
|
||||
]
|
||||
|
||||
if not (CP.flags & HyundaiFlags.CANFD_HDA2):
|
||||
messages.append(("CLUSTER_SPEED_LIMIT", 10))
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)
|
||||
|
|
|
|||
|
|
@ -11,6 +11,8 @@ from openpilot.selfdrive.car.interfaces import CarStateBase
|
|||
from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
|
||||
TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
||||
|
||||
SteerControlType = car.CarParams.SteerControlType
|
||||
|
||||
# These steering fault definitions seem to be common across LKA (torque) and LTA (angle):
|
||||
|
|
@ -46,6 +48,8 @@ class CarState(CarStateBase):
|
|||
|
||||
# FrogPilot variables
|
||||
|
||||
self.traffic_signals = {}
|
||||
|
||||
def update(self, cp, cp_cam, frogpilot_variables):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
|
|
@ -178,8 +182,31 @@ class CarState(CarStateBase):
|
|||
self.fpf.lkas_button_function(frogpilot_variables.conditional_experimental_mode)
|
||||
self.lkas_previously_pressed = lkas_pressed
|
||||
|
||||
# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team!
|
||||
self.update_traffic_signals(cp_cam)
|
||||
SpeedLimitController.car_speed_limit = self.calculate_speed_limit(frogpilot_variables)
|
||||
SpeedLimitController.write_car_state()
|
||||
|
||||
return ret
|
||||
|
||||
def update_traffic_signals(self, cp_cam):
|
||||
signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"]
|
||||
new_values = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals}
|
||||
|
||||
if new_values != self.traffic_signals:
|
||||
self.traffic_signals.update(new_values)
|
||||
|
||||
def calculate_speed_limit(self, frogpilot_variables):
|
||||
tsgn1 = self.traffic_signals.get("TSGN1", None)
|
||||
spdval1 = self.traffic_signals.get("SPDVAL1", None)
|
||||
|
||||
if tsgn1 == 1 and not frogpilot_variables.force_mph_dashboard:
|
||||
return spdval1 * CV.KPH_TO_MS
|
||||
elif tsgn1 == 36 or frogpilot_variables.force_mph_dashboard:
|
||||
return spdval1 * CV.MPH_TO_MS
|
||||
else:
|
||||
return 0
|
||||
|
||||
@staticmethod
|
||||
def get_can_parser(CP):
|
||||
messages = [
|
||||
|
|
@ -234,6 +261,11 @@ class CarState(CarStateBase):
|
|||
def get_cam_can_parser(CP):
|
||||
messages = []
|
||||
|
||||
messages += [
|
||||
("RSA1", 0),
|
||||
("RSA2", 0),
|
||||
]
|
||||
|
||||
if CP.carFingerprint != CAR.PRIUS_V:
|
||||
messages += [
|
||||
("LKAS_HUD", 1),
|
||||
|
|
|
|||
|
|
@ -38,6 +38,8 @@ from openpilot.system.version import get_short_branch
|
|||
|
||||
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
||||
|
||||
SOFT_DISABLE_TIME = 3 # seconds
|
||||
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
|
||||
LANE_DEPARTURE_THRESHOLD = 0.1
|
||||
|
|
@ -184,6 +186,8 @@ class Controls:
|
|||
self.stopped_for_light_previously = False
|
||||
|
||||
self.previous_lead_distance = 0
|
||||
self.previous_speed_limit = SpeedLimitController.desired_speed_limit
|
||||
self.speed_limit_changed_timer = 0
|
||||
|
||||
ignore = self.sensor_packets + ['testJoystick']
|
||||
if SIMULATION:
|
||||
|
|
@ -585,6 +589,47 @@ class Controls:
|
|||
if lead_departing:
|
||||
self.events.add(EventName.leadDeparting)
|
||||
|
||||
# Speed limit changed alert
|
||||
if self.speed_limit_alert or self.speed_limit_confirmation:
|
||||
current_speed_limit = frogpilot_plan.slcSpeedLimit
|
||||
desired_speed_limit = SpeedLimitController.desired_speed_limit
|
||||
|
||||
speed_limit_changed = desired_speed_limit != self.previous_speed_limit and abs(current_speed_limit - desired_speed_limit) > 1
|
||||
|
||||
speed_limit_changed_lower = speed_limit_changed and self.previous_speed_limit > desired_speed_limit
|
||||
speed_limit_changed_higher = speed_limit_changed and self.previous_speed_limit < desired_speed_limit
|
||||
|
||||
self.previous_speed_limit = desired_speed_limit
|
||||
|
||||
if speed_limit_changed_lower:
|
||||
if self.speed_limit_confirmation_lower:
|
||||
self.FPCC.speedLimitChanged = True
|
||||
else:
|
||||
self.params_memory.put_bool("SLCConfirmed", True)
|
||||
if speed_limit_changed_higher:
|
||||
if self.speed_limit_confirmation_higher:
|
||||
self.FPCC.speedLimitChanged = True
|
||||
else:
|
||||
self.params_memory.put_bool("SLCConfirmed", True)
|
||||
|
||||
if self.params_memory.get_bool("SLCConfirmedPressed") or current_speed_limit == desired_speed_limit or not self.speed_limit_confirmation:
|
||||
self.FPCC.speedLimitChanged = False
|
||||
self.params_memory.put_bool("SLCConfirmedPressed", False)
|
||||
|
||||
if speed_limit_changed and self.speed_limit_alert:
|
||||
self.events.add(EventName.speedLimitChanged)
|
||||
|
||||
# Cancel the confirmation message after 10 seconds
|
||||
if self.FPCC.speedLimitChanged:
|
||||
self.speed_limit_timer += 1
|
||||
if self.speed_limit_timer * DT_CTRL >= 10:
|
||||
self.FPCC.speedLimitChanged = False
|
||||
self.speed_limit_timer = 0
|
||||
else:
|
||||
self.speed_limit_timer = 0
|
||||
else:
|
||||
self.FPCC.speedLimitChanged = False
|
||||
|
||||
def data_sample(self):
|
||||
"""Receive data from sockets and update carState"""
|
||||
|
||||
|
|
@ -637,7 +682,7 @@ class Controls:
|
|||
def state_transition(self, CS):
|
||||
"""Compute conditional state transitions and execute actions on state transitions"""
|
||||
|
||||
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.frogpilot_variables)
|
||||
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.FPCC.speedLimitChanged, self.frogpilot_variables)
|
||||
|
||||
# decrement the soft disable timer at every step, as it's reset on
|
||||
# entrance in SOFT_DISABLING state
|
||||
|
|
@ -712,7 +757,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.frogpilot_variables)
|
||||
self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit, self.frogpilot_variables)
|
||||
|
||||
# Check if openpilot is engaged and actuators are enabled
|
||||
self.enabled = self.state in ENABLED_STATES
|
||||
|
|
@ -1048,7 +1093,7 @@ class Controls:
|
|||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if not self.frogpilot_variables.conditional_experimental_mode:
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode")
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode") or SpeedLimitController.experimental_mode
|
||||
else:
|
||||
self.experimental_mode = False
|
||||
if self.CP.notCar:
|
||||
|
|
@ -1104,6 +1149,14 @@ class Controls:
|
|||
self.frogpilot_variables.reverse_cruise_increase = quality_of_life and self.params.get_bool("ReverseCruise")
|
||||
self.frogpilot_variables.set_speed_offset = self.params.get_int("SetSpeedOffset") * (1 if self.is_metric else CV.MPH_TO_KPH) if quality_of_life else 0
|
||||
|
||||
self.speed_limit_controller = self.params.get_bool("SpeedLimitController")
|
||||
self.frogpilot_variables.force_mph_dashboard = self.speed_limit_controller and self.params.get_bool("ForceMPHDashboard")
|
||||
self.frogpilot_variables.set_speed_limit = self.speed_limit_controller and self.params.get_bool("SetSpeedLimit")
|
||||
self.speed_limit_alert = self.speed_limit_controller and self.params.get_bool("SpeedLimitChangedAlert")
|
||||
self.speed_limit_confirmation = self.speed_limit_controller and self.params.get_bool("SLCConfirmation")
|
||||
self.speed_limit_confirmation_lower = self.speed_limit_confirmation and self.params.get_bool("SLCConfirmationLower")
|
||||
self.speed_limit_confirmation_higher = self.speed_limit_confirmation and self.params.get_bool("SLCConfirmationHigher")
|
||||
|
||||
def main():
|
||||
controls = Controls()
|
||||
controls.controlsd_thread()
|
||||
|
|
|
|||
|
|
@ -47,18 +47,19 @@ class VCruiseHelper:
|
|||
self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
|
||||
|
||||
# FrogPilot variables
|
||||
self.params_memory = Params("/dev/shm/params")
|
||||
|
||||
@property
|
||||
def v_cruise_initialized(self):
|
||||
return self.v_cruise_kph != V_CRUISE_UNSET
|
||||
|
||||
def update_v_cruise(self, CS, enabled, is_metric, frogpilot_variables):
|
||||
def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_variables):
|
||||
self.v_cruise_kph_last = self.v_cruise_kph
|
||||
|
||||
if CS.cruiseState.available:
|
||||
if not self.CP.pcmCruise:
|
||||
# if stock cruise is completely disabled, then we can use our own set speed logic
|
||||
self._update_v_cruise_non_pcm(CS, enabled, is_metric, frogpilot_variables)
|
||||
self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, frogpilot_variables)
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
self.update_button_timers(CS, enabled)
|
||||
else:
|
||||
|
|
@ -68,7 +69,7 @@ class VCruiseHelper:
|
|||
self.v_cruise_kph = V_CRUISE_UNSET
|
||||
self.v_cruise_cluster_kph = V_CRUISE_UNSET
|
||||
|
||||
def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, frogpilot_variables):
|
||||
def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_variables):
|
||||
# handle button presses. TODO: this should be in state_control, but a decelCruise press
|
||||
# would have the effect of both enabling and changing speed is checked after the state transition
|
||||
if not enabled:
|
||||
|
|
@ -99,6 +100,17 @@ class VCruiseHelper:
|
|||
if button_type is None:
|
||||
return
|
||||
|
||||
# Confirm or deny the new speed limit value
|
||||
if speed_limit_changed:
|
||||
if button_type == ButtonType.accelCruise:
|
||||
self.params_memory.put_bool("SLCConfirmed", True);
|
||||
self.params_memory.put_bool("SLCConfirmedPressed", True);
|
||||
return
|
||||
elif button_type == ButtonType.decelCruise:
|
||||
self.params_memory.put_bool("SLCConfirmed", False);
|
||||
self.params_memory.put_bool("SLCConfirmedPressed", True);
|
||||
return
|
||||
|
||||
# Don't adjust speed when pressing resume to exit standstill
|
||||
cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill
|
||||
if button_type == ButtonType.accelCruise and cruise_standstill:
|
||||
|
|
@ -138,7 +150,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, frogpilot_variables) -> None:
|
||||
def initialize_v_cruise(self, CS, experimental_mode: bool, desired_speed_limit, frogpilot_variables) -> None:
|
||||
# initializing is handled by the PCM
|
||||
if self.CP.pcmCruise:
|
||||
return
|
||||
|
|
@ -152,11 +164,16 @@ class VCruiseHelper:
|
|||
if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250:
|
||||
self.v_cruise_kph = self.v_cruise_kph_last
|
||||
else:
|
||||
# Initial set speed
|
||||
if desired_speed_limit != 0 and frogpilot_variables.set_speed_limit:
|
||||
# If there's a known speed limit and the corresponding FP toggle is set, push it to the car
|
||||
self.v_cruise_kph = desired_speed_limit * CV.MS_TO_KPH
|
||||
else:
|
||||
# Use fixed initial set speed from mode etc.
|
||||
self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
|
||||
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
|
||||
|
||||
def apply_deadzone(error, deadzone):
|
||||
if error > deadzone:
|
||||
error -= deadzone
|
||||
|
|
|
|||
|
|
@ -1036,6 +1036,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
|
|||
Priority.HIGH, VisualAlert.wrongGear, AudibleAlert.promptRepeat, 4.),
|
||||
},
|
||||
|
||||
EventName.speedLimitChanged: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Speed Limit Changed",
|
||||
"",
|
||||
AlertStatus.frogpilot, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.),
|
||||
},
|
||||
|
||||
EventName.torqueNNLoad: {
|
||||
ET.PERMANENT: torque_nn_load_alert,
|
||||
},
|
||||
|
|
|
|||
|
|
@ -3,6 +3,8 @@ from openpilot.common.numpy_fast import interp
|
|||
|
||||
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, MovingAverageCalculator
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
||||
|
||||
CITY_SPEED_LIMIT = 25 # Speed limit for turn signal check
|
||||
PROBABILITY = 0.6 # 60% chance of condition being true
|
||||
|
||||
|
|
@ -76,6 +78,11 @@ class ConditionalExperimentalMode:
|
|||
self.status_value = 5 if frogpilotNavigation.approachingIntersection else 6
|
||||
return True
|
||||
|
||||
# Speed Limit Controller check
|
||||
if SpeedLimitController.experimental_mode:
|
||||
self.status_value = 7
|
||||
return True
|
||||
|
||||
# Speed check
|
||||
if (not self.lead_detected and v_ego <= self.limit) or (self.lead_detected and v_ego <= self.limit_lead):
|
||||
self.status_value = 8 if self.lead_detected else 9
|
||||
|
|
|
|||
|
|
@ -12,6 +12,7 @@ from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING
|
|||
|
||||
from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode
|
||||
from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController
|
||||
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
||||
|
||||
|
||||
class FrogPilotPlanner:
|
||||
|
|
@ -24,8 +25,12 @@ class FrogPilotPlanner:
|
|||
self.cem = ConditionalExperimentalMode(self.params_memory)
|
||||
self.mtsc = MapTurnSpeedController()
|
||||
|
||||
self.override_slc = False
|
||||
|
||||
self.overridden_speed = 0
|
||||
self.mtsc_target = 0
|
||||
self.road_curvature = 0
|
||||
self.slc_target = 0
|
||||
self.stop_distance = 0
|
||||
self.v_cruise = 0
|
||||
|
||||
|
|
@ -106,7 +111,39 @@ class FrogPilotPlanner:
|
|||
else:
|
||||
self.mtsc_target = v_cruise
|
||||
|
||||
targets = [self.mtsc_target]
|
||||
# Pfeiferj's Speed Limit Controller
|
||||
if self.speed_limit_controller:
|
||||
SpeedLimitController.update_speed_limits()
|
||||
unconfirmed_slc_target = SpeedLimitController.desired_speed_limit
|
||||
|
||||
# Check if the new speed limit has been confirmed by the user
|
||||
if self.speed_limit_confirmation:
|
||||
if self.params_memory.get_bool("SLCConfirmed") or self.slc_target == 0:
|
||||
self.slc_target = unconfirmed_slc_target
|
||||
self.params_memory.put_bool("SLCConfirmed", False)
|
||||
else:
|
||||
self.slc_target = unconfirmed_slc_target
|
||||
|
||||
# Override SLC upon gas pedal press and reset upon brake/cancel button
|
||||
self.override_slc &= self.overridden_speed > self.slc_target
|
||||
self.override_slc |= carState.gasPressed and v_ego > self.slc_target > CRUISING_SPEED
|
||||
self.override_slc &= enabled and not carState.standstill
|
||||
|
||||
# Use the override speed if SLC is being overridden
|
||||
if self.override_slc:
|
||||
if self.speed_limit_controller_override == 1:
|
||||
# Set the speed limit to the manual set speed
|
||||
if carState.gasPressed:
|
||||
self.overridden_speed = np.clip(v_ego + v_ego_diff, self.slc_target, v_cruise + v_cruise_diff)
|
||||
elif self.speed_limit_controller_override == 2:
|
||||
# Set the speed limit to the max set speed
|
||||
self.overridden_speed = v_cruise + v_cruise_diff
|
||||
else:
|
||||
self.overridden_speed = 0
|
||||
else:
|
||||
self.slc_target = v_cruise
|
||||
|
||||
targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff]
|
||||
filtered_targets = [target for target in targets if target > CRUISING_SPEED]
|
||||
|
||||
return (min(filtered_targets) if filtered_targets else v_cruise)
|
||||
|
|
@ -129,6 +166,12 @@ class FrogPilotPlanner:
|
|||
|
||||
frogpilotPlan.redLight = self.cem.red_light_detected
|
||||
|
||||
frogpilotPlan.slcOverridden = bool(self.override_slc)
|
||||
frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed)
|
||||
frogpilotPlan.slcSpeedLimit = float(self.slc_target)
|
||||
frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
|
||||
frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit
|
||||
|
||||
pm.send('frogpilotPlan', frogpilot_plan_send)
|
||||
|
||||
def update_frogpilot_params(self, params):
|
||||
|
|
@ -171,3 +214,8 @@ class FrogPilotPlanner:
|
|||
self.mtsc_curvature_check = params.get_bool("MTSCCurvatureCheck")
|
||||
self.mtsc_limit = params.get_float("MTSCLimit") * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS)
|
||||
self.params_memory.put_float("MapTargetLatA", 2 * (params.get_int("MTSCAggressiveness") / 100))
|
||||
|
||||
self.speed_limit_controller = params.get_bool("SpeedLimitController")
|
||||
if self.speed_limit_controller:
|
||||
self.speed_limit_confirmation = params.get_bool("SLCConfirmation")
|
||||
self.speed_limit_controller_override = params.get_int("SLCOverride")
|
||||
|
|
|
|||
|
|
@ -0,0 +1,114 @@
|
|||
import json
|
||||
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.params import Params
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED
|
||||
|
||||
|
||||
params = Params()
|
||||
params_memory = Params("/dev/shm/params")
|
||||
|
||||
class SpeedLimitController:
|
||||
car_speed_limit: float = 0 # m/s
|
||||
map_speed_limit: float = 0 # m/s
|
||||
nav_speed_limit: float = 0 # m/s
|
||||
prv_speed_limit: float = 0 # m/s
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.update_frogpilot_params()
|
||||
|
||||
def update_speed_limits(self):
|
||||
self.car_speed_limit = json.loads(params_memory.get("CarSpeedLimit") or '0')
|
||||
self.map_speed_limit = json.loads(params_memory.get("MapSpeedLimit") or '0')
|
||||
self.nav_speed_limit = json.loads(params_memory.get("NavSpeedLimit") or '0')
|
||||
|
||||
if params_memory.get_bool("FrogPilotTogglesUpdated"):
|
||||
self.update_frogpilot_params()
|
||||
|
||||
@property
|
||||
def offset(self) -> float:
|
||||
if self.speed_limit < 14:
|
||||
return self.offset1
|
||||
elif self.speed_limit < 24:
|
||||
return self.offset2
|
||||
elif self.speed_limit < 29:
|
||||
return self.offset3
|
||||
else:
|
||||
return self.offset4
|
||||
|
||||
@property
|
||||
def speed_limit(self) -> float:
|
||||
limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit]
|
||||
filtered_limits = [limit for limit in limits if limit > CRUISING_SPEED]
|
||||
|
||||
if self.highest and filtered_limits:
|
||||
return max(filtered_limits)
|
||||
elif self.lowest and filtered_limits:
|
||||
return min(filtered_limits)
|
||||
|
||||
speed_limits = {
|
||||
"Dashboard": self.car_speed_limit,
|
||||
"Navigation": self.nav_speed_limit,
|
||||
"Offline Maps": self.map_speed_limit,
|
||||
}
|
||||
|
||||
for priority in self.priorities:
|
||||
limit = speed_limits.get(priority)
|
||||
if limit and limit > CRUISING_SPEED:
|
||||
self.update_previous_limit(limit)
|
||||
return limit
|
||||
|
||||
if self.use_previous_limit:
|
||||
return self.prv_speed_limit
|
||||
|
||||
return 0
|
||||
|
||||
def update_previous_limit(self, new_limit: float):
|
||||
if self.prv_speed_limit != new_limit:
|
||||
params.put_float("PreviousSpeedLimit", new_limit)
|
||||
self.prv_speed_limit = new_limit
|
||||
|
||||
@property
|
||||
def desired_speed_limit(self) -> float:
|
||||
return self.speed_limit + self.offset if self.speed_limit != 0 else 0
|
||||
|
||||
@property
|
||||
def experimental_mode(self) -> bool:
|
||||
return self.speed_limit == 0 and self.use_experimental_mode
|
||||
|
||||
def write_car_state(self):
|
||||
params_memory.put("CarSpeedLimit", json.dumps(self.car_speed_limit))
|
||||
|
||||
def write_map_state(self):
|
||||
params_memory.put("MapSpeedLimit", json.dumps(self.map_speed_limit))
|
||||
|
||||
def write_nav_state(self):
|
||||
params_memory.put("NavSpeedLimit", json.dumps(self.nav_speed_limit))
|
||||
|
||||
def update_frogpilot_params(self):
|
||||
conversion = CV.KPH_TO_MS if params.get_bool("IsMetric") else CV.MPH_TO_MS
|
||||
|
||||
self.offset1 = params.get_int("Offset1") * conversion
|
||||
self.offset2 = params.get_int("Offset2") * conversion
|
||||
self.offset3 = params.get_int("Offset3") * conversion
|
||||
self.offset4 = params.get_int("Offset4") * conversion
|
||||
|
||||
speed_limit_priority1 = params.get("SLCPriority1", encoding='utf-8')
|
||||
|
||||
self.highest = speed_limit_priority1 == "Highest"
|
||||
self.lowest = speed_limit_priority1 == "Lowest"
|
||||
|
||||
self.priorities = [
|
||||
speed_limit_priority1,
|
||||
params.get("SLCPriority2", encoding='utf-8'),
|
||||
params.get("SLCPriority3", encoding='utf-8'),
|
||||
]
|
||||
|
||||
slc_fallback = params.get_int("SLCFallback")
|
||||
self.use_experimental_mode = slc_fallback == 1
|
||||
self.use_previous_limit = slc_fallback == 2
|
||||
|
||||
self.prv_speed_limit = params.get_float("PreviousSpeedLimit")
|
||||
|
||||
SpeedLimitController = SpeedLimitController()
|
||||
|
|
@ -53,6 +53,23 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
|
|||
{"PauseLateralOnSignal", "Pause Lateral On Turn Signal Below", "Temporarily disable lateral control during turn signal use below the set speed.", ""},
|
||||
{"ReverseCruise", "Reverse Cruise Increase", "Reverses the 'long press' functionality when increasing the max set speed. Useful to increase the max speed quickly.", ""},
|
||||
{"SetSpeedOffset", "Set Speed Offset", "Set an offset for your desired set speed.", ""},
|
||||
|
||||
{"SpeedLimitController", "Speed Limit Controller", "Automatically adjust vehicle speed to match speed limits using 'Open Street Map's, 'Navigate On openpilot', or your car's dashboard (TSS2 Toyotas only).", "../assets/offroad/icon_speed_limit.png"},
|
||||
{"SLCControls", "Controls Settings", "Manage settings for the controls.", ""},
|
||||
{"Offset1", "Speed Limit Offset (0-34 mph)", "Speed limit offset for speed limits between 0-34 mph.", ""},
|
||||
{"Offset2", "Speed Limit Offset (35-54 mph)", "Speed limit offset for speed limits between 35-54 mph.", ""},
|
||||
{"Offset3", "Speed Limit Offset (55-64 mph)", "Speed limit offset for speed limits between 55-64 mph.", ""},
|
||||
{"Offset4", "Speed Limit Offset (65-99 mph)", "Speed limit offset for speed limits between 65-99 mph.", ""},
|
||||
{"SLCFallback", "Fallback Method", "Choose your fallback method for when there are no speed limits currently being obtained from Navigation, OSM, and the car's dashboard.", ""},
|
||||
{"SLCOverride", "Override Method", "Choose your preferred method to override the current speed limit.", ""},
|
||||
{"SLCPriority", "Priority Order", "Determine the priority order for what speed limits to use.", ""},
|
||||
{"SLCQOL", "Quality of Life Settings", "Manage quality of life settings.", ""},
|
||||
{"SLCConfirmation", "Confirm New Speed Limits", "Don't automatically start using the new speed limit until it's been manually confirmed first.", ""},
|
||||
{"ForceMPHDashboard", "Force MPH From Dashboard Readings", "Force MPH readings from the dashboard. Only use this if you live in an area where the speed limits from your dashboard are in KPH but you use MPH.", ""},
|
||||
{"SetSpeedLimit", "Use Current Speed Limit As Set Speed", "Sets your max speed to the current speed limit if one is populated when you initially enable openpilot.", ""},
|
||||
{"SLCVisuals", "Visuals Settings", "Manage visual settings.", ""},
|
||||
{"ShowSLCOffset", "Show Speed Limit Offset", "Show the speed limit offset seperated from the speed limit in the onroad UI when using 'Speed Limit Controller'.", ""},
|
||||
{"UseVienna", "Use Vienna Speed Limit Signs", "Use the Vienna (EU) speed limit style signs as opposed to MUTCD (US).", ""},
|
||||
};
|
||||
|
||||
for (const auto &[param, title, desc, icon] : controlToggles) {
|
||||
|
|
@ -302,6 +319,116 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
|
|||
} else if (param == "LaneDetectionWidth") {
|
||||
toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 100, std::map<int, QString>(), this, false, " feet", 10);
|
||||
|
||||
} else if (param == "SpeedLimitController") {
|
||||
FrogPilotParamManageControl *speedLimitControllerToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
|
||||
QObject::connect(speedLimitControllerToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
|
||||
parentToggleClicked();
|
||||
for (auto &[key, toggle] : toggles) {
|
||||
toggle->setVisible(speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end());
|
||||
}
|
||||
});
|
||||
toggle = speedLimitControllerToggle;
|
||||
} else if (param == "SLCControls") {
|
||||
FrogPilotParamManageControl *manageSLCControlsToggle = new FrogPilotParamManageControl(param, title, desc, icon, this, true);
|
||||
QObject::connect(manageSLCControlsToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
|
||||
parentToggleClicked();
|
||||
for (auto &[key, toggle] : toggles) {
|
||||
toggle->setVisible(speedLimitControllerControlsKeys.find(key.c_str()) != speedLimitControllerControlsKeys.end());
|
||||
subParentToggleClicked();
|
||||
}
|
||||
slcPriorityButton->setVisible(true);
|
||||
});
|
||||
toggle = manageSLCControlsToggle;
|
||||
} else if (param == "SLCQOL") {
|
||||
FrogPilotParamManageControl *manageSLCQOLToggle = new FrogPilotParamManageControl(param, title, desc, icon, this, true);
|
||||
QObject::connect(manageSLCQOLToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
|
||||
parentToggleClicked();
|
||||
for (auto &[key, toggle] : toggles) {
|
||||
toggle->setVisible(speedLimitControllerQOLKeys.find(key.c_str()) != speedLimitControllerQOLKeys.end());
|
||||
subParentToggleClicked();
|
||||
}
|
||||
});
|
||||
toggle = manageSLCQOLToggle;
|
||||
} else if (param == "SLCConfirmation") {
|
||||
std::vector<QString> slcConfirmationToggles{"SLCConfirmationLower", "SLCConfirmationHigher"};
|
||||
std::vector<QString> slcConfirmationNames{tr("Lower Limits"), tr("Higher Limits")};
|
||||
toggle = new FrogPilotParamToggleControl(param, title, desc, icon, slcConfirmationToggles, slcConfirmationNames);
|
||||
} else if (param == "SLCVisuals") {
|
||||
FrogPilotParamManageControl *manageSLCVisualsToggle = new FrogPilotParamManageControl(param, title, desc, icon, this, true);
|
||||
QObject::connect(manageSLCVisualsToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
|
||||
parentToggleClicked();
|
||||
for (auto &[key, toggle] : toggles) {
|
||||
toggle->setVisible(speedLimitControllerVisualsKeys.find(key.c_str()) != speedLimitControllerVisualsKeys.end());
|
||||
subParentToggleClicked();
|
||||
}
|
||||
});
|
||||
toggle = manageSLCVisualsToggle;
|
||||
} else if (param == "Offset1" || param == "Offset2" || param == "Offset3" || param == "Offset4") {
|
||||
toggle = new FrogPilotParamValueControl(param, title, desc, icon, -99, 99, std::map<int, QString>(), this, false, " mph");
|
||||
} else if (param == "ShowSLCOffset") {
|
||||
std::vector<QString> slcOffsetToggles{"ShowSLCOffsetUI"};
|
||||
std::vector<QString> slcOffsetToggleNames{tr("Control Via UI")};
|
||||
toggle = new FrogPilotParamToggleControl(param, title, desc, icon, slcOffsetToggles, slcOffsetToggleNames);
|
||||
} else if (param == "SLCFallback") {
|
||||
std::vector<QString> fallbackOptions{tr("Set Speed"), tr("Experimental Mode"), tr("Previous Limit")};
|
||||
FrogPilotButtonParamControl *fallbackSelection = new FrogPilotButtonParamControl(param, title, desc, icon, fallbackOptions);
|
||||
toggle = fallbackSelection;
|
||||
} else if (param == "SLCOverride") {
|
||||
std::vector<QString> overrideOptions{tr("None"), tr("Manual Set Speed"), tr("Set Speed")};
|
||||
FrogPilotButtonParamControl *overrideSelection = new FrogPilotButtonParamControl(param, title, desc, icon, overrideOptions);
|
||||
toggle = overrideSelection;
|
||||
} else if (param == "SLCPriority") {
|
||||
slcPriorityButton = new ButtonControl(title, tr("SELECT"), desc);
|
||||
QStringList primaryPriorities = {"None", "Dashboard", "Navigation", "Offline Maps", "Highest", "Lowest"};
|
||||
QStringList secondaryTertiaryPriorities = {"None", "Dashboard", "Navigation", "Offline Maps"};
|
||||
QStringList priorityPrompts = {tr("Select your primary priority"), tr("Select your secondary priority"), tr("Select your tertiary priority")};
|
||||
|
||||
QObject::connect(slcPriorityButton, &ButtonControl::clicked, this, [this, primaryPriorities, secondaryTertiaryPriorities, priorityPrompts]() {
|
||||
QStringList availablePriorities = primaryPriorities;
|
||||
QStringList selectedPriorities;
|
||||
for (int i = 1; i <= 3; ++i) {
|
||||
QStringList currentPriorities = (i == 1) ? availablePriorities : secondaryTertiaryPriorities;
|
||||
|
||||
for (const QString &selectedPriority : selectedPriorities) {
|
||||
currentPriorities.removeAll(selectedPriority);
|
||||
}
|
||||
|
||||
QString priorityKey = QString("SLCPriority%1").arg(i);
|
||||
QString selection = MultiOptionDialog::getSelection(priorityPrompts[i - 1], currentPriorities, "", this);
|
||||
if (!selection.isEmpty()) {
|
||||
params.put(priorityKey.toStdString(), selection.toStdString());
|
||||
selectedPriorities.append(selection);
|
||||
|
||||
if (i == 1 && (selection == "Highest" || selection == "Lowest" || selection == "None")) {
|
||||
for (int j = i + 1; j <= 3; ++j) {
|
||||
QString remainingPriorityKeys = QString("SLCPriority%1").arg(j);
|
||||
params.putInt(remainingPriorityKeys.toStdString(), 0);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
updateToggles();
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
selectedPriorities.removeAll("None");
|
||||
slcPriorityButton->setValue(selectedPriorities.join(", "));
|
||||
});
|
||||
|
||||
QStringList initialPriorities;
|
||||
for (int i = 1; i <= 3; ++i) {
|
||||
QString priorityKey = QString("SLCPriority%1").arg(i);
|
||||
std::string priorityValue = params.get(priorityKey.toStdString());
|
||||
QString priorityString = QString::fromStdString(priorityValue);
|
||||
if (!priorityString.isEmpty() && primaryPriorities.contains(priorityString) && priorityString != "None") {
|
||||
initialPriorities.append(priorityString);
|
||||
}
|
||||
}
|
||||
slcPriorityButton->setValue(initialPriorities.join(", "));
|
||||
addItem(slcPriorityButton);
|
||||
|
||||
} else {
|
||||
toggle = new ParamControl(param, title, desc, icon, this);
|
||||
}
|
||||
|
|
@ -382,6 +509,10 @@ void FrogPilotControlsPanel::updateMetric() {
|
|||
params.putIntNonBlocking("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion));
|
||||
params.putIntNonBlocking("LaneDetectionWidth", std::nearbyint(params.getInt("LaneDetectionWidth") * distanceConversion));
|
||||
params.putIntNonBlocking("MTSCLimit", std::nearbyint(params.getInt("MTSCLimit") * speedConversion));
|
||||
params.putIntNonBlocking("Offset1", std::nearbyint(params.getInt("Offset1") * speedConversion));
|
||||
params.putIntNonBlocking("Offset2", std::nearbyint(params.getInt("Offset2") * speedConversion));
|
||||
params.putIntNonBlocking("Offset3", std::nearbyint(params.getInt("Offset3") * speedConversion));
|
||||
params.putIntNonBlocking("Offset4", std::nearbyint(params.getInt("Offset4") * speedConversion));
|
||||
params.putIntNonBlocking("PauseLateralOnSignal", std::nearbyint(params.getInt("PauseLateralOnSignal") * speedConversion));
|
||||
params.putIntNonBlocking("SetSpeedOffset", std::nearbyint(params.getInt("SetSpeedOffset") * speedConversion));
|
||||
params.putIntNonBlocking("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion));
|
||||
|
|
@ -389,22 +520,58 @@ void FrogPilotControlsPanel::updateMetric() {
|
|||
|
||||
FrogPilotParamValueControl *laneWidthToggle = static_cast<FrogPilotParamValueControl*>(toggles["LaneDetectionWidth"]);
|
||||
FrogPilotParamValueControl *mtscLimitToggle = static_cast<FrogPilotParamValueControl*>(toggles["MTSCLimit"]);
|
||||
FrogPilotParamValueControl *offset1Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset1"]);
|
||||
FrogPilotParamValueControl *offset2Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset2"]);
|
||||
FrogPilotParamValueControl *offset3Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset3"]);
|
||||
FrogPilotParamValueControl *offset4Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset4"]);
|
||||
FrogPilotParamValueControl *pauseLateralToggle = static_cast<FrogPilotParamValueControl*>(toggles["PauseLateralOnSignal"]);
|
||||
FrogPilotParamValueControl *setSpeedOffsetToggle = static_cast<FrogPilotParamValueControl*>(toggles["SetSpeedOffset"]);
|
||||
FrogPilotParamValueControl *stoppingDistanceToggle = static_cast<FrogPilotParamValueControl*>(toggles["StoppingDistance"]);
|
||||
|
||||
if (isMetric) {
|
||||
offset1Toggle->setTitle("Speed Limit Offset (0-34 kph)");
|
||||
offset2Toggle->setTitle("Speed Limit Offset (35-54 kph)");
|
||||
offset3Toggle->setTitle("Speed Limit Offset (55-64 kph)");
|
||||
offset4Toggle->setTitle("Speed Limit Offset (65-99 kph)");
|
||||
|
||||
offset1Toggle->setDescription("Set speed limit offset for limits between 0-34 kph.");
|
||||
offset2Toggle->setDescription("Set speed limit offset for limits between 35-54 kph.");
|
||||
offset3Toggle->setDescription("Set speed limit offset for limits between 55-64 kph.");
|
||||
offset4Toggle->setDescription("Set speed limit offset for limits between 65-99 kph.");
|
||||
|
||||
laneWidthToggle->updateControl(0, 30, " meters", 10);
|
||||
|
||||
mtscLimitToggle->updateControl(0, 99, " kph");
|
||||
|
||||
offset1Toggle->updateControl(-99, 99, " kph");
|
||||
offset2Toggle->updateControl(-99, 99, " kph");
|
||||
offset3Toggle->updateControl(-99, 99, " kph");
|
||||
offset4Toggle->updateControl(-99, 99, " kph");
|
||||
|
||||
pauseLateralToggle->updateControl(0, 150, " kph");
|
||||
setSpeedOffsetToggle->updateControl(0, 150, " kph");
|
||||
|
||||
stoppingDistanceToggle->updateControl(0, 5, " meters");
|
||||
} else {
|
||||
offset1Toggle->setTitle("Speed Limit Offset (0-34 mph)");
|
||||
offset2Toggle->setTitle("Speed Limit Offset (35-54 mph)");
|
||||
offset3Toggle->setTitle("Speed Limit Offset (55-64 mph)");
|
||||
offset4Toggle->setTitle("Speed Limit Offset (65-99 mph)");
|
||||
|
||||
offset1Toggle->setDescription("Set speed limit offset for limits between 0-34 mph.");
|
||||
offset2Toggle->setDescription("Set speed limit offset for limits between 35-54 mph.");
|
||||
offset3Toggle->setDescription("Set speed limit offset for limits between 55-64 mph.");
|
||||
offset4Toggle->setDescription("Set speed limit offset for limits between 65-99 mph.");
|
||||
|
||||
laneWidthToggle->updateControl(0, 100, " feet", 10);
|
||||
|
||||
mtscLimitToggle->updateControl(0, 99, " mph");
|
||||
|
||||
offset1Toggle->updateControl(-99, 99, " mph");
|
||||
offset2Toggle->updateControl(-99, 99, " mph");
|
||||
offset3Toggle->updateControl(-99, 99, " mph");
|
||||
offset4Toggle->updateControl(-99, 99, " mph");
|
||||
|
||||
pauseLateralToggle->updateControl(0, 99, " mph");
|
||||
setSpeedOffsetToggle->updateControl(0, 99, " mph");
|
||||
|
||||
|
|
@ -413,6 +580,10 @@ void FrogPilotControlsPanel::updateMetric() {
|
|||
|
||||
laneWidthToggle->refresh();
|
||||
mtscLimitToggle->refresh();
|
||||
offset1Toggle->refresh();
|
||||
offset2Toggle->refresh();
|
||||
offset3Toggle->refresh();
|
||||
offset4Toggle->refresh();
|
||||
pauseLateralToggle->refresh();
|
||||
setSpeedOffsetToggle->refresh();
|
||||
stoppingDistanceToggle->refresh();
|
||||
|
|
@ -425,6 +596,7 @@ void FrogPilotControlsPanel::parentToggleClicked() {
|
|||
conditionalSpeedsImperial->setVisible(false);
|
||||
conditionalSpeedsMetric->setVisible(false);
|
||||
modelSelectorButton->setVisible(false);
|
||||
slcPriorityButton->setVisible(false);
|
||||
standardProfile->setVisible(false);
|
||||
relaxedProfile->setVisible(false);
|
||||
|
||||
|
|
@ -440,6 +612,7 @@ void FrogPilotControlsPanel::hideSubToggles() {
|
|||
conditionalSpeedsImperial->setVisible(false);
|
||||
conditionalSpeedsMetric->setVisible(false);
|
||||
modelSelectorButton->setVisible(true);
|
||||
slcPriorityButton->setVisible(false);
|
||||
standardProfile->setVisible(false);
|
||||
relaxedProfile->setVisible(false);
|
||||
|
||||
|
|
@ -452,6 +625,9 @@ void FrogPilotControlsPanel::hideSubToggles() {
|
|||
mtscKeys.find(key.c_str()) != mtscKeys.end() ||
|
||||
qolKeys.find(key.c_str()) != qolKeys.end() ||
|
||||
speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end() ||
|
||||
speedLimitControllerControlsKeys.find(key.c_str()) != speedLimitControllerControlsKeys.end() ||
|
||||
speedLimitControllerQOLKeys.find(key.c_str()) != speedLimitControllerQOLKeys.end() ||
|
||||
speedLimitControllerVisualsKeys.find(key.c_str()) != speedLimitControllerVisualsKeys.end() ||
|
||||
visionTurnControlKeys.find(key.c_str()) != visionTurnControlKeys.end();
|
||||
toggle->setVisible(!subToggles);
|
||||
}
|
||||
|
|
@ -460,8 +636,10 @@ void FrogPilotControlsPanel::hideSubToggles() {
|
|||
}
|
||||
|
||||
void FrogPilotControlsPanel::hideSubSubToggles() {
|
||||
slcPriorityButton->setVisible(false);
|
||||
|
||||
for (auto &[key, toggle] : toggles) {
|
||||
bool isVisible = false;
|
||||
bool isVisible = speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end();
|
||||
toggle->setVisible(isVisible);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -29,6 +29,8 @@ private:
|
|||
void updateState(const UIState &s);
|
||||
void updateToggles();
|
||||
|
||||
ButtonControl *slcPriorityButton;
|
||||
|
||||
FrogPilotButtonIconControl *modelSelectorButton;
|
||||
|
||||
FrogPilotDualParamControl *aggressiveProfile;
|
||||
|
|
@ -44,10 +46,10 @@ private:
|
|||
std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"};
|
||||
std::set<QString> mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"};
|
||||
std::set<QString> qolKeys = {"DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset"};
|
||||
std::set<QString> speedLimitControllerKeys = {};
|
||||
std::set<QString> speedLimitControllerControlsKeys = {};
|
||||
std::set<QString> speedLimitControllerQOLKeys = {};
|
||||
std::set<QString> speedLimitControllerVisualsKeys = {};
|
||||
std::set<QString> speedLimitControllerKeys = {"SLCControls", "SLCQOL", "SLCVisuals"};
|
||||
std::set<QString> speedLimitControllerControlsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"};
|
||||
std::set<QString> speedLimitControllerQOLKeys = {"SLCConfirmation", "ForceMPHDashboard", "SetSpeedLimit"};
|
||||
std::set<QString> speedLimitControllerVisualsKeys = {"ShowSLCOffset", "UseVienna"};
|
||||
std::set<QString> visionTurnControlKeys = {};
|
||||
|
||||
std::map<std::string, ParamControl*> toggles;
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
|
|||
{"GreenLightAlert", "Green Light Alert", "Get an alert when a traffic light changes from red to green.", ""},
|
||||
{"LeadDepartingAlert", "Lead Departing Alert", "Get an alert when your lead vehicle starts departing when you're at a standstill.", ""},
|
||||
{"LoudBlindspotAlert", "Loud Blindspot Alert", "Enable a louder alert for when a vehicle is detected in your blindspot when attempting to change lanes.", ""},
|
||||
{"SpeedLimitChangedAlert", "Speed Limit Changed Alert", "Trigger an alert whenever the current speed limit changes.", ""},
|
||||
|
||||
{"CustomUI", "Custom Onroad UI", "Customize the Onroad UI with some additional visual functions.", "../assets/offroad/icon_road.png"},
|
||||
{"AccelerationPath", "Acceleration Path", "Visualize the car's intended acceleration or deceleration with a color-coded path.", ""},
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ private:
|
|||
void updateToggles();
|
||||
|
||||
std::set<QString> alertVolumeControlKeys = {"EngageVolume", "DisengageVolume", "RefuseVolume", "PromptVolume", "PromptDistractedVolume", "WarningSoftVolume", "WarningImmediateVolume"};
|
||||
std::set<QString> customAlertsKeys = {"GreenLightAlert", "LeadDepartingAlert", "LoudBlindspotAlert"};
|
||||
std::set<QString> customAlertsKeys = {"GreenLightAlert", "LeadDepartingAlert", "LoudBlindspotAlert", "SpeedLimitChangedAlert"};
|
||||
std::set<QString> customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "FPSCounter", "LeadInfo", "PedalsOnUI", "RoadNameUI"};
|
||||
std::set<QString> customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"};
|
||||
std::set<QString> modelUIKeys = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};
|
||||
|
|
|
|||
|
|
@ -320,7 +320,7 @@ def manager_cleanup() -> None:
|
|||
|
||||
|
||||
def update_frogpilot_params(params, params_memory):
|
||||
keys = ["DisableOnroadUploads", "FireTheBabysitter", "NoLogging", "NoUploads", "RoadNameUI"]
|
||||
keys = ["DisableOnroadUploads", "FireTheBabysitter", "NoLogging", "NoUploads", "RoadNameUI", "SpeedLimitController"]
|
||||
for key in keys:
|
||||
params_memory.put_bool(key, params.get_bool(key))
|
||||
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@ def enable_uploading(started, params, params_memory, CP: car.CarParams) -> bool:
|
|||
return not (params_memory.get_bool("FireTheBabysitter") and params_memory.get_bool("NoUploads"))
|
||||
|
||||
def osm(started, params, params_memory, CP: car.CarParams) -> bool:
|
||||
return params_memory.get_bool("RoadNameUI")
|
||||
return params_memory.get_bool("RoadNameUI") or params_memory.get_bool("SpeedLimitController")
|
||||
|
||||
procs = [
|
||||
DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"),
|
||||
|
|
|
|||
|
|
@ -18,6 +18,8 @@ from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
|
|||
parse_banner_instructions)
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
||||
|
||||
REROUTE_DISTANCE = 25
|
||||
MANEUVER_TRANSITION_THRESHOLD = 10
|
||||
REROUTE_COUNTER_MIN = 3
|
||||
|
|
@ -263,6 +265,11 @@ class RouteEngine:
|
|||
|
||||
if self.step_idx is None:
|
||||
msg.valid = False
|
||||
SpeedLimitController.nav_speed_limit = 0
|
||||
SpeedLimitController.write_nav_state()
|
||||
|
||||
if SpeedLimitController.desired_speed_limit != 0:
|
||||
msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
|
||||
self.pm.send('navInstruction', msg)
|
||||
return
|
||||
|
||||
|
|
@ -337,6 +344,14 @@ class RouteEngine:
|
|||
|
||||
if ('maxspeed' in closest.annotations) and self.localizer_valid:
|
||||
msg.navInstruction.speedLimit = closest.annotations['maxspeed']
|
||||
SpeedLimitController.nav_speed_limit = closest.annotations['maxspeed']
|
||||
SpeedLimitController.write_nav_state()
|
||||
if not self.localizer_valid or ('maxspeed' not in closest.annotations):
|
||||
SpeedLimitController.nav_speed_limit = 0
|
||||
SpeedLimitController.write_nav_state()
|
||||
|
||||
if SpeedLimitController.desired_speed_limit != 0:
|
||||
msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
|
||||
|
||||
# Speed limit sign type
|
||||
if 'speedLimitSign' in step:
|
||||
|
|
|
|||
|
|
@ -128,7 +128,18 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
|
|||
QRect hideSpeedRect(rect().center().x() - 175, 50, 350, 350);
|
||||
bool isSpeedClicked = hideSpeedRect.contains(e->pos());
|
||||
|
||||
if (isMaxSpeedClicked || isSpeedClicked) {
|
||||
// Speed limit confirmation buttons
|
||||
QSize size = this->size();
|
||||
QRect leftRect(0, 0, size.width() / 2, size.height());
|
||||
QRect rightRect = leftRect.translated(size.width() / 2, 0);
|
||||
bool isLeftSideClicked = leftRect.contains(e->pos()) && scene.speed_limit_changed;
|
||||
bool isRightSideClicked = rightRect.contains(e->pos()) && scene.speed_limit_changed;
|
||||
|
||||
// Speed limit offset button
|
||||
QRect speedLimitRect(7, 250, 225, 225);
|
||||
bool isSpeedLimitClicked = speedLimitRect.contains(e->pos());
|
||||
|
||||
if (isMaxSpeedClicked || isSpeedClicked || isSpeedLimitClicked) {
|
||||
if (isMaxSpeedClicked && scene.reverse_cruise_ui) {
|
||||
bool currentReverseCruise = scene.reverse_cruise;
|
||||
|
||||
|
|
@ -140,8 +151,19 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
|
|||
|
||||
uiState()->scene.hide_speed = !currentHideSpeed;
|
||||
params.putBoolNonBlocking("HideSpeed", !currentHideSpeed);
|
||||
} else if (isSpeedLimitClicked && scene.show_slc_offset_ui) {
|
||||
bool currentShowSLCOffset = scene.show_slc_offset;
|
||||
|
||||
uiState()->scene.show_slc_offset = !currentShowSLCOffset;
|
||||
params.putBoolNonBlocking("ShowSLCOffset", !currentShowSLCOffset);
|
||||
}
|
||||
|
||||
widgetClicked = true;
|
||||
} else if (isLeftSideClicked || isRightSideClicked) {
|
||||
bool slcConfirmed = isLeftSideClicked && !scene.right_hand_drive || isRightSideClicked && scene.right_hand_drive;
|
||||
paramsMemory.putBool("SLCConfirmed", slcConfirmed);
|
||||
paramsMemory.putBool("SLCConfirmedPressed", true);
|
||||
|
||||
widgetClicked = true;
|
||||
// If the click wasn't for anything specific, change the value of "ExperimentalMode"
|
||||
} else if (scene.experimental_mode_via_screen && e->pos() != timeoutPoint) {
|
||||
|
|
@ -481,11 +503,14 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
|
|||
speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
|
||||
|
||||
auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
|
||||
speedLimit = nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
|
||||
speedLimit = slcOverridden ? scene.speed_limit_overridden_speed : speedLimitController ? slcSpeedLimit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
|
||||
speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
|
||||
if (speedLimitController && !slcOverridden) {
|
||||
speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0);
|
||||
}
|
||||
|
||||
has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
|
||||
has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
|
||||
has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || (speedLimitController && !useViennaSLCSign);
|
||||
has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA) || (speedLimitController && useViennaSLCSign);
|
||||
is_metric = s.scene.is_metric;
|
||||
speedUnit = s.scene.is_metric ? tr("km/h") : tr("mph");
|
||||
hideBottomIcons = (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE || customSignals && (turnSignalLeft || turnSignalRight)) || fullMapOpen || showDriverCamera;
|
||||
|
|
@ -518,6 +543,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
|
|||
p.fillRect(0, 0, width(), UI_HEADER_HEIGHT, bg);
|
||||
|
||||
QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
|
||||
QString speedLimitOffsetStr = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : "");
|
||||
QString speedStr = QString::number(std::nearbyint(speed));
|
||||
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
|
||||
|
||||
|
|
@ -590,12 +616,24 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
|
|||
p.setPen(QPen(blackColor(), 6));
|
||||
p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
|
||||
|
||||
p.save();
|
||||
p.setOpacity(slcOverridden ? 0.25 : 1.0);
|
||||
if (showSLCOffset) {
|
||||
p.setFont(InterFont(28, QFont::DemiBold));
|
||||
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
|
||||
p.setFont(InterFont(70, QFont::Bold));
|
||||
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
|
||||
p.setFont(InterFont(50, QFont::DemiBold));
|
||||
p.drawText(sign_rect.adjusted(0, 120, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
|
||||
} else {
|
||||
p.setFont(InterFont(28, QFont::DemiBold));
|
||||
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
|
||||
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
|
||||
p.setFont(InterFont(70, QFont::Bold));
|
||||
p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
|
||||
}
|
||||
p.restore();
|
||||
}
|
||||
|
||||
// EU (Vienna style) sign
|
||||
if (has_eu_speed_limit) {
|
||||
|
|
@ -605,10 +643,20 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
|
|||
p.setPen(QPen(Qt::red, 20));
|
||||
p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16));
|
||||
|
||||
p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
|
||||
p.save();
|
||||
p.setOpacity(slcOverridden ? 0.25 : 1.0);
|
||||
p.setPen(blackColor());
|
||||
if (speedLimitController && showSLCOffset && !slcOverridden) {
|
||||
p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
|
||||
p.drawText(sign_rect.adjusted(0, -25, 0, 0), Qt::AlignCenter, speedLimitStr);
|
||||
p.setFont(InterFont(40, QFont::DemiBold));
|
||||
p.drawText(sign_rect.adjusted(0, 100, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
|
||||
} else {
|
||||
p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
|
||||
p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
|
||||
}
|
||||
p.restore();
|
||||
}
|
||||
|
||||
// current speed
|
||||
if (!(scene.hide_speed || fullMapOpen)) {
|
||||
|
|
@ -1136,10 +1184,19 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
|
|||
|
||||
pedalsOnUI = scene.pedals_on_ui;
|
||||
|
||||
rightHandDrive = scene.right_hand_drive;
|
||||
|
||||
roadNameUI = scene.road_name_ui;
|
||||
|
||||
showDriverCamera = scene.show_driver_camera;
|
||||
|
||||
speedLimitController = scene.speed_limit_controller;
|
||||
showSLCOffset = speedLimitController && scene.show_slc_offset;
|
||||
slcOverridden = speedLimitController && scene.speed_limit_overridden;
|
||||
slcSpeedLimit = scene.speed_limit;
|
||||
slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
|
||||
useViennaSLCSign = scene.use_vienna_slc_sign;
|
||||
|
||||
turnSignalLeft = scene.turn_signal_left;
|
||||
turnSignalRight = scene.turn_signal_right;
|
||||
|
||||
|
|
@ -1160,6 +1217,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
|
|||
} else if (animationTimer->isActive()) {
|
||||
animationTimer->stop();
|
||||
}
|
||||
|
||||
if (scene.speed_limit_changed) {
|
||||
drawSLCConfirmation(p);
|
||||
}
|
||||
}
|
||||
|
||||
bool enableCompass = compass && !hideBottomIcons;
|
||||
|
|
@ -1478,6 +1539,39 @@ void PedalIcons::paintEvent(QPaintEvent *event) {
|
|||
p.drawPixmap(gasX, (height() - img_size) / 2, gas_pedal_img);
|
||||
}
|
||||
|
||||
void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) {
|
||||
p.save();
|
||||
|
||||
// Get screen size
|
||||
QSize size = this->size();
|
||||
|
||||
// Configure the screen halves
|
||||
QRect leftRect(0, 0, size.width() / 2, size.height());
|
||||
QRect rightRect = leftRect.translated(size.width() / 2, 0);
|
||||
|
||||
// Fill in the screen halves
|
||||
p.setOpacity(0.5);
|
||||
p.fillRect(leftRect, rightHandDrive ? redColor() : greenColor());
|
||||
p.fillRect(rightRect, rightHandDrive ? greenColor() : redColor());
|
||||
p.setOpacity(1.0);
|
||||
|
||||
// Configure the font
|
||||
p.setFont(InterFont(75, QFont::Bold));
|
||||
p.setPen(Qt::white);
|
||||
|
||||
// Configure the text
|
||||
QString unitText = is_metric ? "kph" : "mph";
|
||||
QString speedText = QString::number(std::nearbyint(scene.unconfirmed_speed_limit * (is_metric ? MS_TO_KPH : MS_TO_MPH))) + " " + unitText;
|
||||
QString confirmText = "Confirm speed limit\n" + speedText;
|
||||
QString ignoreText = "Ignore speed limit\n" + speedText;
|
||||
|
||||
// Draw the text
|
||||
p.drawText(leftRect, Qt::AlignCenter | Qt::AlignTop, confirmText);
|
||||
p.drawText(rightRect, Qt::AlignCenter | Qt::AlignTop, ignoreText);
|
||||
|
||||
p.restore();
|
||||
}
|
||||
|
||||
void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
|
||||
p.save();
|
||||
|
||||
|
|
|
|||
|
|
@ -158,6 +158,7 @@ private:
|
|||
void updateFrogPilotWidgets(QPainter &p);
|
||||
|
||||
void drawLeadInfo(QPainter &p);
|
||||
void drawSLCConfirmation(QPainter &p);
|
||||
void drawStatusBar(QPainter &p);
|
||||
void drawTurnSignals(QPainter &p);
|
||||
|
||||
|
|
@ -183,15 +184,22 @@ private:
|
|||
bool leadInfo;
|
||||
bool mapOpen;
|
||||
bool pedalsOnUI;
|
||||
bool rightHandDrive;
|
||||
bool roadNameUI;
|
||||
bool showDriverCamera;
|
||||
bool showSLCOffset;
|
||||
bool slcOverridden;
|
||||
bool speedLimitController;
|
||||
bool turnSignalLeft;
|
||||
bool turnSignalRight;
|
||||
bool useViennaSLCSign;
|
||||
|
||||
float cruiseAdjustment;
|
||||
float distanceConversion;
|
||||
float laneWidthLeft;
|
||||
float laneWidthRight;
|
||||
float slcSpeedLimit;
|
||||
float slcSpeedLimitOffset;
|
||||
float speedConversion;
|
||||
|
||||
int cameraView;
|
||||
|
|
|
|||
|
|
@ -245,11 +245,18 @@ static void update_state(UIState *s) {
|
|||
scene.enabled = controlsState.getEnabled();
|
||||
scene.experimental_mode = controlsState.getExperimentalMode();
|
||||
}
|
||||
if (sm.updated("driverMonitoringState")) {
|
||||
auto driverMonitoringState = sm["driverMonitoringState"].getDriverMonitoringState();
|
||||
scene.right_hand_drive = driverMonitoringState.getIsRHD();
|
||||
}
|
||||
if (sm.updated("frogpilotCarControl")) {
|
||||
auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl();
|
||||
if (scene.always_on_lateral) {
|
||||
scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral();
|
||||
}
|
||||
if (scene.speed_limit_controller) {
|
||||
scene.speed_limit_changed = frogpilotCarControl.getSpeedLimitChanged();
|
||||
}
|
||||
}
|
||||
if (sm.updated("frogpilotPlan")) {
|
||||
auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
|
||||
|
|
@ -263,6 +270,13 @@ static void update_state(UIState *s) {
|
|||
scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock();
|
||||
scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor();
|
||||
}
|
||||
if (scene.speed_limit_controller) {
|
||||
scene.speed_limit = frogpilotPlan.getSlcSpeedLimit();
|
||||
scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset();
|
||||
scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden();
|
||||
scene.speed_limit_overridden_speed = frogpilotPlan.getSlcOverriddenSpeed();
|
||||
scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit();
|
||||
}
|
||||
scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
|
||||
}
|
||||
if (sm.updated("liveLocationKalman")) {
|
||||
|
|
@ -350,6 +364,11 @@ void ui_update_frogpilot_params(UIState *s) {
|
|||
|
||||
scene.rotating_wheel = params.getBool("RotatingWheel");
|
||||
scene.screen_brightness = params.getInt("ScreenBrightness");
|
||||
|
||||
scene.speed_limit_controller = params.getBool("SpeedLimitController");
|
||||
scene.show_slc_offset = scene.speed_limit_controller && params.getBool("ShowSLCOffset");
|
||||
scene.show_slc_offset_ui = scene.speed_limit_controller && params.getBool("ShowSLCOffsetUI");
|
||||
scene.use_vienna_slc_sign = scene.speed_limit_controller && params.getBool("UseVienna");
|
||||
}
|
||||
|
||||
void UIState::updateStatus() {
|
||||
|
|
|
|||
|
|
@ -204,15 +204,22 @@ typedef struct UIScene {
|
|||
bool pedals_on_ui;
|
||||
bool reverse_cruise;
|
||||
bool reverse_cruise_ui;
|
||||
bool right_hand_drive;
|
||||
bool road_name_ui;
|
||||
bool rotating_wheel;
|
||||
bool show_driver_camera;
|
||||
bool show_slc_offset;
|
||||
bool show_slc_offset_ui;
|
||||
bool speed_limit_changed;
|
||||
bool speed_limit_controller;
|
||||
bool speed_limit_overridden;
|
||||
bool standstill;
|
||||
bool tethering_enabled;
|
||||
bool turn_signal_left;
|
||||
bool turn_signal_right;
|
||||
bool unlimited_road_ui_length;
|
||||
bool use_si;
|
||||
bool use_vienna_slc_sign;
|
||||
|
||||
float acceleration;
|
||||
float adjusted_cruise;
|
||||
|
|
@ -222,6 +229,10 @@ typedef struct UIScene {
|
|||
float path_edge_width;
|
||||
float path_width;
|
||||
float road_edge_width;
|
||||
float speed_limit;
|
||||
float speed_limit_offset;
|
||||
float speed_limit_overridden_speed;
|
||||
float unconfirmed_speed_limit;
|
||||
|
||||
int bearing_deg;
|
||||
int camera_view;
|
||||
|
|
|
|||
Loading…
Reference in New Issue