From a6aa132b3d68bd3b8f0d437c56a2d2b32ccc51bc Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 27 Feb 2024 16:34:47 -0700 Subject: [PATCH] Always On Lateral 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 --- cereal/custom.capnp | 1 + common/params.cc | 2 ++ panda/board/safety.h | 19 ++++++++++++--- panda/board/safety/safety_chrysler.h | 1 + panda/board/safety/safety_ford.h | 1 + panda/board/safety/safety_gm.h | 1 + panda/board/safety/safety_honda.h | 3 ++- panda/board/safety/safety_hyundai_common.h | 4 +++ panda/board/safety/safety_mazda.h | 1 + panda/board/safety/safety_nissan.h | 7 ++++++ panda/board/safety/safety_subaru.h | 1 + panda/board/safety/safety_subaru_preglobal.h | 1 + panda/board/safety/safety_tesla.h | 8 ++++++ panda/board/safety/safety_toyota.h | 5 ++++ panda/board/safety_declarations.h | 4 +++ panda/python/__init__.py | 1 + selfdrive/car/chrysler/carcontroller.py | 2 +- selfdrive/car/chrysler/chryslercan.py | 5 ++-- selfdrive/car/honda/carcontroller.py | 2 +- selfdrive/car/honda/hondacan.py | 4 +-- selfdrive/car/hyundai/carcontroller.py | 6 ++--- selfdrive/car/hyundai/carstate.py | 11 +++++++-- selfdrive/car/hyundai/hyundaican.py | 8 +++--- selfdrive/car/hyundai/hyundaicanfd.py | 6 ++--- selfdrive/car/nissan/carcontroller.py | 2 +- selfdrive/car/nissan/nissancan.py | 8 +++--- selfdrive/car/subaru/carcontroller.py | 2 +- selfdrive/car/subaru/subarucan.py | 6 +++-- selfdrive/car/toyota/carcontroller.py | 2 +- selfdrive/car/toyota/toyotacan.py | 6 ++--- selfdrive/car/volkswagen/carcontroller.py | 2 +- selfdrive/car/volkswagen/mqbcan.py | 6 ++--- selfdrive/car/volkswagen/pqcan.py | 6 ++--- selfdrive/controls/controlsd.py | 21 +++++++++++++++- .../toggle_icons/icon_always_on_lateral.png | Bin 0 -> 9723 bytes selfdrive/frogpilot/ui/control_settings.cc | 23 ++++++++++++++++-- selfdrive/ui/qt/maps/map.cc | 2 +- selfdrive/ui/qt/onroad.cc | 18 ++++++++++---- selfdrive/ui/qt/onroad.h | 2 ++ selfdrive/ui/ui.cc | 7 ++++++ selfdrive/ui/ui.h | 4 +++ 41 files changed, 170 insertions(+), 51 deletions(-) create mode 100644 selfdrive/frogpilot/assets/toggle_icons/icon_always_on_lateral.png diff --git a/cereal/custom.capnp b/cereal/custom.capnp index fcca5e3..28c0308 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -9,6 +9,7 @@ $Cxx.namespace("cereal"); # you can rename the struct, but don't change the identifier struct FrogPilotCarControl @0x81c2f05a394cf4af { + alwaysOnLateral @0 :Bool; } struct FrogPilotDeviceState @0xaedffd8f31e7b55d { diff --git a/common/params.cc b/common/params.cc index bfe0e95..2d3f9d2 100644 --- a/common/params.cc +++ b/common/params.cc @@ -213,6 +213,8 @@ std::unordered_map keys = { {"AccelerationProfile", PERSISTENT}, {"AggressiveAcceleration", PERSISTENT}, {"AlertVolumeControl", PERSISTENT}, + {"AlwaysOnLateral", PERSISTENT}, + {"AlwaysOnLateralMain", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, {"CustomAlerts", PERSISTENT}, {"CustomUI", PERSISTENT}, diff --git a/panda/board/safety.h b/panda/board/safety.h index 1e95244..58f2ed7 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -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; } diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h index fa0e153..b80e3e8 100644 --- a/panda/board/safety/safety_chrysler.h +++ b/panda/board/safety/safety_chrysler.h @@ -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); } diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h index 2f11863..efbb91c 100644 --- a/panda/board/safety/safety_ford.h +++ b/panda/board/safety/safety_ford.h @@ -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); } diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index e341e43..241e09f 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -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); } diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index b79b6c4..c398e40 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -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; diff --git a/panda/board/safety/safety_hyundai_common.h b/panda/board/safety/safety_hyundai_common.h index 54ea0f0..bc30411 100644 --- a/panda/board/safety/safety_hyundai_common.h +++ b/panda/board/safety/safety_hyundai_common.h @@ -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 { diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index 7c6d8be..b3a567f 100644 --- a/panda/board/safety/safety_mazda.h +++ b/panda/board/safety/safety_mazda.h @@ -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); } diff --git a/panda/board/safety/safety_nissan.h b/panda/board/safety/safety_nissan.h index c240626..b8db422 100644 --- a/panda/board/safety/safety_nissan.h +++ b/panda/board/safety/safety_nissan.h @@ -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; } } diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index c445bc4..924b767 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/board/safety/safety_subaru.h @@ -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); } diff --git a/panda/board/safety/safety_subaru_preglobal.h b/panda/board/safety/safety_subaru_preglobal.h index 1047814..7d44fb0 100644 --- a/panda/board/safety/safety_subaru_preglobal.h +++ b/panda/board/safety/safety_subaru_preglobal.h @@ -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); } diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 4fa0df8..a2401c2 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -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 diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index f65e869..538fc79 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -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 diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index 4c62596..7c5ddb9 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -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 diff --git a/panda/python/__init__.py b/panda/python/__init__.py index af169db..871f536 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -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: diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index dd3c841..820c761 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -42,7 +42,7 @@ class CarController: if self.frame % 25 == 0: if CS.lkas_car_model != -1: can_sends.append(chryslercan.create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert, - self.hud_count, CS.lkas_car_model, CS.auto_high_beam)) + self.hud_count, CS.lkas_car_model, CS.auto_high_beam, CC.latActive)) self.hud_count += 1 # steering diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index 96439f3..f1a5d8f 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -4,7 +4,7 @@ from openpilot.selfdrive.car.chrysler.values import RAM_CARS GearShifter = car.CarState.GearShifter VisualAlert = car.CarControl.HUDControl.VisualAlert -def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam): +def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam, lat_active): # LKAS_HUD - Controls what lane-keeping icon is displayed # == Color == @@ -27,7 +27,7 @@ def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, au # 7 Normal # 6 lane departure place hands on wheel - color = 2 if lkas_active else 1 + color = 2 if lkas_active else 1 if lat_active else 0 lines = 3 if lkas_active else 0 alerts = 7 if lkas_active else 0 @@ -48,6 +48,7 @@ def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, au if CP.carFingerprint in RAM_CARS: values['AUTO_HIGH_BEAM_ON'] = auto_high_beam + values['LKAS_DISABLED'] = 0 if lat_active else 1 return packer.make_can_msg("DAS_6", 0, values) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index eccf1b7..7539d5b 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -253,7 +253,7 @@ class CarController: if self.frame % 10 == 0: hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible, hud_control.lanesVisible, fcw_display, acc_alert, steer_required) - can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud)) + can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud, CC.latActive)) if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH: self.speed = pcm_speed diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index a8cbad7..b59f8a5 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -116,7 +116,7 @@ def create_bosch_supplemental_1(packer, car_fingerprint): return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values) -def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud): +def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active): commands = [] bus_pt = get_pt_bus(CP.carFingerprint) radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl @@ -149,7 +149,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud_values = { 'SET_ME_X41': 0x41, 'STEERING_REQUIRED': hud.steer_required, - 'SOLID_LANES': hud.lanes_visible, + 'SOLID_LANES': lat_active, 'BEEP': 0, } diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index f4a4594..28e1ed1 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -120,7 +120,7 @@ class CarController: # LFA and HDA icons if self.frame % 5 == 0 and (not hda2 or hda2_long): - can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled)) + can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled, CC.latActive)) # blinkers if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: @@ -151,11 +151,11 @@ class CarController: use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), hud_control.leadVisible, set_speed_in_units, stopping, - CC.cruiseControl.override, use_fca)) + CC.cruiseControl.override, use_fca, CS.out.cruiseState.available)) # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: - can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled)) + can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, CC.latActive)) # 5 Hz ACC options if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 1f952b0..0687a65 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -53,6 +53,7 @@ class CarState(CarStateBase): self.params = CarControllerParams(CP) # FrogPilot variables + self.main_enabled = False def update(self, cp, cp_cam, frogpilot_variables): if self.CP.carFingerprint in CANFD_CAR: @@ -104,7 +105,7 @@ class CarState(CarStateBase): # cruise state if self.CP.openpilotLongitudinalControl: # These are not used for engage/disengage since openpilot keeps track of state using the buttons - ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0 + ret.cruiseState.available = self.main_enabled ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1 ret.cruiseState.standstill = False ret.cruiseState.nonAdaptive = False @@ -164,7 +165,10 @@ class CarState(CarStateBase): self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE self.prev_cruise_buttons = self.cruise_buttons[-1] self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) + self.prev_main_buttons = self.main_buttons[-1] self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) + if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0: + self.main_enabled = not self.main_enabled return ret @@ -219,7 +223,7 @@ class CarState(CarStateBase): # cruise state # CAN FD cars enable on main button press, set available if no TCS faults preventing engagement - ret.cruiseState.available = cp.vl["TCS"]["ACCEnable"] == 0 + ret.cruiseState.available = self.main_enabled if self.CP.openpilotLongitudinalControl: # These are not used for engage/disengage since openpilot keeps track of state using the buttons ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1 @@ -240,7 +244,10 @@ class CarState(CarStateBase): self.prev_cruise_buttons = self.cruise_buttons[-1] self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"]) + self.prev_main_buttons = self.main_buttons[-1] self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"]) + if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0: + self.main_enabled = not self.main_enabled self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"] ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index bc29aeb..09e940a 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -117,20 +117,20 @@ def create_clu11(packer, frame, clu11, button, car_fingerprint): return packer.make_can_msg("CLU11", bus, values) -def create_lfahda_mfc(packer, enabled, hda_set_speed=0): +def create_lfahda_mfc(packer, enabled, lat_active, hda_set_speed=0): values = { - "LFA_Icon_State": 2 if enabled else 0, + "LFA_Icon_State": 2 if lat_active else 0, "HDA_Active": 1 if hda_set_speed else 0, "HDA_Icon_State": 2 if hda_set_speed else 0, "HDA_VSetReq": hda_set_speed, } return packer.make_can_msg("LFAHDA_MFC", 0, values) -def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca): +def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca, cruise_available): commands = [] scc11_values = { - "MainMode_ACC": 1, + "MainMode_ACC": 1 if cruise_available else 0, "TauGapSet": 4, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index a35fcb7..afb4009 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -41,7 +41,7 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer): values = { "LKA_MODE": 2, - "LKA_ICON": 2 if enabled else 1, + "LKA_ICON": 2 if lat_active else 1, "TORQUE_REQUEST": apply_steer, "LKA_ASSIST": 0, "STEER_REQ": 1 if lat_active else 0, @@ -113,10 +113,10 @@ def create_acc_cancel(packer, CP, CAN, cruise_info_copy): }) return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) -def create_lfahda_cluster(packer, CAN, enabled): +def create_lfahda_cluster(packer, CAN, enabled, lat_active): values = { "HDA_ICON": 1 if enabled else 0, - "LFA_ICON": 2 if enabled else 0, + "LFA_ICON": 2 if lat_active else 0, } return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index beb5d3c..96ec5c6 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -67,7 +67,7 @@ class CarController: if self.CP.carFingerprint != CAR.ALTIMA: if self.frame % 2 == 0: can_sends.append(nissancan.create_lkas_hud_msg(self.packer, CS.lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, - hud_control.leftLaneDepart, hud_control.rightLaneDepart)) + hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.latActive)) if self.frame % 50 == 0: can_sends.append(nissancan.create_lkas_hud_info_msg( diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py index 49dcd6f..d7ff55e 100644 --- a/selfdrive/car/nissan/nissancan.py +++ b/selfdrive/car/nissan/nissancan.py @@ -65,7 +65,7 @@ def create_cancel_msg(packer, cancel_msg, cruise_cancel): return packer.make_can_msg("CANCEL_MSG", 2, values) -def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart): +def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart, lat_active): values = {s: lkas_hud_msg[s] for s in [ "LARGE_WARNING_FLASHING", "SIDE_RADAR_ERROR_FLASHING1", @@ -98,9 +98,9 @@ def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, le values["RIGHT_LANE_YELLOW_FLASH"] = 1 if right_lane_depart else 0 values["LEFT_LANE_YELLOW_FLASH"] = 1 if left_lane_depart else 0 - values["LARGE_STEERING_WHEEL_ICON"] = 2 if enabled else 0 - values["RIGHT_LANE_GREEN"] = 1 if right_line and enabled else 0 - values["LEFT_LANE_GREEN"] = 1 if left_line and enabled else 0 + values["LARGE_STEERING_WHEEL_ICON"] = 2 if lat_active else 0 + values["RIGHT_LANE_GREEN"] = 1 if right_line and lat_active else 0 + values["LEFT_LANE_GREEN"] = 1 if left_line and lat_active else 0 return packer.make_can_msg("PROPILOT_HUD", 0, values) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index bcfc515..1d6d764 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -100,7 +100,7 @@ class CarController: can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, - hud_control.leftLaneDepart, hud_control.rightLaneDepart)) + hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.latActive)) if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert)) diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 86d39ff..4036d7c 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -66,7 +66,7 @@ def create_es_distance(packer, frame, es_distance_msg, bus, pcm_cancel_cmd, long return packer.make_can_msg("ES_Distance", bus, values) -def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): +def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart, lat_active): values = {s: es_lkas_state_msg[s] for s in [ "CHECKSUM", "LKAS_Alert_Msg", @@ -118,9 +118,11 @@ def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert elif right_lane_depart: values["LKAS_Alert"] = 11 # Right lane departure dash alert - if enabled: + if lat_active: values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines values["LKAS_Dash_State"] = 2 # Green enabled indicator + values["LKAS_Left_Line_Enable"] = 1 + values["LKAS_Right_Line_Enable"] = 1 else: values["LKAS_Dash_State"] = 0 # LKAS Not enabled diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 33bd6e7..7df3e00 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -179,7 +179,7 @@ class CarController: if self.frame % 20 == 0 or send_ui: can_sends.append(toyotacan.create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, - hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud)) + hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud, lat_active)) if (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value): can_sends.append(toyotacan.create_fcw_command(self.packer, fcw_alert)) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 3cc08dd..f8d9db5 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -73,12 +73,12 @@ def create_fcw_command(packer, fcw): return packer.make_can_msg("PCS_HUD", 0, values) -def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud): +def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud, lat_active): values = { "TWO_BEEPS": chime, "LDA_ALERT": steer, - "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2, - "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2, + "RIGHT_LINE": 0 if not lat_active else 3 if right_lane_depart else 1 if right_line else 2, + "LEFT_LINE": 0 if not lat_active else 3 if left_lane_depart else 1 if left_line else 2, "BARRIERS": 1 if enabled else 0, # static signals diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 7dd0c38..ee063e2 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -94,7 +94,7 @@ class CarController: if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw): hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"] can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled, - CS.out.steeringPressed, hud_alert, hud_control)) + CS.out.steeringPressed, hud_alert, hud_control, CC.latActive)) if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl: lead_distance = 0 diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 787c7de..418462b 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -28,7 +28,7 @@ def create_eps_update(packer, bus, eps_stock_values, ea_simulated_torque): return packer.make_can_msg("LH_EPS_03", bus, values) -def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control): +def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control, lat_active): values = {} if len(ldw_stock_values): values = {s: ldw_stock_values[s] for s in [ @@ -40,8 +40,8 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres ]} values.update({ - "LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0, - "LDW_Status_LED_gruen": 1 if enabled and not steering_pressed else 0, + "LDW_Status_LED_gelb": 1 if lat_active and steering_pressed else 0, + "LDW_Status_LED_gruen": 1 if lat_active and not steering_pressed else 0, "LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible, "LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible, "LDW_Texte": hud_alert, diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index f42c3cf..ec0b633 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -9,7 +9,7 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled): return packer.make_can_msg("HCA_1", bus, values) -def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control): +def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control, lat_active): values = {} if len(ldw_stock_values): values = {s: ldw_stock_values[s] for s in [ @@ -21,8 +21,8 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres ]} values.update({ - "LDW_Lampe_gelb": 1 if enabled and steering_pressed else 0, - "LDW_Lampe_gruen": 1 if enabled and not steering_pressed else 0, + "LDW_Lampe_gelb": 1 if lat_active and steering_pressed else 0, + "LDW_Lampe_gruen": 1 if lat_active and not steering_pressed else 0, "LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible, "LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible, "LDW_Textbits": hud_alert, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f27bcb6..c7271c2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -197,6 +197,15 @@ class Controls: if not self.disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + # Set "Always On Lateral" conditions + self.always_on_lateral = self.params.get_bool("AlwaysOnLateral") + self.always_on_lateral_main = self.params.get_bool("AlwaysOnLateralMain") + if self.always_on_lateral: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL + if self.disengage_on_accelerator: + self.disengage_on_accelerator = False + self.params.put_bool("DisengageOnAccelerator", False) + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX # read params @@ -684,9 +693,19 @@ class Controls: # Gear Check self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) + # Always on lateral + self.FPCC.alwaysOnLateral |= CS.cruiseState.enabled or self.always_on_lateral_main + self.FPCC.alwaysOnLateral &= self.always_on_lateral + self.FPCC.alwaysOnLateral &= CS.cruiseState.available + self.FPCC.alwaysOnLateral &= self.driving_gear + self.FPCC.alwaysOnLateral &= not self.openpilot_crashed + + if self.FPCC.alwaysOnLateral: + self.current_alert_types.append(ET.WARNING) + # Check which actuators can be enabled standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill - CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ + CC.latActive = (self.active or self.FPCC.alwaysOnLateral) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ (not standstill or self.joystick_mode) CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_always_on_lateral.png b/selfdrive/frogpilot/assets/toggle_icons/icon_always_on_lateral.png new file mode 100644 index 0000000000000000000000000000000000000000..1e55e3feb958850c75f576a8854224c2becc4cee GIT binary patch literal 9723 zcmZ{qWl$VV*M=eJqG5prf&^I>3GVK)XmEE8?yif=LKb&RuoxEH-7P_aI|SDUx0lrS z`|GNnuBn>po>MhFr_X)ev1+RFxL9wnkdTmY6`(R2FXQ09j*0$q{h?Qp|1uzZXvj+; z&kwPLyxag>q52+3NZ5q`IxMJZPS|F~ppx$dnPC?6TDg1ujR@79o2$^>*!!lSn$r?t{+#QKYHO&jUWD;@IS7b$a;JXy!xx@Zd3Ve;85B2=T1DFn*RTqz9%`i&FG$s z`O1LfT$TAI-;UBK3i$v9X_|I?Nf9S>8&`rQ*c64T2lm>c<_V3;7)+6#sXQJ z|K)xVa(IKl%D(U7KohB!Z`%BXgw7z&e9%Wz#anej>)}+7?3eb|_O$-$aqS@NXwh=C z+3O%1@;*P1@9t$5O~sV)y1F_KZ_WXz1xr4W!q53=p^hJiLckSqa;tDA zqoOi)n4X@FQO$n+C;>?e;S`Z$y{Wm&uT1sbeQ@4k>c8&MwHFzh{_}?)^~-)Ei=g>@ zjrH3H;>~->cU~rD73N{n0{S3N{boV>Y3m)a9W13_x!f&)afI2 zny|p6>F{Z$?oIxGM1XNiJSvf!h!c#mHe|Z}TDz&zyZq zeDN31*V@QE!`i%GhZ3}oc7_W=8;pa=cQ37; z!5mZIHEUdSCR{jJaa60qsL6e1^(7Wbwr4Zfr=}GBR^PK^GHx5?lRuTFA<>(YcDs{B zF1{Ej7`<&ccw_AYG#MS7dP|abys;R$yldS@0UO9mLmgdf@f=?J(;oV*Z0!RWYdEfF zeI)El-PCi#*8;Gh(L%{^^gF7Y8gxV8YGOn*nTW5eHbAjgkI!llq3Ng8Kx!ybaQ5yL zo-gckEV8<(p6J=VvizsFJO7+uho%2oBJ+3I`vR(X5bM`_u1$yd<}7n{p*yPh8ne^2 zE^T~nxA(MH<7)aFg%qiKPhMBfnx7M%zWb(RZydiD3}AT4*il( zo(6^l|K@UUJMR=I8(HG|q$yij#xb8*oq|3()Vz*sHtPzzUGl_O4Y~%TQopr+>h0w` z79boe5D$T!1ZNOUV)YT<;~EFOIhjl+%-Y%6xpZS88oeA&d#8JI?7=i?zF6;6;4MnC zhN0Zd5!YjyDh8z|ioS9W&!QOhQrDQVnGY|WEmcp#1LrAFJ6GBi<8r1`UvuB zgUc%adh&Y9XKNYxTrwl)77p7bRi9PLOBLOWcodToU5<8P{fe~`lmP|Ziwu46(UsEg z`R#fvF%3H-DHg2?y=n_V3SQ$;OPiLN<8d(MjYRuEU+BhnH%h|d=uG9JMemp z$;--Rfg9elcstSz-mo$Wnv(`)YzcA*4jC1G&2f1&aQ*73sP(7 z$dGpXln+@BiWcUStkecg?v6Y0kLa~+3T5sZaP|KH{#(6gs!rU>NN^gVgSDZwfHzXVTKcwed~1)Nx0u@$0@19u?e4u_f3RgHCucEP7zB>Ith;*QA>T z&#A}zVhIa3>hTZ~QwCw@iqA(|!GpUSdN%yjr~S5JX!Ek4uwwa-y#lS@&2Kn*;im17 zU>erSdqm+Djg<*H*QboJ19+@UGFeNw0AKu!Sb*)UhXUrZK2;wSX-5=5={{7ZxAs0P zHwNx4Yb~7vzPKRE}gs?(&hr-qAnh9eKc_h{D_TazkRWqCA#7;@717Rxy}dQ{X+rCtus>TUgVT$Yf`llr71C)$r~p5Zf*p z1W22B)s!4pAl}_c>vnZIuebn1^{ud)epScf=y}+H^H6_&QjjVuWnCqq*IyBsRjyaP z?CuVcTv?Nb!yE^o$eam(ek`wBN1rSNJPYXM7g&pF>j;#> zl(Qs(c!%W>hXpcfLe8ClB(+%f*TDT{N>CK;B6oRc-kd^VVKp_~%u8ZuM7)n=<5Ogn zCZU3s`kpmG%l{)8gaXsu4VP4$S&k_kGqKf5*-rHbmeUOynw$Mg6stlAC;|Wd4QL?XEIKNshKwXz*9+>0jvTH~)9M*YaM4{|g+bfn)uOmH^W1m+pI@CuX3(c5D3e3hCB5kj4r%?OBo*JJ z1!&r_AsWN~n3a4K8QDX^kePDF#=NX!{p0qMNpYh}2V@5CNz0_r`$zy!N2!{sWMBPF znXA#HXb%()>GR22WlUXK#B=yv+Q(!V8>fXY|1%!Q#EhvNxH;P#5G3S?mWoFWC-Qz; z580RKeh)}NiWX@UhELol{P&7xefG>B$a#o=>V;wIQbtPn%gvA%a$${xcj&Wlj6WV% zYMM0jO}Da|RX1I6`kI?y{FZ_3W0kB%c{Xz_h9G}F+LIjnxpv+K`Ind1nE)N)z2ABi zu77(f7DNA$qhZQ|%QT95?E9tiy$=^e%6cjLuw|f-_WNE;7wby1ne3o3I4e%p2|fCG zFLg8`3P$=LLDuRJ8zMHaO>K=|d~?Ll{4Fq8S2b>d(M*&Lh9MH6^%nP21`$mEDWgXOjbL09*IQCQ0%HNGb-v|?C2W-VHtFhtC30P_a_BxBx5O$YdQiqMy- zBFuUNX~*jm3NwW94)NsS9u@u=~d2A&1Z1C z*-G-&cG0#230erUO%^6!g72H{*gzl-DSRk_>gP za6izFEVS@C%zo!3z?4TftnuXdLB!*{)OgmHbA}y1pA```WUpOJaT>V*O(ZnAcx0!2 zj=l&~lNqp=Bbwt@c64Xwg5f%W!d6w}UQT?00voUh)-7O72eeAybgkKh0y?<%y|$i) zWbwGtF5yZc1(IjS5@9!q73Xb08b3#UAHTJ`5uHKph*LX@m{aax7Et<*?72kpLYf?s(;)OYIOcGJX_oaVbSJRc*;$u8tVnLOn+5?u(U&N81h=XX zpR=7(ik)vCzqR+I4azkBJGLw&i)*$Y$=Y2=_nQ}y53Fu?NYu27MeN`INpa0(Ar zdz30T^Ev+VTs-=Z`Sapw25C|BJ~R{2dld!zW< z^%2I+XzTBNhJze#8@&KXHy)*s))U$tsK1z#?inQ8*mQBL@xvjyYCb4%x%ZDg(a$3( zbYMJ{X-~_|9niGl!VAq51?!=mfBp-VH43Q~d%Q=h+Ulf?S;Uzlf7BxF) zbU+TO#zCfycQg#23gs!w#~8Q%7D2ZxM~tfy0f(Z#y$t; zk*;E5y>_U0SEW&cO!)S@JZZG0D~|KNidDP7c^8u9H*ZSQ8!O>6nvMIbO$RK~Dqbi0 zvq}81`!NKj;JN`P!&bXMEgjJeBWohfpp}K_WjPL{h)glDZjp33Y6HqTJm$GRqG)`D z8Qk@UYOSE(7&E2|70kykB!>bGCgtkPVgVs&(q^KK^$Wufj6tK_N{4d8XW zf+#&h8aO&{W0z^Cr0EE7SaC^@dO*htPxR|}cU8EFJylu{h_%QQ^M#A#iqP_8TwK}!zesIvl)S#xiFi29Ze1qz+$>*f@LbE@M1tJv}lwFzXFx;k)WVi&72)u^|(Be*LJ?Se8Z}<4(^q;xXipz?GlkrXw5oyMC zOCRt^T4kB{piDwR6_H`I}%yA6VvrAMPm~V&zGmEf#dg6K#oLWxBdRl(G-s z;?^2fZO`eTM7!DL2f9X1tnP^BUFWn<*H__!{+n)HZ8O%0Vxrmf%vUHQO8SE&h)O@} zDIB^q^&2;fh$&0N?|dF_fcpOQ1l#ToX}h=qTm*?6{;A!-p3L)i$cH($Qj)D)SR?6z z{H9qn`IIGl)AY@C?0k_B2V)teMmadSrB zWqm^&2yV$nEMHiyMqN-4L*Wv%He33U0Y&(eK-zK2__y_M!NJ%EB;y9L3OJ;TuQ=x| z`jYmeBUp>o;zC41i%dCzZ#YuvFZSZrv2IEtn=?{gc2qS5-Xn)ea8v{q87RmXtFr++ zEQ2?M*;4h~+R&sTnn-*Q?6-Y$c$p`ZU;To)36~s^#rT1>yWZeNs&kLS zvqgTR@FNPx)lJBsBZqE@f?sb-7EZD)Kc4?qjYrz!cqz0SUY1SH{?n@_;Ih)%gf~!# z3kIS*G=&Pdio=@uc?{qYW2gb1zOeM3dTiT)N<@`n->t? zE7m+o+a(DcO8G|nL`6Th7J4JCXmUnIiFe_}9cvIdoR$Y= zZ`oUHd~dYqi%a7ZW<`lT0&R?c9w*lOFQ$W$^ayHEv?f-i-pDop!f*|6*>=EJ6`{$5OH+8en8WXm+WgUkrSDjwrAA@f3q!Un;mAr(~L4R4CS}l zI8F>S;1mAwDmGep{{hv3cjbE$RT-Fyy0;fB_&1e#F4%kout{?$rRLgM$aIh#o-u)@ zzFEp?sv{cGz|kE3dF$92$WiC;r($CMu$CFT5+}|~3o+7On*))>tI`sA3#JwHE0HkD ze775B+;FJqF_CMqR7BQ<&@?DO-^hQWKmxvz-;VDtqQDDxP5C&wD$~u2YD?4_l#*`3 z;9T`sb8ePmtul8_wyesOCdu*TF)G3Xk_UFn?-V1GWv}L+RSZ_{Cixb%AcPi$ti;2- zB&R9@K%u`n+%BflL9_j7&^GVHosUM$Ae5QOkebYwS^PKzUjHpLnuHnEosRBe?+J1~ z5~n7J@U%Y@j#%*!i5+aUh<0vFpcAQw#mxbsq)cbqexVToN&%L6<3#R1Dk1byQ_z8H`ujH{JL=`haI% zeUnT)J(E&3EY1BN062T^J{dUV`f>ju3Qp;bHH)`*O3S~8(Aa3~Ct|<5&TJBb@R@lvwKEe0xvMD@5l!hxoJT^v-;=ypxMJGw%L+l&up{7pn zne3vf_V#wk25#4JTrG`{uTr*V$oZWg5cU`fZBp-ARn^zE1jm?STeZx;_+Y`5w|$Z$ zP6z<}psiMaL~@qa0{ZpWM881+{W=)6d9$-^B8A|2u6wwtr6;+got>Xro#^idvrQVA zZj^0WK5#oD=0ut?qh6{S>|<&pe=2HK8TEgc)?@AT#GBKqR`YSep}≻}>t1hIjeO zw{Fs4qxdaqi`^(lOu}iYsNh3NM!Hj+p<64-a4-B=#M~!Bm2M{#+cyGg>EYQo?lZTz zWP~}Uh+$pQ83F1?EiyGnVf`lFCYw4rylO|1{qeIrUAo649p=0;`aBVLj(I z?dVLyVJP656bWJagE+U$2=aI>IBHvZo2Yn;!f#2f;-Afbg;NV(dl!jgJJui;Q>TV*_i;OA^hJ=U zg*!H&Get?&V;J}6i|$8a_(<0D&7axu@{$qBCoaMG+TS?rH^71q~O?FGp( zD$zmG{>cix3Dhja&KFh9B!-=OjIKixz?O9_EA`|X3-g>%kJs>Gg}O2`9QK(k-Ds_~ z(Qk`IL2&O8h?u_FKT*na5pq^95B!jNUejh8jYS@tNbre?@YYVyF^pwF9HNr#S*j5IPXUi_&_GI2_A0EdK4YFj zrarrhqKHZN^@1BNwf#>lxD#eD`>*^Fh^ zK`66D1H2QwH&0zS6_8O4<9@)_7QGm%(O|-HN2hwNU(`vcBxMv`_<_t80zy=fCT>)` zM`38=Hi=VwuOM1can;j3+`Bh(z(7YO>V&hIQtE3#imK4i_w*r4cm>k4S=As*LZ3R6 zbPb_}n;2P-Ung*}#u_5V^p!@TJSJ(Lou5^8>l6_)7eJ!0fC}x+rfaTL^Q~5TEu9jB z6Q_@kMG|Puf5rhG$#I!n+cd_E8XJCu z?ygCQQUY3YUX_-vFabMK)&x7v1=D=VN#*AOlpt#jF9@Q71(o~4hrc%`4xSqK7i-qn zgz98#;Xg7=RhoA2X(Jq^Y4`E_MI9v_qX##-A}`ioVSU-vlBqB<#Lx%@ErKipqDV<8 z#Lgm91Fm0x79qp#g)rny0c6EuvbzTwiL`&Jbx;b)pO5A{KFnUVmT(bU_WM<0kIfBIr$qa8lCybU!3=+%%arbr6Y3aw82A) zOn|`1A#ITk+Z%kO^Ux!CTA<4~&9X2?3R;4SrMcnQg*_b>YJELTL zXc{n?TH1|6Yy2_hwR~4QUF=3cqUhipXm~;BlrQ?~7$=fX?K_64$ntG`F8?{A>x>d^ zW14tw2$}R^XF?TIfJhz9|NI-TgEk!AOHOv{josXw;wl_%xk!`JPK0#l`(r~AtkqlY z6HH)eOkpAEswkU-|jJaVw5-f!;DiL8_>N992p!Ubu z|FRi4vXM(xI*7jq8&QNlo3lJUq5A#8#QtATS3R zoZ-VCuRiMsO!trx0txZ-B!ImQ@c{#}dTCCKWT!wu_D@a6O@`k%j98L?k`h)LyS@-1 z2FGEdSOriE3sl>M7A2MxkamGc+iwXhfCZYzBP$6ljjzYV6uYB1Ev-saa0b%6(pq#$ zyycMpS*ZJn7|j}W;sH=tZ)G*NbPC6(3<%YZmbTtF z@}6?v4o+om1_pmOXBuBFionXK4uB`s~gSsTDvW&WKYgagcem$oZ(JGyL*+O|_m zR`Njt^f)Nv`qO62KV=+w-ZP1M;Y1g1-*U^t$Hz94))i3!xefS9UD~K8ok|!lDdpkS zAw@XRNEx*yEmZ{DLg!CbdzPw;w{H~KlDEI?hm(2Sy(L|~<<$op);r!dI=AR^nc{z| zBQ8;YJd8TyN8bj0TA<cMwPD!e6;AB2(_ft)*h9@!wu&a_Cwxf0*j}9{^n#qYAR)JQTE2~Y4w;4#Js zLMy4KN>y^~KWpB6KQA-xCq@<12C1IBfWv(Y=_Bkl4R!}>hNAii zZ|o=~R55*ps@&fo-T30BCcm~H_r_f}RAr3JjzJLfTxOfML(wwzhz9jno8=>DB=NnV z`|FcuZ$xUiAB~w`&a)$EM-J}*R+zr#$Nj$apu{`>~ABC(f4Q-D8xvHas@qBc3Ph099LRz`EpQdd`?#G z!#!Kl1Oi!-4ars?>m}vMX;PZ3>=PN^E@>_S|I*sMfJYF2R=I!dLZMhp!rE6OHIY7V zU^9`&h90=X3LS@Z@{f<{k1$2;N$o|7a%r5JquXP{;j=~~a3xo)hfHmsEIfg++Gy?3 zkLm>iG7W^^h4B3)qei}4dQlH=k8?EDoQAJDX^KJg#;9J8Z07 zrw9l$e}N&<@lD@um4Z%DmyOwgU21dX)n@=MM4*J$GXj>L-d1V!I& zmDj$oE56M~mWw4Mm)QPUP1^&&t@=Uy&-8YP>|?*j8lXu4|n>8`-ri zgZwxJv{_o^CBv~e?pcyOB-c0HawXZ-kP;3`Cwa*5xh=ZOtnvq2!E&k=3#ALR{t`;M zIAySzeMSwg#gY>wObspi&RE&lc==PlXh6F1jxTC@5fKu|-^eze17U_dzZmpUUO!yA zcq~<_S>hs$=>8x0nwgpTc@w(&x$(cVP$!!Gd6yRh3!PQO_x1to^fN60@NJsw+gNFS zcII*O$3+e3jn`A@hNn*TtlWMT?)g7rR z;1(&Q^IH0Kp9^X4M2{4_a_m0y_Tn#lrl7|Ty!u6kzQsq6!=%VWaqHg;t0Xvl9gcxt z@#dcu$$E|nuZxUG`)<@q-v|*uyju1f6f^f3iKaDf@ZqIR{HNd z-Y6j361J8EiBlYr`HC0%s`$@BIXKwF`fq~u7Rc4mi{(0g7kzpuH`g&Y@4J|M)IQN_ zs_-qVrcvj)F(ytKm$V`A6NudxseN4)+G5h#_2<2K_3|LISHA5Va`FuH?R9NLvV7S; zYYxMUi^Jdx^8N59+4%i1p3E;n950k)pPifI^fve=84D+|d_=?8Saj+-u*CP(zIpMG z$mmTlb;>Kem2Wyfn#HvLlx}Ij__yz)Lq z%PM~5I}@vVZdXT~-#=yJAjGzuFZx9r4O4z~^IGdAM>y+t#ETmReJ_KykMDG{?@!6g z-%#|^27;#VI@}Z!HGqZ(2`}+Rv3Q6{BXE+yqzUuW!iy_r%+%WKT@2kjB~akZzlzyN zNogb;Du%32t%-2X(EuvjYIR6)5HC$9Nnr?fTyeOfgv518g$awanx4_$>7qZlJn%n| z2ZyhAw^9BxF&#t(NNMMNshXMhvDMW0f0|> controlToggles { + {"AlwaysOnLateral", "Always on Lateral", "Maintain openpilot lateral control when the brake or gas pedals are used.\n\nDeactivation occurs only through the 'Cruise Control' button.", "../frogpilot/assets/toggle_icons/icon_always_on_lateral.png"}, + {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, @@ -15,7 +17,24 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil for (const auto &[param, title, desc, icon] : controlToggles) { ParamControl *toggle; - if (param == "LateralTune") { + if (param == "AlwaysOnLateral") { + std::vector aolToggles{"AlwaysOnLateralMain"}; + std::vector aolToggleNames{tr("Enable On Cruise Main")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, aolToggles, aolToggleNames); + + QObject::connect(static_cast(toggle), &FrogPilotParamToggleControl::buttonClicked, [this](const bool checked) { + if (checked) { + FrogPilotConfirmationDialog::toggleAlert("WARNING: This is very experimental and isn't guaranteed to work. If you run into any issues, please report it in the FrogPilot Discord!", + "I understand the risks.", this); + } + if (started) { + if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) { + Hardware::soft_reboot(); + } + } + }); + + } else if (param == "LateralTune") { FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { parentToggleClicked(); @@ -88,7 +107,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil }); } - std::set rebootKeys = {}; + std::set rebootKeys = {"AlwaysOnLateral"}; for (const std::string &key : rebootKeys) { QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this, key]() { if (started) { diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index a5644ba..c1f98d6 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -121,7 +121,7 @@ void MapWindow::updateState(const UIState &s) { if (sm.updated("modelV2")) { // set path color on change, and show map on rising edge of navigate on openpilot bool nav_enabled = sm["modelV2"].getModelV2().getNavEnabled() && - sm["controlsState"].getControlsState().getEnabled(); + (sm["controlsState"].getControlsState().getEnabled() || sm["frogpilotCarControl"].getFrogpilotCarControl().getAlwaysOnLateral()); if (nav_enabled != uiState()->scene.navigate_on_openpilot) { if (loaded_once) { m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(nav_enabled)); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index acacdc9..4354a45 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -175,7 +175,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { int margin = 40; int radius = 30; - int offset = true ? 25 : 0; + int offset = scene.always_on_lateral ? 25 : 0; if (alert.size == cereal::ControlsState::AlertSize::FULL) { margin = 0; radius = 0; @@ -242,7 +242,7 @@ void ExperimentalButton::changeMode() { void ExperimentalButton::updateState(const UIState &s) { const auto cs = (*s.sm)["controlsState"].getControlsState(); - bool eng = cs.getEngageable() || cs.getEnabled(); + bool eng = cs.getEngageable() || cs.getEnabled() || scene.always_on_lateral_active; if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) { engageable = eng; experimental_mode = cs.getExperimentalMode(); @@ -255,7 +255,7 @@ void ExperimentalButton::updateState(const UIState &s) { void ExperimentalButton::paintEvent(QPaintEvent *event) { QPainter p(this); QPixmap img = experimental_mode ? experimental_img : engage_img; - drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0); + drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !(engageable || scene.always_on_lateral_active)) ? 0.6 : 1.0); } @@ -545,7 +545,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) // base icon int offset = UI_BORDER_SIZE + btn_size / 2; - offset += true ? 25 : 0; + offset += alwaysOnLateral ? 25 : 0; int x = rightHandDM ? width() - offset : offset; int y = height() - offset; float opacity = dmActive ? 0.65 : 0.2; @@ -734,9 +734,12 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { } void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { + alwaysOnLateral = scene.always_on_lateral; + alwaysOnLateralActive = scene.always_on_lateral_active; + experimentalMode = scene.experimental_mode; - if (true) { + if (alwaysOnLateral) { drawStatusBar(p); } @@ -768,6 +771,11 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { p.setOpacity(1.0); p.drawRoundedRect(statusBarRect, 30, 30); + // Update status text + if (alwaysOnLateralActive) { + newStatus = QString("Always On Lateral active. Press the \"Cruise Control\" button to disable"); + } + // Check if status has changed if (newStatus != lastShownStatus) { displayStatusText = true; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 962a148..9e5809b 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -118,6 +118,8 @@ private: QHBoxLayout *bottom_layout; + bool alwaysOnLateral; + bool alwaysOnLateralActive; bool experimentalMode; protected: diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 560747e..23d01bf 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -210,6 +210,9 @@ static void update_state(UIState *s) { } if (sm.updated("frogpilotCarControl")) { auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl(); + if (scene.always_on_lateral) { + scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral(); + } } if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); @@ -243,6 +246,8 @@ void ui_update_frogpilot_params(UIState *s) { Params params = Params(); UIScene &scene = s->scene; + scene.always_on_lateral = params.getBool("AlwaysOnLateral"); + bool custom_onroad_ui = params.getBool("CustomUI"); scene.acceleration_path = custom_onroad_ui && params.getBool("AccelerationPath"); @@ -257,6 +262,8 @@ void UIState::updateStatus() { auto state = controls_state.getState(); if (state == cereal::ControlsState::OpenpilotState::PRE_ENABLED || state == cereal::ControlsState::OpenpilotState::OVERRIDING) { status = STATUS_OVERRIDE; + } else if (scene.always_on_lateral_active) { + status = STATUS_LATERAL_ACTIVE; } else { status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index fa6ee80..9f603a8 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -107,6 +107,7 @@ typedef enum UIStatus { STATUS_ENGAGED, // FrogPilot statuses + STATUS_LATERAL_ACTIVE, } UIStatus; enum PrimeType { @@ -125,6 +126,7 @@ const QColor bg_colors [] = { [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), // FrogPilot colors + [STATUS_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1), }; static std::map alert_colors = { @@ -168,6 +170,8 @@ typedef struct UIScene { // FrogPilot variables bool acceleration_path; + bool always_on_lateral; + bool always_on_lateral_active; bool enabled; bool experimental_mode;