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
e88916b93d
commit
accfb4759b
|
|
@ -125,6 +125,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
|||
noLaneAvailable @125;
|
||||
openpilotCrashed @126;
|
||||
pedalInterceptorNoBrake @128;
|
||||
speedLimitChanged @129;
|
||||
torqueNNLoad @130;
|
||||
|
||||
radarCanErrorDEPRECATED @15;
|
||||
|
|
|
|||
|
|
@ -33,6 +33,10 @@ struct FrogPilotPlan @0x80ae746ee2596b11 {
|
|||
redLight @5 :Bool;
|
||||
safeObstacleDistance @6 :Int16;
|
||||
safeObstacleDistanceStock @7 :Int16;
|
||||
slcOverridden @8 :Bool;
|
||||
slcOverriddenSpeed @9 :Float64;
|
||||
slcSpeedLimit @10 :Float64;
|
||||
slcSpeedLimitOffset @11 :Float32;
|
||||
stoppedEquivalenceFactor @12 :Int16;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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},
|
||||
|
|
@ -293,6 +294,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},
|
||||
|
|
@ -305,6 +308,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},
|
||||
|
|
@ -312,6 +317,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},
|
||||
|
|
@ -319,6 +328,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||
{"PathWidth", PERSISTENT},
|
||||
{"PauseLateralOnSignal", PERSISTENT},
|
||||
{"PreferredSchedule", PERSISTENT},
|
||||
{"PreviousSpeedLimit", PERSISTENT},
|
||||
{"PromptVolume", PERSISTENT},
|
||||
{"PromptDistractedVolume", PERSISTENT},
|
||||
{"QOLControls", PERSISTENT},
|
||||
|
|
@ -335,16 +345,27 @@ 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},
|
||||
{"SLCFallback", PERSISTENT},
|
||||
{"SLCOverride", PERSISTENT},
|
||||
{"SLCPriority", PERSISTENT},
|
||||
{"SLCPriority1", PERSISTENT},
|
||||
{"SLCPriority2", PERSISTENT},
|
||||
{"SLCPriority3", PERSISTENT},
|
||||
{"SmoothBraking", PERSISTENT},
|
||||
{"SpeedLimitController", PERSISTENT},
|
||||
{"SpeedLimitChangedAlert", PERSISTENT},
|
||||
{"StandardFollow", PERSISTENT},
|
||||
{"StandardJerk", PERSISTENT},
|
||||
{"StockTune", PERSISTENT},
|
||||
|
|
@ -355,6 +376,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||
{"UpdateSchedule", PERSISTENT},
|
||||
{"UpdateTime", PERSISTENT},
|
||||
{"UseSI", PERSISTENT},
|
||||
{"UseVienna", PERSISTENT},
|
||||
{"WarningSoftVolume", PERSISTENT},
|
||||
{"WarningImmediateVolume", PERSISTENT},
|
||||
};
|
||||
|
|
|
|||
|
|
@ -563,3 +563,4 @@ selfdrive/frogpilot/functions/frogpilot_functions.py
|
|||
selfdrive/frogpilot/functions/frogpilot_planner.py
|
||||
selfdrive/frogpilot/functions/map_turn_speed_controller.py
|
||||
selfdrive/frogpilot/functions/model_switcher.py
|
||||
selfdrive/frogpilot/functions/speed_limit_controller.py
|
||||
|
|
|
|||
|
|
@ -55,6 +55,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, conditional_experimental_mode, frogpilot_variables):
|
||||
if self.CP.carFingerprint in CANFD_CAR:
|
||||
return self.update_canfd(cp, cp_cam, conditional_experimental_mode, frogpilot_variables)
|
||||
|
|
@ -177,6 +187,10 @@ class CarState(CarStateBase):
|
|||
self.fpf.lkas_button_function(conditional_experimental_mode)
|
||||
self.lkas_previously_pressed = lkas_pressed
|
||||
|
||||
self.slc.load_state()
|
||||
self.slc.car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_conv
|
||||
self.slc.write_car_state()
|
||||
|
||||
return ret
|
||||
|
||||
def update_canfd(self, cp, cp_cam, conditional_experimental_mode, frogpilot_variables):
|
||||
|
|
@ -269,6 +283,10 @@ class CarState(CarStateBase):
|
|||
self.fpf.lkas_button_function(conditional_experimental_mode)
|
||||
self.lkas_previously_pressed = lkas_pressed
|
||||
|
||||
self.slc.load_state()
|
||||
self.slc.car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_factor
|
||||
self.slc.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)
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@ 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
|
||||
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
|
@ -497,6 +498,7 @@ class CarStateBase(ABC):
|
|||
|
||||
# FrogPilot variables
|
||||
self.fpf = FrogPilotFunctions()
|
||||
self.slc = SpeedLimitController
|
||||
|
||||
self.lkas_previously_pressed = False
|
||||
|
||||
|
|
|
|||
|
|
@ -46,6 +46,8 @@ class CarState(CarStateBase):
|
|||
|
||||
# FrogPilot variables
|
||||
|
||||
self.traffic_signals = {}
|
||||
|
||||
def update(self, cp, cp_cam, conditional_experimental_mode, frogpilot_variables):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
|
|
@ -172,8 +174,32 @@ class CarState(CarStateBase):
|
|||
self.fpf.lkas_button_function(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)
|
||||
self.slc.load_state()
|
||||
self.slc.car_speed_limit = self.calculate_speed_limit()
|
||||
self.slc.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):
|
||||
tsgn1 = self.traffic_signals.get("TSGN1", None)
|
||||
spdval1 = self.traffic_signals.get("SPDVAL1", None)
|
||||
|
||||
if tsgn1 == 1:
|
||||
return spdval1 * CV.KPH_TO_MS
|
||||
elif tsgn1 == 36:
|
||||
return spdval1 * CV.MPH_TO_MS
|
||||
else:
|
||||
return 0
|
||||
|
||||
@staticmethod
|
||||
def get_can_parser(CP):
|
||||
messages = [
|
||||
|
|
@ -225,6 +251,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),
|
||||
|
|
|
|||
|
|
@ -31,6 +31,8 @@ from openpilot.system.hardware import HARDWARE
|
|||
|
||||
import openpilot.selfdrive.sentry as sentry
|
||||
|
||||
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
|
||||
|
|
@ -90,6 +92,7 @@ class Controls:
|
|||
self.stopped_for_light_previously = False
|
||||
|
||||
self.previous_lead_distance = 0
|
||||
self.previous_speed_limit = 0
|
||||
|
||||
ignore = self.sensor_packets + ['testJoystick']
|
||||
if SIMULATION:
|
||||
|
|
@ -487,6 +490,15 @@ class Controls:
|
|||
if lead_departing and lead.vLead > 1 and not CS.gasPressed and CS.standstill:
|
||||
self.events.add(EventName.leadDeparting)
|
||||
|
||||
# Speed limit changed alert
|
||||
if self.speed_limit_alert:
|
||||
speed_limit = SpeedLimitController.desired_speed_limit
|
||||
speed_limit_changed = abs(speed_limit - self.previous_speed_limit) > 1 and self.previous_speed_limit != 0
|
||||
self.previous_speed_limit = speed_limit
|
||||
|
||||
if speed_limit_changed:
|
||||
self.events.add(EventName.speedLimitChanged)
|
||||
|
||||
def data_sample(self):
|
||||
"""Receive data from sockets and update carState"""
|
||||
|
||||
|
|
@ -965,7 +977,7 @@ class Controls:
|
|||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if not self.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:
|
||||
|
|
@ -996,6 +1008,7 @@ class Controls:
|
|||
self.green_light_alert = self.params.get_bool("GreenLightAlert") and custom_alerts
|
||||
self.lead_departing_alert = self.params.get_bool("LeadDepartingAlert") and custom_alerts
|
||||
self.loud_blindspot_alert = self.params.get_bool("LoudBlindspotAlert") and custom_alerts
|
||||
self.speed_limit_alert = self.params.get_bool("SpeedLimitChangedAlert") and self.params.get_bool("SpeedLimitController") and custom_alerts
|
||||
|
||||
custom_theme = self.params.get_bool("CustomTheme")
|
||||
custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0
|
||||
|
|
@ -1019,6 +1032,7 @@ class Controls:
|
|||
quality_of_life = self.params.get_bool("QOLControls")
|
||||
self.pause_lateral_on_signal = self.params.get_int("PauseLateralOnSignal") * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS) if quality_of_life else 0
|
||||
self.frogpilot_variables.reverse_cruise_increase = self.params.get_bool("ReverseCruise") and quality_of_life
|
||||
self.frogpilot_variables.set_speed_limit = self.params.get_bool("SetSpeedLimit") and quality_of_life
|
||||
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
|
||||
|
||||
def main():
|
||||
|
|
|
|||
|
|
@ -5,6 +5,8 @@ from openpilot.common.conversions import Conversions as CV
|
|||
from openpilot.common.numpy_fast import clip, interp
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
||||
|
||||
# WARNING: this value was determined based on the model's training distribution,
|
||||
# model predictions above this speed can be unpredictable
|
||||
# V_CRUISE's are in kph
|
||||
|
|
@ -149,11 +151,13 @@ 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:
|
||||
self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
|
||||
if SpeedLimitController.desired_speed_limit != 0 and frogpilot_variables.set_speed_limit:
|
||||
self.v_cruise_kph = SpeedLimitController.desired_speed_limit * CV.MS_TO_KPH
|
||||
else:
|
||||
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
|
||||
|
|
|
|||
|
|
@ -1037,6 +1037,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,
|
||||
},
|
||||
|
|
|
|||
|
|
@ -4,6 +4,8 @@ from openpilot.common.conversions import Conversions as CV
|
|||
from openpilot.common.numpy_fast import interp
|
||||
from openpilot.common.params import Params
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
||||
|
||||
# Constants
|
||||
PROBABILITY = 0.6 # 60% chance of condition being true
|
||||
THRESHOLD = 5 # Time threshold (0.25s)
|
||||
|
|
@ -99,6 +101,11 @@ class ConditionalExperimentalMode:
|
|||
self.status_value = 5
|
||||
return True
|
||||
|
||||
# Speed Limit Controller check
|
||||
if SpeedLimitController.experimental_mode:
|
||||
self.status_value = 6
|
||||
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 = 7 if self.lead_detected else 8
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@ from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import FrogPilo
|
|||
|
||||
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:
|
||||
def __init__(self, params, params_memory):
|
||||
|
|
@ -17,8 +18,12 @@ class FrogPilotPlanner:
|
|||
self.fpf = FrogPilotFunctions()
|
||||
self.mtsc = MapTurnSpeedController()
|
||||
|
||||
self.override_slc = False
|
||||
|
||||
self.overridden_speed = 0
|
||||
self.mtsc_target = 0
|
||||
self.road_curvature = 0
|
||||
self.slc_target = 0
|
||||
self.v_cruise = 0
|
||||
|
||||
self.accel_limits = [A_CRUISE_MIN, get_max_accel(0)]
|
||||
|
|
@ -73,8 +78,40 @@ class FrogPilotPlanner:
|
|||
else:
|
||||
self.mtsc_target = v_cruise
|
||||
|
||||
# Pfeiferj's Speed Limit Controller
|
||||
if self.speed_limit_controller:
|
||||
SpeedLimitController.update_current_max_velocity(v_cruise)
|
||||
self.slc_target = SpeedLimitController.desired_speed_limit
|
||||
|
||||
# Override SLC upon gas pedal press and reset upon brake/cancel button
|
||||
if self.speed_limit_controller_override:
|
||||
self.override_slc |= carState.gasPressed
|
||||
self.override_slc &= enabled
|
||||
self.override_slc &= v_ego > self.slc_target
|
||||
else:
|
||||
self.override_slc = False
|
||||
|
||||
self.overridden_speed *= enabled
|
||||
|
||||
# Use the override speed if SLC is being overridden
|
||||
if self.override_slc:
|
||||
if self.speed_limit_controller_override == 1:
|
||||
# Set the max speed to the manual set speed
|
||||
if carState.gasPressed:
|
||||
self.overridden_speed = np.clip(v_ego, self.slc_target, v_cruise)
|
||||
self.slc_target = self.overridden_speed
|
||||
elif self.speed_limit_controller_override == 2:
|
||||
self.overridden_speed = v_cruise
|
||||
self.slc_target = v_cruise
|
||||
|
||||
if self.slc_target == 0:
|
||||
self.slc_target = v_cruise
|
||||
else:
|
||||
self.overriden_speed = 0
|
||||
self.slc_target = v_cruise
|
||||
|
||||
v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
|
||||
return min(v_cruise, self.mtsc_target) - v_ego_diff
|
||||
return min(v_cruise, self.mtsc_target, self.slc_target) - v_ego_diff
|
||||
|
||||
def publish(self, sm, pm, mpc):
|
||||
frogpilot_plan_send = messaging.new_message('frogpilotPlan')
|
||||
|
|
@ -94,6 +131,11 @@ class FrogPilotPlanner:
|
|||
|
||||
frogpilotPlan.redLight = self.cem.red_light_detected
|
||||
|
||||
frogpilotPlan.slcOverridden = self.override_slc
|
||||
frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed)
|
||||
frogpilotPlan.slcSpeedLimit = float(self.slc_target)
|
||||
frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
|
||||
|
||||
pm.send('frogpilotPlan', frogpilot_plan_send)
|
||||
|
||||
def update_frogpilot_params(self, params, params_memory):
|
||||
|
|
@ -129,3 +171,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)
|
||||
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_controller_override = params.get_int("SLCOverride")
|
||||
SpeedLimitController.update_frogpilot_params()
|
||||
|
|
|
|||
|
|
@ -0,0 +1,107 @@
|
|||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.params import Params
|
||||
import json
|
||||
|
||||
params = Params()
|
||||
params_memory = Params("/dev/shm/params")
|
||||
|
||||
class SpeedLimitController:
|
||||
car_speed_limit: float = 0 # m/s
|
||||
lst_speed_limit: float = 0 # m/s
|
||||
map_speed_limit: float = 0 # m/s
|
||||
nav_speed_limit: float = 0 # m/s
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.update_frogpilot_params()
|
||||
|
||||
self.write_car_state()
|
||||
self.write_map_state()
|
||||
self.write_nav_state()
|
||||
|
||||
def update_current_max_velocity(self, max_v: float, load_state: bool = True, write_state: bool = True) -> None:
|
||||
if load_state:
|
||||
self.load_state()
|
||||
|
||||
@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 > 0]
|
||||
|
||||
if self.highest and filtered_limits:
|
||||
return max(filtered_limits)
|
||||
if self.lowest and filtered_limits:
|
||||
return min(filtered_limits)
|
||||
|
||||
priority_map = {1: "car", 2: "nav", 3: "map"}
|
||||
priorities = [self.speed_limit_priority1, self.speed_limit_priority2, self.speed_limit_priority3]
|
||||
valid_priorities = [priority_map[p] for p in priorities if p in priority_map]
|
||||
|
||||
for source in valid_priorities:
|
||||
limit = getattr(self, f"{source}_speed_limit", 0)
|
||||
if limit > 0:
|
||||
self.prv_speed_limit = limit
|
||||
return limit
|
||||
|
||||
if self.use_previous_limit:
|
||||
if self.lst_speed_limit != self.prv_speed_limit:
|
||||
params.put_float("PreviousSpeedLimit", self.prv_speed_limit)
|
||||
self.lst_speed_limit = self.prv_speed_limit
|
||||
return self.prv_speed_limit
|
||||
|
||||
return 0
|
||||
|
||||
@property
|
||||
def desired_speed_limit(self):
|
||||
return self.speed_limit + self.offset if self.speed_limit else 0
|
||||
|
||||
@property
|
||||
def experimental_mode(self):
|
||||
return self.speed_limit == 0 and self.use_experimental_mode
|
||||
|
||||
def load_state(self):
|
||||
self.car_speed_limit = json.loads(params_memory.get("CarSpeedLimit"))
|
||||
self.map_speed_limit = json.loads(params_memory.get("MapSpeedLimit"))
|
||||
self.nav_speed_limit = json.loads(params_memory.get("NavSpeedLimit"))
|
||||
|
||||
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
|
||||
|
||||
self.speed_limit_priority1 = params.get_int("SLCPriority1")
|
||||
self.speed_limit_priority2 = params.get_int("SLCPriority2")
|
||||
self.speed_limit_priority3 = params.get_int("SLCPriority3")
|
||||
|
||||
self.highest = self.speed_limit_priority1 == 4
|
||||
self.lowest = self.speed_limit_priority1 == 5
|
||||
|
||||
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()
|
||||
|
|
@ -50,7 +50,16 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
|
|||
{"NavChill", "Navigate on Chill Mode", "Allows cars without longitudinal support to navigate. Allows navigation without experimental mode.", ""},
|
||||
{"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.", ""},
|
||||
{"SetSpeedLimit", "Set Speed to Current Speed Limit", "Sets your max speed to the current speed limit if one is populated when you initially enable openpilot.", ""},
|
||||
{"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"},
|
||||
{"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.", ""},
|
||||
};
|
||||
|
||||
for (const auto &[param, title, desc, icon] : controlToggles) {
|
||||
|
|
@ -288,6 +297,78 @@ 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());
|
||||
}
|
||||
slscPriorityButton->setVisible(true);
|
||||
});
|
||||
toggle = speedLimitControllerToggle;
|
||||
} else if (param == "Offset1" || param == "Offset2" || param == "Offset3" || param == "Offset4") {
|
||||
toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map<int, QString>(), this, false, " mph");
|
||||
} else if (param == "SLCFallback") {
|
||||
std::vector<QString> fallbackOptions{tr("None"), 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("Max Set Speed")};
|
||||
FrogPilotButtonParamControl *overrideSelection = new FrogPilotButtonParamControl(param, title, desc, icon, overrideOptions);
|
||||
toggle = overrideSelection;
|
||||
|
||||
slscPriorityButton = new ButtonControl("Priority Order", tr("SELECT"), "Determine the priority order for what speed limits to use.");
|
||||
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(slscPriorityButton, &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()) {
|
||||
if (selection == "None" || (i == 1 && (selection == "Highest" || selection == "Lowest"))) {
|
||||
for (int j = i; j <= 3; ++j) {
|
||||
QString specialPriorityKey = QString("SLCPriority%1").arg(j);
|
||||
params.putInt(specialPriorityKey.toStdString(), primaryPriorities.indexOf(selection));
|
||||
}
|
||||
selectedPriorities.append(selection);
|
||||
break;
|
||||
}
|
||||
|
||||
int selectedPriority = primaryPriorities.indexOf(selection);
|
||||
params.putInt(priorityKey.toStdString(), selectedPriority);
|
||||
selectedPriorities.append(selection);
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
selectedPriorities.removeAll("None");
|
||||
slscPriorityButton->setValue(selectedPriorities.join(", "));
|
||||
});
|
||||
|
||||
QStringList initialPriorities;
|
||||
for (int i = 1; i <= 3; ++i) {
|
||||
QString priorityKey = QString("SLCPriority%1").arg(i);
|
||||
int priorityIndex = params.getInt(priorityKey.toStdString());
|
||||
if (priorityIndex >= 0 && priorityIndex < primaryPriorities.size() && primaryPriorities[priorityIndex] != "None") {
|
||||
initialPriorities.append(primaryPriorities[priorityIndex]);
|
||||
}
|
||||
}
|
||||
slscPriorityButton->setValue(initialPriorities.join(", "));
|
||||
addItem(slscPriorityButton);
|
||||
|
||||
} else {
|
||||
toggle = new ParamControl(param, title, desc, icon, this);
|
||||
}
|
||||
|
|
@ -359,6 +440,10 @@ void FrogPilotControlsPanel::updateMetric() {
|
|||
params.putIntNonBlocking("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion));
|
||||
params.putIntNonBlocking("MTSCLimit", std::nearbyint(params.getInt("MTSCLimit") * speedConversion));
|
||||
params.putIntNonBlocking("LaneDetectionWidth", std::nearbyint(params.getInt("LaneDetectionWidth") * distanceConversion));
|
||||
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));
|
||||
|
|
@ -366,19 +451,55 @@ 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(0, 99, " kph");
|
||||
offset2Toggle->updateControl(0, 99, " kph");
|
||||
offset3Toggle->updateControl(0, 99, " kph");
|
||||
offset4Toggle->updateControl(0, 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(0, 99, " mph");
|
||||
offset2Toggle->updateControl(0, 99, " mph");
|
||||
offset3Toggle->updateControl(0, 99, " mph");
|
||||
offset4Toggle->updateControl(0, 99, " mph");
|
||||
|
||||
pauseLateralToggle->updateControl(0, 99, " mph");
|
||||
setSpeedOffsetToggle->updateControl(0, 99, " mph");
|
||||
stoppingDistanceToggle->updateControl(0, 10, " feet");
|
||||
|
|
@ -386,6 +507,10 @@ void FrogPilotControlsPanel::updateMetric() {
|
|||
|
||||
laneWidthToggle->refresh();
|
||||
mtscLimitToggle->refresh();
|
||||
offset1Toggle->refresh();
|
||||
offset2Toggle->refresh();
|
||||
offset3Toggle->refresh();
|
||||
offset4Toggle->refresh();
|
||||
pauseLateralToggle->refresh();
|
||||
setSpeedOffsetToggle->refresh();
|
||||
stoppingDistanceToggle->refresh();
|
||||
|
|
@ -398,6 +523,7 @@ void FrogPilotControlsPanel::parentToggleClicked() {
|
|||
conditionalSpeedsImperial->setVisible(false);
|
||||
conditionalSpeedsMetric->setVisible(false);
|
||||
modelSelectorButton->setVisible(false);
|
||||
slscPriorityButton->setVisible(false);
|
||||
standardProfile->setVisible(false);
|
||||
relaxedProfile->setVisible(false);
|
||||
|
||||
|
|
@ -409,6 +535,7 @@ void FrogPilotControlsPanel::hideSubToggles() {
|
|||
conditionalSpeedsImperial->setVisible(false);
|
||||
conditionalSpeedsMetric->setVisible(false);
|
||||
modelSelectorButton->setVisible(true);
|
||||
slscPriorityButton->setVisible(false);
|
||||
standardProfile->setVisible(false);
|
||||
relaxedProfile->setVisible(false);
|
||||
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@ private:
|
|||
void updateState(const UIState &s);
|
||||
void updateToggles();
|
||||
|
||||
ButtonControl *slscPriorityButton;
|
||||
|
||||
FrogPilotButtonIconControl *modelSelectorButton;
|
||||
|
||||
FrogPilotDualParamControl *aggressiveProfile;
|
||||
|
|
@ -39,8 +41,8 @@ private:
|
|||
std::set<QString> lateralTuneKeys = {"ForceAutoTune", "NNFF"};
|
||||
std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "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> qolKeys = {"DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedLimit", "SetSpeedOffset"};
|
||||
std::set<QString> speedLimitControllerKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"};
|
||||
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.", ""},
|
||||
|
|
@ -32,6 +33,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
|
|||
{"FPSCounter", "FPS Counter", "Display the Frames Per Second (FPS) of your onroad UI for monitoring system performance.", ""},
|
||||
{"LeadInfo", "Lead Info and Logics", "Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance.", ""},
|
||||
{"RoadNameUI", "Road Name", "See the name of the road you're on at the bottom of your screen. Sourced from OpenStreetMap.", ""},
|
||||
{"UseVienna", "Use Vienna Speed Limit Signs", "Use the Vienna (EU) speed limit style signs as opposed to MUTCD (US).", ""},
|
||||
|
||||
{"DriverCamera", "Driver Camera On Reverse", "Show the driver's camera feed when you shift to reverse.", "../assets/img_driver_face_static.png"},
|
||||
|
||||
|
|
@ -48,6 +50,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
|
|||
{"DriveStats", "Drive Stats In Home Screen", "Display your device's drive stats in the home screen.", ""},
|
||||
{"FullMap", "Full Sized Map", "Maximize the size of the map in the onroad UI.", ""},
|
||||
{"HideSpeed", "Hide Speed", "Hide the speed indicator in the onroad UI. Additional toggle allows it to be hidden/shown via tapping the speed itself.", ""},
|
||||
{"ShowSLCOffset", "Show Speed Limit Offset", "Show the speed limit offset seperated from the speed limit in the onroad UI when using 'Speed Limit Controller'.", ""},
|
||||
|
||||
{"ScreenBrightness", "Screen Brightness", "Customize your screen brightness.", "../frogpilot/assets/toggle_icons/icon_light.png"},
|
||||
};
|
||||
|
|
@ -162,6 +165,10 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
|
|||
std::vector<QString> hideSpeedToggles{"HideSpeedUI"};
|
||||
std::vector<QString> hideSpeedToggleNames{tr("Control Via UI")};
|
||||
toggle = new FrogPilotParamToggleControl(param, title, desc, icon, hideSpeedToggles, hideSpeedToggleNames);
|
||||
} 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 == "ScreenBrightness") {
|
||||
std::map<int, QString> brightnessLabels;
|
||||
|
|
|
|||
|
|
@ -25,11 +25,11 @@ private:
|
|||
void updateToggles();
|
||||
|
||||
std::set<QString> alertVolumeControlKeys = {"EngageVolume", "DisengageVolume", "RefuseVolume", "PromptVolume", "PromptDistractedVolume", "WarningSoftVolume", "WarningImmediateVolume"};
|
||||
std::set<QString> customAlertsKeys = {"GreenLightAlert", "LeadDepartingAlert", "LoudBlindspotAlert"};
|
||||
std::set<QString> customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "FPSCounter", "LeadInfo", "RoadNameUI"};
|
||||
std::set<QString> customAlertsKeys = {"GreenLightAlert", "LeadDepartingAlert", "LoudBlindspotAlert", "SpeedLimitChangedAlert"};
|
||||
std::set<QString> customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "FPSCounter", "LeadInfo", "RoadNameUI", "UseVienna"};
|
||||
std::set<QString> customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"};
|
||||
std::set<QString> modelUIKeys = {"LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};
|
||||
std::set<QString> qolKeys = {"DriveStats", "FullMap", "HideSpeed"};
|
||||
std::set<QString> qolKeys = {"DriveStats", "FullMap", "HideSpeed", "ShowSLCOffset"};
|
||||
|
||||
std::map<std::string, ParamControl*> toggles;
|
||||
|
||||
|
|
|
|||
|
|
@ -317,7 +317,7 @@ def manager_cleanup() -> None:
|
|||
|
||||
|
||||
def update_frogpilot_params(started, params, params_memory):
|
||||
keys = ["DisableOnroadUploads", "FireTheBabysitter", "NoLogging", "RoadNameUI"]
|
||||
keys = ["DisableOnroadUploads", "FireTheBabysitter", "NoLogging", "RoadNameUI", "SpeedLimitController"]
|
||||
for key in keys:
|
||||
params_memory.put_bool(key, params.get_bool(key))
|
||||
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ def enable_logging(started, params, params_memory, CP: car.CarParams) -> bool:
|
|||
return not (params_memory.get_bool("FireTheBabysitter") and params_memory.get_bool("NoLogging"))
|
||||
|
||||
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,12 @@ class RouteEngine:
|
|||
|
||||
if self.step_idx is None:
|
||||
msg.valid = False
|
||||
SpeedLimitController.load_state()
|
||||
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 +345,16 @@ class RouteEngine:
|
|||
|
||||
if ('maxspeed' in closest.annotations) and self.localizer_valid:
|
||||
msg.navInstruction.speedLimit = closest.annotations['maxspeed']
|
||||
SpeedLimitController.load_state()
|
||||
SpeedLimitController.nav_speed_limit = closest.annotations['maxspeed']
|
||||
SpeedLimitController.write_nav_state()
|
||||
if not self.localizer_valid or ('maxspeed' not in closest.annotations):
|
||||
SpeedLimitController.load_state()
|
||||
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,11 @@ 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 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;
|
||||
|
||||
|
|
@ -142,6 +146,13 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
|
|||
uiState()->scene.hide_speed = !currentHideSpeed;
|
||||
params.putBoolNonBlocking("HideSpeed", !currentHideSpeed);
|
||||
|
||||
widgetClicked = true;
|
||||
} 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;
|
||||
}
|
||||
// If the click wasn't for anything specific, change the value of "ExperimentalMode"
|
||||
|
|
@ -474,11 +485,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 : slcSpeedLimit ? slcSpeedLimit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
|
||||
speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
|
||||
if (slcSpeedLimit && !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) || (slcSpeedLimit && !useViennaSLCSign);
|
||||
has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA) || (slcSpeedLimit && 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;
|
||||
|
|
@ -511,6 +525,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 = (showSLCOffset) ? "+" + QString::number(std::nearbyint(slcSpeedLimitOffset)) : "–";
|
||||
QString speedStr = QString::number(std::nearbyint(speed));
|
||||
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
|
||||
|
||||
|
|
@ -583,11 +598,23 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
|
|||
p.setPen(QPen(blackColor(), 6));
|
||||
p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
|
||||
|
||||
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.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
|
||||
|
|
@ -598,9 +625,19 @@ 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());
|
||||
p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
|
||||
if (showSLCOffset) {
|
||||
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
|
||||
|
|
@ -1118,8 +1155,13 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
|
|||
obstacleDistanceStock = scene.obstacle_distance_stock;
|
||||
roadNameUI = scene.road_name_ui;
|
||||
showDriverCamera = scene.show_driver_camera;
|
||||
showSLCOffset = scene.show_slc_offset;
|
||||
slcOverridden = scene.speed_limit_controller ? scene.speed_limit_overridden : 0;
|
||||
slcSpeedLimit = scene.speed_limit_controller ? scene.speed_limit : 0;
|
||||
slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
|
||||
turnSignalLeft = scene.turn_signal_left;
|
||||
turnSignalRight = scene.turn_signal_right;
|
||||
useViennaSLCSign = scene.use_vienna_slc_sign;
|
||||
|
||||
if (!(showDriverCamera || fullMapOpen)) {
|
||||
if (leadInfo) {
|
||||
|
|
|
|||
|
|
@ -162,13 +162,18 @@ private:
|
|||
bool mapOpen;
|
||||
bool roadNameUI;
|
||||
bool showDriverCamera;
|
||||
bool showSLCOffset;
|
||||
bool slcOverridden;
|
||||
bool turnSignalLeft;
|
||||
bool turnSignalRight;
|
||||
bool useViennaSLCSign;
|
||||
|
||||
float cruiseAdjustment;
|
||||
float distanceConversion;
|
||||
float laneWidthLeft;
|
||||
float laneWidthRight;
|
||||
float slcSpeedLimit;
|
||||
float slcSpeedLimitOffset;
|
||||
float speedConversion;
|
||||
|
||||
int bearingDeg;
|
||||
|
|
|
|||
|
|
@ -249,6 +249,12 @@ 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.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
|
||||
}
|
||||
if (sm.updated("liveLocationKalman")) {
|
||||
|
|
@ -303,6 +309,7 @@ void ui_update_frogpilot_params(UIState *s) {
|
|||
scene.lead_info = params.getBool("LeadInfo") && custom_onroad_ui;
|
||||
scene.use_si = params.getBool("UseSI") && scene.lead_info;
|
||||
scene.road_name_ui = params.getBool("RoadNameUI") && custom_onroad_ui;
|
||||
scene.use_vienna_slc_sign = params.getBool("UseVienna") && custom_onroad_ui;
|
||||
|
||||
bool custom_theme = params.getBool("CustomTheme");
|
||||
scene.custom_colors = custom_theme ? params.getInt("CustomColors") : 0;
|
||||
|
|
@ -326,6 +333,8 @@ void ui_update_frogpilot_params(UIState *s) {
|
|||
bool quality_of_life_controls = params.getBool("QOLControls");
|
||||
scene.reverse_cruise = params.getBool("ReverseCruise") && quality_of_life_controls;
|
||||
scene.reverse_cruise_ui = params.getBool("ReverseCruiseUI") && scene.reverse_cruise;
|
||||
scene.show_slc_offset = params.getBool("ShowSLCOffset") && quality_of_life_controls;
|
||||
scene.show_slc_offset_ui = params.getBool("ShowSLCOffsetUI") && quality_of_life_controls;
|
||||
|
||||
bool quality_of_life_visuals = params.getBool("QOLVisuals");
|
||||
scene.full_map = params.getBool("FullMap") && quality_of_life_visuals;
|
||||
|
|
@ -334,6 +343,7 @@ 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");
|
||||
}
|
||||
|
||||
void UIState::updateStatus() {
|
||||
|
|
|
|||
|
|
@ -204,11 +204,16 @@ typedef struct UIScene {
|
|||
bool road_name_ui;
|
||||
bool rotating_wheel;
|
||||
bool show_driver_camera;
|
||||
bool show_slc_offset;
|
||||
bool show_slc_offset_ui;
|
||||
bool speed_limit_controller;
|
||||
bool speed_limit_overridden;
|
||||
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 adjusted_cruise;
|
||||
float lane_line_width;
|
||||
|
|
@ -217,6 +222,9 @@ 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;
|
||||
|
||||
int bearing_deg;
|
||||
int camera_view;
|
||||
|
|
|
|||
Loading…
Reference in New Issue