diff --git a/board/safety.h b/board/safety.h index 78c02941e42..776dfd4e3d7 100644 --- a/board/safety.h +++ b/board/safety.h @@ -256,18 +256,45 @@ void generic_rx_checks(bool stock_ecu_detected) { if (gas_pressed && !gas_pressed_prev && !(alternative_experience & ALT_EXP_DISABLE_DISENGAGE_ON_GAS)) { controls_allowed = false; } + // PFEIFER - AOL {{ + if (gas_pressed && !gas_pressed_prev && !(alternative_experience & ALT_EXP_ENABLE_ALWAYS_ON_LATERAL)) { + lateral_controls_allowed = 0; + } + // }} PFEIFER - AOL gas_pressed_prev = gas_pressed; // exit controls on rising edge of brake press if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { controls_allowed = false; } + // PFEIFER - AOL {{ + if (brake_pressed && (!brake_pressed_prev || vehicle_moving) && !(alternative_experience & ALT_EXP_ENABLE_ALWAYS_ON_LATERAL)) { + lateral_controls_allowed = 0; + } + // }} PFEIFER - AOL + if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) { + controls_allowed = 0; + } brake_pressed_prev = brake_pressed; // exit controls on rising edge of regen paddle if (regen_braking && (!regen_braking_prev || vehicle_moving)) { controls_allowed = false; } + // PFEIFER - AOL {{ + if (regen_braking && (!regen_braking_prev || vehicle_moving) && !(alternative_experience & ALT_EXP_ENABLE_ALWAYS_ON_LATERAL)) { + lateral_controls_allowed = 0; + } + + // If always on lateral is not enabled we set lateral_controls_allowed to controls_allowed to maintain stock behavior + if (!(alternative_experience & ALT_EXP_ENABLE_ALWAYS_ON_LATERAL)) { + lateral_controls_allowed = controls_allowed; + } + + if (controls_allowed == 1) { // always allow lateral_controls when long controls are allowed, makes syncing easier + lateral_controls_allowed = 1; + } + // }} PFEIFER - AOL regen_braking_prev = regen_braking; // check if stock ECU is on bus broken by car harness @@ -335,6 +362,9 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { vehicle_moving = false; acc_main_on = false; cruise_button_prev = 0; + // PFEIFER - AOL {{ + main_button_prev = 0; + // }} PFEIFER - AOL desired_torque_last = 0; rt_torque_last = 0; ts_angle_last = 0; @@ -541,7 +571,10 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi bool violation = false; uint32_t ts = microsecond_timer_get(); - if (controls_allowed) { +// if (controls_allowed) { +// PFEIFER - AOL {{ + if(lateral_controls_allowed) { +// }} PFEIFER - AOL // *** global torque limit check *** violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer); @@ -568,7 +601,10 @@ 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 && (desired_torque != 0)) { +// PFEIFER - AOL {{ + if(!lateral_controls_allowed && (desired_torque != 0)) { +// }} PFEIFER - AOL violation = true; } @@ -610,7 +646,10 @@ 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) { +// PFEIFER - AOL {{ + if (violation || !lateral_controls_allowed) { +// }} PFEIFER - AOL valid_steer_req_count = 0; invalid_steer_req_count = 0; desired_torque_last = 0; @@ -626,7 +665,10 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) { bool violation = false; - if (controls_allowed && steer_control_enabled) { +// if (controls_allowed && steer_control_enabled) { +// PFEIFER - AOL {{ + if (lateral_controls_allowed && steer_control_enabled) { +// }} PFEIFER - AOL // 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 @@ -669,7 +711,10 @@ 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 && steer_control_enabled; +// PFEIFER - AOL {{ + violation |= !lateral_controls_allowed && steer_control_enabled; +// }} PFEIFER - AOL return violation; } diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 324f73a22d7..e50135ce502 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -196,6 +196,15 @@ static int chrysler_rx_hook(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)) { + // PFEIFER - AOL {{ + bool cruise_available = GET_BIT(to_push, 20U) == 1U; + if(!cruise_available) { + lateral_controls_allowed = 0; + } + if(alternative_experience & ALT_EXP_AOL_ENABLE_ON_MAIN) { + lateral_controls_allowed = cruise_available; + } + // }} PFEIFER - AOL bool cruise_engaged = GET_BIT(to_push, 21U) == 1U; pcm_cruise_check(cruise_engaged); } diff --git a/board/safety/safety_defaults.h b/board/safety/safety_defaults.h index 90c8a0655d1..4edc74414b6 100644 --- a/board/safety/safety_defaults.h +++ b/board/safety/safety_defaults.h @@ -49,6 +49,7 @@ bool alloutput_passthrough = false; static const addr_checks* alloutput_init(uint16_t param) { controls_allowed = true; + lateral_controls_allowed = true; alloutput_passthrough = GET_FLAG(param, ALLOUTPUT_PARAM_PASSTHROUGH); return &default_rx_checks; } diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 8f48868a2d5..62905523ba3 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -264,6 +264,15 @@ static int ford_rx_hook(CANPacket_t *to_push) { // Signal: CcStat_D_Actl unsigned int cruise_state = GET_BYTE(to_push, 1) & 0x07U; + // PFEIFER - AOL {{ + bool cruise_available = (cruise_state == 3U) ||(cruise_state == 4U) || (cruise_state == 5U); + if(!cruise_available) { + lateral_controls_allowed = 0; + } + if(alternative_experience & ALT_EXP_AOL_ENABLE_ON_MAIN) { + lateral_controls_allowed = cruise_available; + } + // }} PFEIFER - AOL bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U); pcm_cruise_check(cruise_engaged); } diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 82c154765aa..19b97faefe7 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -112,6 +112,18 @@ static int gm_rx_hook(CANPacket_t *to_push) { brake_pressed = GET_BYTE(to_push, 1) >= 8U; } + // PFEIFER - AOL {{ + if (addr == 201) { + bool cruise_available = GET_BIT(to_push, 29U) != 0U; + if(!cruise_available) { + lateral_controls_allowed = 0; + } + if(alternative_experience & ALT_EXP_AOL_ENABLE_ON_MAIN) { + lateral_controls_allowed = cruise_available; + } + } + // }} PFEIFER - AOL + if ((addr == 0xC9) && (gm_hw == GM_CAM)) { brake_pressed = GET_BIT(to_push, 40U) != 0U; } diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index d1c6eba76e0..33ee600ee05 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -143,7 +143,15 @@ static int honda_rx_hook(CANPacket_t *to_push) { acc_main_on = GET_BIT(to_push, ((addr == 0x326) ? 28U : 47U)); if (!acc_main_on) { controls_allowed = false; + // PFEIFER - AOL {{ + lateral_controls_allowed = false; + // }} PFEIFER - AOL } + // PFEIFER - AOL {{ + if(alternative_experience & ALT_EXP_AOL_ENABLE_ON_MAIN) { + lateral_controls_allowed = acc_main_on; + } + // }} PFEIFER - AOL } // enter controls when PCM enters cruise state @@ -334,7 +342,10 @@ static int honda_tx_hook(CANPacket_t *to_send) { // STEER: safety check if ((addr == 0xE4) || (addr == 0x194)) { - if (!controls_allowed) { + // if (!controls_allowed) { + // PFEIFER - AOL {{ + if (!lateral_controls_allowed) { + // }} PFEIFER - AOL bool steer_applied = GET_BYTE(to_send, 0) | GET_BYTE(to_send, 1); if (steer_applied) { tx = 0; diff --git a/board/safety/safety_hyundai_canfd.h b/board/safety/safety_hyundai_canfd.h index f0af9060eb3..4b85b047e87 100644 --- a/board/safety/safety_hyundai_canfd.h +++ b/board/safety/safety_hyundai_canfd.h @@ -199,6 +199,15 @@ static int hyundai_canfd_rx_hook(CANPacket_t *to_push) { // brake press if (addr == 0x175) { + // PFEIFER - AOL {{ + bool cruise_available = ((GET_BYTE(to_push, 8) >> 3) & 0x8U) == 0U; + if(!cruise_available) { + lateral_controls_allowed = 0; + } + if(alternative_experience & ALT_EXP_AOL_ENABLE_ON_MAIN) { + lateral_controls_allowed = cruise_available; + } + // }} PFEIFER - AOL brake_pressed = GET_BIT(to_push, 81U) != 0U; } diff --git a/board/safety/safety_hyundai_common.h b/board/safety/safety_hyundai_common.h index 8fec9b06cd8..40da8936f78 100644 --- a/board/safety/safety_hyundai_common.h +++ b/board/safety/safety_hyundai_common.h @@ -70,6 +70,12 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const int main hyundai_last_button_interaction = MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES); } + // PFEIFER - AOL {{ + if (main_button != 0 && main_button_prev == 0) { // main_button was pressed + lateral_controls_allowed = (lateral_controls_allowed + 1) % 2; // toggle + } + // }} PFEIFER - AOL + if (hyundai_longitudinal) { // enter controls on falling edge of resume or set bool set = (cruise_button != HYUNDAI_BTN_SET) && (cruise_button_prev == HYUNDAI_BTN_SET); @@ -84,6 +90,7 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const int main } cruise_button_prev = cruise_button; + main_button_prev = main_button; } } diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h index e17f1ff4b3b..b9da2fd0d78 100644 --- a/board/safety/safety_mazda.h +++ b/board/safety/safety_mazda.h @@ -55,6 +55,15 @@ static int mazda_rx_hook(CANPacket_t *to_push) { // enter controls on rising edge of ACC, exit controls on ACC off if (addr == MAZDA_CRZ_CTRL) { + // PFEIFER - AOL {{ + bool cruise_available = GET_BIT(to_push, 17U); + if(!cruise_available) { + lateral_controls_allowed = 0; + } + if(alternative_experience & ALT_EXP_AOL_ENABLE_ON_MAIN) { + lateral_controls_allowed = cruise_available; + } + // }} PFEIFER - AOL bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U; pcm_cruise_check(cruise_engaged); } diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index bfe800142a3..7df9f72d5b1 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -165,6 +165,15 @@ static int subaru_rx_hook(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)) { + // PFEIFER - AOL {{ + bool cruise_available = GET_BIT(to_push, 40U) != 0U; + if (!cruise_available) { + lateral_controls_allowed = 0; + } + if(alternative_experience & ALT_EXP_AOL_ENABLE_ON_MAIN) { + lateral_controls_allowed = cruise_available; + } + // }} PFEIFER - AOL bool cruise_engaged = GET_BIT(to_push, 41U) != 0U; pcm_cruise_check(cruise_engaged); } diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h index 676670aaaf6..928e432efd7 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -98,6 +98,20 @@ static int tesla_rx_hook(CANPacket_t *to_push) { if(addr == (tesla_powertrain ? 0x256 : 0x368)) { // Cruise state int cruise_state = (GET_BYTE(to_push, 1) >> 4); + // PFEIFER - AOL {{ + bool cruise_available = (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 + if(!cruise_available) { + lateral_controls_allowed = 0; + } + if(alternative_experience & ALT_EXP_AOL_ENABLE_ON_MAIN) { + lateral_controls_allowed = cruise_available; + } + // }} PFEIFER - AOL bool cruise_engaged = (cruise_state == 2) || // ENABLED (cruise_state == 3) || // STANDSTILL (cruise_state == 4) || // OVERRIDE diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 0bc7f3c6df3..0c757668168 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -30,12 +30,18 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805; const CanMsg TOYOTA_TX_MSGS[] = {{0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8}, // DSU bus 0 {0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, // DSU bus 1 {0x2E4, 0, 5}, {0x191, 0, 8}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, // LKAS + ACC + // PFEIFER - AOL {{ + {0x1D3, 0, 8}, + // }} PFEIFER - AOL {0x200, 0, 6}}; // interceptor AddrCheckStruct toyota_addr_checks[] = { {.msg = {{ 0xaa, 0, 8, .check_checksum = false, .expected_timestep = 12000U}, { 0 }, { 0 }}}, {.msg = {{0x260, 0, 8, .check_checksum = true, .expected_timestep = 20000U}, { 0 }, { 0 }}}, {.msg = {{0x1D2, 0, 8, .check_checksum = true, .expected_timestep = 30000U}, { 0 }, { 0 }}}, + // PFEIFER - AOL {{ + {.msg = {{0x1D3, 0, 8, .check_checksum = true, .expected_timestep = 30000U}, { 0 }, { 0 }}}, + // }} PFEIFER - AOL {.msg = {{0x224, 0, 8, .check_checksum = false, .expected_timestep = 25000U}, {0x226, 0, 8, .check_checksum = false, .expected_timestep = 25000U}, { 0 }}}, }; @@ -94,6 +100,21 @@ static int toyota_rx_hook(CANPacket_t *to_push) { torque_meas.max++; } + // PFEIFER - AOL {{ + // wrap lateral controls on main + if (addr == 0x1D3) { + // ACC main switch on is a prerequisite to enter controls, exit controls immediately on main switch off + // Signal: PCM_CRUISE_2/MAIN_ON at 15th bit + acc_main_on = GET_BIT(to_push, 15U); + if (!acc_main_on) { + lateral_controls_allowed = 0; + } + if(alternative_experience & ALT_EXP_AOL_ENABLE_ON_MAIN) { + lateral_controls_allowed = acc_main_on; + } + } + // }} PFEIFER - AOL + // enter controls on rising edge of ACC, exit controls on ACC off // exit controls on rising edge of gas press if (addr == 0x1D2) { diff --git a/board/safety/safety_volkswagen_mqb.h b/board/safety/safety_volkswagen_mqb.h index 0716cdf8fb0..77a59c1ea91 100644 --- a/board/safety/safety_volkswagen_mqb.h +++ b/board/safety/safety_volkswagen_mqb.h @@ -158,7 +158,15 @@ static int volkswagen_mqb_rx_hook(CANPacket_t *to_push) { if (!acc_main_on) { controls_allowed = false; + // PFEIFER - AOL {{ + lateral_controls_allowed = 0; + // }} PFEIFER - AOL } + // PFEIFER - AOL {{ + if(alternative_experience & ALT_EXP_AOL_ENABLE_ON_MAIN) { + lateral_controls_allowed = acc_main_on; + } + // }} PFEIFER - AOL } if (addr == MSG_GRA_ACC_01) { diff --git a/board/safety/safety_volkswagen_pq.h b/board/safety/safety_volkswagen_pq.h index 93d180a8114..af08ce7dfba 100644 --- a/board/safety/safety_volkswagen_pq.h +++ b/board/safety/safety_volkswagen_pq.h @@ -124,6 +124,18 @@ static int volkswagen_pq_rx_hook(CANPacket_t *to_push) { update_sample(&torque_driver, torque_driver_new); } + // PFEIFER - AOL {{ + if (addr == MSG_MOTOR_5) { + acc_main_on = GET_BIT(to_push, 50U); + if (!acc_main_on) { + lateral_controls_allowed = 0; + } + if(alternative_experience & ALT_EXP_AOL_ENABLE_ON_MAIN) { + lateral_controls_allowed = acc_main_on; + } + } + // }} PFEIFER - AOL + if (volkswagen_longitudinal) { if (addr == MSG_MOTOR_5) { // ACC main switch on is a prerequisite to enter controls, exit controls immediately on main switch off diff --git a/board/safety_declarations.h b/board/safety_declarations.h index cbef3a25d49..44fcf6801c5 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -195,6 +195,9 @@ void safety_tick(const addr_checks *addr_checks); // This can be set by the safety hooks bool controls_allowed = false; +// PFEIFER - AOL {{ +bool lateral_controls_allowed = false; +// }} PFEIFER - AOL bool relay_malfunction = false; bool gas_interceptor_detected = false; int gas_interceptor_prev = 0; @@ -209,6 +212,10 @@ 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; +// PFEIFER - AOL {{ +int main_button_prev = 0; +bool set_me_prev = false; +// }} PFEIFER - AOL bool safety_rx_checks_invalid = false; // for safety modes with torque steering control @@ -251,3 +258,8 @@ 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; + +// PFEIFER - AOL {{ +#define ALT_EXP_ENABLE_ALWAYS_ON_LATERAL 16 +#define ALT_EXP_AOL_ENABLE_ON_MAIN 32 +// }} PFEIFER - AOL diff --git a/python/__init__.py b/python/__init__.py index bfa376840e3..f8007edc6bd 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -111,6 +111,10 @@ class ALTERNATIVE_EXPERIENCE: DISABLE_DISENGAGE_ON_GAS = 1 DISABLE_STOCK_AEB = 2 RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8 + # PFEIFER - AOL {{ + ENABLE_ALWAYS_ON_LATERAL = 16 + AOL_ENABLE_ON_MAIN = 32 + # }} PFEIFER - AOL class Panda: diff --git a/tests/libpanda/safety_helpers.h b/tests/libpanda/safety_helpers.h index 22a3c4f13a6..dc07538d999 100644 --- a/tests/libpanda/safety_helpers.h +++ b/tests/libpanda/safety_helpers.h @@ -21,6 +21,9 @@ bool addr_checks_valid() { void set_controls_allowed(bool c){ controls_allowed = c; + // PFEIFER - AOL {{ + lateral_controls_allowed = c; + // }} PFEIFER - AOL } void set_alternative_experience(int mode){