From d882e78e7c396057ab52ce1bc514db2ff2a81ae4 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 20 Oct 2025 15:33:27 -0500 Subject: [PATCH] Upstream Throttle --- cereal/custom.capnp | 57 ++++++++++--------- frogpilot/controls/frogpilot_planner.py | 10 +++- frogpilot/controls/lib/frogpilot_vcruise.py | 13 +---- .../controls/lib/longitudinal_planner.py | 1 + 4 files changed, 41 insertions(+), 40 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 44409b9c5..029709a26 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -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 { diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index 99e66778b..d0b854778 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -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 diff --git a/frogpilot/controls/lib/frogpilot_vcruise.py b/frogpilot/controls/lib/frogpilot_vcruise.py index c2c1b6eff..35aae36b5 100644 --- a/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/frogpilot/controls/lib/frogpilot_vcruise.py @@ -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) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2911d7eae..22d779b5d 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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])