mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-02 20:12:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Upstream Throttle

Browse Source
This commit is contained in:
firestar5683
2025-10-20 15:33:27 -05:00
parent b3c8edab9b
commit d882e78e7c
4 changed files with 41 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/custom.capnp
+29 -28
View File
@@ -193,34 +193,35 @@ struct FrogPilotPlan @0xa1680744031fdb2d {
cscTraining @4 :Bool;
dangerJerk @5 :Float32;
desiredFollowDistance @6 :Int64;
experimentalMode @7 :Bool;
forcingStop @8 :Bool;
forcingStopLength @9 :Float32;
frogpilotEvents @10 :List(FrogPilotCarEvent);
increasedStoppedDistance @11 :Float32;
lateralCheck @12 :Bool;
laneWidthLeft @13 :Float32;
laneWidthRight @14 :Float32;
maxAcceleration @15 :Float32;
minAcceleration @16 :Float32;
redLight @17 :Bool;
roadCurvature @18 :Float32;
slcMapSpeedLimit @19 :Float32;
slcMapboxSpeedLimit @20 :Float32;
slcNextSpeedLimit @21 :Float32;
slcOverriddenSpeed @22 :Float32;
slcSpeedLimit @23 :Float32;
slcSpeedLimitOffset @24 :Float32;
slcSpeedLimitSource @25 :Text;
speedJerk @26 :Float32;
speedJerkStock @27 :Float32;
speedLimitChanged @28 :Bool;
tFollow @29 :Float32;
themeUpdated @30 :Bool;
togglesUpdated @31 :Bool;
trackingLead @32 :Bool;
unconfirmedSlcSpeedLimit @33 :Float32;
vCruise @34 :Float32;
disableThrottle @7 :Bool;
experimentalMode @8 :Bool;
forcingStop @9 :Bool;
forcingStopLength @10 :Float32;
frogpilotEvents @11 :List(FrogPilotCarEvent);
increasedStoppedDistance @12 :Float32;
lateralCheck @13 :Bool;
laneWidthLeft @14 :Float32;
laneWidthRight @15 :Float32;
maxAcceleration @16 :Float32;
minAcceleration @17 :Float32;
redLight @18 :Bool;
roadCurvature @19 :Float32;
slcMapSpeedLimit @20 :Float32;
slcMapboxSpeedLimit @21 :Float32;
slcNextSpeedLimit @22 :Float32;
slcOverriddenSpeed @23 :Float32;
slcSpeedLimit @24 :Float32;
slcSpeedLimitOffset @25 :Float32;
slcSpeedLimitSource @26 :Text;
speedJerk @27 :Float32;
speedJerkStock @28 :Float32;
speedLimitChanged @29 :Bool;
tFollow @30 :Float32;
themeUpdated @31 :Bool;
togglesUpdated @32 :Bool;
trackingLead @33 :Bool;
unconfirmedSlcSpeedLimit @34 :Float32;
vCruise @35 :Float32;
}
struct FrogPilotRadarState @0xcb9fd56c7057593a {
frogpilot/controls/frogpilot_planner.py
+9 -1
View File
@@ -9,7 +9,7 @@ from openpilot.common.conversions import Conversions as CV
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, COMFORT_BRAKE, DANGER_ZONE_COST, J_EGO_COST
from openpilot.frogpilot.common.frogpilot_utilities import calculate_lane_width, calculate_road_curvature
from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD, params, params_memory
@@ -32,6 +32,7 @@ class FrogPilotPlanner:
self.tracking_lead_filter = FirstOrderFilter(0, 0.5, DT_MDL)
self.disable_throttle = False
self.driving_in_curve = False
self.lateral_check = False
self.model_stopped = False
@@ -65,6 +66,11 @@ class FrogPilotPlanner:
self.cem.curve_detected = False
self.cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2)
self.disable_throttle = self.tracking_lead and not self.frogpilot_following.following_lead
self.disable_throttle &= self.lead_one.vLead + COMFORT_BRAKE < v_ego > CRUISING_SPEED
self.disable_throttle &= sm["controlsState"].enabled
self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION
self.frogpilot_events.update(v_cruise, sm, frogpilot_toggles)
@@ -140,6 +146,8 @@ class FrogPilotPlanner:
frogpilotPlan.desiredFollowDistance = self.frogpilot_following.desired_follow_distance
frogpilotPlan.disableThrottle = self.disable_throttle
frogpilotPlan.experimentalMode = self.cem.experimental_mode or self.frogpilot_vcruise.slc.experimental_mode
frogpilotPlan.forcingStop = self.frogpilot_vcruise.forcing_stop
frogpilot/controls/lib/frogpilot_vcruise.py
+2 -11
View File
@@ -18,6 +18,7 @@ class FrogPilotVCruise:
self.override_force_stop = False
self.override_force_stop_timer = 0
self.force_stop_timer = 0.0
def update(self, gps_position, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles):
force_stop = self.frogpilot_planner.cem.stop_light_detected and sm["controlsState"].enabled and frogpilot_toggles.force_stops
@@ -58,16 +59,6 @@ class FrogPilotVCruise:
self.csc_target = v_cruise
# Mike's extended lead linear braking
if self.frogpilot_planner.lead_one.vLead < v_ego > CRUISING_SPEED and sm["controlsState"].enabled and self.frogpilot_planner.tracking_lead and frogpilot_toggles.human_following:
if not self.frogpilot_planner.frogpilot_following.following_lead:
decel_rate = (v_ego - self.frogpilot_planner.lead_one.vLead)**2 / self.frogpilot_planner.lead_one.dRel
self.braking_target = max(v_ego - (decel_rate * DT_MDL), self.frogpilot_planner.lead_one.vLead + CRUISING_SPEED)
else:
self.braking_target = v_cruise
else:
self.braking_target = v_cruise
# Pfeiferj's Speed Limit Controller
self.slc.frogpilot_toggles = frogpilot_toggles
@@ -97,7 +88,7 @@ class FrogPilotVCruise:
self.tracked_model_length = self.frogpilot_planner.model_length
targets = [self.braking_target, self.csc_target, v_cruise]
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)
selfdrive/controls/lib/longitudinal_planner.py
+1
View File
@@ -220,6 +220,7 @@ class LongitudinalPlanner:
x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, frogpilot_toggles.taco_tune)
# Don't clip at low speeds since throttle_prob doesn't account for creep
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
self.allow_throttle &= not sm['frogpilotPlan'].disableThrottle
if not self.allow_throttle:
clipped_accel_coast = max(accel_coast, accel_limits_turns[0])