brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Pedal interceptor longitudinal control

Browse Source 
Co-Authored-By: Eric Brown <13560103+nworb-cire@users.noreply.github.com>
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent 104932e207
commit 003f15fa87
9 changed files with 158 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
selfdrive/car/gm/carcontroller.py
View File

@@ -1,10 +1,10 @@
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
from openpilot.common.numpy_fast import interp, clip
from openpilot.common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_driver_steer_torque_limits
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
from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone
@@ -58,6 +58,21 @@ class CarController:
def update_frogpilot_variables(self, params):
self.long_pitch = params.get_bool("LongPitch")
@staticmethod
def calc_pedal_command(accel: float, long_active: bool) -> float:
if not long_active: return 0.
zero = 0.15625 # 40/256
if accel > 0.:
# Scales the accel from 0-1 to 0.156-1
pedal_gas = clip(((1 - zero) * accel + zero), 0., 1.)
else:
# if accel is negative, -0.1 -> 0.015625
pedal_gas = clip(zero + accel, 0., zero) # Make brake the same size as gas, but clip to regen
return pedal_gas
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
accel = actuators.accel
@@ -107,6 +122,7 @@ class CarController:
# Gas/regen, brakes, and UI commands - all at 25Hz
if self.frame % 4 == 0:
stopping = actuators.longControlState == LongCtrlState.stopping
interceptor_gas_cmd = 0
# Pitch compensated acceleration;
# TODO: include future pitch (sm['modelDataV2'].orientation.y) to account for long actuator delay
@@ -125,6 +141,9 @@ class CarController:
# 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 = self.calc_pedal_command(actuators.accel, CC.longActive)
idx = (self.frame // 4) % 4
@@ -134,6 +153,8 @@ class CarController:
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
@@ -174,6 +195,15 @@ class CarController:
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

27
selfdrive/car/gm/carstate.py
View File

@@ -9,6 +9,7 @@ from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRES
TransmissionType = car.CarParams.TransmissionType
NetworkLocation = car.CarParams.NetworkLocation
GearShifter = car.CarState.GearShifter
STANDSTILL_THRESHOLD = 10 * 0.0311 * CV.KPH_TO_MS
@@ -26,6 +27,8 @@ class CarState(CarStateBase):
self.cam_lka_steering_cmd_counter = 0
self.buttons_counter = 0
self.single_pedal_mode = False
def update(self, pt_cp, cam_cp, loopback_cp):
ret = car.CarState.new_message()
@@ -37,7 +40,8 @@ class CarState(CarStateBase):
self.cruise_buttons = cam_cp.vl["ASCMSteeringButton"]["ACCButtons"]
self.buttons_counter = cam_cp.vl["ASCMSteeringButton"]["RollingCounter"]
self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
self.moving_backward = pt_cp.vl["EBCMWheelSpdRear"]["MovingBackward"] != 0
moving_forward = pt_cp.vl["EBCMWheelSpdRear"]["MovingForward"] != 0
self.moving_backward = (pt_cp.vl["EBCMWheelSpdRear"]["MovingBackward"] != 0) and not moving_forward
if self.CP.enableBsm:
ret.leftBlindspot = pt_cp.vl["BCMBSM"]["Left_BSM"] == 1
@@ -80,10 +84,15 @@ class CarState(CarStateBase):
# Regen braking is braking
if self.CP.transmissionType == TransmissionType.direct:
ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
self.single_pedal_mode = ret.gearShifter == GearShifter.low or pt_cp.vl["EVDriveMode"]["SinglePedalModeActive"] == 1
ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
threshold = 15 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4
ret.gasPressed = ret.gas > threshold
if self.CP.enableGasInterceptor:
ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
threshold = 15 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4
ret.gasPressed = ret.gas > threshold
else:
ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
ret.gasPressed = ret.gas > 1e-5
ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"]
ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"]
@@ -229,13 +238,21 @@ class CarState(CarStateBase):
]
if CP.transmissionType == TransmissionType.direct:
messages.append(("EBCMRegenPaddle", 50))
messages += [
("EBCMRegenPaddle", 50),
("EVDriveMode", 0),
]
if CP.carFingerprint in CC_ONLY_CAR:
messages += [
("ECMCruiseControl", 10),
]
if CP.enableGasInterceptor:
messages += [
("GAS_SENSOR", 50),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus.POWERTRAIN)
@staticmethod

43
selfdrive/car/gm/interface.py
View File

@@ -22,6 +22,7 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D
ACCELERATOR_POS_MSG = 0xbe
BSM_MSG = 0x142
PEDAL_MSG = 0x201
NON_LINEAR_TORQUE_PARAMS = {
CAR.BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
@@ -84,6 +85,9 @@ class CarInterface(CarInterfaceBase):
ret.carName = "gm"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)]
ret.autoResumeSng = False
if PEDAL_MSG in fingerprint[0]:
ret.enableGasInterceptor = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_GAS_INTERCEPTOR
if candidate in EV_CAR:
ret.transmissionType = TransmissionType.direct
@@ -141,12 +145,9 @@ class CarInterface(CarInterfaceBase):
# Tuning
ret.longitudinalTuning.kpV = [2.4, 1.5]
ret.longitudinalTuning.kiV = [0.36]
# These cars have been put into dashcam only due to both a lack of users and test coverage.
# These cars likely still work fine. Once a user confirms each car works and a test route is
# added to selfdrive/car/tests/routes.py, we can remove it from this list.
ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL, CAR.EQUINOX} or \
(ret.networkLocation == NetworkLocation.gateway and ret.radarUnavailable)
if ret.enableGasInterceptor:
# Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_ASCM_LONG
# Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below.
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
@@ -248,6 +249,10 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if ret.enableGasInterceptor:
# ACC Bolts use pedal for full longitudinal control, not just sng
ret.flags |= GMFlags.PEDAL_LONG.value
elif candidate == CAR.SILVERADO:
ret.mass = 2450.
ret.wheelbase = 3.75
@@ -312,6 +317,26 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 17.7
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if ret.enableGasInterceptor:
ret.networkLocation = NetworkLocation.fwdCamera
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
ret.minEnableSpeed = -1
ret.pcmCruise = False
ret.openpilotLongitudinalControl = True
ret.stoppingControl = True
ret.autoResumeSng = True
if candidate in CC_ONLY_CAR:
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
ret.longitudinalTuning.kpBP = [5., 35.]
ret.longitudinalTuning.kpV = [0.35, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.0]
ret.longitudinalTuning.kiV = [0.1, 0.1]
ret.longitudinalTuning.kf = 0.15
ret.stoppingDecelRate = 0.8
elif candidate in CC_ONLY_CAR:
ret.flags |= GMFlags.CC_LONG.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_CC_LONG
@@ -385,6 +410,12 @@ class CarInterface(CarInterfaceBase):
if (self.CP.flags & GMFlags.CC_LONG.value) and ret.vEgo < self.CP.minEnableSpeed and ret.cruiseState.enabled:
events.add(EventName.speedTooLow)
if (self.CP.flags & GMFlags.PEDAL_LONG.value) and \
self.CP.transmissionType == TransmissionType.direct and \
not self.CS.single_pedal_mode and \
c.longActive:
events.add(FrogPilotEventName.pedalInterceptorNoBrake)
ret.events = events.to_msg()
return ret

1
selfdrive/car/gm/values.py
View File

@@ -175,6 +175,7 @@ class CanBus:
DROPPED = 192
class GMFlags(IntFlag):
PEDAL_LONG = 1
CC_LONG = 2
NO_ACCELERATOR_POS_MSG = 8