mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-03 04:22:09 +08:00
firestar5683
99 lines
3.7 KiB
Python
99 lines
3.7 KiB
Python
#!/usr/bin/env python3
|
|
from openpilot.common.constants import CV
|
|
from openpilot.common.realtime import DT_MDL
|
|
|
|
from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME
|
|
from openpilot.frogpilot.controls.lib.curve_speed_controller import CurveSpeedController
|
|
from openpilot.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
|
|
|
|
OVERRIDE_FORCE_STOP_TIMER = 10
|
|
|
|
class FrogPilotVCruise:
|
|
def __init__(self, FrogPilotPlanner):
|
|
self.frogpilot_planner = FrogPilotPlanner
|
|
|
|
self.csc = CurveSpeedController(self)
|
|
self.slc = SpeedLimitController(self)
|
|
|
|
self.forcing_stop = False
|
|
self.override_force_stop = False
|
|
|
|
self.override_force_stop_timer = 0
|
|
self.force_stop_timer = 0.0
|
|
self.tracked_model_length = 0.0
|
|
|
|
def update(self, long_control_active, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles):
|
|
force_stop = self.frogpilot_planner.frogpilot_cem.stop_light_detected and long_control_active and frogpilot_toggles.force_stops
|
|
force_stop &= self.frogpilot_planner.model_stopped
|
|
force_stop &= self.override_force_stop_timer <= 0
|
|
|
|
self.force_stop_timer = self.force_stop_timer + DT_MDL if force_stop else 0
|
|
|
|
force_stop_enabled = self.force_stop_timer >= 1
|
|
|
|
self.override_force_stop |= sm["carState"].gasPressed
|
|
self.override_force_stop |= sm["frogpilotCarState"].accelPressed
|
|
self.override_force_stop &= force_stop_enabled
|
|
|
|
if self.override_force_stop:
|
|
self.override_force_stop_timer = OVERRIDE_FORCE_STOP_TIMER
|
|
elif self.override_force_stop_timer > 0:
|
|
self.override_force_stop_timer -= DT_MDL
|
|
|
|
v_cruise_cluster = max(sm["carState"].vCruiseCluster * CV.KPH_TO_MS, v_cruise)
|
|
v_cruise_diff = v_cruise_cluster - v_cruise
|
|
|
|
v_ego_cluster = max(sm["carState"].vEgoCluster, v_ego)
|
|
v_ego_diff = v_ego_cluster - v_ego
|
|
|
|
# FrogsGoMoo's Curve Speed Controller
|
|
if long_control_active and v_ego > CRUISING_SPEED and self.frogpilot_planner.road_curvature_detected and frogpilot_toggles.curve_speed_controller:
|
|
self.csc.update_target(v_ego)
|
|
|
|
self.csc_controlling_speed = True
|
|
|
|
self.csc_target = self.csc.target
|
|
else:
|
|
self.csc.log_data(long_control_active, v_ego, sm)
|
|
|
|
self.csc_controlling_speed = False
|
|
self.csc.target_set = False
|
|
|
|
self.csc_target = v_cruise
|
|
|
|
# Pfeiferj's Speed Limit Controller
|
|
self.slc.frogpilot_toggles = frogpilot_toggles
|
|
|
|
if frogpilot_toggles.speed_limit_controller:
|
|
self.slc.update_limits(sm["frogpilotCarState"].dashboardSpeedLimit, now, time_validated, v_cruise, v_ego, sm)
|
|
self.slc.update_override(v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm)
|
|
|
|
self.slc_offset = self.slc.offset
|
|
self.slc_target = self.slc.target
|
|
elif frogpilot_toggles.show_speed_limits:
|
|
self.slc.update_limits(sm["frogpilotCarState"].dashboardSpeedLimit, now, time_validated, v_cruise, v_ego, sm)
|
|
|
|
self.slc_offset = 0
|
|
self.slc_target = self.slc.target
|
|
else:
|
|
self.slc_offset = 0
|
|
self.slc_target = 0
|
|
|
|
if force_stop_enabled and not self.override_force_stop:
|
|
self.forcing_stop |= not sm["carState"].standstill
|
|
|
|
self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0)
|
|
v_cruise = min((self.tracked_model_length // PLANNER_TIME), v_cruise)
|
|
|
|
else:
|
|
self.forcing_stop = False
|
|
|
|
self.tracked_model_length = self.frogpilot_planner.model_length
|
|
|
|
targets = [self.csc_target, v_cruise]
|
|
if frogpilot_toggles.speed_limit_controller:
|
|
targets.append(max(self.slc.overridden_speed, self.slc_target + self.slc_offset) - v_ego_diff)
|
|
v_cruise = min([target if target >= CRUISING_SPEED else v_cruise for target in targets])
|
|
|
|
return v_cruise
|