mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-16 06:42:12 +08:00
Pedal interceptor longitudinal control
Co-Authored-By: Eric Brown <13560103+nworb-cire@users.noreply.github.com>
This commit is contained in:
@@ -126,6 +126,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
||||
leadDeparting @129;
|
||||
noLaneAvailable @130;
|
||||
openpilotCrashed @131;
|
||||
pedalInterceptorNoBrake @133;
|
||||
torqueNNLoad @135;
|
||||
|
||||
radarCanErrorDEPRECATED @15;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -41,7 +41,12 @@ 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
|
||||
@@ -49,7 +54,7 @@ const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {
|
||||
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
|
||||
@@ -77,6 +82,8 @@ const uint16_t GM_PARAM_CC_LONG = 8;
|
||||
const uint16_t GM_PARAM_HW_ASCM_LONG = 16;
|
||||
const uint16_t GM_PARAM_NO_CAMERA = 32;
|
||||
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,
|
||||
@@ -89,10 +96,12 @@ enum {GM_ASCM, GM_CAM, GM_SDGM} gm_hw = GM_ASCM;
|
||||
bool gm_cam_long = false;
|
||||
bool gm_pcm_cruise = false;
|
||||
bool gm_cc_long = false;
|
||||
bool gm_has_acc = true;
|
||||
bool gm_pedal_long = false;
|
||||
bool gm_force_ascm = false;
|
||||
|
||||
static void handle_gm_wheel_buttons(const CANPacket_t *to_push) {
|
||||
if ((addr == 0x1E1) && gm_cc_long) {
|
||||
int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4;
|
||||
int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4;
|
||||
|
||||
// enter controls on falling edge of set or rising edge of resume (avoids fault)
|
||||
bool set = (button != GM_BTN_SET) && (cruise_button_prev == GM_BTN_SET);
|
||||
@@ -151,23 +160,42 @@ 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;
|
||||
}
|
||||
|
||||
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);
|
||||
@@ -199,6 +227,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);
|
||||
@@ -215,7 +250,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;
|
||||
@@ -269,7 +304,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 {
|
||||
@@ -284,12 +321,12 @@ static safety_config gm_init(uint16_t param) {
|
||||
} else {
|
||||
}
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG);
|
||||
#endif
|
||||
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_hw == GM_SDGM);
|
||||
gm_pcm_cruise = ((gm_hw == GM_CAM) && (!gm_cam_long || gm_cc_long) && !gm_force_ascm && !gm_pedal_long) || (gm_hw == GM_SDGM);
|
||||
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) {
|
||||
|
||||
@@ -236,6 +236,8 @@ class Panda:
|
||||
FLAG_GM_CC_LONG = 8
|
||||
FLAG_GM_HW_ASCM_LONG = 16
|
||||
FLAG_GM_NO_ACC = 64
|
||||
FLAG_GM_PEDAL_LONG = 128 # TODO: This can be inferred
|
||||
FLAG_GM_GAS_INTERCEPTOR = 256
|
||||
|
||||
FLAG_FORD_LONG_CONTROL = 1
|
||||
FLAG_FORD_CANFD = 2
|
||||
|
||||
@@ -255,6 +255,91 @@ class TestGmSdgmSafety(TestGmSafetyBase):
|
||||
self._rx(self._pcm_status_msg(enabled))
|
||||
self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL)))
|
||||
|
||||
##### OPGM TESTS #####
|
||||
|
||||
def interceptor_msg(gas, addr):
|
||||
to_send = common.make_msg(0, addr, 6)
|
||||
to_send[0].data[0] = (gas & 0xFF00) >> 8
|
||||
to_send[0].data[1] = gas & 0xFF
|
||||
to_send[0].data[2] = (gas & 0xFF00) >> 8
|
||||
to_send[0].data[3] = gas & 0xFF
|
||||
return to_send
|
||||
|
||||
|
||||
class TestGmInterceptorSafety(common.GasInterceptorSafetyTest, TestGmCameraSafety):
|
||||
INTERCEPTOR_THRESHOLD = 515
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("gm_global_a_powertrain_generated")
|
||||
self.packer_chassis = CANPackerPanda("gm_global_a_chassis")
|
||||
self.safety = libpanda_py.libpanda
|
||||
self.safety.set_safety_hooks(
|
||||
Panda.SAFETY_GM,
|
||||
Panda.FLAG_GM_HW_CAM | Panda.FLAG_GM_NO_ACC | Panda.FLAG_GM_PEDAL_LONG | Panda.FLAG_GM_GAS_INTERCEPTOR)
|
||||
self.safety.init_tests()
|
||||
|
||||
def test_pcm_sets_cruise_engaged(self):
|
||||
for enabled in [True, False]:
|
||||
self._rx(self._pcm_status_msg(enabled))
|
||||
self.assertEqual(enabled, self.safety.get_cruise_engaged_prev())
|
||||
|
||||
def test_no_pcm_enable(self):
|
||||
self._rx(self._interceptor_user_gas(0))
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self._rx(self._pcm_status_msg(True))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self.safety.get_cruise_engaged_prev())
|
||||
|
||||
def test_no_response_to_acc_pcm_message(self):
|
||||
self._rx(self._interceptor_user_gas(0))
|
||||
def _acc_pcm_msg(enable):
|
||||
values = {"CruiseState": enable}
|
||||
return self.packer.make_can_msg_panda("AcceleratorPedal2", 0, values)
|
||||
for enable in [True, False]:
|
||||
self.safety.set_controls_allowed(enable)
|
||||
self._rx(_acc_pcm_msg(True))
|
||||
self.assertEqual(enable, self.safety.get_controls_allowed())
|
||||
self._rx(_acc_pcm_msg(False))
|
||||
self.assertEqual(enable, self.safety.get_controls_allowed())
|
||||
|
||||
def test_buttons(self):
|
||||
self._rx(self._interceptor_user_gas(0))
|
||||
# Only CANCEL button is allowed while cruise is enabled
|
||||
self.safety.set_controls_allowed(0)
|
||||
for btn in range(8):
|
||||
self.assertFalse(self._tx(self._button_msg(btn)))
|
||||
|
||||
self.safety.set_controls_allowed(1)
|
||||
for btn in range(8):
|
||||
self.assertFalse(self._tx(self._button_msg(btn)))
|
||||
|
||||
self.safety.set_controls_allowed(1)
|
||||
for enabled in (True, False):
|
||||
self._rx(self._pcm_status_msg(enabled))
|
||||
self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_fwd_hook(self):
|
||||
pass
|
||||
|
||||
def test_disable_control_allowed_from_cruise(self):
|
||||
pass
|
||||
|
||||
def test_enable_control_allowed_from_cruise(self):
|
||||
pass
|
||||
|
||||
def _interceptor_gas_cmd(self, gas):
|
||||
return interceptor_msg(gas, 0x200)
|
||||
|
||||
def _interceptor_user_gas(self, gas):
|
||||
return interceptor_msg(gas, 0x201)
|
||||
|
||||
def _pcm_status_msg(self, enable):
|
||||
values = {"CruiseActive": enable}
|
||||
return self.packer.make_can_msg_panda("ECMCruiseControl", 0, values)
|
||||
|
||||
|
||||
class TestGmCcLongitudinalSafety(TestGmCameraSafety):
|
||||
TX_MSGS = [[384, 0], [481, 0], [388, 2]]
|
||||
FWD_BLACKLISTED_ADDRS = {2: [384], 0: [388]} # block LKAS message and PSCMStatus
|
||||
|
||||
@@ -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.car.interfaces import CarControllerBase
|
||||
@@ -54,6 +54,21 @@ class CarController(CarControllerBase):
|
||||
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
|
||||
@@ -103,6 +118,7 @@ class CarController(CarControllerBase):
|
||||
# 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
|
||||
@@ -126,6 +142,9 @@ class CarController(CarControllerBase):
|
||||
# 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
|
||||
|
||||
@@ -135,6 +154,8 @@ class CarController(CarControllerBase):
|
||||
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
|
||||
@@ -175,6 +196,15 @@ class CarController(CarControllerBase):
|
||||
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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -30,6 +31,7 @@ class CarState(CarStateBase):
|
||||
self.distance_button = 0
|
||||
|
||||
# FrogPilot variables
|
||||
self.single_pedal_mode = False
|
||||
|
||||
def update(self, pt_cp, cam_cp, loopback_cp, frogpilot_variables):
|
||||
ret = car.CarState.new_message()
|
||||
@@ -89,9 +91,14 @@ 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.
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
if self.CP.enableGasInterceptor:
|
||||
ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
|
||||
ret.gasPressed = ret.gas > 3
|
||||
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"]
|
||||
@@ -237,13 +244,21 @@ class CarState(CarStateBase):
|
||||
messages.append(("EBCMBrakePedalPosition", 100))
|
||||
|
||||
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
|
||||
|
||||
@@ -21,6 +21,7 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D
|
||||
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel}
|
||||
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
|
||||
|
||||
PEDAL_MSG = 0x201
|
||||
ACCELERATOR_POS_MSG = 0xbe
|
||||
|
||||
NON_LINEAR_TORQUE_PARAMS = {
|
||||
@@ -96,6 +97,9 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)]
|
||||
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
|
||||
@@ -155,11 +159,9 @@ class CarInterface(CarInterfaceBase):
|
||||
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.]]
|
||||
@@ -207,6 +209,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:
|
||||
# On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop
|
||||
# with foot on brake to allow engagement, but this platform only has that check in the camera.
|
||||
@@ -242,6 +248,26 @@ class CarInterface(CarInterfaceBase):
|
||||
elif candidate == CAR.CT6_CC:
|
||||
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 = not disable_openpilot_long
|
||||
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
|
||||
@@ -320,6 +346,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
|
||||
|
||||
@@ -262,6 +262,7 @@ class CanBus:
|
||||
DROPPED = 192
|
||||
|
||||
class GMFlags(IntFlag):
|
||||
PEDAL_LONG = 1
|
||||
CC_LONG = 2
|
||||
NO_ACCELERATOR_POS_MSG = 8
|
||||
|
||||
|
||||
@@ -1064,6 +1064,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
Priority.HIGHEST, VisualAlert.none, AudibleAlert.none, 10.),
|
||||
},
|
||||
|
||||
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,
|
||||
},
|
||||
|
||||
Reference in New Issue
Block a user