mawei
/
gm
mirror of https://github.com/slims2016/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Pedal interceptor longitudinal control 

Co-Authored-By: Eric Brown <13560103+nworb-cire@users.noreply.github.com>
This commit is contained in:
FrogAi 2024-02-27 16:34:47 -07:00
parent 64e64fee63
commit 6f51245dfa
9 changed files with 156 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cereal/car.capnp
View File

@ -124,6 +124,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
leadDeparting @124;
noLaneAvailable @125;
openpilotCrashed @126;
pedalInterceptorNoBrake @128;
torqueNNLoad @130;
radarCanErrorDEPRECATED @15;

2
panda/board/safety.h
View File

@ -544,7 +544,7 @@ bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limit
}
bool longitudinal_interceptor_checks(const CANPacket_t *to_send) {
return !get_longitudinal_allowed() && (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1));
return (!get_longitudinal_allowed() || brake_pressed_prev) && (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1));
}
// Safety checks for torque-based steering commands

61
panda/board/safety/safety_gm.h
View File

@ -41,15 +41,20 @@ const LongitudinalLimits *gm_long_limits;
const int GM_STANDSTILL_THRSLD = 10; // 0.311kph
const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6}, // pt bus
// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches
// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state
const int GM_GAS_INTERCEPTOR_THRESHOLD = 515; // (610 + 306.25) / 2 ratio between offset and gain from dbc file
#define GM_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks
const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, // pt bus
{0xA1, 1, 7}, {0x306, 1, 8}, {0x308, 1, 7}, {0x310, 1, 2}, // obs bus
{0x315, 2, 5}, // ch bus
{0x104c006c, 3, 3}, {0x10400060, 3, 5}}; // gmlan
const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus
const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x200, 0, 6}, {0x1E1, 0, 7}, // pt bus
{0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus
const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, // pt bus
const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6}, // pt bus
{0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus
const CanMsg GM_SDGM_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus
@ -74,6 +79,10 @@ const uint16_t GM_PARAM_HW_CAM = 1;
const uint16_t GM_PARAM_HW_CAM_LONG = 2;
const uint16_t GM_PARAM_HW_SDGM = 4;
const uint16_t GM_PARAM_CC_LONG = 8;
const uint16_t GM_PARAM_HW_ASCM_LONG = 16;
const uint16_t GM_PARAM_NO_ACC = 64;
const uint16_t GM_PARAM_PEDAL_LONG = 128; // TODO: this can be inferred
const uint16_t GM_PARAM_PEDAL_INTERCEPTOR = 256;
enum {
GM_BTN_UNPRESS = 1,
@ -85,7 +94,10 @@ enum {
enum {GM_ASCM, GM_CAM, GM_SDGM} gm_hw = GM_ASCM;
bool gm_cam_long = false;
bool gm_pcm_cruise = false;
bool gm_has_acc = true;
bool gm_pedal_long = false;
bool gm_cc_long = false;
bool gm_force_ascm = false;
static void handle_gm_wheel_buttons(const CANPacket_t *to_push) {
int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4;
@ -145,23 +157,43 @@ static void gm_rx_hook(const CANPacket_t *to_push) {
}
if (addr == 0x1C4) {
gas_pressed = GET_BYTE(to_push, 5) != 0U;
if (!enable_gas_interceptor) {
gas_pressed = GET_BYTE(to_push, 5) != 0U;
}
// enter controls on rising edge of ACC, exit controls when ACC off
if (gm_pcm_cruise) {
if (gm_pcm_cruise && gm_has_acc) {
bool cruise_engaged = (GET_BYTE(to_push, 1) >> 5) != 0U;
pcm_cruise_check(cruise_engaged);
}
}
// Cruise check for CC only cars
if ((addr == 0x3D1) && !gm_has_acc) {
bool cruise_engaged = (GET_BYTE(to_push, 4) >> 7) != 0U;
if (gm_cc_long) {
pcm_cruise_check(cruise_engaged);
} else {
cruise_engaged_prev = cruise_engaged;
}
}
if (addr == 0xBD) {
regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U;
}
// Pedal Interceptor
if ((addr == 0x201) && enable_gas_interceptor) {
int gas_interceptor = GM_GET_INTERCEPTOR(to_push);
gas_pressed = gas_interceptor > GM_GAS_INTERCEPTOR_THRESHOLD;
gas_interceptor_prev = gas_interceptor;
// gm_pcm_cruise = false;
}
bool stock_ecu_detected = (addr == 0x180); // ASCMLKASteeringCmd
// Check ASCMGasRegenCmd only if we're blocking it
if (!gm_pcm_cruise && (addr == 0x2CB)) {
if (!gm_pcm_cruise && !gm_pedal_long && (addr == 0x2CB)) {
stock_ecu_detected = true;
}
generic_rx_checks(stock_ecu_detected);
@ -193,6 +225,13 @@ static bool gm_tx_hook(const CANPacket_t *to_send) {
}
}
// GAS: safety check (interceptor)
if (addr == 0x200) {
if (longitudinal_interceptor_checks(to_send)) {
tx = 0;
}
}
// GAS/REGEN: safety check
if (addr == 0x2CB) {
bool apply = GET_BIT(to_send, 0U);
@ -209,7 +248,7 @@ static bool gm_tx_hook(const CANPacket_t *to_send) {
}
// BUTTONS: used for resume spamming and cruise cancellation with stock longitudinal
if ((addr == 0x1E1) && (gm_pcm_cruise || gm_cc_long)) {
if ((addr == 0x1E1) && (gm_pcm_cruise || gm_pedal_long || gm_cc_long)) {
int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U;
bool allowed_btn = (button == GM_BTN_CANCEL) && cruise_engaged_prev;
@ -263,7 +302,9 @@ static safety_config gm_init(uint16_t param) {
gm_hw = GM_ASCM;
}
if (gm_hw == GM_ASCM) {
gm_force_ascm = GET_FLAG(param, GM_PARAM_HW_ASCM_LONG);
if (gm_hw == GM_ASCM || gm_force_ascm) {
if (sport_mode) {
gm_long_limits = &GM_ASCM_LONG_LIMITS_SPORT;
} else {
@ -278,9 +319,13 @@ static safety_config gm_init(uint16_t param) {
} else {
}
gm_pedal_long = GET_FLAG(param, GM_PARAM_PEDAL_LONG);
gm_cc_long = GET_FLAG(param, GM_PARAM_CC_LONG);
gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG) && !gm_cc_long;
gm_pcm_cruise = ((gm_hw == GM_CAM) && (!gm_cam_long || gm_cc_long) && !gm_force_ascm && !gm_pedal_long) || (gm_hw == GM_SDGM);
gm_skip_relay_check = GET_FLAG(param, GM_PARAM_NO_CAMERA);
gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC);
enable_gas_interceptor = GET_FLAG(param, GM_PARAM_PEDAL_INTERCEPTOR);
safety_config ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_ASCM_TX_MSGS);
if (gm_hw == GM_CAM) {

3
panda/python/__init__.py
View File

@ -236,6 +236,9 @@ class Panda:
FLAG_GM_HW_SDGM = 4
FLAG_GM_CC_LONG = 8
FLAG_GM_HW_ASCM_LONG = 16
FLAG_GM_NO_ACC = 64
FLAG_GM_PEDAL_LONG = 128
FLAG_GM_GAS_INTERCEPTOR = 256
FLAG_FORD_LONG_CONTROL = 1
FLAG_FORD_CANFD = 2

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
@ -53,6 +53,21 @@ class CarController:
self.pitch = FirstOrderFilter(0., 0.09 * 4, DT_CTRL * 4) # runs at 25 Hz
self.accel_g = 0.0
@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, frogpilot_variables):
actuators = CC.actuators
accel = actuators.accel
@ -102,6 +117,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

23
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
@ -27,6 +28,7 @@ class CarState(CarStateBase):
self.buttons_counter = 0
# FrogPilot variables
self.single_pedal_mode = False
def update(self, pt_cp, cam_cp, loopback_cp, frogpilot_variables):
ret = car.CarState.new_message()
@ -80,10 +82,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"]
@ -230,13 +237,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

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

@ -22,6 +22,7 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D
ACCELERATOR_POS_MSG = 0xbe
PEDAL_MSG = 0x201
NON_LINEAR_TORQUE_PARAMS = {
CAR.BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
@ -95,6 +96,10 @@ class CarInterface(CarInterfaceBase):
ret.autoResumeSng = False
ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN]
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
else:
@ -152,12 +157,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} 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.]]
@ -259,6 +261,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
@ -321,6 +327,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
@ -391,6 +417,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(EventName.pedalInterceptorNoBrake)
ret.events = events.to_msg()
return ret

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

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

8
selfdrive/controls/lib/events.py
View File

@ -1028,6 +1028,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
Priority.HIGH, VisualAlert.none, AudibleAlert.none, .1),
},
EventName.pedalInterceptorNoBrake: {
ET.WARNING: Alert(
"Braking Unavailable",
"Shift to L",
AlertStatus.userPrompt, AlertSize.mid,
Priority.HIGH, VisualAlert.wrongGear, AudibleAlert.promptRepeat, 4.),
},
EventName.torqueNNLoad: {
ET.PERMANENT: torque_nn_load_alert,
},