diff --git a/common/libcommon.a b/common/libcommon.a index adac862e..4a5f3490 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_pyx.so b/common/params_pyx.so index ddfd9e62..0c02c0f0 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index fa383a62..cd8305bb 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -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) diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index 9905b630..cf8e5632 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -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) diff --git a/frogpilot/controls/lib/frogpilot_following.py b/frogpilot/controls/lib/frogpilot_following.py index 8c236b8d..176a4fe8 100644 --- a/frogpilot/controls/lib/frogpilot_following.py +++ b/frogpilot/controls/lib/frogpilot_following.py @@ -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 diff --git a/frogpilot/controls/lib/frogpilot_vcruise.py b/frogpilot/controls/lib/frogpilot_vcruise.py index 6a560082..b17260f0 100644 --- a/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/frogpilot/controls/lib/frogpilot_vcruise.py @@ -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 diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index b60bc9c6..6fa99105 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -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)) diff --git a/opendbc_repo/opendbc/car/gm/gmcan.py b/opendbc_repo/opendbc/car/gm/gmcan.py index df10e8cf..3faaf11d 100644 --- a/opendbc_repo/opendbc/car/gm/gmcan.py +++ b/opendbc_repo/opendbc/car/gm/gmcan.py @@ -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) diff --git a/opendbc_repo/opendbc/safety/modes/gm.h b/opendbc_repo/opendbc/safety/modes/gm.h index 39e2fb4a..6e550053 100644 --- a/opendbc_repo/opendbc/safety/modes/gm.h +++ b/opendbc_repo/opendbc/safety/modes/gm.h @@ -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 diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 9fc57018..3f58ab9e 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/bootstub.elf b/panda/board/obj/body_h7/bootstub.elf index 70bfe2e5..c8c21709 100755 Binary files a/panda/board/obj/body_h7/bootstub.elf and b/panda/board/obj/body_h7/bootstub.elf differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index 7059f4fc..3e832805 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index 5ce712e4..c88d0a11 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index 3525569a..ae7faadf 100755 Binary files a/panda/board/obj/bootstub.body_h7.bin and b/panda/board/obj/bootstub.body_h7.bin differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index e7929e1b..3e63a65e 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index 180c6273..b6dcac36 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote.bin b/panda/board/obj/bootstub.panda_h7_remote.bin index 180c6273..b6dcac36 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote.bin and b/panda/board/obj/bootstub.panda_h7_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_jungle_h7.bin b/panda/board/obj/bootstub.panda_jungle_h7.bin index f785c32a..0b0e1826 100755 Binary files a/panda/board/obj/bootstub.panda_jungle_h7.bin and b/panda/board/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_remote.bin b/panda/board/obj/bootstub.panda_remote.bin index e7929e1b..3e63a65e 100755 Binary files a/panda/board/obj/bootstub.panda_remote.bin and b/panda/board/obj/bootstub.panda_remote.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index 074a85a8..697cfb96 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -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"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 12190579..5364585d 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda/bootstub.elf b/panda/board/obj/panda/bootstub.elf index 652b6b96..4c2e5d76 100755 Binary files a/panda/board/obj/panda/bootstub.elf and b/panda/board/obj/panda/bootstub.elf differ diff --git a/panda/board/obj/panda/main.bin b/panda/board/obj/panda/main.bin index 74d5a7bb..bf3f2f89 100755 Binary files a/panda/board/obj/panda/main.bin and b/panda/board/obj/panda/main.bin differ diff --git a/panda/board/obj/panda/main.elf b/panda/board/obj/panda/main.elf index 4758df03..804265ea 100755 Binary files a/panda/board/obj/panda/main.elf and b/panda/board/obj/panda/main.elf differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index 659eaa9c..f241344f 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/bootstub.elf b/panda/board/obj/panda_h7/bootstub.elf index a59f31b1..dab396e5 100755 Binary files a/panda/board/obj/panda_h7/bootstub.elf and b/panda/board/obj/panda_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index 723750da..560b1001 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index 08730e27..ceaffdb4 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index 936615ab..1dabcd91 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote/bootstub.elf b/panda/board/obj/panda_h7_remote/bootstub.elf index 50782234..90bbfcf8 100755 Binary files a/panda/board/obj/panda_h7_remote/bootstub.elf and b/panda/board/obj/panda_h7_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote/main.bin b/panda/board/obj/panda_h7_remote/main.bin index 3830dc1d..d5212bc9 100755 Binary files a/panda/board/obj/panda_h7_remote/main.bin and b/panda/board/obj/panda_h7_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_remote/main.elf b/panda/board/obj/panda_h7_remote/main.elf index 82e1391b..a5704c85 100755 Binary files a/panda/board/obj/panda_h7_remote/main.elf and b/panda/board/obj/panda_h7_remote/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index 6286fded..40ab47a2 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/bootstub.elf b/panda/board/obj/panda_jungle_h7/bootstub.elf index e185e9d8..088d81ed 100755 Binary files a/panda/board/obj/panda_jungle_h7/bootstub.elf and b/panda/board/obj/panda_jungle_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index 6b438ea5..adf7d66e 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index 2460d22c..5aeb0998 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed index c8c33bf9..f1b0509f 100644 Binary files a/panda/board/obj/panda_remote.bin.signed and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote/bootstub.elf b/panda/board/obj/panda_remote/bootstub.elf index 25dbad77..7bb7c1a6 100755 Binary files a/panda/board/obj/panda_remote/bootstub.elf and b/panda/board/obj/panda_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_remote/main.bin b/panda/board/obj/panda_remote/main.bin index 44535f30..c10a6dd7 100755 Binary files a/panda/board/obj/panda_remote/main.bin and b/panda/board/obj/panda_remote/main.bin differ diff --git a/panda/board/obj/panda_remote/main.elf b/panda/board/obj/panda_remote/main.elf index 93371214..ab1a676d 100755 Binary files a/panda/board/obj/panda_remote/main.elf and b/panda/board/obj/panda_remote/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index 76455bbe..5cd3cf67 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-6a639c86-DEBUG \ No newline at end of file +DEV-10d7d7d1-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index fd096c27..e7caee56 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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' diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 24b97a42..5311a3b5 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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') diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index acfc5148..5340e20f 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 112f58c0..0698ae15 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -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: diff --git a/selfdrive/ui/qt/onroad/hud.cc b/selfdrive/ui/qt/onroad/hud.cc index c08072da..1e376bc4 100644 --- a/selfdrive/ui/qt/onroad/hud.cc +++ b/selfdrive/ui/qt/onroad/hud.cc @@ -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; } diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 7cbb5242..1a47393c 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 29777620..daa4c808 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index be5da2c8..41c64ec9 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index be0a7528..132b781b 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index a63c4372..b7c1a173 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ diff --git a/system/manager/manager.py b/system/manager/manager.py index 007e6ca1..4702d1a3 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -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: