long
This commit is contained in:
Binary file not shown.
Binary file not shown.
@@ -263,6 +263,7 @@ def update_frogpilot_toggles():
|
||||
class FrogPilotVariables:
|
||||
def __init__(self):
|
||||
self.params = Params(return_defaults=True)
|
||||
self.params_raw = Params()
|
||||
self.params_memory = Params(memory=True)
|
||||
|
||||
self.frogpilot_toggles = SimpleNamespace()
|
||||
@@ -487,10 +488,10 @@ class FrogPilotVariables:
|
||||
ev_vehicle = gm_ev_vehicle or (toggle.car_make == "hyundai" and CP.carFingerprint in HYUNDAI_EV_CAR)
|
||||
ev_vehicle |= CP.transmissionType == car.CarParams.TransmissionType.direct
|
||||
|
||||
if self.params.get("EVTuning") == b"":
|
||||
if self.params_raw.get("EVTuning") in (None, b""):
|
||||
self.params.put_bool("EVTuning", ev_vehicle)
|
||||
|
||||
if self.params.get("TruckTuning") == b"":
|
||||
if self.params_raw.get("TruckTuning") in (None, b""):
|
||||
self.params.put_bool("TruckTuning", False)
|
||||
|
||||
ev_tuning_param = self.params.get_bool("EVTuning")
|
||||
@@ -504,7 +505,7 @@ class FrogPilotVariables:
|
||||
toggle.ev_tuning = ev_tuning_param if advanced_longitudinal_tuning else ev_vehicle
|
||||
toggle.truck_tuning = truck_tuning_param if advanced_longitudinal_tuning else False
|
||||
toggle.longitudinalActuatorDelay = self.get_value("LongitudinalActuatorDelay", cast=float, condition=advanced_longitudinal_tuning, default=longitudinalActuatorDelay, min=0, max=1)
|
||||
toggle.max_desired_acceleration = self.get_value("MaxDesiredAcceleration", cast=float, condition=advanced_longitudinal_tuning, min=0.1, max=MAX_ACCELERATION)
|
||||
toggle.max_desired_acceleration = self.get_value("MaxDesiredAcceleration", cast=float, condition=advanced_longitudinal_tuning, default=MAX_ACCELERATION, min=0.1, max=MAX_ACCELERATION)
|
||||
toggle.startAccel = self.get_value("StartAccel", cast=float, condition=advanced_longitudinal_tuning, default=startAccel, min=0, max=MAX_ACCELERATION)
|
||||
toggle.stopAccel = self.get_value("StopAccel", cast=float, condition=advanced_longitudinal_tuning, default=stopAccel, min=-MAX_ACCELERATION, max=0)
|
||||
toggle.stoppingDecelRate = self.get_value("StoppingDecelRate", cast=float, condition=advanced_longitudinal_tuning, default=toggle.stoppingDecelRate, min=0.001, max=1)
|
||||
|
||||
@@ -20,6 +20,7 @@ from openpilot.frogpilot.controls.lib.frogpilot_following import FrogPilotFollow
|
||||
from openpilot.frogpilot.controls.lib.frogpilot_vcruise import FrogPilotVCruise
|
||||
from openpilot.frogpilot.controls.lib.weather_checker import WeatherChecker
|
||||
|
||||
|
||||
class FrogPilotPlanner:
|
||||
def __init__(self, error_log, ThemeManager):
|
||||
self.params = Params(return_defaults=True)
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, STOP_DISTANCE, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
|
||||
|
||||
from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT, MAX_T_FOLLOW
|
||||
|
||||
@@ -25,8 +25,6 @@ class FrogPilotFollowing:
|
||||
self.t_follow = 0
|
||||
|
||||
def update(self, long_control_active, v_ego, sm, frogpilot_toggles):
|
||||
stop_distance = max(float(getattr(frogpilot_toggles, "stop_distance", STOP_DISTANCE)), 4.0)
|
||||
|
||||
if long_control_active and sm["frogpilotCarState"].trafficModeEnabled:
|
||||
if sm["carState"].aEgo >= 0:
|
||||
self.base_acceleration_jerk = np.interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_acceleration)
|
||||
@@ -85,43 +83,41 @@ class FrogPilotFollowing:
|
||||
lead_distance = self.frogpilot_planner.lead_one.dRel
|
||||
v_lead = self.frogpilot_planner.lead_one.vLead
|
||||
closing_speed = max(0.0, v_ego - v_lead)
|
||||
desired_gap = float(desired_follow_distance(v_ego, v_lead, self.t_follow, stop_distance))
|
||||
desired_gap = float(desired_follow_distance(v_ego, v_lead, self.t_follow))
|
||||
ttc = lead_distance / max(closing_speed, 1e-3) if closing_speed > 0.1 else 1e6
|
||||
|
||||
coast_window_open = lead_distance > desired_gap + max(4.0, 0.2 * v_ego)
|
||||
coast_window_far = lead_distance < desired_gap + max(25.0, 1.2 * v_ego)
|
||||
gentle_closing = closing_speed < max(2.0, 0.12 * v_ego)
|
||||
|
||||
self.disable_throttle = (not self.following_lead and v_ego > HIGHWAY_DISABLE_THROTTLE_MIN_SPEED and coast_window_open and
|
||||
coast_window_far and gentle_closing)
|
||||
self.disable_throttle &= ttc > 6.0 and lead_distance > desired_gap + 6.0
|
||||
|
||||
if long_control_active and self.frogpilot_planner.tracking_lead:
|
||||
if not sm["frogpilotCarState"].trafficModeEnabled:
|
||||
self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead, frogpilot_toggles)
|
||||
self.desired_follow_distance = int(desired_follow_distance(v_ego, self.frogpilot_planner.lead_one.vLead, self.t_follow, stop_distance))
|
||||
self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead, frogpilot_toggles)
|
||||
self.desired_follow_distance = int(desired_follow_distance(v_ego, self.frogpilot_planner.lead_one.vLead, self.t_follow))
|
||||
else:
|
||||
self.desired_follow_distance = 0
|
||||
|
||||
def update_follow_values(self, lead_distance, v_ego, v_lead, frogpilot_toggles):
|
||||
stop_distance = max(float(getattr(frogpilot_toggles, "stop_distance", STOP_DISTANCE)), 4.0)
|
||||
|
||||
# Keep StarPilot behavior for lead response, while still honoring stop distance/weather offsets.
|
||||
if frogpilot_toggles.human_following and v_lead > v_ego:
|
||||
distance_factor = max(lead_distance - (v_ego * self.t_follow), 1)
|
||||
accelerating_offset = np.clip(stop_distance - v_ego, 1, distance_factor)
|
||||
from frogpilot.common.frogpilot_variables import get_frogpilot_toggles
|
||||
fp_toggles = get_frogpilot_toggles()
|
||||
accelerating_offset = float(np.clip(fp_toggles.stop_distance - v_ego, 1, distance_factor))
|
||||
|
||||
self.acceleration_jerk /= accelerating_offset
|
||||
self.speed_jerk /= accelerating_offset
|
||||
self.t_follow /= accelerating_offset
|
||||
|
||||
# Only apply the stronger slower-lead follow tightening for human-following, like StarPilot.
|
||||
if (frogpilot_toggles.conditional_slower_lead or frogpilot_toggles.human_following) and v_lead < v_ego:
|
||||
distance_factor = max(lead_distance - (v_lead * self.t_follow), 1)
|
||||
braking_offset = np.clip(min(v_ego - v_lead, v_lead) - COMFORT_BRAKE, 1, distance_factor)
|
||||
braking_offset = float(np.clip(min(v_ego - v_lead, v_lead) - COMFORT_BRAKE, 1, distance_factor))
|
||||
|
||||
if frogpilot_toggles.human_following:
|
||||
far_lead_offset = 0.0
|
||||
if lead_distance >= 100:
|
||||
far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - stop_distance, 0)
|
||||
from frogpilot.common.frogpilot_variables import get_frogpilot_toggles
|
||||
fp_toggles = get_frogpilot_toggles()
|
||||
far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - fp_toggles.stop_distance, 0)
|
||||
self.t_follow /= braking_offset + far_lead_offset
|
||||
self.slower_lead = braking_offset > 1
|
||||
|
||||
@@ -8,6 +8,7 @@ from openpilot.frogpilot.controls.lib.speed_limit_controller import SpeedLimitCo
|
||||
|
||||
OVERRIDE_FORCE_STOP_TIMER = 10
|
||||
|
||||
|
||||
class FrogPilotVCruise:
|
||||
def __init__(self, FrogPilotPlanner):
|
||||
self.frogpilot_planner = FrogPilotPlanner
|
||||
|
||||
@@ -178,7 +178,7 @@ class CarController(CarControllerBase):
|
||||
else:
|
||||
small_cmd_scale = np.interp(abs(accel), [0.0, 0.35, 0.8, 1.5, 2.5], [0.44, 0.54, 0.70, 0.89, 1.0])
|
||||
accel_cmd = accel * small_cmd_scale
|
||||
if (not press_regen_paddle) and accel < -2.0:
|
||||
if accel < -2.0:
|
||||
accel_cmd *= np.interp(abs(accel), [2.0, 2.5, 3.0], [1.0, 1.03, 1.06])
|
||||
raw_pedal_gas = float(np.clip(pedaloffset + accel_cmd * accel_gain * accel_term_scale, 0.0, 1.0))
|
||||
|
||||
@@ -428,12 +428,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
if self.CP.carFingerprint in CC_ONLY_CAR:
|
||||
# gas interceptor only used for full long control on cars without ACC
|
||||
pedal_accel_cmd = actuators.accel
|
||||
if (long_pitch_for_powertrain and
|
||||
len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping):
|
||||
pedal_accel_cmd += math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
|
||||
|
||||
interceptor_gas_cmd, press_regen_paddle = self.calc_pedal_command(pedal_accel_cmd, CC.longActive, CS.out.vEgo)
|
||||
interceptor_gas_cmd, press_regen_paddle = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo)
|
||||
|
||||
if self.CP.enableGasInterceptorDEPRECATED and self.apply_gas > self.params.INACTIVE_REGEN and CS.out.cruiseState.standstill:
|
||||
interceptor_gas_cmd = self.params.SNG_INTERCEPTOR_GAS
|
||||
@@ -493,7 +488,7 @@ class CarController(CarControllerBase):
|
||||
# GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
|
||||
can_sends.append(gmcan.create_gas_regen_command(
|
||||
self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, acc_engaged, at_full_stop,
|
||||
include_always_one3=self.CP.carFingerprint in kaofui_cars))
|
||||
include_always_one3=self.CP.carFingerprint in kaofui_cars, use_volt_layout=self.is_volt))
|
||||
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake,
|
||||
idx, CC.enabled, near_stop, at_full_stop, self.CP))
|
||||
|
||||
|
||||
@@ -115,9 +115,29 @@ def create_adas_keepalive(bus):
|
||||
return [CanData(0x409, dat, bus), CanData(0x40a, dat, bus)]
|
||||
|
||||
|
||||
def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop, include_always_one3=False):
|
||||
# Keep GM camera-long GasRegen bytes aligned with StarPilot's legacy layout.
|
||||
# The regenerated DBC shape does not pack to the same wire format on Global A.
|
||||
def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop, include_always_one3=False, use_volt_layout=False):
|
||||
if use_volt_layout:
|
||||
values = {
|
||||
"GasRegenCmdActive": enabled,
|
||||
"RollingCounter": idx,
|
||||
"GasRegenCmdActiveInv": 1 - enabled,
|
||||
"GasRegenCmd": throttle,
|
||||
"GasRegenFullStopActive": at_full_stop,
|
||||
"GasRegenAlwaysOne": 1,
|
||||
"GasRegenAlwaysOne2": 1,
|
||||
}
|
||||
if include_always_one3:
|
||||
values["GasRegenAlwaysOne3"] = 1
|
||||
|
||||
dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[1]
|
||||
values["GasRegenChecksum"] = (((0xff - dat[1]) & 0xff) << 16) | \
|
||||
(((0xff - dat[2]) & 0xff) << 8) | \
|
||||
((0x100 - dat[3] - idx) & 0xff)
|
||||
|
||||
return packer.make_can_msg("ASCMGasRegenCmd", bus, values)
|
||||
|
||||
# Keep GM Global A camera-long GasRegen bytes aligned with StarPilot's legacy layout.
|
||||
# The regenerated DBC shape does not expose the same wire-format helper fields.
|
||||
throttle = int(throttle)
|
||||
dat = bytearray(8)
|
||||
dat[0] = ((idx & 0x3) << 6) | int(enabled)
|
||||
|
||||
@@ -317,7 +317,6 @@ static void gm_rx_hook(const CANPacket_t *msg) {
|
||||
gm_update_periodic_phase(&gm_bd_state, now_us, GM_PADDLE_PERIOD_US, GM_PADDLE_LOCK_TOLERANCE_US);
|
||||
gm_bd_state.next_tx_us = now_us + GM_PADDLE_TX_OFFSET_US;
|
||||
gm_try_send_periodic_spoof(now_us, 0xBDU, 7U, &gm_bd_state, GM_PADDLE_PERIOD_US);
|
||||
gm_try_send_periodic_spoof(now_us, 0x1F5U, 8U, &gm_prndl2_state, GM_PADDLE_PERIOD_US);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -326,7 +325,6 @@ static void gm_rx_hook(const CANPacket_t *msg) {
|
||||
gm_update_periodic_phase(&gm_prndl2_state, now_us, GM_PADDLE_PERIOD_US, GM_PADDLE_LOCK_TOLERANCE_US);
|
||||
gm_prndl2_state.next_tx_us = now_us + GM_PADDLE_TX_OFFSET_US;
|
||||
gm_try_send_periodic_spoof(now_us, 0x1F5U, 8U, &gm_prndl2_state, GM_PADDLE_PERIOD_US);
|
||||
gm_try_send_periodic_spoof(now_us, 0xBDU, 7U, &gm_bd_state, GM_PADDLE_PERIOD_US);
|
||||
}
|
||||
|
||||
// Pedal Interceptor
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-6a639c86-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-10d7d7d1-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
DEV-6a639c86-DEBUG
|
||||
DEV-10d7d7d1-DEBUG
|
||||
@@ -105,8 +105,6 @@ FCW_IDXS = T_IDXS < 5.0
|
||||
T_DIFFS = np.diff(T_IDXS, prepend=[0.])
|
||||
COMFORT_BRAKE = 2.5
|
||||
STOP_DISTANCE = 6.0
|
||||
CRUISE_MIN_ACCEL = -1.2
|
||||
CRUISE_MAX_ACCEL = 1.6
|
||||
|
||||
def get_jerk_factor(aggressive_jerk_acceleration=0.5, aggressive_jerk_danger=0.5, aggressive_jerk_speed=0.5,
|
||||
standard_jerk_acceleration=1.0, standard_jerk_danger=1.0, standard_jerk_speed=1.0,
|
||||
@@ -155,13 +153,17 @@ def get_T_FOLLOW(aggressive_follow=1.25, standard_follow=1.45, relaxed_follow=1.
|
||||
def get_stopped_equivalence_factor(v_lead):
|
||||
return (v_lead**2) / (2 * COMFORT_BRAKE)
|
||||
|
||||
def get_safe_obstacle_distance(v_ego, t_follow, stop_distance=STOP_DISTANCE):
|
||||
def get_safe_obstacle_distance(v_ego, t_follow):
|
||||
from openpilot.common.params import Params
|
||||
params = Params()
|
||||
stop_str = params.get("StopDistance", encoding="utf8")
|
||||
stop_distance = float(stop_str) if stop_str else STOP_DISTANCE
|
||||
return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance
|
||||
|
||||
def desired_follow_distance(v_ego, v_lead, t_follow=None, stop_distance=STOP_DISTANCE):
|
||||
def desired_follow_distance(v_ego, v_lead, t_follow=None):
|
||||
if t_follow is None:
|
||||
t_follow = get_T_FOLLOW()
|
||||
return get_safe_obstacle_distance(v_ego, t_follow, stop_distance) - get_stopped_equivalence_factor(v_lead)
|
||||
return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
|
||||
|
||||
|
||||
def gen_long_model():
|
||||
@@ -303,7 +305,6 @@ class LongitudinalMpc:
|
||||
self.source = SOURCES[2]
|
||||
# Initialize smoothing filters with default time constants
|
||||
self.current_filter_time = LEAD_FILTER_TIME_LOW
|
||||
self.prev_filter_time = self.current_filter_time
|
||||
self.lead_a_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt)
|
||||
self.lead_v_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt)
|
||||
# Slew-limited filter factor to avoid abrupt 0.50↔1.00 jumps
|
||||
@@ -318,7 +319,6 @@ class LongitudinalMpc:
|
||||
# Initialize acceleration limits to prevent AttributeError
|
||||
self.cruise_min_a = ACCEL_MIN
|
||||
self.max_a = min(ACCEL_MAX, 1.2)
|
||||
self.stop_distance = STOP_DISTANCE
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
@@ -368,9 +368,7 @@ class LongitudinalMpc:
|
||||
|
||||
def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True,
|
||||
personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0,
|
||||
uncertainty=0.0, accel_reengage=False, panic_bypass=False, stop_distance=STOP_DISTANCE):
|
||||
self.stop_distance = max(float(stop_distance), 4.0)
|
||||
|
||||
uncertainty=0.0, accel_reengage=False, panic_bypass=False):
|
||||
# Update parameters based on current speed with interpolation for smooth scaling
|
||||
speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph
|
||||
|
||||
@@ -527,7 +525,7 @@ class LongitudinalMpc:
|
||||
self.status = (lead_one.status and tracking_lead) or lead_two.status
|
||||
|
||||
lead_xv_0 = self.process_lead(lead_one, tracking_lead)
|
||||
lead_xv_1 = self.process_lead(lead_two, True)
|
||||
lead_xv_1 = self.process_lead(lead_two, v_ego)
|
||||
|
||||
# To estimate a safe distance from a moving lead, we calculate how much stopping
|
||||
# distance that lead needs as a minimum. We can add that to the current distance
|
||||
@@ -550,7 +548,7 @@ class LongitudinalMpc:
|
||||
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
|
||||
v_lower,
|
||||
v_upper)
|
||||
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow, self.stop_distance)
|
||||
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
|
||||
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
|
||||
self.source = SOURCES[np.argmin(x_obstacles[0])]
|
||||
|
||||
@@ -596,9 +594,9 @@ class LongitudinalMpc:
|
||||
# Check if it got within lead comfort range
|
||||
# TODO This should be done cleaner
|
||||
if self.mode == 'blended':
|
||||
if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow, self.stop_distance))- self.x_sol[:,0] < 0.0):
|
||||
if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow))- self.x_sol[:,0] < 0.0):
|
||||
self.source = 'lead0'
|
||||
if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow, self.stop_distance))- self.x_sol[:,0] < 0.0) and \
|
||||
if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow))- self.x_sol[:,0] < 0.0) and \
|
||||
(lead_1_obstacle[0] - lead_0_obstacle[0]):
|
||||
self.source = 'lead1'
|
||||
|
||||
|
||||
@@ -145,8 +145,6 @@ class LongitudinalPlanner:
|
||||
return float(np.clip(model_msg.temporalPose.trans[0] - v_ego, -5.0, 5.0))
|
||||
except AttributeError:
|
||||
pass
|
||||
if len(model_msg.velocity.x) == ModelConstants.IDX_N:
|
||||
return float(np.clip(model_msg.velocity.x[0] - v_ego, -5.0, 5.0))
|
||||
return 0.0
|
||||
|
||||
@staticmethod
|
||||
@@ -345,8 +343,7 @@ class LongitudinalPlanner:
|
||||
v_ego=v_ego,
|
||||
lead_dist=self.lead_dist_f if self.lead_dist_f is not None else lead_dist,
|
||||
uncertainty=uncertainty,
|
||||
panic_bypass=panic_bypass,
|
||||
stop_distance=getattr(frogpilot_toggles, "stop_distance", 6.0))
|
||||
panic_bypass=panic_bypass)
|
||||
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
# After deciding the MPC mode via get_mpc_mode(), ensure MPC uses that mode when not mlsim
|
||||
@@ -398,20 +395,33 @@ class LongitudinalPlanner:
|
||||
if -0.05 < self.a_desired < 0.05:
|
||||
self.a_desired = 0.0
|
||||
|
||||
action_t = frogpilot_toggles.longitudinalActuatorDelay + DT_MDL
|
||||
output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory,
|
||||
action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping)
|
||||
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
|
||||
output_should_stop_e2e = sm['modelV2'].action.shouldStop
|
||||
classic_model = bool(getattr(frogpilot_toggles, "classic_model", False))
|
||||
tinygrad_model = bool(getattr(frogpilot_toggles, "tinygrad_model", False))
|
||||
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
|
||||
|
||||
if self.mode == 'acc' or self.generation == 'v9':
|
||||
output_a_target = output_a_target_mpc
|
||||
self.output_should_stop = output_should_stop_mpc
|
||||
if classic_model:
|
||||
output_a_target, output_should_stop = get_accel_from_plan_classic(
|
||||
self.CP, self.v_desired_trajectory, self.a_desired_trajectory, frogpilot_toggles.vEgoStopping)
|
||||
elif tinygrad_model:
|
||||
output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(
|
||||
self.v_desired_trajectory, self.a_desired_trajectory,
|
||||
action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping)
|
||||
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
|
||||
output_should_stop_e2e = sm['modelV2'].action.shouldStop
|
||||
|
||||
if self.mode == 'acc' or self.generation == 'v9':
|
||||
output_a_target = output_a_target_mpc
|
||||
output_should_stop = output_should_stop_mpc
|
||||
else:
|
||||
output_a_target = min(output_a_target_mpc, output_a_target_e2e)
|
||||
output_should_stop = output_should_stop_e2e or output_should_stop_mpc
|
||||
else:
|
||||
output_a_target = min(output_a_target_mpc, output_a_target_e2e)
|
||||
self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
|
||||
output_a_target, output_should_stop = get_accel_from_plan(
|
||||
self.v_desired_trajectory, self.a_desired_trajectory,
|
||||
action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping)
|
||||
|
||||
self.output_a_target = float(output_a_target)
|
||||
self.output_should_stop = bool(output_should_stop)
|
||||
|
||||
def publish(self, sm, pm):
|
||||
plan_send = messaging.new_message('longitudinalPlan')
|
||||
|
||||
Binary file not shown.
@@ -418,8 +418,9 @@ class SelfdriveD:
|
||||
desired_lateral_accel = self.sm['modelV2'].action.desiredCurvature * (clipped_speed**2)
|
||||
undershooting = abs(desired_lateral_accel) / abs(1e-3 + actual_lateral_accel) > 1.2
|
||||
turning = abs(desired_lateral_accel) > 1.0
|
||||
commanded_torque_at_max = abs(lac.output) > 0.99
|
||||
# TODO: lac.saturated includes speed and other checks, should be pulled out
|
||||
if undershooting and turning and lac.saturated:
|
||||
if undershooting and turning and (lac.saturated or commanded_torque_at_max):
|
||||
if self.frogpilot_toggles.goat_scream_alert:
|
||||
self.frogpilot_events.add(FrogPilotEventName.goatSteerSaturated)
|
||||
else:
|
||||
|
||||
@@ -28,6 +28,10 @@ void HudRenderer::updateState(const UIState &s) {
|
||||
is_cruise_set = set_speed > 0 && set_speed != SET_SPEED_NA;
|
||||
is_cruise_available = set_speed != -1;
|
||||
|
||||
if (is_cruise_set && frogpilot_toggles.value("set_speed_offset").toDouble() > 0) {
|
||||
set_speed += frogpilot_toggles.value("set_speed_offset").toDouble();
|
||||
}
|
||||
|
||||
if (is_cruise_set && !is_metric) {
|
||||
set_speed *= KM_TO_MILE;
|
||||
}
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -126,6 +126,20 @@ def migrate_starpilot_default_parity(params: Params, params_cache: Params) -> No
|
||||
params.put_float("CEModelStopTime", 7.0)
|
||||
params_cache.put_float("CEModelStopTime", 7.0)
|
||||
|
||||
# Rebase default regression fix:
|
||||
# EVTuning must default to enabled on EV/direct-drive platforms to preserve
|
||||
# StarPilot acceleration profile behavior.
|
||||
carparams_blob = params.get("CarParamsPersistent") or params.get("CarParams")
|
||||
if carparams_blob is not None:
|
||||
try:
|
||||
with car.CarParams.from_bytes(carparams_blob) as cp:
|
||||
is_ev_platform = cp.transmissionType == car.CarParams.TransmissionType.direct
|
||||
if is_ev_platform and not params.get_bool("TruckTuning"):
|
||||
params.put_bool("EVTuning", True)
|
||||
params_cache.put_bool("EVTuning", True)
|
||||
except Exception:
|
||||
cloudlog.exception("Failed EVTuning EV default parity migration")
|
||||
|
||||
cloudlog.warning("Applied one-time StarPilot default parity migration for lateral/longitudinal toggles")
|
||||
|
||||
try:
|
||||
|
||||
Reference in New Issue
Block a user