brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Speed limit controller

Browse Source 
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
2024-02-27 16:34:47 -07:00
parent 6907410750
commit 3a926ddfbe
23 changed files with 703 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
selfdrive/controls/controlsd.py
View File

@@ -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
@@ -185,6 +187,7 @@ class Controls:
self.stopped_for_light_previously = False
self.previous_lead_distance = 0
self.previous_speed_limit = SpeedLimitController.desired_speed_limit
ignore = self.sensor_packets + ['testJoystick']
if SIMULATION:
@@ -589,6 +592,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 abs(current_speed_limit - desired_speed_limit) < 1 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"""
@@ -641,7 +685,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
@@ -716,7 +760,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
@@ -1052,7 +1096,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:
@@ -1110,6 +1154,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()

29
selfdrive/controls/lib/drive_helpers.py
View File

@@ -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:
self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
# 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

8
selfdrive/controls/lib/events.py
View File

@@ -1065,6 +1065,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,
},