mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-25 02:03:43 +08:00
Compare commits
23 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 235c1cab79 | |||
| eb507d5ea8 | |||
| 159ca85be2 | |||
| 40d8eb8a4e | |||
| ebf77baf33 | |||
| 7c2fa00aec | |||
| 9bd0463c64 | |||
| e247249033 | |||
| ea48457d72 | |||
| 8a4e817f6b | |||
| a8767be7b1 | |||
| 0c249d253e | |||
| 9bb9d6ed2c | |||
| ceac092b67 | |||
| 2d23f740cc | |||
| 80c20399f1 | |||
| 4f65f90567 | |||
| 0a217baf18 | |||
| b81d6fb1eb | |||
| 944fc51f8a | |||
| 086570b29f | |||
| 8face05df0 | |||
| 7231aeb5ee |
Binary file not shown.
|
After Width: | Height: | Size: 371 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 661 KiB After Width: | Height: | Size: 503 KiB |
@@ -1,5 +1,6 @@
|
||||
#!/usr/bin/bash
|
||||
|
||||
|
||||
if [ -z "$BASEDIR" ]; then
|
||||
BASEDIR="/data/openpilot"
|
||||
fi
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 671 KiB After Width: | Height: | Size: 418 KiB |
@@ -1,4 +1,3 @@
|
||||
from typing import Tuple
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
@@ -8,7 +7,7 @@ from openpilot.common.params_pyx import Params
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, create_gas_interceptor_command
|
||||
from openpilot.selfdrive.car.gm import gmcan
|
||||
from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR, AccState, CC_REGEN_PADDLE_CAR
|
||||
from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR, AccState
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone
|
||||
from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
||||
@@ -23,6 +22,7 @@ TransmissionType = car.CarParams.TransmissionType
|
||||
CAMERA_CANCEL_DELAY_FRAMES = 10
|
||||
# Enforce a minimum interval between steering messages to avoid a fault
|
||||
MIN_STEER_MSG_INTERVAL_MS = 15
|
||||
|
||||
# Constants for pitch compensation
|
||||
PITCH_DEADZONE = 0.01 # [radians] 0.01 ≈ 1% grade
|
||||
BRAKE_PITCH_FACTOR_BP = [5., 10.] # [m/s] smoothly revert to planned accel at low speeds
|
||||
@@ -38,10 +38,6 @@ class CarController(CarControllerBase):
|
||||
self.apply_speed = 0
|
||||
self.frame = 0
|
||||
self.last_steer_frame = 0
|
||||
self.last_steer_ts_ns = 0
|
||||
self.last_regen_active = False
|
||||
self.prev_steer_ts_ns = 0
|
||||
self.last_spoof_ts_ns = 0
|
||||
self.last_button_frame = 0
|
||||
self.cancel_counter = 0
|
||||
self.pedal_steady = 0.
|
||||
@@ -59,64 +55,24 @@ class CarController(CarControllerBase):
|
||||
# FrogPilot variables
|
||||
self.pitch = FirstOrderFilter(0., 0.09 * 4, DT_CTRL * 4) # runs at 25 Hz
|
||||
self.accel_g = 0.0
|
||||
self.regen_paddle_pressed = False
|
||||
self.aego = 0.0
|
||||
self.regen_paddle_timer = 0
|
||||
|
||||
# Midpoint + overflow spoof accumulator and flags
|
||||
self.spoof_accum = 0.0
|
||||
self.spoof_mid_sent = False
|
||||
self.spoof_over_sent = False
|
||||
self.last_interval_ns = 0
|
||||
@staticmethod
|
||||
def calc_pedal_command(accel: float, long_active: bool, car_velocity) -> float:
|
||||
if not long_active: return 0.
|
||||
|
||||
def calc_pedal_command(self, accel: float, long_active: bool, car_velocity) -> Tuple[float, bool]:
|
||||
if not long_active:
|
||||
return 0., False
|
||||
if accel < -0.5:
|
||||
pedal_gas = 0
|
||||
else:
|
||||
|
||||
# Regen paddle hysteresis (frame-based): hold 30 frames, with decrement dead-zone
|
||||
if not hasattr(self, 'regen_paddle_timer'):
|
||||
self.regen_paddle_timer = 0 # frames
|
||||
|
||||
# Regen paddle hysteresis (frame‑based): count frames when decelerating hard, decrement only when truly released
|
||||
if self.aego < -0.7:
|
||||
self.regen_paddle_timer += 1
|
||||
elif self.aego > -0.3:
|
||||
self.regen_paddle_timer = max(self.regen_paddle_timer - 1, 0)
|
||||
# else: hold timer between -0.7 and -0.3
|
||||
|
||||
# Base paddle press hysteresis
|
||||
self.regen_paddle_pressed = self.regen_paddle_timer >= 20 # 30 frames
|
||||
press_regen_paddle = self.regen_paddle_pressed
|
||||
|
||||
# Regen gain ratios from bin-averaged 60–0 deceleration sweep; Calculates stronger decel from paddle
|
||||
speed_mps = [0.559, 1.678, 2.797, 3.916, 5.035, 6.154, 7.273, 8.392, 9.511, 10.63,
|
||||
11.749, 12.868, 13.987, 15.106, 16.225, 17.344, 18.463, 19.582, 20.701, 21.820,
|
||||
22.939, 24.058, 25.177, 26.296]
|
||||
regen_gain_ratio = [1.01, 1.01, 1.02, 1.05, 1.08, 1.345979, 1.369975,
|
||||
1.376302, 1.388052, 1.370367, 1.388498, 1.386030, 1.405950, 1.387555,
|
||||
1.390392, 1.394946, 1.414915, 1.428535, 1.439611, 1.440106, 1.441438,
|
||||
1.439395, 1.446909, 1.445738]
|
||||
|
||||
gain = interp(car_velocity, speed_mps, regen_gain_ratio)
|
||||
pedaloffset = interp(car_velocity, [0., 3, 6, 30], [0.10, 0.175, 0.240, 0.240])
|
||||
|
||||
# Compute raw pedal gas
|
||||
raw_pedal_gas = clip((pedaloffset + (accel / gain) * 0.6), 0.0, 1.0) if press_regen_paddle else clip((pedaloffset + accel * 0.6), 0.0, 1.0)
|
||||
|
||||
# --- Immediate application of raw pedal gas, no blending ---
|
||||
pedal_gas = raw_pedal_gas
|
||||
# Safety cap: ramp from 22% at 0 m/s to 37.25% at 10 mph (4.47 m/s), then allow full throttleAdd commentMore actions
|
||||
pedal_gas_max = interp(car_velocity, [0.0, 4.47, 4.48], [0.22, 0.3725, 1.0])
|
||||
pedal_gas = clip(pedal_gas, 0.0, pedal_gas_max)
|
||||
return pedal_gas, press_regen_paddle
|
||||
pedaloffset = interp(car_velocity, [0., 3, 6, 30], [0.10, 0.175, 0.240, 0.240])
|
||||
pedal_gas = clip((pedaloffset + accel * 0.6), 0.0, 1.0)
|
||||
|
||||
return pedal_gas
|
||||
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_toggles):
|
||||
self.CS = CS
|
||||
self.aego = CS.out.aEgo
|
||||
actuators = CC.actuators
|
||||
accel = brake_accel = actuators.accel
|
||||
press_regen_paddle = False
|
||||
hud_control = CC.hudControl
|
||||
hud_alert = hud_control.visualAlert
|
||||
hud_v_cruise = hud_control.setSpeed
|
||||
@@ -125,98 +81,6 @@ class CarController(CarControllerBase):
|
||||
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
paddle_sends = []
|
||||
|
||||
raw_regen_active = (
|
||||
self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and
|
||||
self.CP.openpilotLongitudinalControl and
|
||||
CC.longActive and
|
||||
self.CP.enableGasInterceptor and
|
||||
self.regen_paddle_timer >= 20 # raw hysteresis-only
|
||||
)
|
||||
regen_active = raw_regen_active
|
||||
|
||||
# === Spoof scheduling: midpoint + overflow (~40Hz) ===
|
||||
# Implement 1s hold-off after CAN reconnect
|
||||
# Detect reconnect: loopback_lka_steering_cmd_ts_nanos == 0 and self.last_steer_ts_ns != 0
|
||||
if CS.loopback_lka_steering_cmd_ts_nanos == 0 and self.last_steer_ts_ns != 0:
|
||||
self.loopback_holdoff_until = now_nanos + 1_000_000_000
|
||||
# Remove holdoff if expired
|
||||
if hasattr(self, "loopback_holdoff_until") and now_nanos >= self.loopback_holdoff_until:
|
||||
del self.loopback_holdoff_until
|
||||
|
||||
# Rising-edge reset on regen start
|
||||
if raw_regen_active and not self.last_regen_active:
|
||||
self.prev_steer_ts_ns = self.last_steer_ts_ns
|
||||
self.last_spoof_ts_ns = 0
|
||||
self.spoof_accum = 0.0
|
||||
self.spoof_mid_sent = False
|
||||
self.spoof_over_sent = False
|
||||
|
||||
if raw_regen_active:
|
||||
# Interval between last two bus-0 steer sends
|
||||
interval_ns = self.last_steer_ts_ns - self.prev_steer_ts_ns
|
||||
|
||||
# New steer interval? clear per-interval flags
|
||||
if interval_ns != self.last_interval_ns:
|
||||
self.spoof_mid_sent = False
|
||||
self.spoof_over_sent = False
|
||||
self.last_interval_ns = interval_ns
|
||||
|
||||
# Accumulate extra spoofs needed above 33Hz base to reach 40Hz
|
||||
self.spoof_accum += (40.0/33.0 - 1.0)
|
||||
|
||||
# 500ms holdoff after reconnect: skip spoof scheduling if active
|
||||
skip_spoof = hasattr(self, "loopback_holdoff_until") and now_nanos < self.loopback_holdoff_until
|
||||
|
||||
# Midpoint spoof: one per interval; avoid sending if loopback stale
|
||||
if not skip_spoof and not self.spoof_mid_sent and interval_ns > 0:
|
||||
midpoint_ns = self.prev_steer_ts_ns + interval_ns // 2
|
||||
if (CS.loopback_lka_steering_cmd_ts_nanos != 0 and
|
||||
now_nanos - CS.loopback_lka_steering_cmd_ts_nanos < 10_000_000 and
|
||||
now_nanos >= midpoint_ns and
|
||||
now_nanos - self.last_steer_ts_ns >= 26_000_000):
|
||||
paddle_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, True))
|
||||
paddle_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, True))
|
||||
self.last_spoof_ts_ns = now_nanos
|
||||
self.spoof_mid_sent = True
|
||||
|
||||
# Overflow spoof: insert extra when accumulator allows
|
||||
if not skip_spoof and self.spoof_accum >= 0.5 and not self.spoof_over_sent and interval_ns > 0:
|
||||
slot2_ns = self.prev_steer_ts_ns + (interval_ns * 2) // 3
|
||||
if (CS.loopback_lka_steering_cmd_ts_nanos != 0 and
|
||||
now_nanos - CS.loopback_lka_steering_cmd_ts_nanos < 10_000_000 and
|
||||
now_nanos >= slot2_ns and
|
||||
now_nanos - self.last_steer_ts_ns >= 28_000_000):
|
||||
paddle_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, True))
|
||||
paddle_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, True))
|
||||
self.last_spoof_ts_ns = now_nanos
|
||||
self.spoof_over_sent = True
|
||||
self.spoof_accum -= 0.5
|
||||
# === End Spoof scheduling ===
|
||||
|
||||
# === Off-pulse scheduling on regen release ===
|
||||
if not raw_regen_active and self.last_regen_active:
|
||||
# schedule two off-slots at 1/3 and 2/3 of the last steer interval
|
||||
if self.prev_steer_ts_ns and self.last_steer_ts_ns:
|
||||
intv = self.last_steer_ts_ns - self.prev_steer_ts_ns
|
||||
self.off_schedule_ns = [
|
||||
self.prev_steer_ts_ns + intv // 3,
|
||||
self.prev_steer_ts_ns + (2 * intv) // 3
|
||||
]
|
||||
self.off_sent = [False, False]
|
||||
|
||||
if hasattr(self, "off_schedule_ns"):
|
||||
for i, t_ns in enumerate(self.off_schedule_ns):
|
||||
if not self.off_sent[i] and now_nanos >= t_ns and now_nanos - self.last_steer_ts_ns >= 18_000_000:
|
||||
paddle_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, False))
|
||||
paddle_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, False))
|
||||
self.off_sent[i] = True
|
||||
# clean up once both off pulses are sent
|
||||
if hasattr(self, "off_sent") and all(self.off_sent):
|
||||
del self.off_schedule_ns
|
||||
del self.off_sent
|
||||
# === End off-pulse scheduling ===
|
||||
|
||||
# Steering (Active: 50Hz, inactive: 10Hz)
|
||||
steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP
|
||||
@@ -246,24 +110,11 @@ class CarController(CarControllerBase):
|
||||
else:
|
||||
apply_steer = 0
|
||||
|
||||
# shift previous steer timestamp
|
||||
self.prev_steer_ts_ns = self.last_steer_ts_ns
|
||||
self.last_steer_ts_ns = now_nanos
|
||||
self.last_steer_frame = self.frame
|
||||
self.apply_steer_last = apply_steer
|
||||
idx = self.lka_steering_cmd_counter % 4
|
||||
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive))
|
||||
|
||||
# Update regen_active state and last_regen_paddle_pressed for next loop
|
||||
self.last_regen_active = regen_active
|
||||
self.last_regen_paddle_pressed = self.regen_paddle_pressed
|
||||
|
||||
# Merge paddle spoof CAN frames, time-guarded only
|
||||
if paddle_sends:
|
||||
# wait at least 15 ms after the last bus0 steer send
|
||||
if now_nanos - self.last_steer_ts_ns >= 15_000_000:
|
||||
can_sends.extend(paddle_sends)
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
# Gas/regen, brakes, and UI commands - all at 25Hz
|
||||
if self.frame % 4 == 0:
|
||||
@@ -290,11 +141,12 @@ class CarController(CarControllerBase):
|
||||
else:
|
||||
# Normal operation
|
||||
if self.CP.carFingerprint in EV_CAR:
|
||||
self.params.update_ev_gas_brake_threshold(CS.out.vEgo)
|
||||
if frogpilot_toggles.sport_plus:
|
||||
self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
|
||||
self.apply_gas = int(round(interp(accel, self.params.EV_GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
|
||||
else:
|
||||
self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
|
||||
self.apply_brake = int(round(interp(brake_accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
|
||||
self.apply_gas = int(round(interp(accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
|
||||
self.apply_brake = int(round(interp(brake_accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
|
||||
else:
|
||||
if frogpilot_toggles.sport_plus:
|
||||
self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
|
||||
@@ -307,13 +159,12 @@ class CarController(CarControllerBase):
|
||||
self.apply_gas = self.params.INACTIVE_REGEN
|
||||
if self.CP.carFingerprint in CC_ONLY_CAR:
|
||||
# gas interceptor only used for full long control on cars without ACC
|
||||
interceptor_gas_cmd, press_regen_paddle = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo)
|
||||
interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo)
|
||||
|
||||
if self.CP.enableGasInterceptor and self.apply_gas > self.params.INACTIVE_REGEN and CS.out.cruiseState.standstill:
|
||||
# "Tap" the accelerator pedal to re-engage ACC
|
||||
interceptor_gas_cmd = self.params.SNG_INTERCEPTOR_GAS
|
||||
self.apply_brake = 0
|
||||
press_regen_paddle = False
|
||||
self.apply_gas = self.params.INACTIVE_REGEN
|
||||
|
||||
idx = (self.frame // 4) % 4
|
||||
@@ -344,30 +195,33 @@ 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))
|
||||
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))
|
||||
idx, CC.enabled, near_stop, at_full_stop, self.CP))
|
||||
|
||||
# Send dashboard UI commands (ACC status)
|
||||
send_fcw = hud_alert == VisualAlert.fcw
|
||||
if self.frame % 10 == 5:
|
||||
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled,
|
||||
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled,
|
||||
hud_v_cruise * CV.MS_TO_KPH, hud_control, send_fcw))
|
||||
else:
|
||||
# to keep accel steady for logs when not sending gas
|
||||
accel += self.accel_g
|
||||
|
||||
# Radar needs to know current speed and yaw rate (50hz),
|
||||
# and that ADAS is alive (5hz, previously 10hz)
|
||||
# and that ADAS is alive (10hz)
|
||||
if not self.CP.radarUnavailable:
|
||||
tt = self.frame * DT_CTRL
|
||||
time_and_headlights_step = 20
|
||||
time_and_headlights_step = 10
|
||||
if self.frame % time_and_headlights_step == 0:
|
||||
idx = (self.frame // time_and_headlights_step) % 4
|
||||
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx))
|
||||
can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE))
|
||||
|
||||
speed_and_accelerometer_step = 2
|
||||
if self.frame % speed_and_accelerometer_step == 0:
|
||||
idx = (self.frame // speed_and_accelerometer_step) % 4
|
||||
can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
|
||||
can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx))
|
||||
|
||||
if self.CP.networkLocation == NetworkLocation.gateway and self.frame % (self.params.ADAS_KEEPALIVE_STEP * 2) == 0:
|
||||
if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0:
|
||||
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
|
||||
|
||||
# TODO: integrate this with the code block below?
|
||||
@@ -395,7 +249,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
|
||||
if self.frame % 20 == 0:
|
||||
if self.frame % 10 == 0:
|
||||
can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status))
|
||||
|
||||
new_actuators = actuators.as_builder()
|
||||
|
||||
@@ -56,10 +56,6 @@ class CarState(CarStateBase):
|
||||
self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0
|
||||
if self.loopback_lka_steering_cmd_updated:
|
||||
self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"]
|
||||
|
||||
# Track timestamps for OEM PRNDL2 and Regen Paddle messages (used to sync spoofing timing)
|
||||
self.prndl2_ts_nanos = pt_cp.ts_nanos["ECMPRDNL2"]["PRNDL2"]
|
||||
self.regen_paddle_ts_nanos = pt_cp.ts_nanos["EBCMRegenPaddle"]["RegenPaddle"]
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.flags & GMFlags.NO_CAMERA.value:
|
||||
self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
|
||||
self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
|
||||
@@ -105,7 +101,7 @@ class CarState(CarStateBase):
|
||||
|
||||
if self.CP.enableGasInterceptor:
|
||||
ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
|
||||
threshold = 12 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 # Panda 595 threshold = 10.88. Set lower to avoid panda blocking messages and GasInterceptor faulting.
|
||||
threshold = 12 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 # Panda 515 threshold = 10.88. Set lower to avoid panda blocking messages and GasInterceptor faulting.
|
||||
ret.gasPressed = ret.gas > threshold
|
||||
else:
|
||||
ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
|
||||
@@ -178,7 +174,7 @@ class CarState(CarStateBase):
|
||||
ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
|
||||
ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
|
||||
|
||||
|
||||
# FrogPilot CarState functions
|
||||
self.lkas_previously_enabled = self.lkas_enabled
|
||||
if self.CP.carFingerprint in SDGM_CAR:
|
||||
self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"]
|
||||
@@ -239,7 +235,7 @@ class CarState(CarStateBase):
|
||||
]
|
||||
else:
|
||||
messages += [
|
||||
("ECMPRDNL2", 40),
|
||||
("ECMPRDNL2", 10),
|
||||
("AcceleratorPedal2", 33),
|
||||
("ECMEngineStatus", 100),
|
||||
("BCMTurnSignals", 1),
|
||||
@@ -261,7 +257,7 @@ class CarState(CarStateBase):
|
||||
|
||||
if CP.transmissionType == TransmissionType.direct:
|
||||
messages += [
|
||||
("EBCMRegenPaddle", 40),
|
||||
("EBCMRegenPaddle", 50),
|
||||
("EVDriveMode", 0),
|
||||
]
|
||||
|
||||
|
||||
@@ -177,33 +177,6 @@ def create_lka_icon_command(bus, active, critical, steer):
|
||||
dat = b"\x00\x00\x00"
|
||||
return make_can_msg(0x104c006c, dat, bus)
|
||||
|
||||
def create_prndl2_command(packer, bus, press_regen_paddle):
|
||||
prndl2_value = 7 if press_regen_paddle else 6
|
||||
manual_mode = 1 if press_regen_paddle else 0
|
||||
values = {
|
||||
"Byte0": 0x0C,
|
||||
"Byte1": 0x0C,
|
||||
"Byte2": 0x00,
|
||||
"PRNDL2": prndl2_value,
|
||||
"Byte4": 0x00,
|
||||
"ManualMode": manual_mode,
|
||||
"TransmissionState": 1,
|
||||
"Byte7": 0x00
|
||||
}
|
||||
return packer.make_can_msg("ECMPRDNL2", bus, values)
|
||||
|
||||
def create_regen_paddle_command(packer, bus, press_regen_paddle):
|
||||
regen_paddle_value = 2 if press_regen_paddle else 0
|
||||
values = {
|
||||
"RegenPaddle": regen_paddle_value,
|
||||
"Byte1": 0,
|
||||
"Byte2": 0,
|
||||
"Byte3": 0,
|
||||
"Byte4": 0,
|
||||
"Byte5": 0,
|
||||
"Byte6": 0
|
||||
}
|
||||
return packer.make_can_msg("EBCMRegenPaddle", bus, values)
|
||||
|
||||
def create_gm_cc_spam_command(packer, controller, CS, actuators):
|
||||
if controller.params_.get_bool("IsMetric"):
|
||||
|
||||
@@ -12,8 +12,8 @@ from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerP
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
|
||||
|
||||
from openpilot.common.params import Params
|
||||
params = Params()
|
||||
from openpilot.frogpilot.common.frogpilot_variables import params
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
|
||||
EventName = car.CarEvent.EventName
|
||||
@@ -29,10 +29,10 @@ CAM_MSG = 0x320 # AEBCmd
|
||||
ACCELERATOR_POS_MSG = 0xbe
|
||||
|
||||
NON_LINEAR_TORQUE_PARAMS = {
|
||||
CAR.CHEVROLET_BOLT_EUV: {"left": [2.15, 1.0, 0.17, -0.04], "right": [2.15, 1.0, 0.21, -0.04]},
|
||||
CAR.CHEVROLET_BOLT_CC: {"left": [2.15, 1.0, 0.17, -0.04], "right": [2.15, 1.0, 0.21, -0.04]},
|
||||
CAR.GMC_ACADIA: {"left": [4.78, 1.0, 0.31, 0.05], "right": [4.78, 1.0, 0.31, 0.05]},
|
||||
CAR.CHEVROLET_SILVERADO: {"left": [3.30, 1.0, 0.25, 0.05], "right": [3.30, 1.0, 0.25, 0.05]}
|
||||
CAR.CHEVROLET_BOLT_EUV: [1.8, 1.1, 0.290, -0.045],
|
||||
CAR.CHEVROLET_BOLT_CC: [1.8, 1.1, 0.290, -0.045],
|
||||
CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
|
||||
CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122]
|
||||
}
|
||||
|
||||
NEURAL_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/neural_ff_weights.json')
|
||||
@@ -74,11 +74,7 @@ class CarInterface(CarInterfaceBase):
|
||||
# ToDo: To generalize to other GMs, explore tanh function as the nonlinear
|
||||
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
|
||||
assert non_linear_torque_params, "The params are not defined"
|
||||
if latcontrol_inputs.lateral_acceleration >= 0:
|
||||
params = non_linear_torque_params["right"]
|
||||
else:
|
||||
params = non_linear_torque_params["left"]
|
||||
a, b, c, d = params
|
||||
a, b, c, d = non_linear_torque_params
|
||||
steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c) + d
|
||||
return float(steer_torque) + friction
|
||||
|
||||
@@ -122,13 +118,14 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
|
||||
|
||||
# Tuning for experimental long
|
||||
ret.longitudinalTuning.kiV = [2.0, 1.5]
|
||||
ret.longitudinalTuning.kiV = [0.5, 0.5, 0.5]
|
||||
ret.vEgoStopping = 0.1
|
||||
ret.vEgoStarting = 0.1
|
||||
|
||||
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
|
||||
ret.stoppingDecelRate = 1.0 # reach brake quickly after enabling
|
||||
ret.vEgoStopping = 0.25
|
||||
ret.vEgoStarting = 0.25
|
||||
ret.stopAccel = -0.25
|
||||
|
||||
if experimental_long:
|
||||
ret.pcmCruise = False
|
||||
@@ -136,7 +133,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
|
||||
|
||||
elif candidate in SDGM_CAR:
|
||||
ret.longitudinalTuning.kiV = [0., 0.] # TODO: tuning
|
||||
ret.longitudinalTuning.kiV = [0., 0., 0.] # TODO: tuning
|
||||
ret.experimentalLongitudinalAvailable = False
|
||||
ret.networkLocation = NetworkLocation.fwdCamera
|
||||
ret.pcmCruise = True
|
||||
@@ -146,7 +143,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_SDGM
|
||||
|
||||
else: # ASCM, OBD-II harness
|
||||
ret.openpilotLongitudinalControl = not params.get_bool("DisableOpenpilotLongitudinal")
|
||||
ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long
|
||||
ret.networkLocation = NetworkLocation.gateway
|
||||
ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs
|
||||
ret.pcmCruise = False # stock non-adaptive cruise control is kept off
|
||||
@@ -155,7 +152,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
|
||||
|
||||
# Tuning
|
||||
ret.longitudinalTuning.kiV = [2.4, 1.5]
|
||||
ret.longitudinalTuning.kiV = [0.5, 0.5, 0.5]
|
||||
|
||||
if ret.enableGasInterceptor:
|
||||
# Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits
|
||||
@@ -206,7 +203,7 @@ class CarInterface(CarInterfaceBase):
|
||||
elif candidate in (CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_BOLT_CC):
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
ret.lateralTuning.torque.kp = 0.4
|
||||
ret.lateralTuning.torque.kp = 0.6
|
||||
|
||||
if ret.enableGasInterceptor:
|
||||
# ACC Bolts use pedal for full longitudinal control, not just sng
|
||||
@@ -268,11 +265,11 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
|
||||
ret.minEnableSpeed = -1
|
||||
ret.pcmCruise = False
|
||||
ret.openpilotLongitudinalControl = not params.get_bool("DisableOpenpilotLongitudinal")
|
||||
ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long
|
||||
ret.stoppingControl = True
|
||||
ret.autoResumeSng = True
|
||||
|
||||
if candidate in CC_ONLY_CAR:
|
||||
if candidate in CC_ONLY_CAR: #pedal interceptor tuning
|
||||
ret.flags |= GMFlags.PEDAL_LONG.value
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_PEDAL_LONG
|
||||
# Note: Low speed, stop and go not tested. Should be fairly smooth on highway
|
||||
@@ -292,10 +289,11 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.radarUnavailable = True
|
||||
ret.experimentalLongitudinalAvailable = False
|
||||
ret.minEnableSpeed = 24 * CV.MPH_TO_MS
|
||||
ret.openpilotLongitudinalControl = not params.get_bool("DisableOpenpilotLongitudinal")
|
||||
ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long
|
||||
ret.pcmCruise = False
|
||||
experimental_long = False
|
||||
|
||||
if not ret.enableGasInterceptor and candidate in CC_ONLY_CAR:
|
||||
if not ret.enableGasInterceptor and candidate in CC_ONLY_CAR: #redneck tuning
|
||||
ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph
|
||||
ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
|
||||
+20
-10
@@ -11,13 +11,13 @@ Ecu = car.CarParams.Ecu
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_MAX = 450 # GM limit is 3Nm. Used by carcontroller to generate LKA output
|
||||
STEER_MAX = 300 # GM limit is 3Nm. Used by carcontroller to generate LKA output
|
||||
STEER_STEP = 3 # Active control frames per command (~33hz)
|
||||
INACTIVE_STEER_STEP = 10 # Inactive control frames per command (10hz)
|
||||
STEER_DELTA_UP = 15 # Delta rates require review due to observed EPS weakness
|
||||
STEER_DELTA_DOWN = 34
|
||||
STEER_DRIVER_ALLOWANCE = 78
|
||||
STEER_DRIVER_MULTIPLIER = 6
|
||||
STEER_DELTA_UP = 10 # Delta rates require review due to observed EPS weakness
|
||||
STEER_DELTA_DOWN = 15
|
||||
STEER_DRIVER_ALLOWANCE = 65
|
||||
STEER_DRIVER_MULTIPLIER = 4
|
||||
STEER_DRIVER_FACTOR = 100
|
||||
NEAR_STOP_BRAKE_PHASE = 0.25 # m/s
|
||||
SNG_INTERCEPTOR_GAS = 18. / 255.
|
||||
@@ -41,7 +41,7 @@ class CarControllerParams:
|
||||
self.ZERO_GAS = 6144 # Coasting
|
||||
self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen
|
||||
|
||||
if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_CAR and CP.carFingerprint != CAR.CHEVROLET_BOLT_EUV:
|
||||
if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_CAR:
|
||||
self.MAX_GAS = 7496
|
||||
self.MAX_GAS_PLUS = 8848
|
||||
self.MAX_ACC_REGEN = 5610
|
||||
@@ -53,18 +53,18 @@ class CarControllerParams:
|
||||
elif CP.carFingerprint in SDGM_CAR:
|
||||
self.MAX_GAS = 7496
|
||||
self.MAX_GAS_PLUS = 7496
|
||||
self.MAX_ACC_REGEN = 7110
|
||||
self.MAX_ACC_REGEN = 5610
|
||||
self.INACTIVE_REGEN = 5650
|
||||
self.max_regen_acceleration = 0.
|
||||
|
||||
else:
|
||||
self.MAX_GAS = 7168 # Safety limit, not ACC max. Stock ACC >8192 from standstill.
|
||||
self.MAX_GAS_PLUS = 8191 # 8292 uses new bit, possible but not tested. Matches Twilsonco tw-main max
|
||||
self.MAX_ACC_REGEN = 7110 # Increased for stronger regen braking
|
||||
self.MAX_ACC_REGEN = 5500 # Max ACC regen is slightly less than max paddle regen
|
||||
self.INACTIVE_REGEN = 5500
|
||||
# ICE has much less engine braking force compared to regen in EVs,
|
||||
# lower threshold removes some braking deadzone
|
||||
self.max_regen_acceleration = -3. if CP.carFingerprint in EV_CAR else -0.1 # More aggressive regen for EVs
|
||||
self.max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1
|
||||
|
||||
self.GAS_LOOKUP_BP = [self.max_regen_acceleration, 0., self.ACCEL_MAX]
|
||||
self.GAS_LOOKUP_BP_PLUS = [self.max_regen_acceleration, 0., self.ACCEL_MAX_PLUS]
|
||||
@@ -74,7 +74,18 @@ class CarControllerParams:
|
||||
self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, self.max_regen_acceleration]
|
||||
self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.]
|
||||
|
||||
# determined by letting Volt regen to a stop in L gear from 89mph,
|
||||
# and by letting off gas and allowing car to creep, for determining
|
||||
# the positive threshold values at very low speed
|
||||
EV_GAS_BRAKE_THRESHOLD_BP = [1.29, 1.52, 1.55, 1.6, 1.7, 1.8, 2.0, 2.2, 2.5, 5.52, 9.6, 20.5, 23.5, 35.0] # [m/s]
|
||||
EV_GAS_BRAKE_THRESHOLD_V = [0.0, -0.14, -0.16, -0.18, -0.215, -0.255, -0.32, -0.41, -0.5, -0.72, -0.895, -1.125, -1.145, -1.16] # [m/s^s]
|
||||
|
||||
def update_ev_gas_brake_threshold(self, v_ego):
|
||||
gas_brake_threshold = interp(v_ego, self.EV_GAS_BRAKE_THRESHOLD_BP, self.EV_GAS_BRAKE_THRESHOLD_V)
|
||||
self.GAS_LOOKUP_BP_PLUS = [self.max_regen_acceleration, 0., self.ACCEL_MAX_PLUS]
|
||||
self.EV_GAS_LOOKUP_BP = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX]
|
||||
self.EV_GAS_LOOKUP_BP_PLUS = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX_PLUS]
|
||||
self.EV_BRAKE_LOOKUP_BP = [self.ACCEL_MIN, gas_brake_threshold]
|
||||
|
||||
@dataclass
|
||||
class GMCarDocs(CarDocs):
|
||||
@@ -311,7 +322,6 @@ FW_QUERY_CONFIG = FwQueryConfig(
|
||||
|
||||
EV_CAR = {CAR.CHEVROLET_VOLT, CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC}
|
||||
CC_ONLY_CAR = {CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_BOLT_CC, CAR.CHEVROLET_EQUINOX_CC, CAR.CHEVROLET_SUBURBAN_CC, CAR.GMC_YUKON_CC, CAR.CADILLAC_CT6_CC, CAR.CHEVROLET_TRAILBLAZER_CC, CAR.CADILLAC_XT5_CC, CAR.CHEVROLET_MALIBU_CC}
|
||||
CC_REGEN_PADDLE_CAR = {CAR.CHEVROLET_BOLT_CC, CAR.CHEVROLET_BOLT_EUV}
|
||||
# CC_ONLY_CAR = set(c for c in CAR if str(c).endswith('_CC'))
|
||||
|
||||
# We're integrated at the Safety Data Gateway Module on these cars
|
||||
|
||||
@@ -412,7 +412,7 @@ class CarInterfaceBase(ABC):
|
||||
|
||||
tune.init('torque')
|
||||
tune.torque.useSteeringAngle = use_steering_angle
|
||||
tune.torque.kp = 0.4
|
||||
tune.torque.kp = 0.6
|
||||
tune.torque.kf = 1.0
|
||||
tune.torque.ki = 0.1
|
||||
tune.torque.friction = params['FRICTION']
|
||||
|
||||
@@ -43,7 +43,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
||||
"CADILLAC_ESCALADE" = [1.899999976158142, 1.842270016670227, 0.1120000034570694]
|
||||
"CADILLAC_ESCALADE_ESV_2019" = [1.15, 1.3, 0.2]
|
||||
"CADILLAC_XT4" = [1.45, 1.6, 0.2]
|
||||
"CHEVROLET_BOLT_EUV" = [3.0, 2.0, 0.11]
|
||||
"CHEVROLET_BOLT_EUV" = [2.0, 2.0, 0.05]
|
||||
"CHEVROLET_MALIBU_CC" = [1.85, 1.85, 0.075]
|
||||
"CHEVROLET_SILVERADO" = [1.9, 1.9, 0.112]
|
||||
"CHEVROLET_TRAILBLAZER" = [1.33, 1.9, 0.16]
|
||||
|
||||
@@ -1,13 +1,18 @@
|
||||
[
|
||||
{
|
||||
"name": "boot",
|
||||
"url": "https://boot.frogpilot.download",
|
||||
"url": "https://www.dropbox.com/scl/fi/z8gcamb7n78xqb515kfgq/boot.img.xz?rlkey=r2zxothb3pz0q9rtqysr1zhwv&st=f0acze3w&dl=1",
|
||||
"hash": "b997aae3f1c93de82449ef7f23f30ff482b0978f3d0ac08219366f9ce362ad7a",
|
||||
"hash_raw": "b997aae3f1c93de82449ef7f23f30ff482b0978f3d0ac08219366f9ce362ad7a",
|
||||
"size": 16029696,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true
|
||||
"has_ab": true,
|
||||
"alt": {
|
||||
"hash": "5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/boot-5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f.img.xz",
|
||||
"size": 16029696
|
||||
}
|
||||
},
|
||||
{
|
||||
"name": "abl",
|
||||
@@ -61,7 +66,7 @@
|
||||
},
|
||||
{
|
||||
"name": "system",
|
||||
"url": "https://system.frogpilot.download",
|
||||
"url": "https://www.dropbox.com/scl/fi/n22f3eex1z52dbrhhxqry/system.img.xz?rlkey=yw4ult7s3sdm6b7d31hrm3zx8&st=of6m7zis&dl=1",
|
||||
"hash": "be1c6bb9ee5e06779087b1b81e09b6df61d942566b0f8d4539c452179c661782",
|
||||
"hash_raw": "a5f84e68d199466fda5c9aead760b90a4cd2d2ef9a418708b9794d95bb03ec5b",
|
||||
"size": 10737418240,
|
||||
@@ -69,9 +74,9 @@
|
||||
"full_check": false,
|
||||
"has_ab": true,
|
||||
"alt": {
|
||||
"hash": "bc11d2148f29862ee1326aca2af1cf6bbf5fed831e3f8f6b8f7a0f110dfe8d26",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a.img.xz",
|
||||
"size": 4548070000
|
||||
"hash": "328e90c62068222dfd98f71dd3f6251fcb962f082b49c6be66ab2699f5db6f4f",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a.img.xz",
|
||||
"size": 10737418240
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
Reference in New Issue
Block a user