OPGM - Pedal interceptor longitudinal control
Co-Authored-By: Eric Brown <13560103+nworb-cire@users.noreply.github.com>
This commit is contained in:
FrogAi
parent
0bbf9db7f8
commit
3e34c11f0d
@ -117,6 +117,9 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
||||
paramsdTemporaryError @50;
|
||||
paramsdPermanentError @119;
|
||||
|
||||
# FrogPilot Events
|
||||
pedalInterceptorNoBrake @133;
|
||||
|
||||
radarCanErrorDEPRECATED @15;
|
||||
communityFeatureDisallowedDEPRECATED @62;
|
||||
radarCommIssueDEPRECATED @67;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -27,15 +27,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}, // pt bus
|
||||
const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, {0x200, 0, 6}, // 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
|
||||
{0x184, 2, 8}}; // camera bus
|
||||
|
||||
// TODO: do checksum and counter checks. Add correct timestep, 0.1s for now.
|
||||
@ -52,7 +57,11 @@ RxCheck gm_rx_checks[] = {
|
||||
|
||||
const uint16_t GM_PARAM_HW_CAM = 1;
|
||||
const uint16_t GM_PARAM_HW_CAM_LONG = 2;
|
||||
const uint16_t GM_PARAM_HW_ASCM_LONG = 8;
|
||||
const uint16_t GM_PARAM_NO_CAMERA = 16;
|
||||
const uint16_t GM_PARAM_NO_ACC = 32;
|
||||
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,
|
||||
@ -64,7 +73,10 @@ enum {
|
||||
enum {GM_ASCM, GM_CAM} 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_skip_relay_check = false;
|
||||
bool gm_force_ascm = false;
|
||||
|
||||
static void gm_rx_hook(const CANPacket_t *to_push) {
|
||||
if (GET_BUS(to_push) == 0U) {
|
||||
@ -114,23 +126,39 @@ static void gm_rx_hook(const CANPacket_t *to_push) {
|
||||
}
|
||||
|
||||
if (addr == 0x1C4) {
|
||||
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;
|
||||
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);
|
||||
@ -162,6 +190,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);
|
||||
@ -178,7 +213,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) {
|
||||
if ((addr == 0x1E1) && (gm_pcm_cruise || gm_pedal_long)) {
|
||||
int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U;
|
||||
|
||||
bool allowed_cancel = (button == 6) && cruise_engaged_prev;
|
||||
@ -219,18 +254,20 @@ static int gm_fwd_hook(int bus_num, int addr) {
|
||||
static safety_config gm_init(uint16_t param) {
|
||||
gm_hw = GET_FLAG(param, GM_PARAM_HW_CAM) ? GM_CAM : 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) {
|
||||
gm_long_limits = &GM_ASCM_LONG_LIMITS;
|
||||
} else if (gm_hw == GM_CAM) {
|
||||
gm_long_limits = &GM_CAM_LONG_LIMITS;
|
||||
} else {
|
||||
}
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG);
|
||||
#endif
|
||||
gm_pcm_cruise = (gm_hw == GM_CAM) && !gm_cam_long;
|
||||
gm_pedal_long = GET_FLAG(param, GM_PARAM_PEDAL_LONG);
|
||||
gm_pcm_cruise = ((gm_hw == GM_CAM) && !gm_cam_long && !gm_force_ascm && !gm_pedal_long);
|
||||
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) {
|
||||
|
||||
@ -229,7 +229,8 @@ class Panda:
|
||||
|
||||
FLAG_GM_HW_CAM = 1
|
||||
FLAG_GM_HW_CAM_LONG = 2
|
||||
FLAG_GM_NO_CAMERA = 16
|
||||
FLAG_GM_PEDAL_LONG = 128 # TODO: This can be inferred
|
||||
FLAG_GM_GAS_INTERCEPTOR = 256
|
||||
|
||||
FLAG_FORD_LONG_CONTROL = 1
|
||||
FLAG_FORD_CANFD = 2
|
||||
|
||||
@ -102,5 +102,3 @@ class PandaSafety(Protocol):
|
||||
def set_honda_alt_brake_msg(self, c: bool) -> None: ...
|
||||
def set_honda_bosch_long(self, c: bool) -> None: ...
|
||||
def get_honda_hw(self) -> int: ...
|
||||
|
||||
|
||||
|
||||
@ -849,7 +849,7 @@ class PandaSafetyTest(PandaSafetyTestBase):
|
||||
continue
|
||||
if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}):
|
||||
continue
|
||||
if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety'}):
|
||||
if {attr, current_test}.issubset({'TestGmCameraSafety', 'TestGmCameraLongitudinalSafety', 'TestGmInterceptorSafety'}):
|
||||
continue
|
||||
if attr.startswith('TestFord') and current_test.startswith('TestFord'):
|
||||
continue
|
||||
|
||||
@ -224,5 +224,90 @@ class TestGmCameraLongitudinalSafety(GmLongitudinalBase, TestGmCameraSafetyBase)
|
||||
self.safety.init_tests()
|
||||
|
||||
|
||||
##### 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)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
||||
@ -1,9 +1,9 @@
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
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
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
@ -39,6 +39,21 @@ class CarController(CarControllerBase):
|
||||
self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar'])
|
||||
self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis'])
|
||||
|
||||
@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
|
||||
hud_control = CC.hudControl
|
||||
@ -87,6 +102,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
|
||||
if not CC.longActive:
|
||||
# ASCM sends max regen when not enabled
|
||||
self.apply_gas = self.params.INACTIVE_REGEN
|
||||
@ -98,11 +114,16 @@ 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
|
||||
|
||||
at_full_stop = CC.longActive and CS.out.standstill
|
||||
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
|
||||
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
|
||||
@ -140,6 +161,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
|
||||
|
||||
|
||||
@ -29,6 +30,9 @@ class CarState(CarStateBase):
|
||||
self.prev_distance_button = 0
|
||||
self.distance_button = 0
|
||||
|
||||
# FrogPilot variables
|
||||
self.single_pedal_mode = False
|
||||
|
||||
def update(self, pt_cp, cam_cp, loopback_cp):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
@ -82,7 +86,12 @@ 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
|
||||
|
||||
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
|
||||
|
||||
@ -181,13 +190,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
|
||||
|
||||
@ -20,6 +20,7 @@ NetworkLocation = car.CarParams.NetworkLocation
|
||||
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
|
||||
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel}
|
||||
|
||||
PEDAL_MSG = 0x201
|
||||
CAM_MSG = 0x320 # AEBCmd
|
||||
# TODO: Is this always linked to camera presence?
|
||||
ACCELERATOR_POS_MSG = 0xbe
|
||||
@ -92,6 +93,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
|
||||
@ -137,12 +141,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.]]
|
||||
@ -189,6 +190,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.
|
||||
@ -215,6 +220,25 @@ 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 = 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
|
||||
if candidate in CC_ONLY_CAR:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC
|
||||
|
||||
@ -260,6 +284,12 @@ class CarInterface(CarInterfaceBase):
|
||||
if ret.vEgo < self.CP.minSteerSpeed:
|
||||
events.add(EventName.belowSteerSpeed)
|
||||
|
||||
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
|
||||
|
||||
@ -234,6 +234,7 @@ class CanBus:
|
||||
DROPPED = 192
|
||||
|
||||
class GMFlags(IntFlag):
|
||||
PEDAL_LONG = 1
|
||||
NO_CAMERA = 4
|
||||
NO_ACCELERATOR_POS_MSG = 8
|
||||
|
||||
|
||||
@ -956,6 +956,15 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"),
|
||||
},
|
||||
|
||||
# FrogPilot Events
|
||||
EventName.pedalInterceptorNoBrake: {
|
||||
ET.WARNING: Alert(
|
||||
"Braking Unavailable",
|
||||
"Shift to L",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.HIGH, VisualAlert.wrongGear, AudibleAlert.promptRepeat, 4.),
|
||||
},
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user