diff --git a/cereal/log.capnp b/cereal/log.capnp index b715d14220..75ca7dd941 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1096,6 +1096,14 @@ struct AndroidLogEntry { message @6 :Text; } +struct DriverAssistance { + # Lane Departure Warnings + leftLaneDeparture @0 :Bool; + rightLaneDeparture @1 :Bool; + + # FCW, AEB, etc. will go here +} + struct LongitudinalPlan @0xe00b5b3eba12876c { modelMonoTime @9 :UInt64; hasLead @7 :Bool; @@ -2342,6 +2350,7 @@ struct Event { carControl @23 :Car.CarControl; carOutput @127 :Car.CarOutput; longitudinalPlan @24 :LongitudinalPlan; + driverAssistance @132 :DriverAssistance; ubloxGnss @34 :UbloxGnss; ubloxRaw @39 :Data; qcomGnss @31 :QcomGnss; diff --git a/cereal/services.py b/cereal/services.py index b50e47be60..926461aec2 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -40,6 +40,7 @@ _services: dict[str, tuple] = { "carControl": (True, 100., 10), "carOutput": (True, 100., 10), "longitudinalPlan": (True, 20., 10), + "driverAssistance": (True, 20., 20), "procLog": (True, 0.5, 15), "gpsLocationExternal": (True, 10., 10), "gpsLocation": (True, 1., 1), diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 14ea9ebb0d..a7fac2a1ff 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -22,7 +22,6 @@ from openpilot.common.gps import get_gps_location_service from opendbc.car.car_helpers import get_car_interface from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_startup_event -from openpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning from openpilot.selfdrive.controls.lib.events import Events, ET from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -95,7 +94,7 @@ class Controls: self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'testJoystick'] + self.camera_packets + self.sensor_packets + self.gps_packets, + 'testJoystick', 'driverAssistance'] + self.camera_packets + self.sensor_packets + self.gps_packets, ignore_alive=ignore, ignore_avg_freq=ignore+['testJoystick'], ignore_valid=['testJoystick', ], frequency=int(1/DT_CTRL)) @@ -128,8 +127,6 @@ class Controls: self.LoC = LongControl(self.CP) self.VM = VehicleModel(self.CP) - self.ldw = LaneDepartureWarning() - self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.LaC = LatControlAngle(self.CP, self.CI) @@ -245,11 +242,9 @@ class Controls: self.events.add(EventName.calibrationInvalid) # Lane departure warning - if self.sm.valid['modelV2'] and CS.canValid: - self.ldw.update(self.sm.frame, self.sm['modelV2'], CS, self.CC_prev) - if self.is_ldw_enabled and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated: - if self.ldw.warning: - self.events.add(EventName.ldw) + if self.is_ldw_enabled and self.sm.valid['driverAssistance']: + if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture: + self.events.add(EventName.ldw) # Handle lane change if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange: @@ -606,9 +601,9 @@ class Controls: hudControl.rightLaneVisible = True hudControl.leftLaneVisible = True - if self.sm.valid['modelV2'] and CS.canValid and self.is_ldw_enabled and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated: - hudControl.leftLaneDepart = self.ldw.left - hudControl.rightLaneDepart = self.ldw.right + if self.is_ldw_enabled and self.sm.valid['driverAssistance']: + hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture + hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture if self.AM.current_alert: hudControl.visualAlert = self.AM.current_alert.visual_alert diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 9de70c767a..ae8301871d 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -3,11 +3,12 @@ from cereal import car from openpilot.common.params import Params from openpilot.common.realtime import Priority, config_realtime_process from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner import cereal.messaging as messaging -def plannerd_thread(): +def main(): config_realtime_process(5, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") @@ -15,8 +16,9 @@ def plannerd_thread(): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("plannerd got CarParams: %s", CP.carName) + ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) - pm = messaging.PubMaster(['longitudinalPlan']) + pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState'], poll='modelV2', ignore_avg_freq=['radarState']) @@ -26,9 +28,12 @@ def plannerd_thread(): longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) - -def main(): - plannerd_thread() + ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl']) + msg = messaging.new_message('driverAssistance') + msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2']) + msg.driverAssistance.leftLaneDeparture = ldw.left + msg.driverAssistance.rightLaneDeparture = ldw.right + pm.send('driverAssistance', msg) if __name__ == "__main__": diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index 723a9ff5b3..9c680ada9a 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -18,6 +18,7 @@ def migrate_all(lr, manager_states=False, panda_states=False, camera_states=Fals msgs = migrate_controlsState(msgs) msgs = migrate_liveLocationKalman(msgs) msgs = migrate_liveTracks(msgs) + msgs = migrate_driverAssistance(msgs) if manager_states: msgs = migrate_managerState(msgs) if panda_states: @@ -28,6 +29,15 @@ def migrate_all(lr, manager_states=False, panda_states=False, camera_states=Fals return msgs +def migrate_driverAssistance(lr): + all_msgs = [] + for msg in lr: + all_msgs.append(msg) + if msg.which() == 'longitudinalPlan': + all_msgs.append(messaging.new_message('driverAssistance', valid=True, logMonoTime=msg.logMonoTime).as_reader()) + if msg.which() == 'driverAssistance': + return lr + return all_msgs def migrate_liveTracks(lr): all_msgs = [] diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 78008ca909..39fec7b669 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -467,7 +467,7 @@ CONFIGS = [ "longitudinalPlan", "livePose", "liveParameters", "radarState", "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput", - "gpsLocationExternal", "gpsLocation", + "gpsLocationExternal", "gpsLocation", "driverAssistance" ], subs=["selfdriveState", "controlsState", "carControl", "onroadEvents"], ignore=["logMonoTime", "controlsState.cumLagMs"], @@ -499,7 +499,7 @@ CONFIGS = [ ProcessConfig( proc_name="plannerd", pubs=["modelV2", "carControl", "carState", "controlsState", "radarState", "selfdriveState"], - subs=["longitudinalPlan"], + subs=["longitudinalPlan", "driverAssistance"], ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"], init_callback=get_car_params_callback, should_recv_callback=FrequencyBasedRcvCallback("modelV2"), diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3a90b7a5fb..9ff5403d3c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -7f9255ef41d0257f8665d441c666cb2721cdf3ee \ No newline at end of file +fc1344b16b802cdb4ec542791f3b4d76a82da68b \ No newline at end of file