mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-12 19:33:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

Controls - Always On Lateral

Browse Source 
Maintain openpilot lateral control when the brake or gas pedals are used. Deactivation occurs only through the 'Cruise Control' button.

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-06-19 19:47:58 -07:00
parent c474b89fa7
commit ea6c0db16f
39 changed files with 148 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
panda/board/safety.h
+15 -4
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;
}
panda/board/safety/safety_chrysler.h
+1
View File
@@ -192,6 +192,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);
}
panda/board/safety/safety_ford.h
+1
View File
@@ -245,6 +245,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);
}
panda/board/safety/safety_gm.h
+5 -1
View File
@@ -143,7 +143,11 @@ static void gm_rx_hook(const CANPacket_t *to_push) {
}
if ((addr == 0xC9) && ((gm_hw == GM_CAM) || (gm_hw == GM_SDGM))) {
brake_pressed = GET_BIT(to_push, 40U);
brake_pressed = GET_BIT(to_push, 40U) != 0U;
}
if (addr == 0xC9) {
acc_main_on = GET_BIT(to_push, 29U) != 0U;
}
if (addr == 0x1C4) {
panda/board/safety/safety_honda.h
+2 -1
View File
@@ -340,7 +340,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;
panda/board/safety/safety_hyundai_common.h
+4
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 && 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 {
panda/board/safety/safety_mazda.h
+1
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);
}
panda/board/safety/safety_nissan.h
+7
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;
}
}
panda/board/safety/safety_subaru.h
+1
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);
}
panda/board/safety/safety_subaru_preglobal.h
+1
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);
}
panda/board/safety/safety_tesla.h
+8
View File
@@ -100,6 +100,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
panda/board/safety/safety_toyota.h
+6
View File
@@ -53,6 +53,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
@@ -71,6 +72,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 }}}, \
@@ -177,6 +179,10 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
}
}
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
if (addr == 0x1D2) {
panda/board/safety_declarations.h
+4
View File
@@ -224,6 +224,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
@@ -269,3 +270,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
panda/python/__init__.py
+1
View File
@@ -111,6 +111,7 @@ class ALTERNATIVE_EXPERIENCE:
DISABLE_STOCK_AEB = 2
RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8
ALLOW_AEB = 16
ALWAYS_ON_LATERAL = 32
class Panda: