diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e80ece8..2b42759 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -311,28 +311,30 @@ class Controls: # Handle lane change # CLEARPILOT - Disabled lane change helper # CLEARPILOT TODO: Make this a toggle - # if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange: - # direction = self.sm['modelV2'].meta.laneChangeDirection - # if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ - # (CS.rightBlindspot and direction == LaneChangeDirection.right): - # if self.loud_blindspot_alert: - # self.events.add(EventName.laneChangeBlockedLoud) - # else: - # self.events.add(EventName.laneChangeBlocked) - # else: - # if direction == LaneChangeDirection.left: - # if self.sm['frogpilotPlan'].laneWidthLeft >= self.lane_detection_width: - # self.events.add(EventName.preLaneChangeLeft) - # else: - # self.events.add(EventName.noLaneAvailable) - # else: - # if self.sm['frogpilotPlan'].laneWidthRight >= self.lane_detection_width: - # self.events.add(EventName.preLaneChangeRight) - # else: - # self.events.add(EventName.noLaneAvailable) - # elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting, - # LaneChangeState.laneChangeFinishing): - # self.events.add(EventName.laneChange) + NoLaneChange = False + if not NoLaneChange: + if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange: + direction = self.sm['modelV2'].meta.laneChangeDirection + if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ + (CS.rightBlindspot and direction == LaneChangeDirection.right): + if self.loud_blindspot_alert: + self.events.add(EventName.laneChangeBlockedLoud) + else: + self.events.add(EventName.laneChangeBlocked) + else: + if direction == LaneChangeDirection.left: + if self.sm['frogpilotPlan'].laneWidthLeft >= self.lane_detection_width: + self.events.add(EventName.preLaneChangeLeft) + else: + self.events.add(EventName.noLaneAvailable) + else: + if self.sm['frogpilotPlan'].laneWidthRight >= self.lane_detection_width: + self.events.add(EventName.preLaneChangeRight) + else: + self.events.add(EventName.noLaneAvailable) + elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting, + LaneChangeState.laneChangeFinishing): + self.events.add(EventName.laneChange) 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 @@ -628,10 +630,15 @@ class Controls: actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state + # CLEARPILOT test lane change + clearpilot_disable_lat_on_lane_change = True + # Enable blinkers while lane changing if model_v2.meta.laneChangeState != LaneChangeState.off: CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right + if clearpilot_disable_lat_on_lane_change: + CC.latActive = False if CS.leftBlinker or CS.rightBlinker: self.last_blinker_frame = self.sm.frame diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 027a441..612c127 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -10,6 +10,7 @@ TurnDirection = log.Desire LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. + DESIRES = { LaneChangeDirection.none: { LaneChangeState.off: log.Desire.none, @@ -61,6 +62,9 @@ class DesireHelper: self.update_frogpilot_params() def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan): + # Clearpilot test lane change no lat + clearpilot_disable_lat_on_lane_change = True + v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN @@ -71,7 +75,7 @@ class DesireHelper: desired_lane = frogpilotPlan.laneWidthLeft if carstate.leftBlinker else frogpilotPlan.laneWidthRight lane_available = desired_lane >= self.lane_detection_width - if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: + if (not clearpilot_disable_lat_on_lane_change and 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 @@ -115,12 +119,16 @@ class DesireHelper: # LaneChangeState.laneChangeStarting elif self.lane_change_state == LaneChangeState.laneChangeStarting: - # fade out over .5s - self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0) + if clearpilot_disable_lat_on_lane_change: + if not one_blinker: + self.lane_change_state = LaneChangeState.laneChangeFinishing + else: + # fade out over .5s + self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0) - # 98% certainty - if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: - self.lane_change_state = LaneChangeState.laneChangeFinishing + # 98% certainty + if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: + self.lane_change_state = LaneChangeState.laneChangeFinishing # LaneChangeState.laneChangeFinishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 2ac392a..0274b6e 100644 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -747,4 +747,4 @@ int main() { Localizer localizer; return localizer.locationd_thread(); -} +} \ No newline at end of file diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index de4bbcf..0cfae89 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -271,4 +271,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index b5c71a6..b4c8b4f 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -5,9 +5,7 @@ from openpilot.common.params import Params from openpilot.system.hardware import PC, TICI from openpilot.selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess -from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS -RADARLESS = Params().get("Model", encoding='utf-8') in RADARLESS_MODELS WEBCAM = os.getenv("USE_WEBCAM") is not None def driverview(started: bool, params: Params, CP: car.CarParams) -> bool: @@ -80,16 +78,15 @@ procs = [ PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad), PythonProcess("deleter", "system.loggerd.deleter", always_run), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)), - PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", always_run, enabled=TICI), - # PythonProcess("ugpsd", "system.ugpsd", only_onroad, enabled=TICI), + PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI), + #PythonProcess("ugpsd", "system.ugpsd", only_onroad, enabled=TICI), PythonProcess("navd", "selfdrive.navd.navd", only_onroad), PythonProcess("pandad", "selfdrive.boardd.pandad", always_run), PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad), NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI), PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI), PythonProcess("plannerd", "selfdrive.controls.plannerd", only_onroad), - PythonProcess("radard", "selfdrive.controls.radard", only_onroad, enabled=not RADARLESS), - PythonProcess("radardless", "selfdrive.controls.radardless", only_onroad, enabled=RADARLESS), + PythonProcess("radard", "selfdrive.controls.radard", only_onroad), PythonProcess("thermald", "selfdrive.thermald.thermald", always_run), PythonProcess("tombstoned", "selfdrive.tombstoned", always_run, enabled=not PC), PythonProcess("updated", "selfdrive.updated.updated", always_run, enabled=not PC), @@ -107,4 +104,4 @@ procs = [ PythonProcess("mapd", "selfdrive.frogpilot.navigation.mapd", always_run), ] -managed_processes = {p.name: p for p in procs} +managed_processes = {p.name: p for p in procs} \ No newline at end of file