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

28
selfdrive/car/hyundai/carstate.py
View File

@@ -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)
@@ -180,6 +192,9 @@ class CarState(CarStateBase):
self.fpf.update_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):
@@ -272,6 +287,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):
@@ -324,6 +342,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
@@ -379,6 +401,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
@@ -392,4 +417,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)