diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index b969c91d4..2f60b7e02 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -14,6 +14,7 @@ from cereal import car, custom, log from opendbc.car import gen_empty_fingerprint from opendbc.car.car_helpers import interfaces from opendbc.car.gm.values import GMFlags +from opendbc.car.hyundai.values import HyundaiFlags from opendbc.car.interfaces import CarInterfaceBase, GearShifter from opendbc.car.mock.values import CAR as MOCK from opendbc.car.subaru.values import SubaruFlags @@ -207,9 +208,11 @@ class FrogPilotVariables: toggle.has_zss = toggle.car_make == "toyota" and bool(FPCP.flags & ToyotaFrogPilotFlags.ZSS.value) is_angle_car = CP.steerControlType == car.CarParams.SteerControlType.angle latAccelFactor = CP.lateralTuning.torque.latAccelFactor + toggle.lkas_allowed_for_aol = toggle.car_make == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD or CP.flags & HyundaiFlags.HAS_LDA_BUTTON) longitudinalActuatorDelay = CP.longitudinalActuatorDelay toggle.openpilot_longitudinal = CP.openpilotLongitudinalControl and not toggle.disable_openpilot_long pcm_cruise = CP.pcmCruise + prohibited_main_aol = not toggle.openpilot_longitudinal and toggle.car_make == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD or CP.flags & HyundaiFlags.HAS_LDA_BUTTON) startAccel = CP.startAccel stopAccel = CP.stopAccel steerActuatorDelay = CP.steerActuatorDelay @@ -265,6 +268,11 @@ class FrogPilotVariables: toggle.warningSoft_volume = self.get_value("WarningSoftVolume", cast=float, condition=toggle.alert_volume_controller) toggle.warningImmediate_volume = max(self.get_value("WarningImmediateVolume", cast=float, condition=toggle.alert_volume_controller, default=25), 25) + toggle.always_on_lateral = self.get_value("AlwaysOnLateral") + toggle.always_on_lateral_lkas = toggle.always_on_lateral and toggle.lkas_allowed_for_aol and self.get_value("AlwaysOnLateralLKAS") + toggle.always_on_lateral_main = toggle.always_on_lateral and not prohibited_main_aol and not toggle.always_on_lateral_lkas + toggle.always_on_lateral_pause_speed = self.get_value("PauseAOLOnBrake", cast=float, condition=toggle.always_on_lateral) + toggle.automatic_updates = self.get_value("AutomaticUpdates", condition=(self.release_branch or self.vetting_branch), default=True) and not BACKUP_PATH.is_file() car_model = self.params.get("CarModel") diff --git a/frogpilot/controls/frogpilot_card.py b/frogpilot/controls/frogpilot_card.py index 3dd99e0f9..07a4913df 100644 --- a/frogpilot/controls/frogpilot_card.py +++ b/frogpilot/controls/frogpilot_card.py @@ -1,6 +1,10 @@ #!/usr/bin/env python3 +from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.common.params import Params from openpilot.selfdrive.car.cruise import ButtonType +from openpilot.selfdrive.selfdrived.events import ET + +from openpilot.frogpilot.common.frogpilot_variables import NON_DRIVING_GEARS class FrogPilotCard: def __init__(self, CP, FPCP): @@ -10,9 +14,28 @@ class FrogPilotCard: self.params_memory = Params(memory=True) self.accel_pressed = False + self.always_on_lateral_allowed = False self.decel_pressed = False + self.always_on_lateral_set = bool(FPCP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) + def update(self, carState, frogpilotCarState, sm, frogpilot_toggles): + if self.CP.brand == "hyundai": + for be in carState.buttonEvents: + if be.type == ButtonType.lkas and be.pressed and frogpilot_toggles.always_on_lateral_lkas: + self.always_on_lateral_allowed = not self.always_on_lateral_allowed + elif be.type == ButtonType.mainCruise and be.pressed and frogpilot_toggles.always_on_lateral_main: + self.always_on_lateral_allowed = not self.always_on_lateral_allowed + elif frogpilot_toggles.always_on_lateral_main: + self.always_on_lateral_allowed = carState.cruiseState.available + + self.always_on_lateral_enabled = self.always_on_lateral_allowed and self.always_on_lateral_set + self.always_on_lateral_enabled &= carState.gearShifter not in NON_DRIVING_GEARS + self.always_on_lateral_enabled &= sm["frogpilotPlan"].lateralCheck + self.always_on_lateral_enabled &= sm["liveCalibration"].calPerc >= 1 + self.always_on_lateral_enabled &= (ET.IMMEDIATE_DISABLE not in sm["selfdriveState"].alertType + sm["frogpilotSelfdriveState"].alertType) + self.always_on_lateral_enabled &= not (carState.brakePressed and carState.vEgo < frogpilot_toggles.always_on_lateral_pause_speed) or carState.standstill + if sm.updated["frogpilotPlan"] or any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents): self.accel_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents) @@ -20,6 +43,7 @@ class FrogPilotCard: self.decel_pressed = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents) frogpilotCarState.accelPressed = self.accel_pressed + frogpilotCarState.alwaysOnLateralEnabled = self.always_on_lateral_enabled frogpilotCarState.decelPressed = self.decel_pressed return frogpilotCarState diff --git a/frogpilot/ui/frogpilot_ui.cc b/frogpilot/ui/frogpilot_ui.cc index c67ba2abd..49dc46ec4 100644 --- a/frogpilot/ui/frogpilot_ui.cc +++ b/frogpilot/ui/frogpilot_ui.cc @@ -12,6 +12,7 @@ static void update_state(FrogPilotUIState *fs) { } if (fpsm.updated("frogpilotCarState")) { const cereal::FrogPilotCarState::Reader &frogpilotCarState = fpsm["frogpilotCarState"].getFrogpilotCarState(); + frogpilot_scene.always_on_lateral_active = !frogpilot_scene.enabled && frogpilotCarState.getAlwaysOnLateralEnabled(); } if (fpsm.updated("frogpilotPlan")) { const cereal::FrogPilotPlan::Reader &frogpilotPlan = fpsm["frogpilotPlan"].getFrogpilotPlan(); diff --git a/frogpilot/ui/frogpilot_ui.h b/frogpilot/ui/frogpilot_ui.h index cdfe78eba..b3b3ae78c 100644 --- a/frogpilot/ui/frogpilot_ui.h +++ b/frogpilot/ui/frogpilot_ui.h @@ -6,6 +6,7 @@ #include "frogpilot/ui/qt/widgets/frogpilot_controls.h" struct FrogPilotUIScene { + bool always_on_lateral_active; bool enabled; bool frogpilot_panel_active; bool online; diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 24a7a4e13..c29286b7f 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -70,6 +70,7 @@ class HyundaiSafetyFlags(IntFlag): # FrogPilot variables class HyundaiFrogPilotSafetyFlags(IntFlag): + HAS_LDA_BUTTON = 1024 class HyundaiFlags(IntFlag): diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index 69e1d3911..056fd464b 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -199,6 +199,9 @@ class CarInterfaceBase(ABC): fp_ret.isHDA2 = hda2 + if CP.flags & HyundaiFlags.HAS_LDA_BUTTON: + fp_ret.safetyConfigs[-1].safetyParam |= HyundaiFrogPilotSafetyFlags.HAS_LDA_BUTTON.value + elif platform in TOYOTA: fp_ret.canUsePedal = not CP.autoResumeSng fp_ret.canUseSDSU = candidate not in UNSUPPORTED_DSU_CAR and candidate not in TSS2_CAR diff --git a/opendbc_repo/opendbc/car/rivian/carstate.py b/opendbc_repo/opendbc/car/rivian/carstate.py index acf205ecf..52445a201 100644 --- a/opendbc_repo/opendbc/car/rivian/carstate.py +++ b/opendbc_repo/opendbc/car/rivian/carstate.py @@ -54,7 +54,7 @@ class CarState(CarStateBase): ret.cruiseState.speed = self.last_speed * CV.MPH_TO_MS # detected speed limit if not self.CP.openpilotLongitudinalControl: ret.cruiseState.speed = -1 - ret.cruiseState.available = True # cp.vl["VDM_AdasSts"]["VDM_AdasInterfaceStatus"] == 1 + ret.cruiseState.available = cp.vl["VDM_AdasSts"]["VDM_AdasInterfaceStatus"] in (1, 2) ret.cruiseState.standstill = cp.vl["VDM_AdasSts"]["VDM_AdasVehicleHoldStatus"] == 1 # ACM_Status->ACM_FaultSupervisorState normally 1, appears to go to 3 when either: diff --git a/opendbc_repo/opendbc/safety/__init__.py b/opendbc_repo/opendbc/safety/__init__.py index 910638551..fe0261279 100644 --- a/opendbc_repo/opendbc/safety/__init__.py +++ b/opendbc_repo/opendbc/safety/__init__.py @@ -10,3 +10,4 @@ class ALTERNATIVE_EXPERIENCE: ALLOW_AEB = 16 # FrogPilot variables + ALWAYS_ON_LATERAL = 32 diff --git a/opendbc_repo/opendbc/safety/declarations.h b/opendbc_repo/opendbc/safety/declarations.h index 18b451b88..2fdef6811 100644 --- a/opendbc_repo/opendbc/safety/declarations.h +++ b/opendbc_repo/opendbc/safety/declarations.h @@ -269,6 +269,10 @@ extern int cruise_button_prev; extern bool safety_rx_checks_invalid; // FrogPilot variables +extern bool aol_allowed; +extern bool lkas_button_prev; +extern bool lkas_on; +extern bool main_button_prev; // for safety modes with torque steering control extern int desired_torque_last; // last desired steer torque @@ -312,6 +316,7 @@ extern bool enable_gas_interceptor; #define ALT_EXP_ALLOW_AEB 16 // FrogPilot variables +#define ALT_EXP_ALWAYS_ON_LATERAL 32 extern int alternative_experience; diff --git a/opendbc_repo/opendbc/safety/lateral.h b/opendbc_repo/opendbc/safety/lateral.h index cfe15af36..94b57ce57 100644 --- a/opendbc_repo/opendbc/safety/lateral.h +++ b/opendbc_repo/opendbc/safety/lateral.h @@ -61,7 +61,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee bool violation = false; uint32_t ts = microsecond_timer_get(); - if (controls_allowed) { + if (aol_allowed || controls_allowed) { // Some safety models support variable torque limit based on vehicle speed int max_torque = limits.max_torque; if (limits.dynamic_max_torque) { @@ -96,7 +96,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee } // no torque if controls is not allowed - if (!controls_allowed && (desired_torque != 0)) { + if (!(aol_allowed || controls_allowed) && (desired_torque != 0)) { violation = true; } @@ -138,7 +138,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee } // reset to 0 if either controls is not allowed or there's a violation - if (violation || !controls_allowed) { + if (violation || !(aol_allowed || controls_allowed)) { valid_steer_req_count = 0; invalid_steer_req_count = 0; desired_torque_last = 0; @@ -176,7 +176,7 @@ static bool rt_angle_rate_limit_check(AngleSteeringLimits limits) { bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits) { bool violation = false; - if (controls_allowed && steer_control_enabled) { + if ((aol_allowed || controls_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 @@ -262,12 +262,12 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const } // No angle control allowed when controls are not allowed - if (!controls_allowed) { + if (!(aol_allowed || controls_allowed)) { violation |= steer_control_enabled; } // reset to current angle if either controls is not allowed or there's a violation - if (violation || !controls_allowed) { + if (violation || !(aol_allowed || controls_allowed)) { if (limits.inactive_angle_is_zero) { desired_angle_last = 0; } else { @@ -304,7 +304,7 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co bool violation = false; - if (controls_allowed && steer_control_enabled) { + if ((aol_allowed || controls_allowed) && steer_control_enabled) { // *** ISO lateral jerk limit *** // calculate maximum angle rate per second const float max_curvature_rate_sec = MAX_LATERAL_JERK / (fudged_speed * fudged_speed); @@ -340,12 +340,12 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co } // No angle control allowed when controls are not allowed - if (!controls_allowed) { + if (!(aol_allowed || controls_allowed)) { violation |= steer_control_enabled; } // reset to current angle if either controls is not allowed or there's a violation - if (violation || !controls_allowed) { + if (violation || !(aol_allowed || controls_allowed)) { desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle); } diff --git a/opendbc_repo/opendbc/safety/modes/chrysler.h b/opendbc_repo/opendbc/safety/modes/chrysler.h index 30d9096a7..9348e8fbc 100644 --- a/opendbc_repo/opendbc/safety/modes/chrysler.h +++ b/opendbc_repo/opendbc/safety/modes/chrysler.h @@ -80,6 +80,7 @@ static void chrysler_rx_hook(const CANPacket_t *msg) { pcm_cruise_check(cruise_engaged); // FrogPilot variables + acc_main_on = GET_BIT(msg, 20U); } // TODO: use the same message for both diff --git a/opendbc_repo/opendbc/safety/modes/ford.h b/opendbc_repo/opendbc/safety/modes/ford.h index 447cc7d11..a1957ad0e 100644 --- a/opendbc_repo/opendbc/safety/modes/ford.h +++ b/opendbc_repo/opendbc/safety/modes/ford.h @@ -160,6 +160,7 @@ static void ford_rx_hook(const CANPacket_t *msg) { pcm_cruise_check(cruise_engaged); // FrogPilot variables + acc_main_on = (cruise_state == 3U) || cruise_engaged; } } } diff --git a/opendbc_repo/opendbc/safety/modes/gm.h b/opendbc_repo/opendbc/safety/modes/gm.h index 73806c9dc..ea86e277d 100644 --- a/opendbc_repo/opendbc/safety/modes/gm.h +++ b/opendbc_repo/opendbc/safety/modes/gm.h @@ -125,6 +125,9 @@ static void gm_rx_hook(const CANPacket_t *msg) { } // FrogPilot variables + if (msg->addr == 0xC9U) { + acc_main_on = GET_BIT(msg, 29U); + } } static bool gm_tx_hook(const CANPacket_t *msg) { diff --git a/opendbc_repo/opendbc/safety/modes/honda.h b/opendbc_repo/opendbc/safety/modes/honda.h index 9f49ef762..1848ee43f 100644 --- a/opendbc_repo/opendbc/safety/modes/honda.h +++ b/opendbc_repo/opendbc/safety/modes/honda.h @@ -238,7 +238,7 @@ static bool honda_tx_hook(const CANPacket_t *msg) { // STEER: safety check if ((msg->addr == 0xE4U) || (msg->addr == 0x194U)) { - if (!controls_allowed) { + if (!(aol_allowed || controls_allowed)) { bool steer_applied = msg->data[0] | msg->data[1]; if (steer_applied) { tx = false; diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index 70903e7a8..2d9173588 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai.h @@ -53,6 +53,8 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = { {.msg = {{0x91, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ // FrogPilot variables +#define HYUNDAI_LDA_BUTTON_ADDR_CHECK \ + {.msg = {{0x391, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ static const CanMsg HYUNDAI_TX_MSGS[] = { HYUNDAI_COMMON_TX_MSGS(0) @@ -177,6 +179,9 @@ static void hyundai_rx_hook(const CANPacket_t *msg) { } // FrogPilot variables + if (msg->addr == 0x391U) { + hyundai_lkas_button_check(GET_BIT(msg, 4U)); + } } } @@ -284,11 +289,29 @@ static safety_config hyundai_init(uint16_t param) { }; // FrogPilot variables + static RxCheck hyundai_long_rx_checks_lda[] = { + HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_LDA_BUTTON_ADDR_CHECK + }; + + static RxCheck hyundai_fcev_long_rx_checks_lda[] = { + HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_FCEV_GAS_ADDR_CHECK + HYUNDAI_LDA_BUTTON_ADDR_CHECK + }; if (hyundai_fcev_gas_signal) { - SET_RX_CHECKS(hyundai_fcev_long_rx_checks, ret); + if (hyundai_has_lda_button) { + SET_RX_CHECKS(hyundai_fcev_long_rx_checks_lda, ret); + } else { + SET_RX_CHECKS(hyundai_fcev_long_rx_checks, ret); + } } else { - SET_RX_CHECKS(hyundai_long_rx_checks, ret); + if (hyundai_has_lda_button) { + SET_RX_CHECKS(hyundai_long_rx_checks_lda, ret); + } else { + SET_RX_CHECKS(hyundai_long_rx_checks, ret); + } } if (hyundai_camera_scc) { SET_TX_MSGS(HYUNDAI_CAMERA_SCC_LONG_TX_MSGS, ret); @@ -303,8 +326,17 @@ static safety_config hyundai_init(uint16_t param) { }; // FrogPilot variables + static RxCheck hyundai_cam_scc_rx_checks_lda[] = { + HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_SCC12_ADDR_CHECK(2) + HYUNDAI_LDA_BUTTON_ADDR_CHECK + }; - ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks, HYUNDAI_CAMERA_SCC_TX_MSGS); + if (hyundai_has_lda_button) { + ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks_lda, HYUNDAI_CAMERA_SCC_TX_MSGS); + } else { + ret = BUILD_SAFETY_CFG(hyundai_cam_scc_rx_checks, HYUNDAI_CAMERA_SCC_TX_MSGS); + } } else { static RxCheck hyundai_rx_checks[] = { HYUNDAI_COMMON_RX_CHECKS(false) @@ -318,12 +350,32 @@ static safety_config hyundai_init(uint16_t param) { }; // FrogPilot variables + static RxCheck hyundai_rx_checks_lda[] = { + HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_SCC12_ADDR_CHECK(0) + HYUNDAI_LDA_BUTTON_ADDR_CHECK + }; + + static RxCheck hyundai_fcev_rx_checks_lda[] = { + HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_SCC12_ADDR_CHECK(0) + HYUNDAI_FCEV_GAS_ADDR_CHECK + HYUNDAI_LDA_BUTTON_ADDR_CHECK + }; SET_TX_MSGS(HYUNDAI_TX_MSGS, ret); if (hyundai_fcev_gas_signal) { - SET_RX_CHECKS(hyundai_fcev_rx_checks, ret); + if (hyundai_has_lda_button) { + SET_RX_CHECKS(hyundai_fcev_rx_checks_lda, ret); + } else { + SET_RX_CHECKS(hyundai_fcev_rx_checks, ret); + } } else { - SET_RX_CHECKS(hyundai_rx_checks, ret); + if (hyundai_has_lda_button) { + SET_RX_CHECKS(hyundai_rx_checks_lda, ret); + } else { + SET_RX_CHECKS(hyundai_rx_checks, ret); + } } } return ret; diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h index fca509847..3a7b5c9e4 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h @@ -90,11 +90,13 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) { main_button = GET_BIT(msg, 19U); // FrogPilot variables + hyundai_lkas_button_check(GET_BIT(msg, 23U)); } else { cruise_button = (msg->data[4] >> 4) & 0x7U; main_button = GET_BIT(msg, 34U); // FrogPilot variables + hyundai_lkas_button_check(GET_BIT(msg, 39U)); } hyundai_common_cruise_buttons_check(cruise_button, main_button); } diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_common.h b/opendbc_repo/opendbc/safety/modes/hyundai_common.h index 6a11f852f..f57b34e2c 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_common.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_common.h @@ -43,6 +43,8 @@ extern bool hyundai_alt_limits_2; bool hyundai_alt_limits_2 = false; // FrogPilot variables +extern bool hyundai_has_lda_button; +bool hyundai_has_lda_button = false; static uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button @@ -56,6 +58,7 @@ void hyundai_common_init(uint16_t param) { const uint16_t HYUNDAI_PARAM_ALT_LIMITS_2 = 512; // FrogPilot variables + const int HYUNDAI_PARAM_HAS_LDA_BUTTON = 1024; hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS); hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS); @@ -66,6 +69,7 @@ void hyundai_common_init(uint16_t param) { hyundai_alt_limits_2 = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS_2); // FrogPilot variables + hyundai_has_lda_button = GET_FLAG(param, HYUNDAI_PARAM_HAS_LDA_BUTTON); hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; @@ -118,6 +122,10 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai } // FrogPilot variables + if (main_button && !main_button_prev) { + acc_main_on = !acc_main_on; + } + main_button_prev = main_button; } #ifdef CANFD @@ -148,3 +156,9 @@ uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *msg) { #endif // FrogPilot variables +void hyundai_lkas_button_check(const bool lkas_button) { + if (lkas_button && !lkas_button_prev) { + lkas_on = !lkas_on; + } + lkas_button_prev = lkas_button; +} diff --git a/opendbc_repo/opendbc/safety/modes/mazda.h b/opendbc_repo/opendbc/safety/modes/mazda.h index b4624c933..cb9d9f376 100644 --- a/opendbc_repo/opendbc/safety/modes/mazda.h +++ b/opendbc_repo/opendbc/safety/modes/mazda.h @@ -36,6 +36,7 @@ static void mazda_rx_hook(const CANPacket_t *msg) { pcm_cruise_check(cruise_engaged); // FrogPilot variables + acc_main_on = GET_BIT(msg, 17U); } if (msg->addr == MAZDA_ENGINE_DATA) { diff --git a/opendbc_repo/opendbc/safety/modes/nissan.h b/opendbc_repo/opendbc/safety/modes/nissan.h index f41034c42..23765687e 100644 --- a/opendbc_repo/opendbc/safety/modes/nissan.h +++ b/opendbc_repo/opendbc/safety/modes/nissan.h @@ -52,6 +52,13 @@ static void nissan_rx_hook(const CANPacket_t *msg) { } // FrogPilot variables + if ((msg->addr == 0x1B6U) && (msg->bus == (nissan_alt_eps ? 2U : 1U))) { + acc_main_on = GET_BIT(msg, 36U); + } + + if ((msg->addr == 0x239U) && (msg->bus == 0U)) { + acc_main_on = GET_BIT(msg, 17U); + } } diff --git a/opendbc_repo/opendbc/safety/modes/psa.h b/opendbc_repo/opendbc/safety/modes/psa.h index 971922e69..04e12b7c5 100644 --- a/opendbc_repo/opendbc/safety/modes/psa.h +++ b/opendbc_repo/opendbc/safety/modes/psa.h @@ -11,6 +11,7 @@ #define PSA_LANE_KEEP_ASSIST 1010U // TX from OP, EPS // FrogPilot variables +#define PSA_HS2_DYN1_MDD_ETAT_2B6 694U // RX from BSI, ACC status // CAN bus #define PSA_MAIN_BUS 0U @@ -24,6 +25,8 @@ static uint8_t psa_get_counter(const CANPacket_t *msg) { } else if (msg->addr == PSA_HS2_DYN_ABR_38D) { cnt = (msg->data[5] >> 4) & 0xFU; // FrogPilot variables + } else if (msg->addr == PSA_HS2_DYN1_MDD_ETAT_2B6) { + cnt = (msg->data[7] >> 4) & 0xFU; } else { } return cnt; @@ -36,6 +39,8 @@ static uint32_t psa_get_checksum(const CANPacket_t *msg) { } else if (msg->addr == PSA_HS2_DYN_ABR_38D) { chksum = msg->data[5] & 0xFU; // FrogPilot variables + } else if (msg->addr == PSA_HS2_DYN1_MDD_ETAT_2B6) { + chksum = msg->data[7] & 0xFU; } else { } return chksum; @@ -64,6 +69,8 @@ static uint32_t psa_compute_checksum(const CANPacket_t *msg) { } else if (msg->addr == PSA_HS2_DYN_ABR_38D) { chk = _psa_compute_checksum(msg, 0x7, 5); // FrogPilot variables + } else if (msg->addr == PSA_HS2_DYN1_MDD_ETAT_2B6) { + chk = _psa_compute_checksum(msg, 0x3, 7); } else { } return chk; @@ -90,6 +97,9 @@ static void psa_rx_hook(const CANPacket_t *msg) { pcm_cruise_check((msg->data[2U] >> 7U) & 1U); // RVV_ACC_ACTIVATION_REQ } // FrogPilot variables + if (msg->addr == PSA_HS2_DYN1_MDD_ETAT_2B6) { + acc_main_on = (msg->data[3] & 0x0FU) > 2; + } } @@ -144,6 +154,7 @@ static safety_config psa_init(uint16_t param) { {.msg = {{PSA_DAT_BSI, PSA_CAM_BUS, 8, 20U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // brake // FrogPilot variables + {.msg = {{PSA_HS2_DYN1_MDD_ETAT_2B6, PSA_ADAS_BUS, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // ACC status }; return BUILD_SAFETY_CFG(psa_rx_checks, PSA_TX_MSGS); diff --git a/opendbc_repo/opendbc/safety/modes/rivian.h b/opendbc_repo/opendbc/safety/modes/rivian.h index 879034fce..176694b7c 100644 --- a/opendbc_repo/opendbc/safety/modes/rivian.h +++ b/opendbc_repo/opendbc/safety/modes/rivian.h @@ -99,6 +99,7 @@ static void rivian_rx_hook(const CANPacket_t *msg) { pcm_cruise_check(feature_status == 1); // FrogPilot variables + acc_main_on = (feature_status == 0) || (feature_status == 1); } } } diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index 381670471..d772c485d 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -111,6 +111,7 @@ static void subaru_rx_hook(const CANPacket_t *msg) { pcm_cruise_check(cruise_engaged); // FrogPilot variables + acc_main_on = GET_BIT(msg, 40U); } // update vehicle moving with any non-zero wheel speed diff --git a/opendbc_repo/opendbc/safety/modes/subaru_preglobal.h b/opendbc_repo/opendbc/safety/modes/subaru_preglobal.h index 1d025410f..673728d6c 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru_preglobal.h +++ b/opendbc_repo/opendbc/safety/modes/subaru_preglobal.h @@ -35,6 +35,7 @@ static void subaru_preglobal_rx_hook(const CANPacket_t *msg) { pcm_cruise_check(cruise_engaged); // FrogPilot variables + acc_main_on = GET_BIT(msg, 48U); } // update vehicle moving with any non-zero wheel speed diff --git a/opendbc_repo/opendbc/safety/modes/tesla.h b/opendbc_repo/opendbc/safety/modes/tesla.h index 1a2586005..dd1394b34 100644 --- a/opendbc_repo/opendbc/safety/modes/tesla.h +++ b/opendbc_repo/opendbc/safety/modes/tesla.h @@ -161,6 +161,7 @@ static void tesla_rx_hook(const CANPacket_t *msg) { pcm_cruise_check(cruise_engaged); // FrogPilot variables + acc_main_on = ((cruise_state == 1) || cruise_engaged) && !tesla_autopark; } if (msg->addr == 0x155U) { diff --git a/opendbc_repo/opendbc/safety/modes/toyota.h b/opendbc_repo/opendbc/safety/modes/toyota.h index 0e58d8db6..78880cf00 100644 --- a/opendbc_repo/opendbc/safety/modes/toyota.h +++ b/opendbc_repo/opendbc/safety/modes/toyota.h @@ -40,6 +40,7 @@ {.msg = {{ 0xaa, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ {.msg = {{0x260, 0, 8, 50U, .ignore_counter = true, .ignore_quality_flag=!(lta)}, { 0 }, { 0 }}}, \ /* FrogPilot Variables */ \ + {.msg = {{0x1D3, 0, 8, 33U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ #define TOYOTA_RX_CHECKS(lta) \ TOYOTA_COMMON_RX_CHECKS(lta) \ @@ -162,6 +163,13 @@ static void toyota_rx_hook(const CANPacket_t *msg) { } // FrogPilot variables + if (msg->addr == 0x1D3U) { + acc_main_on = GET_BIT(msg, 15U); + } + + if (msg->addr == 0x365U) { + acc_main_on = GET_BIT(msg, 0U); + } } } diff --git a/opendbc_repo/opendbc/safety/safety.h b/opendbc_repo/opendbc/safety/safety.h index 2c0683e37..d5f0f6c31 100644 --- a/opendbc_repo/opendbc/safety/safety.h +++ b/opendbc_repo/opendbc/safety/safety.h @@ -61,6 +61,10 @@ int cruise_button_prev = 0; bool safety_rx_checks_invalid = false; // FrogPilot variables +bool aol_allowed = false; +bool lkas_button_prev = false; +bool lkas_on = false; +bool main_button_prev = false; // for safety modes with torque steering control int desired_torque_last = 0; // last desired steer torque @@ -370,6 +374,7 @@ static void generic_rx_checks(void) { steering_disengage_prev = steering_disengage; // FrogPilot variables + aol_allowed = (acc_main_on || lkas_on) && (alternative_experience & ALT_EXP_ALWAYS_ON_LATERAL); } static void stock_ecu_check(bool stock_ecu_detected) { @@ -494,6 +499,10 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { enable_gas_interceptor = false; // FrogPilot variables + aol_allowed = false; + lkas_button_prev = false; + lkas_on = false; + main_button_prev = false; return set_status; } diff --git a/opendbc_repo/opendbc/safety/tests/common.py b/opendbc_repo/opendbc/safety/tests/common.py index 5440b4551..46b34a575 100644 --- a/opendbc_repo/opendbc/safety/tests/common.py +++ b/opendbc_repo/opendbc/safety/tests/common.py @@ -302,6 +302,40 @@ class TorqueSteeringSafetyTestBase(SafetyTestBase, abc.ABC): self.assertFalse(self._tx(self._torque_cmd_msg(self.MAX_TORQUE, 1))) # FrogPilot variables + def _toggle_aol(self, toggle_on): + """Toggles "Always On Lateral" On/Off""" + pass + + def test_always_on_lateral(self): + if self._toggle_aol(True) is None: + raise unittest.SkipTest("AOL message not implemented for this safety mode") + + self.safety.set_controls_allowed(False) + + torque_cmd = self.MAX_RATE_UP # Use the max rate + + # Without alt exp, make sure steering is blocked + self.safety.set_alternative_experience(0) + self._set_prev_torque(0) + self.assertFalse(self._tx(self._torque_cmd_msg(torque_cmd))) + + # With alt exp, but without main on, steering should be blocked + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) + self._rx(self._toggle_aol(False)) + self._set_prev_torque(0) + self.assertFalse(self._tx(self._torque_cmd_msg(torque_cmd))) + self.assertFalse(self.safety.get_longitudinal_allowed()) + + # With alt exp and main on, steering should be allowed + self._rx(self._toggle_aol(True)) + self._set_prev_torque(0) + self.assertTrue(self._tx(self._torque_cmd_msg(torque_cmd))) + self.assertFalse(self.safety.get_longitudinal_allowed()) + + # Turn off main, steering should be blocked again + self._rx(self._toggle_aol(False)) + self.safety.set_desired_torque_last(torque_cmd) + self.assertFalse(self._tx(self._torque_cmd_msg(torque_cmd))) class SteerRequestCutSafetyTest(TorqueSteeringSafetyTestBase, abc.ABC): @@ -809,6 +843,42 @@ class AngleSteeringSafetyTest(VehicleSpeedSafetyTest): self.assertTrue(self._tx(self._angle_cmd_msg(0, True, increment_timer=False))) # FrogPilot variables + def _toggle_aol(self, toggle_on): + """Toggles "Always On Lateral" on/off""" + pass + + def test_always_on_lateral(self): + if self._toggle_aol(True) is None: + raise unittest.SkipTest("AOL message not implemented for this safety mode") + + self.safety.set_controls_allowed(False) + + self._reset_angle_measurement(0) + self._reset_speed_measurement(1) + angle_cmd = self.ANGLE_RATE_UP[0] / 2.0 # Use half of the max angle rate + + # Without alt exp, make sure steering is blocked + self.safety.set_alternative_experience(0) + self._set_prev_desired_angle(0) + self.assertFalse(self._tx(self._angle_cmd_msg(angle=angle_cmd, enabled=True))) + + # With alt exp, but without main on, steering should be blocked + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) + self._rx(self._toggle_aol(False)) + self._set_prev_desired_angle(0) + self.assertFalse(self._tx(self._angle_cmd_msg(angle=angle_cmd, enabled=True))) + self.assertFalse(self.safety.get_longitudinal_allowed()) + + # With alt exp and main on, steering should be allowed + self._rx(self._toggle_aol(True)) + self._set_prev_desired_angle(0) + self.assertTrue(self._tx(self._angle_cmd_msg(angle=angle_cmd, enabled=True))) + self.assertFalse(self.safety.get_longitudinal_allowed()) + + # Turn off main, steering should be blocked again + self._rx(self._toggle_aol(False)) + self._set_prev_desired_angle(angle_cmd) + self.assertFalse(self._tx(self._angle_cmd_msg(angle=angle_cmd, enabled=True))) class SafetyTest(SafetyTestBase): diff --git a/opendbc_repo/opendbc/safety/tests/hyundai_common.py b/opendbc_repo/opendbc/safety/tests/hyundai_common.py index df7dd8c70..fb18e4957 100644 --- a/opendbc_repo/opendbc/safety/tests/hyundai_common.py +++ b/opendbc_repo/opendbc/safety/tests/hyundai_common.py @@ -72,6 +72,25 @@ class HyundaiButtonBase: self._rx(self._button_msg(Buttons.NONE)) # FrogPilot variables + def _toggle_aol(self, toggle_on): + """ + Simulates toggling the main cruise button. The safety model requires a + press and release to change the main cruise state. This function + resets the safety model to a known state before each call. + """ + if not hasattr(self, "_aol_state"): + self._aol_state = False + + # Already in the requested state + if toggle_on == self._aol_state: + return None + + # Toggle: press + release sequence + self._rx(self._button_msg(Buttons.NONE, main_button=1)) + self._rx(self._button_msg(Buttons.NONE, main_button=0)) + + self._aol_state = toggle_on + return None # avoid duplicate message in harness class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest): diff --git a/opendbc_repo/opendbc/safety/tests/test_chrysler.py b/opendbc_repo/opendbc/safety/tests/test_chrysler.py index 8a3f1c325..be424584b 100755 --- a/opendbc_repo/opendbc/safety/tests/test_chrysler.py +++ b/opendbc_repo/opendbc/safety/tests/test_chrysler.py @@ -72,6 +72,10 @@ class TestChryslerSafety(common.CarSafetyTest, common.MotorTorqueSteeringSafetyT self.assertFalse(self._tx(self._button_msg(cancel=False, resume=False))) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # DAS_3, bit 20 is ACC_AVAILABLE + values = {"ACC_AVAILABLE": 1 if toggle_on else 0} + return self.packer.make_can_msg_panda("DAS_3", self.DAS_BUS, values) class TestChryslerRamDTSafety(TestChryslerSafety): diff --git a/opendbc_repo/opendbc/safety/tests/test_ford.py b/opendbc_repo/opendbc/safety/tests/test_ford.py index 88c90a1ff..3c9ab26f0 100755 --- a/opendbc_repo/opendbc/safety/tests/test_ford.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford.py @@ -379,6 +379,15 @@ class TestFordSafetyBase(common.CarSafetyTest): self.assertEqual(enabled, self._tx(self._acc_button_msg(Buttons.CANCEL, bus))) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # EngBrakeData, CcStat_D_Actl is the cruise state + # 3 is standby (main on), 5 is active (engaged) + brake = self.safety.get_brake_pressed_prev() + values = { + "BpedDrvAppl_D_Actl": 2 if brake else 1, + "CcStat_D_Actl": 3 if toggle_on else 0, + } + return self.packer.make_can_msg_panda("EngBrakeData", 0, values) class TestFordCANFDStockSafety(TestFordSafetyBase): diff --git a/opendbc_repo/opendbc/safety/tests/test_gm.py b/opendbc_repo/opendbc/safety/tests/test_gm.py index e3955191e..8343ac6e6 100755 --- a/opendbc_repo/opendbc/safety/tests/test_gm.py +++ b/opendbc_repo/opendbc/safety/tests/test_gm.py @@ -133,6 +133,10 @@ class TestGmSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTe return self.packer.make_can_msg_safety("ASCMSteeringButton", self.BUTTONS_BUS, values) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # ECMEngineStatus, bit 29 is CruiseMainOn + values = {"CruiseMainOn": 1 if toggle_on else 0} + return self.packer.make_can_msg_panda("ECMEngineStatus", 0, values) class TestGmEVSafetyBase(TestGmSafetyBase): diff --git a/opendbc_repo/opendbc/safety/tests/test_honda.py b/opendbc_repo/opendbc/safety/tests/test_honda.py index 387147415..571f7cf0d 100755 --- a/opendbc_repo/opendbc/safety/tests/test_honda.py +++ b/opendbc_repo/opendbc/safety/tests/test_honda.py @@ -238,6 +238,11 @@ class HondaBase(common.CarSafetyTest): self.assertFalse(self._tx(self._send_steer_msg(0x1000))) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # SCM_FEEDBACK, bit 28 is MAIN_ON + values = {"MAIN_ON": 1 if toggle_on else 0, "COUNTER": self.cnt_acc_state % 4} + self.__class__.cnt_acc_state += 1 + return self.packer.make_can_msg_panda("SCM_FEEDBACK", self.PT_BUS, values) # ********************* Honda Nidec ********************** diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index f638f2b0e..7264453d0 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -82,6 +82,26 @@ class TestHyundaiCanfdBase(HyundaiButtonBase, common.CarSafetyTest, common.Drive return self.packer.make_can_msg_safety("CRUISE_BUTTONS", bus, values) # FrogPilot variables + def _toggle_aol(self, toggle_on): + if not hasattr(self, "_aol_state"): + self._aol_state = False + + # Already in the requested state + if toggle_on == self._aol_state: + return None + + # Simulate button press + release + values = { + "CRUISE_BUTTONS": 0, + "ADAPTIVE_CRUISE_MAIN_BTN": 0, + "LFA_BTN": 1, + "COUNTER": 0, + } + self._rx(self.packer.make_can_msg_panda("CRUISE_BUTTONS", self.PT_BUS, values)) + self._rx(self.packer.make_can_msg_panda("CRUISE_BUTTONS", self.PT_BUS, {**values, "LFA_BTN": 0})) + + self._aol_state = toggle_on + return None # avoid duplicate message in harness class TestHyundaiCanfdLFASteeringBase(TestHyundaiCanfdBase): @@ -153,6 +173,26 @@ class TestHyundaiCanfdLFASteeringAltButtonsBase(TestHyundaiCanfdLFASteeringBase) self.assertFalse(self._tx(self._acc_cancel_msg(False))) # FrogPilot variables + def _toggle_aol(self, toggle_on): + if not hasattr(self, "_aol_state"): + self._aol_state = False + + # Already in the requested state + if toggle_on == self._aol_state: + return None + + # Simulate button press + release + values = { + "CRUISE_BUTTONS_ALT": 0, + "ADAPTIVE_CRUISE_MAIN_BTN": 0, + "LFA_BTN": 1, + "COUNTER": 0, + } + self._rx(self.packer.make_can_msg_panda("CRUISE_BUTTONS_ALT", self.PT_BUS, values)) + self._rx(self.packer.make_can_msg_panda("CRUISE_BUTTONS_ALT", self.PT_BUS, {**values, "LFA_BTN": 0})) + + self._aol_state = toggle_on + return None # avoid duplicate message in harness @parameterized_class(ALL_GAS_EV_HYBRID_COMBOS) diff --git a/opendbc_repo/opendbc/safety/tests/test_mazda.py b/opendbc_repo/opendbc/safety/tests/test_mazda.py index 5d228826a..70ca85814 100755 --- a/opendbc_repo/opendbc/safety/tests/test_mazda.py +++ b/opendbc_repo/opendbc/safety/tests/test_mazda.py @@ -81,6 +81,10 @@ class TestMazdaSafety(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTes self.assertTrue(self._tx(self._button_msg(resume=True))) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # CRZ_CTRL, CRZ_AVAILABLE is the main on button + values = {"CRZ_AVAILABLE": 1 if toggle_on else 0} + return self.packer.make_can_msg_panda("CRZ_CTRL", 0, values) if __name__ == "__main__": diff --git a/opendbc_repo/opendbc/safety/tests/test_nissan.py b/opendbc_repo/opendbc/safety/tests/test_nissan.py index 7090695be..8f9940a8e 100755 --- a/opendbc_repo/opendbc/safety/tests/test_nissan.py +++ b/opendbc_repo/opendbc/safety/tests/test_nissan.py @@ -80,6 +80,10 @@ class TestNissanSafety(common.CarSafetyTest, common.AngleSteeringSafetyTest): self.assertEqual(tx, should_tx) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # PRO_PILOT, CRUISE_ON is the main on button for X-Trail/Rogue/Altima + values = {"CRUISE_ON": 1 if toggle_on else 0} + return self.packer.make_can_msg_panda("PRO_PILOT", 2, values) class TestNissanSafetyAltEpsBus(TestNissanSafety): @@ -116,6 +120,10 @@ class TestNissanLeafSafety(TestNissanSafety): pass # FrogPilot variables + def _toggle_aol(self, toggle_on): + # CRUISE_THROTTLE, CRUISE_AVAILABLE is the main on button for Leaf + values = {"CRUISE_AVAILABLE": 1 if toggle_on else 0} + return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 0, values) if __name__ == "__main__": diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru.py b/opendbc_repo/opendbc/safety/tests/test_subaru.py index c7c3c8d63..06f83f34a 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru.py @@ -112,6 +112,10 @@ class TestSubaruSafetyBase(common.CarSafetyTest): return self.packer.make_can_msg_safety("CruiseControl", self.ALT_MAIN_BUS, values) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # CruiseControl, Cruise_On is the main on button + values = {"Cruise_On": 1 if toggle_on else 0} + return self.packer.make_can_msg_panda("CruiseControl", self.ALT_MAIN_BUS, values) class TestSubaruStockLongitudinalSafetyBase(TestSubaruSafetyBase): diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py b/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py index 5d470187f..405b62d27 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru_preglobal.py @@ -60,6 +60,10 @@ class TestSubaruPreglobalSafety(common.CarSafetyTest, common.DriverTorqueSteerin return self.packer.make_can_msg_safety("CruiseControl", 0, values) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # CruiseControl, Cruise_On is the main on button + values = {"Cruise_On": 1 if toggle_on else 0} + return self.packer.make_can_msg_panda("CruiseControl", 0, values) class TestSubaruPreglobalReversedDriverTorqueSafety(TestSubaruPreglobalSafety): diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla.py b/opendbc_repo/opendbc/safety/tests/test_tesla.py index 07df31d82..95088954f 100755 --- a/opendbc_repo/opendbc/safety/tests/test_tesla.py +++ b/opendbc_repo/opendbc/safety/tests/test_tesla.py @@ -354,6 +354,10 @@ class TestTeslaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest, self.assertTrue(self._tx(self._angle_cmd_msg(0, True))) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # DI_state, DI_cruiseState is the cruise state, 1 is standby + values = {"DI_cruiseState": 1 if toggle_on else 0} + return self.packer.make_can_msg_panda("DI_state", 0, values) class TestTeslaStockSafety(TestTeslaSafetyBase): diff --git a/opendbc_repo/opendbc/safety/tests/test_toyota.py b/opendbc_repo/opendbc/safety/tests/test_toyota.py old mode 100755 new mode 100644 index 3bdff15ca..e52d11832 --- a/opendbc_repo/opendbc/safety/tests/test_toyota.py +++ b/opendbc_repo/opendbc/safety/tests/test_toyota.py @@ -10,7 +10,7 @@ from opendbc.safety.tests.libsafety import libsafety_py import opendbc.safety.tests.common as common from opendbc.safety.tests.common import CANPackerSafety -TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0]] # LKAS + LTA + ACC & PCM cancel cmds +TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0], [0x1D3, 0]] # LKAS + LTA + ACC & PCM cancel cmds TOYOTA_SECOC_TX_MSGS = [[0x131, 0], [0x183, 0]] + TOYOTA_COMMON_TX_MSGS TOYOTA_COMMON_LONG_TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0], # DSU bus 0 [0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1], # DSU bus 1 @@ -123,6 +123,10 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT self.assertFalse(self.safety.get_controls_allowed()) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # pcm_cruise_2, bit 15 is toggle_on + values = {"MAIN_ON": 1 if toggle_on else 0} + return self.packer.make_can_msg_panda("PCM_CRUISE_2", 0, values) class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest): diff --git a/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py b/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py index 5418f913e..a8bb66d8c 100755 --- a/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py +++ b/opendbc_repo/opendbc/safety/tests/test_volkswagen_mqb.py @@ -127,6 +127,9 @@ class TestVolkswagenMqbSafetyBase(common.CarSafetyTest, common.DriverTorqueSteer self.assertEqual(0, self.safety.get_torque_driver_min()) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # TSK_06, TSK_Status is the cruise state, 2 is standby + return self._tsk_status_msg(False, main_switch=toggle_on) class TestVolkswagenMqbStockSafety(TestVolkswagenMqbSafetyBase): diff --git a/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py b/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py index bb0995738..79ab10291 100755 --- a/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py +++ b/opendbc_repo/opendbc/safety/tests/test_volkswagen_pq.py @@ -109,6 +109,9 @@ class TestVolkswagenPqSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeri self.assertEqual(0, self.safety.get_torque_driver_min()) # FrogPilot variables + def _toggle_aol(self, toggle_on): + # Motor_5, GRA_Hauptschalter is the main cruise switch + return self._motor_5_msg(main_switch=toggle_on) class TestVolkswagenPqStockSafety(TestVolkswagenPqSafetyBase): diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 6e0f32ac8..db73d79ab 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -17,6 +17,7 @@ from opendbc.car.carlog import carlog from opendbc.car.fw_versions import ObdCallback from opendbc.car.car_helpers import get_car, interfaces from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase +from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car.cruise import VCruiseHelper from openpilot.selfdrive.car.car_specific import MockCarState @@ -174,6 +175,9 @@ class Car: # FrogPilot variables self.frogpilot_toggles = get_frogpilot_toggles() + if self.frogpilot_toggles.always_on_lateral: + self.FPCP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL + fpcp_bytes = self.FPCP.to_bytes() self.params.put("FrogPilotCarParams", fpcp_bytes) self.params.put_nonblocking("FrogPilotCarParamsPersistent", fpcp_bytes) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9e141a709..8a4d98565 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -107,7 +107,7 @@ class Controls: # Check which actuators can be enabled standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill - CC.latActive = self.sm['selfdriveState'].active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ + CC.latActive = (self.sm['selfdriveState'].active or self.sm['frogpilotCarState'].alwaysOnLateralEnabled) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ (not standstill or self.CP.steerAtStandstill) and self.sm['frogpilotPlan'].lateralCheck CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index bad4cbf9e..ac18efb0b 100644 Binary files a/selfdrive/modeld/models/driving_vision.onnx and b/selfdrive/modeld/models/driving_vision.onnx differ diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index 1ed04e705..e0788c59e 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -436,7 +436,7 @@ class DriverMonitoring: rpyCalib = [0., 0., 0.] else: highway_speed = sm['carState'].vEgo - enabled = sm['selfdriveState'].enabled + enabled = sm['selfdriveState'].enabled or sm['frogpilotCarState'].alwaysOnLateralEnabled wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low) standstill = sm['carState'].standstill driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 3d4689002..b9bb1e658 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -552,7 +552,7 @@ class SelfdriveD: CS = self.data_sample() self.update_events(CS) if not self.CP.passive and self.initialized: - self.enabled, self.active = self.state_machine.update(self.events, self.frogpilot_events) + self.enabled, self.active = self.state_machine.update(self.events, self.frogpilot_events, self.sm['frogpilotCarState'].alwaysOnLateralEnabled) self.update_alerts(CS) self.publish_selfdriveState(CS) diff --git a/selfdrive/selfdrived/state.py b/selfdrive/selfdrived/state.py index 311517005..244ebc265 100644 --- a/selfdrive/selfdrived/state.py +++ b/selfdrive/selfdrived/state.py @@ -16,7 +16,7 @@ class StateMachine: self.state = State.disabled self.soft_disable_timer = 0 - def update(self, events: Events, frogpilot_events: Events): + def update(self, events: Events, frogpilot_events: Events, alwaysOnLateralEnabled: bool): # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state self.soft_disable_timer = max(0, self.soft_disable_timer - 1) @@ -94,7 +94,7 @@ class StateMachine: # Check if openpilot is engaged and actuators are enabled enabled = self.state in ENABLED_STATES active = self.state in ACTIVE_STATES - if active: + if active or alwaysOnLateralEnabled: self.current_alert_types.append(ET.WARNING) return enabled, active diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index 459016344..36245b297 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -37,7 +37,7 @@ void ExperimentalButton::changeMode() { void ExperimentalButton::updateState(const UIState &s, const FrogPilotUIState &fs) { const auto cs = (*s.sm)["selfdriveState"].getSelfdriveState(); - bool eng = cs.getEngageable() || cs.getEnabled(); + bool eng = cs.getEngageable() || cs.getEnabled() || fs.frogpilot_scene.always_on_lateral_active; if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) { engageable = eng; experimental_mode = cs.getExperimentalMode(); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index a7a713e92..998403ad2 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -97,6 +97,8 @@ void UIState::updateStatus(FrogPilotUIState *fs) { if (state == cereal::SelfdriveState::OpenpilotState::PRE_ENABLED || state == cereal::SelfdriveState::OpenpilotState::OVERRIDING) { status = STATUS_OVERRIDE; + } else if (frogpilot_scene.always_on_lateral_active) { + status = STATUS_ALWAYS_ON_LATERAL_ACTIVE; } else { status = ss.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index b992d19a7..484a4f7e5 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -46,6 +46,7 @@ typedef enum UIStatus { STATUS_ENGAGED, // FrogPilot variables + STATUS_ALWAYS_ON_LATERAL_ACTIVE, STATUS_EXPERIMENTAL_MODE_ENABLED, } UIStatus; @@ -55,6 +56,7 @@ const QColor bg_colors [] = { [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), // FrogPilot variables + [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1), [STATUS_EXPERIMENTAL_MODE_ENABLED] = QColor(0xda, 0x6f, 0x25, 0xf1), };