From 205ec0e39793c4b57c2a555e5054d97148bb1b69 Mon Sep 17 00:00:00 2001
From: jakethesake420 <fastermaster12@gmail.com>
Date: Sun, 27 Apr 2025 13:13:47 -0500
Subject: [PATCH] cancel lane change

---
 selfdrive/controls/lib/desire_helper.py | 37 +++++++++++++++++++++++++
 1 file changed, 37 insertions(+)

diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py
index a7a0643e8..d25b8edc4 100644
--- a/selfdrive/controls/lib/desire_helper.py
+++ b/selfdrive/controls/lib/desire_helper.py
@@ -45,9 +45,11 @@ class DesireHelper:
     self.keep_pulse_timer = 0.0
     self.prev_one_blinker = False
     self.desire = log.Desire.none
+    self.turn_direction = TurnDirection.none
 
     # FrogPilot variables
     self.lane_change_completed = False
+    self.lane_change_canceled = False
 
     self.lane_change_wait_timer = 0
 
@@ -55,6 +57,41 @@ class DesireHelper:
     v_ego = carstate.vEgo
     one_blinker = carstate.leftBlinker != carstate.rightBlinker
     below_lane_change_speed = v_ego < frogpilot_toggles.minimum_lane_change_speed
+    # Cancel nudgeless lane change in any state under these conditions:
+    # - Brake is pressed
+    # - Driver is applying opposing steering torque.
+
+    # Reset canceled flag when blinker is off or driver applies torque
+    if not one_blinker or carstate.steeringPressed:
+      self.lane_change_canceled = False
+    # Cancel logic
+    cancel_lane_change = (carstate.brakePressed and frogpilot_toggles.nudgeless)
+    if self.lane_change_direction == LaneChangeDirection.left and (carstate.steeringPressed and carstate.steeringTorque < 0):
+      cancel_lane_change = frogpilot_toggles.nudgeless
+    elif self.lane_change_direction == LaneChangeDirection.right and (carstate.steeringPressed and carstate.steeringTorque > 0):
+      cancel_lane_change = frogpilot_toggles.nudgeless
+
+    if cancel_lane_change:
+      self.lane_change_state = LaneChangeState.off
+      self.lane_change_direction = LaneChangeDirection.none
+      self.lane_change_timer = 0.0
+      self.lane_change_ll_prob = 1.0
+      self.lane_change_completed = False
+      self.lane_change_canceled = True
+      self.desire = log.Desire.none
+      self.prev_one_blinker = one_blinker
+      return
+
+    # Prevent lane change if canceled
+    if self.lane_change_canceled:
+      self.lane_change_state = LaneChangeState.off
+      self.lane_change_direction = LaneChangeDirection.none
+      self.lane_change_timer = 0.0
+      self.lane_change_ll_prob = 1.0
+      self.lane_change_completed = False
+      self.desire = log.Desire.none
+      self.prev_one_blinker = one_blinker
+      return
 
     if not (frogpilot_toggles.lane_detection and one_blinker) or below_lane_change_speed:
       lane_available = True