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

Always On Lateral

Browse Source 
Added toggle for Always On Lateral / No disengage lateral on gas and brake.

Lots of credit goes to "pfeiferj"! Couldn't of done it without him!

https: //github.com/pfeiferj/openpilot
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent a76689801d
commit a6aa132b3d
41 changed files with 170 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
panda/board/safety.h
View File

@@ -552,7 +552,13 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
bool violation = false;
uint32_t ts = microsecond_timer_get();
bool aol_allowed = acc_main_on && (alternative_experience & ALT_EXP_ALWAYS_ON_LATERAL);
if (controls_allowed) {
// acc main must be on if controls are allowed
acc_main_on = controls_allowed;
}
if (controls_allowed || aol_allowed) {
// *** global torque limit check ***
violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer);
@@ -579,7 +585,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
}
// no torque if controls is not allowed
if (!controls_allowed && (desired_torque != 0)) {
if (!(controls_allowed || aol_allowed) && (desired_torque != 0)) {
violation = true;
}
@@ -621,7 +627,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
}
// reset to 0 if either controls is not allowed or there's a violation
if (violation || !controls_allowed) {
if (violation || !(controls_allowed || aol_allowed)) {
valid_steer_req_count = 0;
invalid_steer_req_count = 0;
desired_torque_last = 0;
@@ -636,8 +642,13 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
// Safety checks for angle-based steering commands
bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) {
bool violation = false;
bool aol_allowed = acc_main_on && (alternative_experience & ALT_EXP_ALWAYS_ON_LATERAL);
if (controls_allowed) {
// acc main must be on if controls are allowed
acc_main_on = controls_allowed;
}
if (controls_allowed && steer_control_enabled) {
if ((controls_allowed || aol_allowed) && steer_control_enabled) {
// convert floating point angle rate limits to integers in the scale of the desired angle on CAN,
// add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are
// always slightly above openpilot's in case we read an updated speed in between angle commands
@@ -680,7 +691,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
}
// No angle control allowed when controls are not allowed
violation |= !controls_allowed && steer_control_enabled;
violation |= !(controls_allowed || aol_allowed) && steer_control_enabled;
return violation;
}

1
panda/board/safety/safety_chrysler.h
View File

@@ -185,6 +185,7 @@ static void chrysler_rx_hook(const CANPacket_t *to_push) {
// enter controls on rising edge of ACC, exit controls on ACC off
const int das_3_bus = (chrysler_platform == CHRYSLER_PACIFICA) ? 0 : 2;
if ((bus == das_3_bus) && (addr == chrysler_addrs->DAS_3)) {
acc_main_on = GET_BIT(to_push, 20U);
bool cruise_engaged = GET_BIT(to_push, 21U);
pcm_cruise_check(cruise_engaged);
}

1
panda/board/safety/safety_ford.h
View File

@@ -251,6 +251,7 @@ static void ford_rx_hook(const CANPacket_t *to_push) {
// Signal: CcStat_D_Actl
unsigned int cruise_state = GET_BYTE(to_push, 1) & 0x07U;
acc_main_on = (cruise_state == 3U) ||(cruise_state == 4U) || (cruise_state == 5U);
bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U);
pcm_cruise_check(cruise_engaged);
}

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

@@ -138,6 +138,7 @@ static void gm_rx_hook(const CANPacket_t *to_push) {
}
if ((addr == 0xC9) && ((gm_hw == GM_CAM) || (gm_hw == GM_SDGM))) {
acc_main_on = GET_BIT(to_push, 29U);
brake_pressed = GET_BIT(to_push, 40U);
}

3
panda/board/safety/safety_honda.h
View File

@@ -350,7 +350,8 @@ static bool honda_tx_hook(const CANPacket_t *to_send) {
// STEER: safety check
if ((addr == 0xE4) || (addr == 0x194)) {
if (!controls_allowed) {
bool aol_allowed = acc_main_on && (alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS);
if (!(controls_allowed || aol_allowed)) {
bool steer_applied = GET_BYTE(to_send, 0) | GET_BYTE(to_send, 1);
if (steer_applied) {
tx = false;

4
panda/board/safety/safety_hyundai_common.h
View File

@@ -63,6 +63,10 @@ void hyundai_common_cruise_state_check(const bool cruise_engaged) {
}
void hyundai_common_cruise_buttons_check(const int cruise_button, const bool main_button) {
if (main_button != 0 && main_button != cruise_main_prev) {
acc_main_on = !acc_main_on;
}
cruise_main_prev = main_button;
if ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) || main_button) {
hyundai_last_button_interaction = 0U;
} else {

1
panda/board/safety/safety_mazda.h
View File

@@ -52,6 +52,7 @@ static void mazda_rx_hook(const CANPacket_t *to_push) {
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == MAZDA_CRZ_CTRL) {
acc_main_on = GET_BIT(to_push, 17U);
bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U;
pcm_cruise_check(cruise_engaged);
}

7
panda/board/safety/safety_nissan.h
View File

@@ -21,6 +21,8 @@ const CanMsg NISSAN_TX_MSGS[] = {
// Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model.
RxCheck nissan_rx_checks[] = {
{.msg = {{0x1b6, 0, 8, .frequency = 100U},
{0x1b6, 1, 8, .frequency = 100U}, { 0 }}}, // PRO_PILOT (100HZ)
{.msg = {{0x2, 0, 5, .frequency = 100U},
{0x2, 1, 5, .frequency = 100U}, { 0 }}}, // STEER_ANGLE_SENSOR
{.msg = {{0x285, 0, 8, .frequency = 50U},
@@ -64,11 +66,16 @@ static void nissan_rx_hook(const CANPacket_t *to_push) {
UPDATE_VEHICLE_SPEED((right_rear + left_rear) / 2.0 * 0.005 / 3.6);
}
if (addr == 0x1b6) {
acc_main_on = GET_BIT(to_push, 36U);
}
// X-Trail 0x15c, Leaf 0x239
if ((addr == 0x15c) || (addr == 0x239)) {
if (addr == 0x15c){
gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3U)) > 3U;
} else {
acc_main_on = GET_BIT(to_push, 17U);
gas_pressed = GET_BYTE(to_push, 0) > 3U;
}
}

1
panda/board/safety/safety_subaru.h
View File

@@ -152,6 +152,7 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
acc_main_on = GET_BIT(to_push, 40U);
bool cruise_engaged = GET_BIT(to_push, 41U);
pcm_cruise_check(cruise_engaged);
}

1
panda/board/safety/safety_subaru_preglobal.h
View File

@@ -56,6 +56,7 @@ static void subaru_preglobal_rx_hook(const CANPacket_t *to_push) {
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == MSG_SUBARU_PG_CruiseControl) {
acc_main_on = GET_BIT(to_push, 48U);
bool cruise_engaged = GET_BIT(to_push, 49U);
pcm_cruise_check(cruise_engaged);
}

8
panda/board/safety/safety_tesla.h
View File

@@ -88,6 +88,14 @@ static void tesla_rx_hook(const CANPacket_t *to_push) {
if(addr == (tesla_powertrain ? 0x256 : 0x368)) {
// Cruise state
int cruise_state = (GET_BYTE(to_push, 1) >> 4);
acc_main_on = (cruise_state == 1) || // STANDBY
(cruise_state == 2) || // ENABLED
(cruise_state == 3) || // STANDSTILL
(cruise_state == 4) || // OVERRIDE
(cruise_state == 6) || // PRE_FAULT
(cruise_state == 7); // PRE_CANCEL
bool cruise_engaged = (cruise_state == 2) || // ENABLED
(cruise_state == 3) || // STANDSTILL
(cruise_state == 4) || // OVERRIDE

5
panda/board/safety/safety_toyota.h
View File

@@ -58,6 +58,7 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805;
{0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, /* DSU bus 1 */ \
{0x411, 0, 8}, /* PCS_HUD */ \
{0x750, 0, 8}, /* radar diagnostic address */ \
{0x1D3, 0, 8}, \
const CanMsg TOYOTA_TX_MSGS[] = {
TOYOTA_COMMON_TX_MSGS
@@ -76,6 +77,7 @@ const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = {
{.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}}, \
{.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \
{.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}}, \
{.msg = {{0x1D3, 0, 8, .check_checksum = true, .frequency = 33U}, { 0 }, { 0 }}}, \
{.msg = {{0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \
{0x226, 0, 8, .check_checksum = false, .frequency = 40U}, { 0 }}}, \
@@ -181,6 +183,9 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
update_sample(&angle_meas, angle_meas_new);
}
}
if (addr == 0x1D3) {
acc_main_on = GET_BIT(to_push, 15U);
}
// enter controls on rising edge of ACC, exit controls on ACC off
// exit controls on rising edge of gas press

4
panda/board/safety_declarations.h
View File

@@ -225,6 +225,7 @@ struct sample_t vehicle_speed;
bool vehicle_moving = false;
bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018
int cruise_button_prev = 0;
int cruise_main_prev = 0;
bool safety_rx_checks_invalid = false;
// for safety modes with torque steering control
@@ -270,3 +271,6 @@ int alternative_experience = 0;
uint32_t safety_mode_cnt = 0U;
// allow 1s of transition timeout after relay changes state before assessing malfunctioning
const uint32_t RELAY_TRNS_TIMEOUT = 1U;
// Always on Lateral
#define ALT_EXP_ALWAYS_ON_LATERAL 32

1
panda/python/__init__.py
View File

@@ -113,6 +113,7 @@ class ALTERNATIVE_EXPERIENCE:
DISABLE_STOCK_AEB = 2
RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8
ALLOW_AEB = 16
ALWAYS_ON_LATERAL = 32
class Panda: