brianhansonxyz/clearpilot
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-02-27 16:34:47 -07:00
parent 1a33c93961
commit fc3b8ead19
9 changed files with 156 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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) {