diff --git a/cereal/car.capnp b/cereal/car.capnp index 7972533dd..01c99d001 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -134,6 +134,8 @@ struct CarEvent @0x9b1657f34caf3ad3 { pedalInterceptorNoBrake @133; speedLimitChanged @134; torqueNNLoad @135; + turningLeft @136; + turningRight @137; vCruise69 @138; yourFrogTriedToKillMe @139; diff --git a/cereal/log.capnp b/cereal/log.capnp index f35767ee1..8c94c943f 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -327,6 +327,12 @@ enum LaneChangeDirection { right @2; } +enum TurnDirection { + none @0; + turnLeft @1; + turnRight @2; +} + struct CanData { address @0 :UInt32; busTime @1 :UInt16; @@ -952,6 +958,7 @@ struct ModelDataV2 { hardBrakePredicted @7 :Bool; laneChangeState @8 :LaneChangeState; laneChangeDirection @9 :LaneChangeDirection; + turnDirection @10 :TurnDirection; # deprecated diff --git a/common/params.cc b/common/params.cc index 4a0626695..2b92674ad 100644 --- a/common/params.cc +++ b/common/params.cc @@ -435,6 +435,7 @@ std::unordered_map keys = { {"TrafficJerk", PERSISTENT}, {"TrafficMode", PERSISTENT}, {"TrafficModeActive", CLEAR_ON_OFFROAD_TRANSITION}, + {"TurnDesires", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"UnlockDoors", PERSISTENT}, {"Updated", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0b1ff5d26..fe20daf2d 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -995,6 +995,12 @@ class Controls: if lead_departing: self.events.add(EventName.leadDeparting) + if not CS.standstill: + if self.sm['modelV2'].meta.turnDirection == Desire.turnLeft: + self.events.add(EventName.turningLeft) + elif self.sm['modelV2'].meta.turnDirection == Desire.turnRight: + self.events.add(EventName.turningRight) + if os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')) and not self.openpilot_crashed_triggered: if self.random_events: self.events.add(EventName.openpilotCrashedRandomEvents) diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 6bfb4374c..027a441a8 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -5,6 +5,7 @@ from openpilot.common.realtime import DT_MDL LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection +TurnDirection = log.Desire LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. @@ -30,6 +31,11 @@ DESIRES = { }, } +TURN_DESIRES = { + TurnDirection.none: log.Desire.none, + TurnDirection.turnLeft: log.Desire.turnLeft, + TurnDirection.turnRight: log.Desire.turnRight, +} class DesireHelper: def __init__(self): @@ -45,7 +51,10 @@ class DesireHelper: self.params = Params() self.params_memory = Params("/dev/shm/params") + self.turn_direction = TurnDirection.none + self.lane_change_completed = False + self.turn_completed = False self.lane_change_wait_timer = 0 @@ -65,7 +74,14 @@ class DesireHelper: if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none + self.turn_direction = TurnDirection.none + elif one_blinker and below_lane_change_speed and self.turn_desires: + self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight + # Set the "turn_completed" flag to prevent lane changes after completing a turn + self.turn_completed = True else: + self.turn_direction = TurnDirection.none + # LaneChangeState.off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange @@ -125,8 +141,14 @@ class DesireHelper: self.lane_change_completed &= one_blinker self.prev_one_blinker = one_blinker + self.turn_completed &= one_blinker - self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + if self.turn_direction != TurnDirection.none: + self.desire = TURN_DESIRES[self.turn_direction] + elif not self.turn_completed: + self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + else: + self.desire = log.Desire.none # Send keep pulse once per second during LaneChangeStart.preLaneChange if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting): @@ -145,6 +167,7 @@ class DesireHelper: is_metric = self.params.get_bool("IsMetric") lateral_tune = self.params.get_bool("LateralTune") + self.turn_desires = lateral_tune and self.params.get_bool("TurnDesires") self.nudgeless = self.params.get_bool("NudgelessLaneChange") self.lane_change_delay = self.params.get_int("LaneChangeTime") if self.nudgeless else 0 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index f08144bce..119fac3e2 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1084,6 +1084,22 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.PERMANENT: torque_nn_load_alert, }, + EventName.turningLeft: { + ET.WARNING: Alert( + "Turning Left", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75), + }, + + EventName.turningRight: { + ET.WARNING: Alert( + "Turning Right", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75), + }, + # Random Events EventName.accel30: { ET.WARNING: Alert( diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index 5c07b1df6..28233b9fc 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -81,6 +81,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"NNFF", tr("NNFF"), tr("Use Twilsonco's Neural Network Feedforward for enhanced precision in lateral control."), ""}, {"NNFFLite", tr("NNFF-Lite"), tr("Use Twilsonco's Neural Network Feedforward for enhanced precision in lateral control for cars without available NNFF logs."), ""}, {"TacoTune", tr("Taco Tune"), tr("Use comma's 'Taco Tune' designed for handling left and right turns."), ""}, + {"TurnDesires", tr("Use Turn Desires"), tr("Use turn desires for greater precision in turns below the minimum lane change speed."), ""}, {"LongitudinalTune", tr("Longitudinal Tuning"), tr("Modify openpilot's acceleration and braking behavior."), "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, {"AccelerationProfile", tr("Acceleration Profile"), tr("Change the acceleration rate to be either sporty or eco-friendly."), ""}, diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index 16d97bdec..16e326fa6 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -44,7 +44,7 @@ private: std::set deviceManagementKeys = {"DeviceShutdown", "HigherBitrate", "IncreaseThermalLimits", "LowVoltageShutdown", "NoLogging", "NoUploads", "OfflineMode"}; std::set experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap"}; std::set laneChangeKeys = {"LaneChangeTime", "LaneDetectionWidth", "OneLaneChange"}; - std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite", "TacoTune"}; + std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite", "TacoTune", "TurnDesires"}; std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "LeadDetectionThreshold", "SmoothBraking", "StoppingDistance", "TrafficMode"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"}; std::set qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise", "SetSpeedOffset"}; diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index e3b3b730b..8ea7b308e 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -322,6 +322,7 @@ def main(demo=False): DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['frogpilotPlan']) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction + modelv2_send.modelV2.meta.turnDirection = DH.turn_direction fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen) pm.send('modelV2', modelv2_send)