mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-24 07:42:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

missed

Browse Source
This commit is contained in:
Jason Wen
2024-09-15 22:07:34 -04:00
parent 028a468290
commit ed5dbc4dd2
3 changed files with 38 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py
+29 -24
View File
@@ -5,7 +5,7 @@ from openpilot.common.conversions import Conversions as CV
from openpilot.common.params import Params
from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_controller import ACTIVE_STATES
from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.states import InactiveState, \
AcceleratingState, DeceleratingState, HoldingState, ResettingState, LoadingState
LoadingState, AcceleratingState, DeceleratingState, HoldingState
from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.helpers import get_set_point, \
speed_hysteresis, update_manual_button_timers
@@ -26,10 +26,10 @@ class CustomStockLongitudinalControllerBase(ABC):
self.CP = CP
self.params = Params()
self.final_speed_kph = 0
self.target_speed = 0
self.v_cruise = 0
self.v_target = 0
self.v_cruise_cluster = 0
self.v_cruise_min = 0
self.cruise_button = None
self.button_state = ButtonControlState.inactive
self.v_tsc_state = VisionTurnControllerState.disabled
@@ -41,8 +41,6 @@ class CustomStockLongitudinalControllerBase(ABC):
self.is_ready = False
self.is_ready_prev = False
self.cruise_button = None
self.speed = 0
self.speed_steady = 0
self.accel_button = None
@@ -51,11 +49,10 @@ class CustomStockLongitudinalControllerBase(ABC):
self.button_states = {
ButtonControlState.inactive: InactiveState(self),
ButtonControlState.loading: LoadingState(self),
ButtonControlState.accelerating: AcceleratingState(self),
ButtonControlState.decelerating: DeceleratingState(self),
ButtonControlState.holding: HoldingState(self),
ButtonControlState.resetting: ResettingState(self),
ButtonControlState.loading: LoadingState(self),
}
@abstractmethod
@@ -66,9 +63,8 @@ class CustomStockLongitudinalControllerBase(ABC):
customStockLongitudinalControl = custom.CarControlSP.CustomStockLongitudinalControl.new_message()
customStockLongitudinalControl.state = self.button_state
customStockLongitudinalControl.cruiseButton = 0 if self.cruise_button is None else int(self.cruise_button)
customStockLongitudinalControl.finalSpeedKph = float(self.final_speed_kph)
customStockLongitudinalControl.targetSpeed = float(self.target_speed)
customStockLongitudinalControl.vCruise = float(self.v_cruise)
customStockLongitudinalControl.vTarget = float(self.v_target)
customStockLongitudinalControl.vCruiseCluster = float(self.v_cruise_cluster)
return customStockLongitudinalControl
def update_msgs(self) -> None:
@@ -85,19 +81,30 @@ class CustomStockLongitudinalControllerBase(ABC):
self.m_tsc = self.car.sm['longitudinalPlanSP'].turnSpeed
def update_calculations(self, CS: car.CarState, CC: car.CarControl) -> None:
self.v_cruise_min = get_set_point(self.car_state.params_list.is_metric)
is_metric = self.car_state.params_list.is_metric
v_cruise_kph = CC.vCruise
v_cruise = v_cruise_kph * CV.KPH_TO_MS
self.v_cruise_min = get_set_point(is_metric)
self.v_cruise_cluster = round(CS.cruiseState.speed * (CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH))
v_tsc_target = self.v_tsc * CV.MS_TO_KPH if self.v_tsc_state != VisionTurnControllerState.disabled else 255
slc_target = self.speed_limit_offseted * CV.MS_TO_KPH if self.slc_state in ACTIVE_STATES else 255
m_tsc_target = self.m_tsc * CV.MS_TO_KPH if self.m_tsc_state > TurnSpeedControlState.tempInactive else 255
tsc_target = min(v_tsc_target, slc_target, m_tsc_target)
self.speed = speed_hysteresis(tsc_target, self.speed_steady, 1.5 * (1 if self.car_state.params_list.is_metric else CV.MPH_TO_KPH))
self.speed_steady = self.speed
v_targets = {'cruise': CC.vCruise}
self.final_speed_kph = min(self.speed, CC.vCruise)
if self.v_tsc_state != VisionTurnControllerState.disabled:
v_targets['v_tsc'] = self.v_tsc
self.target_speed = round(self.final_speed_kph * (CV.KPH_TO_MPH if not self.car_state.params_list.is_metric else 1))
self.v_cruise = round(CS.cruiseState.speed * (CV.MS_TO_MPH if not self.car_state.params_list.is_metric else CV.MS_TO_KPH))
if self.slc_state in ACTIVE_STATES:
v_targets['slc'] = self.speed_limit_offseted
if self.m_tsc_state > TurnSpeedControlState.tempInactive:
v_targets['m_tsc'] = self.m_tsc
source = min(v_targets, key=v_targets.get)
v_target = speed_hysteresis(self, v_targets[source], self.speed_steady, 1.5 * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS))
v_target = min(v_target, v_cruise)
v_target = round(v_target * (CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH))
self.v_target = v_target
def update_state(self, CS: car.CarState, CC: car.CarControl) -> None:
update_manual_button_timers(CS, self.cruise_buttons)
@@ -112,14 +119,12 @@ class CustomStockLongitudinalControllerBase(ABC):
self.is_ready_prev = self.is_ready
def update(self, CS: car.CarState, CC: car.CarControl) -> list[SendCan]:
can_sends = []
self.update_msgs()
self.update_calculations(CS, CC)
self.update_state(CS, CC)
can_sends.extend(self.create_mock_button_messages())
can_sends = self.create_mock_button_messages()
return can_sends
sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py
+4 -1
View File
@@ -7,11 +7,14 @@ def get_set_point(is_metric: bool) -> float:
return 30 if is_metric else 20
def speed_hysteresis(speed: float, speed_steady: float, hyst: float) -> float:
def speed_hysteresis(controller, speed: float, speed_steady: float, hyst: float) -> float:
if speed > speed_steady + hyst:
speed_steady = speed - hyst
elif speed < speed_steady - hyst:
speed_steady = speed + hyst
controller.speed_steady = speed
return speed_steady
sunnypilot/controls/lib/custom_stock_longitudinal_controller/states.py
+5 -11
View File
@@ -47,9 +47,9 @@ class InactiveState(ButtonStateBase):
class LoadingState(ButtonStateBase):
def handle(self) -> None:
if self.controller.target_speed > self.controller.v_cruise:
if self.controller.v_target > self.controller.v_cruise_cluster:
self.controller.button_state = ButtonControlState.accelerating
elif self.controller.target_speed < self.controller.v_cruise and self.controller.v_cruise > self.controller.v_cruise_min:
elif self.controller.v_target < self.controller.v_cruise_cluster and self.controller.v_cruise_cluster > self.controller.v_cruise_min:
self.controller.button_state = ButtonControlState.decelerating
else:
self.controller.button_state = ButtonControlState.holding
@@ -59,7 +59,7 @@ class LoadingState(ButtonStateBase):
class AcceleratingState(ButtonStateBase):
def handle(self) -> int | None:
self.button_count += 1
if self.controller.target_speed <= self.controller.v_cruise or self.button_count > RESET_COUNT:
if self.controller.v_target <= self.controller.v_cruise_cluster or self.button_count > RESET_COUNT:
self.button_count = 0
self.controller.button_state = ButtonControlState.holding
return None
@@ -69,7 +69,7 @@ class AcceleratingState(ButtonStateBase):
class DeceleratingState(ButtonStateBase):
def handle(self) -> int | None:
self.button_count += 1
if self.controller.target_speed >= self.controller.v_cruise or self.controller.v_cruise <= self.controller.v_cruise_min or self.button_count > RESET_COUNT:
if self.controller.v_target >= self.controller.v_cruise_cluster or self.controller.v_cruise_cluster <= self.controller.v_cruise_min or self.button_count > RESET_COUNT:
self.button_count = 0
self.controller.button_state = ButtonControlState.holding
return None
@@ -81,11 +81,5 @@ class HoldingState(ButtonStateBase):
self.button_count += 1
if self.button_count > HOLD_TIME:
self.button_count = 0
self.controller.button_state = ButtonControlState.resetting
return None
class ResettingState(ButtonStateBase):
def handle(self) -> None:
self.controller.button_state = ButtonControlState.loading
self.controller.button_state = ButtonControlState.loading
return None