Merge branch 'master' of https://github.com/sunnypilot/sunnypilot

cereal/custom.capnp

@@ -172,6 +172,8 @@ struct OnroadEventSP @0xda96579883444c35 {
     experimentalModeSwitched @14;
     wrongCarModeAlertOnly @15;
     pedalPressedAlertOnly @16;
+    laneTurnLeft @17;
+    laneTurnRight @18;
   }
 }
 
@@ -258,9 +260,16 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
   roadName @5 :Text;
 }
 
-struct CustomReserved9 @0xa1680744031fdb2d {
+struct ModelDataV2SP @0xa1680744031fdb2d {
+  laneTurnDirection @0 :TurnDirection;
 }
 
+  enum TurnDirection {
+    none @0;
+    turnLeft @1;
+    turnRight @2;
+  }
+
 struct CustomReserved10 @0xcb9fd56c7057593a {
 }
 

cereal/log.capnp

@@ -2633,7 +2633,7 @@ struct Event {
     backupManagerSP @113 :Custom.BackupManagerSP;
     carStateSP @114 :Custom.CarStateSP;
     liveMapDataSP @115 :Custom.LiveMapDataSP;
-    customReserved9 @116 :Custom.CustomReserved9;
+    modelDataV2SP @116 :Custom.ModelDataV2SP;
     customReserved10 @136 :Custom.CustomReserved10;
     customReserved11 @137 :Custom.CustomReserved11;
     customReserved12 @138 :Custom.CustomReserved12;

cereal/services.py

@@ -88,6 +88,7 @@ _services: dict[str, tuple] = {
   "carControlSP": (True, 100., 10),
   "carStateSP": (True, 100., 10),
   "liveMapDataSP": (True, 1., 1),
+  "modelDataV2SP": (True, 20.),
 
   # debug
   "uiDebug": (True, 0., 1),

common/params_keys.h

@@ -204,10 +204,12 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
-    // model panel params
+    // sunnypilot model params
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"LagdToggleDelay", {PERSISTENT | BACKUP, FLOAT, "0.2"}},
     {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}},
+    {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
 
     // mapd
     {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}},

selfdrive/controls/lib/desire_helper.py

@@ -1,7 +1,8 @@
-from cereal import log
+from cereal import log, custom
 from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode
+from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController
 
 LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
@@ -30,6 +31,12 @@ DESIRES = {
   },
 }
 
+TURN_DESIRES = {
+  custom.TurnDirection.none: log.Desire.none,
+  custom.TurnDirection.turnLeft: log.Desire.turnLeft,
+  custom.TurnDirection.turnRight: log.Desire.turnRight,
+}
+
 
 class DesireHelper:
   def __init__(self):
@@ -41,13 +48,21 @@ class DesireHelper:
     self.prev_one_blinker = False
     self.desire = log.Desire.none
     self.alc = AutoLaneChangeController(self)
+    self.lane_turn_controller = LaneTurnController(self)
+    self.lane_turn_direction = custom.TurnDirection.none
 
   def update(self, carstate, lateral_active, lane_change_prob):
     self.alc.update_params()
+    self.lane_turn_controller.update_params()
     v_ego = carstate.vEgo
     one_blinker = carstate.leftBlinker != carstate.rightBlinker
     below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
 
+    # Lane turn controller update
+    self.lane_turn_controller.update_lane_turn(blindspot_left=carstate.leftBlindspot, blindspot_right=carstate.rightBlindspot,
+                                               left_blinker=carstate.leftBlinker, right_blinker=carstate.rightBlinker, v_ego=v_ego)
+    self.lane_turn_direction = self.lane_turn_controller.get_turn_direction()
+
     if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX or self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF:
       self.lane_change_state = LaneChangeState.off
       self.lane_change_direction = LaneChangeDirection.none
@@ -106,7 +121,10 @@ class DesireHelper:
 
     self.prev_one_blinker = one_blinker
 
-    self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
+    if self.lane_turn_direction != custom.TurnDirection.none:
+      self.desire = TURN_DESIRES[self.lane_turn_direction]
+    else:
+      self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
 
     # Send keep pulse once per second during LaneChangeStart.preLaneChange
     if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):

selfdrive/modeld/modeld.py

@@ -216,7 +216,7 @@ def main(demo=False):
     cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
 
   # messaging
-  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
+  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"])
   sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
 
   publish_state = PublishState()
@@ -333,6 +333,7 @@ def main(demo=False):
       modelv2_send = messaging.new_message('modelV2')
       drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')
+      mdv2sp_send = messaging.new_message('modelDataV2SP')
 
       action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego)
       prev_action = action
@@ -347,6 +348,7 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
+      mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
       drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
       drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
 
@@ -354,6 +356,7 @@ def main(demo=False):
       pm.send('modelV2', modelv2_send)
       pm.send('drivingModelData', drivingdata_send)
       pm.send('cameraOdometry', posenet_send)
+      pm.send('modelDataV2SP', mdv2sp_send)
     last_vipc_frame_id = meta_main.frame_id
 
 

selfdrive/selfdrived/selfdrived.py

@@ -86,7 +86,7 @@ class SelfdriveD(CruiseHelper):
     # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
     self.car_state_sock = messaging.sub_sock('carState', timeout=20)
 
-    ignore = self.sensor_packets + self.gps_packets + ['alertDebug']
+    ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP']
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
     if REPLAY:
@@ -95,7 +95,8 @@ class SelfdriveD(CruiseHelper):
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
-                                   'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback'] + \
+                                   'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
+                                   'modelDataV2SP'] + \
                                    self.camera_packets + self.sensor_packets + self.gps_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore,
                                   ignore_valid=ignore, frequency=int(1/DT_CTRL))
@@ -300,6 +301,13 @@ class SelfdriveD(CruiseHelper):
                                                     LaneChangeState.laneChangeFinishing):
       self.events.add(EventName.laneChange)
 
+    # Handle lane turn
+    lane_turn_direction = self.sm['modelDataV2SP'].laneTurnDirection
+    if lane_turn_direction == custom.TurnDirection.turnLeft:
+      self.events_sp.add(custom.OnroadEventSP.EventName.laneTurnLeft)
+    elif lane_turn_direction == custom.TurnDirection.turnRight:
+      self.events_sp.add(custom.OnroadEventSP.EventName.laneTurnRight)
+
     for i, pandaState in enumerate(self.sm['pandaStates']):
       # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
       if i < len(self.CP.safetyConfigs):

selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc

@@ -48,6 +48,25 @@ static const QString progressStyleError = progressStyleActive +
     "  background-color: transparent;"
     "}";
 
+std::optional<cereal::Event::Reader> safeParamEventLoad(Params& params, const std::string& paramName) {
+  std::string raw = params.get(paramName);
+  if (raw.empty()) {
+    return std::nullopt;
+  }
+
+  try {
+    AlignedBuffer alignedBuf;
+    auto buf = alignedBuf.align(raw.data(), raw.size());
+
+    capnp::FlatArrayMessageReader msg(kj::ArrayPtr<const capnp::word>(buf.begin(), buf.size()));
+    return msg.getRoot<cereal::Event>();
+  }
+  catch (const kj::Exception& e) {
+    qInfo() << "Invalid param" << QString::fromStdString(paramName) << ":" << e.getDescription().cStr();
+    return std::nullopt;
+  }
+}
+
 ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
   QVBoxLayout *main_layout = new QVBoxLayout(this);
   main_layout->setContentsMargins(50, 20, 50, 20);
@@ -101,9 +120,32 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
   QString policyType = tr("Policy Model");
   policyFrame = createModelDetailFrame(this, policyType, policyProgressBar);
   list->addItem(policyFrame);
-
   list->addItem(horizontal_line());
 
+  // Lane Turn Desire toggle
+  lane_turn_desire_toggle = new ParamControlSP("LaneTurnDesire", tr("Use Lane Turn Desires"),
+                            "If you’re driving at 20 mph (32 km/h) or below and have your blinker on, "
+                            "the car will plan a turn in that direction at the nearest drivable path. "
+                            "This prevents situations (like at red lights) where the car might plan the wrong turn direction.",
+                             "../assets/offroad/icon_shell.png");
+  list->addItem(lane_turn_desire_toggle);
+
+  // Lane Turn Value control
+  int max_value_mph = 20;
+  bool is_metric_initial = params.getBool("IsMetric");
+  const float K = 1.609344f;
+  int per_value_change_scaled = is_metric_initial ? static_cast<int>(std::round((1.0f / K) * 100.0f)) : 100; // 100 -> 1 mph
+  lane_turn_value_control = new OptionControlSP("LaneTurnValue", tr("Adjust Lane Turn Speed"),
+    tr("Set the maximum speed for lane turn desires. Default is 19 %1.").arg(is_metric_initial ? "km/h" : "mph"),
+    "", {5 * 100, max_value_mph * 100}, per_value_change_scaled, false, nullptr, true, true);
+  lane_turn_value_control->showDescription();
+  list->addItem(lane_turn_value_control);
+
+  // Show based on toggle
+  refreshLaneTurnValueControl();
+  connect(lane_turn_desire_toggle, &ParamControlSP::toggleFlipped, this, &ModelsPanel::refreshLaneTurnValueControl);
+  connect(lane_turn_value_control, &OptionControlSP::updateLabels, this, &ModelsPanel::refreshLaneTurnValueControl);
+
   // LiveDelay toggle
   lagd_toggle_control = new ParamControlSP("LagdToggle", tr("Live Learning Steer Delay"), "", "../assets/offroad/icon_shell.png");
   lagd_toggle_control->showDescription();
@@ -111,17 +153,10 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
 
   // Software delay control
   int liveDelayMaxInt = 30;
-  std::string liveDelayBytes = params.get("LiveDelay");
-  if (!liveDelayBytes.empty()) {
-    capnp::FlatArrayMessageReader msg(kj::ArrayPtr<const capnp::word>(
-      reinterpret_cast<const capnp::word*>(liveDelayBytes.data()),
-      liveDelayBytes.size() / sizeof(capnp::word)));
-    auto event = msg.getRoot<cereal::Event>();
-    if (event.hasLiveDelay()) {
-      auto liveDelay = event.getLiveDelay();
-      float lateralDelay = liveDelay.getLateralDelay();
-      liveDelayMaxInt = static_cast<int>(lateralDelay * 100.0f) + 20;
-    }
+  if (const auto event = safeParamEventLoad(params, "LiveDelay"); event && event->hasLiveDelay()) {
+    auto liveDelay = event->getLiveDelay();
+    float lateralDelay = liveDelay.getLateralDelay();
+    liveDelayMaxInt = static_cast<int>(lateralDelay * 100.0f) + 20;
   }
   delay_control = new OptionControlSP("LagdToggleDelay", tr("Adjust Software Delay"),
                                      tr("Adjust the software delay when Live Learning Steer Delay is toggled off."
@@ -159,6 +194,19 @@ QFrame* ModelsPanel::createModelDetailFrame(QWidget *parent, QString &typeName,
   return frame;
 }
 
+void ModelsPanel::refreshLaneTurnValueControl() {
+  if (!lane_turn_value_control) return;
+  float stored_mph = QString::fromStdString(params.get("LaneTurnValue")).toFloat();
+  bool is_metric = params.getBool("IsMetric");
+  QString unit = is_metric ? "km/h" : "mph";
+  float display_value = stored_mph;
+  if (is_metric) {
+    display_value = stored_mph * 1.609344f;
+  }
+  lane_turn_value_control->setLabel(QString::number(static_cast<int>(std::round(display_value))) + " " + unit);
+  lane_turn_value_control->setVisible(params.getBool("LaneTurnDesire"));
+}
+
 /**
  * @brief Updates the UI with bundle download progress information
  * Reads status from modelManagerSP cereal message and displays status for all models
@@ -401,18 +449,11 @@ void ModelsPanel::updateLabels() {
                    "Disable to use a fixed steering response time. Keeping this on provides the stock openpilot experience.");
   bool lagdEnabled = params.getBool("LagdToggle");
   if (lagdEnabled) {
-    std::string liveDelayBytes = params.get("LiveDelay");
-    if (!liveDelayBytes.empty()) {
-      capnp::FlatArrayMessageReader msg(kj::ArrayPtr<const capnp::word>(
-        reinterpret_cast<const capnp::word*>(liveDelayBytes.data()),
-        liveDelayBytes.size() / sizeof(capnp::word)));
-      auto event = msg.getRoot<cereal::Event>();
-      if (event.hasLiveDelay()) {
-        auto liveDelay = event.getLiveDelay();
-        float lateralDelay = liveDelay.getLateralDelay();
-        desc += QString("<br><br><b><span style=\"color:#e0e0e0\">%1</span></b> <span style=\"color:#e0e0e0\">%2 s</span>")
+    if (const auto event = safeParamEventLoad(params, "LiveDelay"); event && event->hasLiveDelay()) {
+      auto liveDelay = event->getLiveDelay();
+      float lateralDelay = liveDelay.getLateralDelay();
+      desc += QString("<br><br><b><span style=\"color:#e0e0e0\">%1</span></b> <span style=\"color:#e0e0e0\">%2 s</span>")
                 .arg(tr("Live Steer Delay:")).arg(QString::number(lateralDelay, 'f', 3));
-      }
     }
   } else {
     std::string carParamsBytes = params.get("CarParamsPersistent");
@@ -439,6 +480,9 @@ void ModelsPanel::updateLabels() {
     delay_control->setLabel(QString::number(value, 'f', 2) + "s");
   }
 
+  // Update lane turn desire label and visibility
+  refreshLaneTurnValueControl();
+
   clearModelCacheBtn->setValue(QString::number(calculateCacheSize(), 'f', 2) + " MB");
 }
 

selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h

@@ -37,6 +37,7 @@ private:
   void updateLabels();
   void handleCurrentModelLblBtnClicked();
   void handleBundleDownloadProgress();
+  void refreshLaneTurnValueControl();
   void showResetParamsDialog();
   QProgressBar* createProgressBar(QWidget *parent);
   QFrame* createModelDetailFrame(QWidget *parent, QString &typeName, QProgressBar *progressBar);
@@ -81,5 +82,6 @@ private:
   Params params;
   ButtonControlSP *clearModelCacheBtn;
   ButtonControlSP *refreshAvailableModelsBtn;
-
+  ParamControlSP *lane_turn_desire_toggle;
+  OptionControlSP *lane_turn_value_control;
 };

sunnypilot/modeld/modeld.py

@@ -177,7 +177,7 @@ def main(demo=False):
     cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
 
   # messaging
-  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
+  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"])
   sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
 
   publish_state = PublishState()
@@ -304,6 +304,7 @@ def main(demo=False):
       modelv2_send = messaging.new_message('modelV2')
       drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')
+      mdv2sp_send = messaging.new_message('modelDataV2SP')
       action = model.get_action_from_model(model_output, prev_action, long_delay + DT_MDL)
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
                      frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen,
@@ -316,6 +317,7 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
+      mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
       drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
       drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
 
@@ -323,6 +325,7 @@ def main(demo=False):
       pm.send('modelV2', modelv2_send)
       pm.send('drivingModelData', drivingdata_send)
       pm.send('cameraOdometry', posenet_send)
+      pm.send('modelDataV2SP', mdv2sp_send)
 
     last_vipc_frame_id = meta_main.frame_id
 

sunnypilot/modeld_v2/modeld.py

@@ -202,7 +202,7 @@ def main(demo=False):
     cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
 
   # messaging
-  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
+  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"])
   sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
 
   publish_state = PublishState()
@@ -322,6 +322,7 @@ def main(demo=False):
       modelv2_send = messaging.new_message('modelV2')
       drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')
+      mdv2sp_send = messaging.new_message('modelDataV2SP')
 
       action = model.get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego)
       prev_action = action
@@ -336,6 +337,7 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
+      mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
       drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
       drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
 
@@ -343,6 +345,7 @@ def main(demo=False):
       pm.send('modelV2', modelv2_send)
       pm.send('drivingModelData', drivingdata_send)
       pm.send('cameraOdometry', posenet_send)
+      pm.send('modelDataV2SP', mdv2sp_send)
     last_vipc_frame_id = meta_main.frame_id
 
 

sunnypilot/selfdrive/controls/lib/lane_turn_desire.py

@@ -0,0 +1,45 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from cereal import custom
+
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+
+LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
+
+
+class LaneTurnController:
+  def __init__(self, desire_helper):
+    self.DH = desire_helper
+    self.turn_direction = custom.TurnDirection.none
+    self.params = Params()
+    self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
+    self.param_read_counter = 0
+    self.enabled = self.params.get_bool("LaneTurnDesire")
+
+  def read_params(self):
+    self.enabled = self.params.get_bool("LaneTurnDesire")
+    value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
+    self.lane_turn_value = min(float(LANE_CHANGE_SPEED_MIN), value)
+
+  def update_params(self) -> None:
+    if self.param_read_counter % 50 == 0:
+      self.read_params()
+    self.param_read_counter += 1
+
+  def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float) -> None:
+    if left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left:
+      self.turn_direction = custom.TurnDirection.turnLeft
+    elif right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right:
+      self.turn_direction = custom.TurnDirection.turnRight
+    else:
+      self.turn_direction = custom.TurnDirection.none
+
+  def get_turn_direction(self):
+    if not self.enabled:
+      return custom.TurnDirection.none
+    return self.turn_direction

sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py

@@ -0,0 +1,113 @@
+import pytest
+from cereal import log
+from openpilot.common.params import Params
+
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
+from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
+
+
+class TurnDirection:
+    none = 0
+    turnLeft = 1
+    turnRight = 2
+
+
+@pytest.mark.parametrize("left_blinker,right_blinker,v_ego,blindspot_left,blindspot_right,expected", [
+    (True, False, 5, False, False, TurnDirection.turnLeft),
+    (False, True, 6, False, False, TurnDirection.turnRight),
+    (True, False, 9, False, False, TurnDirection.none),
+    (True, False, 7, True, False, TurnDirection.none),
+    (False, True, 6, False, True, TurnDirection.none),
+    (False, False, 5, False, False, TurnDirection.none),
+    (True, True, 5, False, False, TurnDirection.none),
+])
+def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindspot_left, blindspot_right, expected):
+    dh = DesireHelper()
+    controller = LaneTurnController(dh)
+    controller.enabled = True
+    controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+    controller.turn_direction = TurnDirection.none
+    controller.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego)
+    assert controller.get_turn_direction() == expected
+
+
+def test_lane_turn_desire_disabled():
+    dh = DesireHelper()
+    controller = LaneTurnController(dh)
+    controller.enabled = False
+    controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+    controller.turn_direction = TurnDirection.none
+    controller.update_lane_turn(False, False, True, False, 7)
+    assert controller.get_turn_direction() == TurnDirection.none
+
+
+def test_lane_turn_overrides_lane_change():
+    dh = DesireHelper()
+    controller = LaneTurnController(dh)
+    controller.enabled = True
+    controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+    controller.turn_direction = TurnDirection.none
+    # left turn desire
+    controller.update_lane_turn(False, False, True, False, 5)
+    assert controller.get_turn_direction() == TurnDirection.turnLeft
+    # right turn desire
+    controller.update_lane_turn(False, False, False, True, 6)
+    assert controller.get_turn_direction() == TurnDirection.turnRight
+    # no turn
+    controller.update_lane_turn(False, False, False, False, 7)
+    assert controller.get_turn_direction() == TurnDirection.none
+
+
+@pytest.mark.parametrize("v_ego,expected", [
+    (8.93, TurnDirection.turnLeft),   # just below threshold
+    (8.96, TurnDirection.none),       # above threshold
+    (8.95, TurnDirection.none),       # just above threshold
+])
+def test_lane_turn_desire_speed_boundary(v_ego, expected):
+    dh = DesireHelper()
+    controller = LaneTurnController(dh)
+    controller.enabled = True
+    controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+    controller.turn_direction = TurnDirection.none
+    controller.update_lane_turn(False, True, True, False, v_ego)
+    assert controller.get_turn_direction() == expected
+
+
+class DummyCarState:
+    def __init__(self, vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+                 steeringPressed=False, steeringTorque=0, brakePressed=False):
+        self.vEgo = vEgo
+        self.leftBlinker = leftBlinker
+        self.rightBlinker = rightBlinker
+        self.leftBlindspot = leftBlindspot
+        self.rightBlindspot = rightBlindspot
+        self.steeringPressed = steeringPressed
+        self.steeringTorque = steeringTorque
+        self.brakePressed = brakePressed
+
+@pytest.fixture
+def set_lane_turn_params():
+    params = Params()
+    params.put("LaneTurnDesire", True)
+    params.put("LaneTurnValue", 20.0)
+
+@pytest.mark.parametrize("carstate, lateral_active, lane_change_prob, expected_desire", [
+    # Lane turn desire overrides lane change desire
+    (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnLeft),
+    (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnRight),
+    # Lane change desire only (no turn desires)
+    (DummyCarState(vEgo=9, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+                   steeringPressed=True, steeringTorque=1), True, 1.0, log.Desire.laneChangeLeft),
+    (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False,
+                   steeringPressed=True, steeringTorque=-1), True, 1.0, log.Desire.laneChangeRight),
+    # No desire (inactive)
+    (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=False), False, 1.0, log.Desire.none),
+    (DummyCarState(vEgo=4, leftBlinker=False, rightBlinker=False), True, 1.0, log.Desire.none),  # No blinkers? no desire!
+])
+def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
+    dh = DesireHelper()
+    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+    for _ in range(10):
+        dh.update(carstate, lateral_active, lane_change_prob)
+    assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers

sunnypilot/selfdrive/selfdrived/events.py

@@ -132,6 +132,21 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
 
   EventNameSP.pedalPressedAlertOnly: {
     ET.WARNING: NoEntryAlert("Pedal Pressed")
-  }
+  },
 
+  EventNameSP.laneTurnLeft: {
+    ET.WARNING: Alert(
+      "Turning Left",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
+  },
+
+  EventNameSP.laneTurnRight: {
+    ET.WARNING: Alert(
+      "Turning Right",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
+  }
 }