mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-09 01:23:43 +08:00
Code Issues Packages Projects Releases Wiki Activity

Controls - Quality of Life - Force Standstill State - Only For Stop Lights/Stop Signs

Browse Source
This commit is contained in:
FrogAi
2024-06-30 17:40:38 -07:00
parent e791efd144
commit 958abbdef8
2 changed files with 22 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/frogpilot/controls/frogpilot_planner.py
+16 -2
View File
@@ -52,6 +52,7 @@ class FrogPilotPlanner:
self.mtsc_target = 0
self.road_curvature = 0
self.speed_jerk = 0
self.tracked_model_length = 0
self.v_cruise = 0
def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarState, frogpilot_toggles):
@@ -74,7 +75,7 @@ class FrogPilotPlanner:
stopping_distance = STOP_DISTANCE + distance_offset
if frogpilot_toggles.conditional_experimental_mode and controlsState.enabled:
self.cem.update(carState, frogpilotNavigation, self.lead_one, modelData, self.model_length, self.road_curvature, self.slower_lead, self.tracking_lead, v_ego, v_lead, frogpilot_toggles)
self.cem.update(carState, frogpilotNavigation, self.lead_one, modelData, self.model_length, self.road_curvature, self.slower_lead, self.tracking_lead, self.v_cruise, v_ego, v_lead, frogpilot_toggles)
check_lane_width = frogpilot_toggles.lane_detection
if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed:
@@ -90,6 +91,9 @@ class FrogPilotPlanner:
if v_ego > CRUISING_SPEED:
self.override_force_stop = False
self.tracking_lead = self.lead_one.status
self.tracked_model_length = 0
elif carState.standstill and frogpilot_toggles.force_stops:
self.override_force_stop = True
else:
self.tracking_lead &= self.lead_one.status
@@ -192,12 +196,22 @@ class FrogPilotPlanner:
else:
self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
if frogpilot_toggles.force_standstill and v_ego < 1 and not self.override_force_stop:
if (frogpilot_toggles.force_standstill or frogpilot_toggles.force_stops) and v_ego < 1 and not self.override_force_stop:
if carState.gasPressed:
self.override_force_stop = True
else:
self.v_cruise = -1
elif frogpilot_toggles.force_stops and v_ego < CRUISING_SPEED and controlsState.experimentalMode and not self.override_force_stop:
if carState.gasPressed or self.tracking_lead or abs(carState.steeringAngleDeg) > 15:
self.override_force_stop = True
else:
if self.tracked_model_length == 0:
self.tracked_model_length = self.model_length
self.tracked_model_length -= v_ego * DT_MDL
self.v_cruise = self.tracked_model_length / ModelConstants.T_IDXS[TRAJECTORY_SIZE - 1]
else:
targets = [self.mtsc_target]
self.v_cruise = min([target if target > CRUISING_SPEED else v_cruise for target in targets])
selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
+6 -6
View File
@@ -15,14 +15,14 @@ class ConditionalExperimentalMode:
self.slow_lead_mac = MovingAverageCalculator()
self.stop_light_mac = MovingAverageCalculator()
def update(self, carState, frogpilotNavigation, lead, modelData, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles):
def update(self, carState, frogpilotNavigation, lead, modelData, model_length, road_curvature, slower_lead, tracking_lead, v_cruise, v_ego, v_lead, frogpilot_toggles):
if frogpilot_toggles.experimental_mode_via_press:
self.status_value = self.params_memory.get_int("CEStatus")
else:
self.status_value = 0
if self.status_value not in {1, 2, 3, 4, 5, 6} and not carState.standstill:
self.update_conditions(lead.dRel, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles)
self.update_conditions(lead.dRel, model_length, road_curvature, slower_lead, tracking_lead, v_cruise, v_ego, v_lead, frogpilot_toggles)
self.experimental_mode = self.check_conditions(carState, frogpilotNavigation, modelData, tracking_lead, v_ego, v_lead, frogpilot_toggles)
self.params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else 0)
else:
@@ -56,10 +56,10 @@ class ConditionalExperimentalMode:
return False
def update_conditions(self, lead_distance, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles):
def update_conditions(self, lead_distance, model_length, road_curvature, slower_lead, tracking_lead, v_cruise, v_ego, v_lead, frogpilot_toggles):
self.road_curvature(road_curvature, v_ego, frogpilot_toggles)
self.slow_lead(slower_lead, tracking_lead, v_lead, frogpilot_toggles)
self.stop_sign_and_light(lead_distance, model_length, tracking_lead, v_ego, v_lead, frogpilot_toggles)
self.stop_sign_and_light(lead_distance, model_length, tracking_lead, v_cruise, v_ego, v_lead, frogpilot_toggles)
def road_curvature(self, road_curvature, v_ego, frogpilot_toggles):
curve_detected = (1 / road_curvature)**0.5 < v_ego
@@ -79,7 +79,7 @@ class ConditionalExperimentalMode:
self.slow_lead_mac.reset_data()
self.slow_lead_detected = False
def stop_sign_and_light(self, lead_distance, model_length, tracking_lead, v_ego, v_lead, frogpilot_toggles):
def stop_sign_and_light(self, lead_distance, model_length, tracking_lead, v_cruise, v_ego, v_lead, frogpilot_toggles):
lead_close = lead_distance < CITY_SPEED_LIMIT
lead_far = lead_distance > CITY_SPEED_LIMIT and v_ego < CRUISING_SPEED
lead_stopped = v_lead < 1
@@ -87,7 +87,7 @@ class ConditionalExperimentalMode:
following_lead = tracking_lead and (lead_close or lead_stopped or lead_stopping) and not lead_far
model_projection = ModelConstants.T_IDXS[TRAJECTORY_SIZE - (5 if frogpilot_toggles.less_sensitive_lights else 3)]
model_stopped = model_length < TRAJECTORY_SIZE
model_stopped = model_length < TRAJECTORY_SIZE or v_cruise < CRUISING_SPEED
model_threshold = v_ego * model_projection
model_stopping = model_length < model_threshold and not self.curve_detected