mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ICBM: use longitudinalPlanSP.vTarget directly for evaluation (#1266)

Browse Source 
* ICBM: add SCC-V to v_targets list

* Longitudinal planner: expose custom vTarget and aTarget

* use the source directly
This commit is contained in:
Jason Wen
2025-09-20 14:22:10 -04:00
committed by GitHub
parent 5f3821c1f9
commit 632b416f2a
2 changed files with 6 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
selfdrive/selfdrived/selfdrived.py
View File

@@ -96,7 +96,7 @@ class SelfdriveD(CruiseHelper):
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
'modelDataV2SP'] + \
'modelDataV2SP', 'longitudinalPlanSP'] + \
self.camera_packets + self.sensor_packets + self.gps_packets,
ignore_alive=ignore, ignore_avg_freq=ignore,
ignore_valid=ignore, frequency=int(1/DT_CTRL))
@@ -444,7 +444,7 @@ class SelfdriveD(CruiseHelper):
self.events.add(EventName.personalityChanged)
self.experimental_mode_switched = False
self.icbm.run(CS, self.sm['carControl'], self.is_metric)
self.icbm.run(CS, self.sm['carControl'], self.sm['longitudinalPlanSP'], self.is_metric)
def data_sample(self):
_car_state = messaging.recv_one(self.car_state_sock)

16
sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py
View File

@@ -49,19 +49,11 @@ class IntelligentCruiseButtonManagement:
def v_cruise_equal(self) -> bool:
return self.v_target == self.v_cruise_cluster
def update_calculations(self, CS: car.CarState) -> None:
def update_calculations(self, CS: car.CarState, LP_SP: custom.LongitudinalPlanSP) -> None:
speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
ms_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
v_cruise_ms = CS.vCruise * CV.KPH_TO_MS
# all targets in m/s
v_targets = {
LongitudinalPlanSource.cruise: v_cruise_ms
}
source = min(v_targets, key=lambda k: v_targets[k])
v_target_ms = v_targets[source]
self.v_target_ms_last = apply_hysteresis(v_target_ms, self.v_target_ms_last, HYST_GAP * ms_conv)
self.v_target_ms_last = apply_hysteresis(LP_SP.vTarget, self.v_target_ms_last, HYST_GAP * ms_conv)
self.v_target = round(self.v_target_ms_last * speed_conv)
self.v_cruise_min = get_minimum_set_speed(self.is_metric)
@@ -123,13 +115,13 @@ class IntelligentCruiseButtonManagement:
self.is_ready = ready and not button_pressed
def run(self, CS: car.CarState, CC: car.CarControl, is_metric: bool) -> None:
def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None:
if self.CP_SP.pcmCruiseSpeed:
return
self.is_metric = is_metric
self.update_calculations(CS)
self.update_calculations(CS, LP_SP)
self.update_readiness(CS, CC)
self.cruise_button = self.update_state_machine()