mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-12 03:03:51 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
deb5322c408c9a3cb7152f245fbcdceb248734e6
StarPilot/selfdrive/car/gm/carcontroller.py
T
firestar5683 deb5322c40 guard off frames.
2025-05-30 16:54:15 -05:00

363 lines
17 KiB
Python
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
from typing import Tuple
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.numpy_fast import interp, clip
from openpilot.common.realtime import DT_CTRL
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.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
VisualAlert = car.CarControl.HUDControl.VisualAlert
NetworkLocation = car.CarParams.NetworkLocation
LongCtrlState = car.CarControl.Actuators.LongControlState
GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType
# Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s
CAMERA_CANCEL_DELAY_FRAMES = 10
# Enforce a minimum interval between steering messages to avoid a fault
MIN_STEER_MSG_INTERVAL_MS = 10
# 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
BRAKE_PITCH_FACTOR_V = [0., 1.] # [unitless in [0,1]]; don't touch
class CarController(CarControllerBase):
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.start_time = 0.
self.apply_steer_last = 0
self.apply_gas = 0
self.apply_brake = 0
self.apply_speed = 0
self.frame = 0
self.last_steer_frame = 0
self.last_button_frame = 0
self.cancel_counter = 0
self.pedal_steady = 0.
self.lka_steering_cmd_counter = 0
self.lka_icon_status_last = (False, False)
self.last_oem_prndl2_ts_nanos = 0
self.last_oem_regen_paddle_ts_nanos = 0
self.params = CarControllerParams(self.CP)
self.params_ = Params()
self.packer_pt = CANPacker(DBC[self.CP.carFingerprint]['pt'])
self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar'])
self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis'])
# 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
# Number of consecutive off-frames to send after regen release
self.off_spoof_frames = 0
def calc_pedal_command(self, accel: float, long_active: bool, car_velocity) -> Tuple[float, bool]:
if not long_active:
return 0., False
# Regen paddle hysteresis (200ms = 20 frames)
if not hasattr(self, 'regen_paddle_timer'):
self.regen_paddle_timer = 0
if self.aego < -0.7 and accel <= 0.0:
self.regen_paddle_timer += 1
else:
self.regen_paddle_timer = max(self.regen_paddle_timer - 1, 0)
self.regen_paddle_pressed = self.regen_paddle_timer >= 20
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])
if press_regen_paddle:
pedal_gas = clip((pedaloffset + (accel / gain) * 0.6), 0.0, 1.0)
else:
pedal_gas = clip((pedaloffset + accel * 0.6), 0.0, 1.0)
return pedal_gas, press_regen_paddle
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
hud_control = CC.hudControl
hud_alert = hud_control.visualAlert
hud_v_cruise = hud_control.setSpeed
if hud_v_cruise > 70:
hud_v_cruise = 0
# Send CAN commands.
can_sends = []
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_pressed
)
# Hybrid midpoint + overflow paddle/PRNDL2 scheduling (>=40Hz, avoid steer)
# Initialize spoof timer on first regen active
if regen_active and not hasattr(self, "last_spoof_frame"):
self.last_spoof_frame = self.frame
# Steer-frame tracking is handled in the steer-sending block below:
# we update prev_steer_frame and last_steer_frame after sending steering.
# Send midpoint spoof (halfway between last two steer frames)
send_spoof = False
if regen_active and hasattr(self, "prev_steer_frame"):
steer_interval = self.last_steer_frame - self.prev_steer_frame
half_interval = max(1, steer_interval // 2)
midpoint_frame = self.prev_steer_frame + half_interval
if self.frame == midpoint_frame:
send_spoof = True
# Overflow spoof: ensure no gap >25 ms without spoof
if regen_active and hasattr(self, "last_spoof_frame"):
time_since_spoof = (self.frame - self.last_spoof_frame) * DT_CTRL
if time_since_spoof >= 0.025 and self.frame != self.last_steer_frame:
send_spoof = True
# Execute spoof sends
if send_spoof:
can_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, True))
can_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, True))
self.last_spoof_frame = self.frame
# Send off commands for two consecutive frames on regen release
if not regen_active and getattr(self, "last_regen_active", False):
# schedule two off-frames
self.off_spoof_frames = 3
# while frames remain, send off spoof if not colliding with steer
if getattr(self, "off_spoof_frames", 0) > 0:
if self.frame != self.last_steer_frame:
# send off messages
can_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, False))
can_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, False))
# decrement only when sent
self.off_spoof_frames -= 1
# Update regen_active state
self.last_regen_active = regen_active
# Steering (Active: 50Hz, inactive: 10Hz)
steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP
if self.CP.networkLocation == NetworkLocation.fwdCamera:
# Also send at 50Hz:
# - on startup, first few msgs are blocked
# - until we're in sync with camera so counters align when relay closes, preventing a fault.
# openpilot can subtly drift, so this is activated throughout a drive to stay synced
out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.cam_lka_steering_cmd_counter + 1) % 4
if CS.loopback_lka_steering_cmd_ts_nanos == 0 or out_of_sync:
steer_step = self.params.STEER_STEP
self.lka_steering_cmd_counter += 1 if CS.loopback_lka_steering_cmd_updated else 0
# Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we
# received the ASCMLKASteeringCmd loopback confirmation too recently
last_lka_steer_msg_ms = (now_nanos - CS.loopback_lka_steering_cmd_ts_nanos) * 1e-6
if (self.frame - self.last_steer_frame) >= steer_step and last_lka_steer_msg_ms > MIN_STEER_MSG_INTERVAL_MS:
# Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus
if CS.loopback_lka_steering_cmd_ts_nanos == 0:
self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1
if CC.latActive:
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
else:
apply_steer = 0
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))
# After sending steering control:
# Update steer-frame tracking for midpoint calculation
if not hasattr(self, "prev_steer_frame"):
self.prev_steer_frame = self.last_steer_frame
else:
self.prev_steer_frame = self.last_steer_frame
self.last_steer_frame = self.frame
if self.CP.openpilotLongitudinalControl:
# Gas/regen, brakes, and UI commands - all at 25Hz
if self.frame % 4 == 0:
stopping = actuators.longControlState == LongCtrlState.stopping
# Pitch compensated acceleration;
# TODO: include future pitch (sm['modelDataV2'].orientation.y) to account for long actuator delay
if frogpilot_toggles.long_pitch and len(CC.orientationNED) > 1:
self.pitch.update(CC.orientationNED[1])
self.accel_g = ACCELERATION_DUE_TO_GRAVITY * apply_deadzone(self.pitch.x, PITCH_DEADZONE) # driving uphill is positive pitch
accel += self.accel_g
brake_accel = actuators.accel + self.accel_g * interp(CS.out.vEgo, BRAKE_PITCH_FACTOR_BP, BRAKE_PITCH_FACTOR_V)
at_full_stop = CC.longActive and CS.out.standstill
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
interceptor_gas_cmd = 0
if not CC.longActive:
# ASCM sends max regen when not enabled
self.apply_gas = self.params.INACTIVE_REGEN
self.apply_brake = 0
elif near_stop and stopping and not CC.cruiseControl.resume:
self.apply_gas = self.params.INACTIVE_REGEN
self.apply_brake = int(min(-100 * self.CP.stopAccel, self.params.MAX_BRAKE))
else:
# Normal operation
if self.CP.carFingerprint in EV_CAR:
if frogpilot_toggles.sport_plus:
self.apply_gas = int(round(interp(accel, self.params.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)))
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)))
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)))
# Don't allow any gas above inactive regen while stopping
# FIXME: brakes aren't applied immediately when enabling at a stop
if stopping:
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)
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
if self.CP.flags & GMFlags.CC_LONG.value:
if CC.longActive and CS.out.vEgo > self.CP.minEnableSpeed:
# Using extend instead of append since the message is only sent intermittently
can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators))
if self.CP.enableGasInterceptor:
can_sends.append(create_gas_interceptor_command(self.packer_pt, interceptor_gas_cmd, idx))
if self.CP.carFingerprint not in CC_ONLY_CAR:
friction_brake_bus = CanBus.CHASSIS
# GM Camera exceptions
# TODO: can we always check the longControlState?
if self.CP.networkLocation == NetworkLocation.fwdCamera and self.CP.carFingerprint not in CC_ONLY_CAR:
at_full_stop = at_full_stop and stopping
friction_brake_bus = CanBus.POWERTRAIN
if self.CP.autoResumeSng:
resume = actuators.longControlState != LongCtrlState.starting or CC.cruiseControl.resume
at_full_stop = at_full_stop and not resume
if CC.cruiseControl.resume and CS.pcm_acc_status == AccState.STANDSTILL and frogpilot_toggles.volt_sng:
acc_engaged = False
else:
acc_engaged = CC.enabled
# 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))
# Send dashboard UI commands (ACC status)
send_fcw = hud_alert == VisualAlert.fcw
if self.frame % 10 == 0:
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 (10hz)
if not self.CP.radarUnavailable:
tt = self.frame * DT_CTRL
time_and_headlights_step = 20
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 = 4
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 == 0:
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
# TODO: integrate this with the code block below?
if (
(self.CP.flags & GMFlags.PEDAL_LONG.value) # Always cancel stock CC when using pedal interceptor
or (self.CP.flags & GMFlags.CC_LONG.value and not CC.enabled) # Cancel stock CC if OP is not active
) and CS.out.cruiseState.enabled:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
self.last_button_frame = self.frame
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, (CS.buttons_counter + 1) % 4, CruiseButtons.CANCEL))
else:
# While car is braking, cancel button causes ECM to enter a soft disable state with a fault status.
# A delayed cancellation allows camera to cancel and avoids a fault when user depresses brake quickly
self.cancel_counter = self.cancel_counter + 1 if CC.cruiseControl.cancel else 0
# Stock longitudinal, integrated at camera
if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
if self.cancel_counter > CAMERA_CANCEL_DELAY_FRAMES:
self.last_button_frame = self.frame
if self.CP.carFingerprint in SDGM_CAR:
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, CS.buttons_counter, CruiseButtons.CANCEL))
else:
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
if self.CP.networkLocation == NetworkLocation.fwdCamera:
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
if self.frame % 20 == 0:
can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status))
new_actuators = actuators.as_builder()
new_actuators.accel = accel
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last
new_actuators.gas = self.apply_gas
new_actuators.brake = self.apply_brake
new_actuators.speed = self.apply_speed
self.frame += 1
return new_actuators, can_sends
Reference in New Issue View Git Blame Copy Permalink