diff --git a/cereal/custom.capnp b/cereal/custom.capnp index e9ef970..7f7f4ee 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -22,6 +22,8 @@ enum FrogPilotEvents @0xf35cc4560bbf6ec2 { greenLight @1; pedalInterceptorNoBrake @2; torqueNNLoad @3; + turningLeft @4; + turningRight @5; } struct FrogPilotLateralPlan @0xda96579883444c35 { diff --git a/common/params.cc b/common/params.cc index 63b59a4..622d91a 100644 --- a/common/params.cc +++ b/common/params.cc @@ -343,6 +343,7 @@ std::unordered_map keys = { {"StoppingDistance", PERSISTENT}, {"TetheringEnabled", PERSISTENT}, {"TSS2Tune", PERSISTENT}, + {"TurnDesires", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"Updated", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c510715..6dbe1d3 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -340,6 +340,13 @@ class Controls: LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) + # Handle turning + if not CS.standstill: + if self.sm['lateralPlan'].desire == Desire.turnLeft: + self.events.add(FrogPilotEventName.turningLeft) + elif self.sm['lateralPlan'].desire == Desire.turnRight: + self.events.add(FrogPilotEventName.turningRight) + 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): diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index f6f6578..22a582a 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -6,6 +6,7 @@ from openpilot.selfdrive.frogpilot.functions.frogpilot_planner import calculate_ LaneChangeState = log.LateralPlan.LaneChangeState LaneChangeDirection = log.LateralPlan.LaneChangeDirection +TurnDirection = log.LateralPlan.Desire LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. @@ -31,6 +32,12 @@ DESIRES = { }, } +TURN_DESIRES = { + TurnDirection.none: log.LateralPlan.Desire.none, + TurnDirection.turnLeft: log.LateralPlan.Desire.turnLeft, + TurnDirection.turnRight: log.LateralPlan.Desire.turnRight, +} + class DesireHelper: def __init__(self): @@ -43,7 +50,10 @@ class DesireHelper: self.desire = log.LateralPlan.Desire.none # FrogPilot variables + self.turn_direction = TurnDirection.none + self.lane_change_completed = False + self.turn_completed = False self.lane_change_wait_timer = 0 self.lane_width_left = 0 @@ -81,7 +91,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 + elif one_blinker and below_lane_change_speed and frogpilot_planner.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: + # TurnDirection.turnLeft / turnRight + 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 @@ -145,8 +162,14 @@ class DesireHelper: # Reset the flags self.lane_change_completed &= 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.LateralPlan.Desire.none # Send keep pulse once per second during LaneChangeStart.preLaneChange if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting): diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 1610588..2c4f262 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1012,6 +1012,22 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.PERMANENT: torque_nn_load_alert, }, + FrogPilotEventName.turningLeft: { + ET.WARNING: Alert( + "Turning Left", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75), + }, + + FrogPilotEventName.turningRight: { + ET.WARNING: Alert( + "Turning Right", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, alert_rate=0.75), + }, + } diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index ea2b80b..18e12fe 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -209,3 +209,5 @@ class FrogPilotPlanner: if self.speed_limit_controller: self.speed_limit_controller_override = params.get_int("SLCOverride") SpeedLimitController.update_frogpilot_params() + + self.turn_desires = params.get_bool("TurnDesires") diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index f6db926..807b1bb 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -51,6 +51,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"SLCFallback", "Fallback Method", "Choose your fallback method for when there are no speed limits currently being obtained from Navigation, OSM, and the car's dashboard.", ""}, {"SLCOverride", "Override Method", "Choose your preferred method to override the current speed limit.", ""}, {"SLCPriority", "Priority Order", "Determine the priority order for what speed limits to use.", ""}, + + {"TurnDesires", "Use Turn Desires", "Use turn desires for enhanced precision in turns below the minimum lane change speed.", "../assets/navigation/direction_continue_right.png"}, }; for (const auto &[param, title, desc, icon] : controlToggles) {