diff --git a/cereal/car.capnp b/cereal/car.capnp index 026aa7baa5..6da9c14c32 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -100,7 +100,7 @@ struct OnroadEvent @0x9b1657f34caf3ad3 { cameraFrameRate @110; processNotRunning @95; dashcamMode @96; - controlsInitializing @98; + selfdriveInitializing @98; usbError @99; roadCameraError @100; driverCameraError @101; @@ -109,7 +109,7 @@ struct OnroadEvent @0x9b1657f34caf3ad3 { cruiseMismatch @106; lkasDisabled @107; canBusMissing @111; - controlsdLagging @112; + selfdrivedLagging @112; resumeBlocked @113; steerTimeLimit @115; vehicleSensorsInvalid @116; diff --git a/cereal/log.capnp b/cereal/log.capnp index 75ca7dd941..258c1f7ab4 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -698,6 +698,7 @@ struct SelfdriveState { alertSize @6 :AlertSize; alertType @7 :Text; alertSound @8 :Car.CarControl.HUDControl.AudibleAlert; + alertHudVisual @12 :Car.CarControl.HUDControl.VisualAlert; # configurable driving settings experimentalMode @10 :Bool; @@ -726,7 +727,6 @@ struct SelfdriveState { } struct ControlsState @0x97ff69c53601abf1 { - cumLagMs @15 :Float32; longitudinalPlanMonoTime @28 :UInt64; lateralPlanMonoTime @50 :UInt64; @@ -880,6 +880,7 @@ struct ControlsState @0x97ff69c53601abf1 { vCruiseDEPRECATED @22 :Float32; # actual set speed vCruiseClusterDEPRECATED @63 :Float32; # set speed to display in the UI startMonoTimeDEPRECATED @48 :UInt64; + cumLagMsDEPRECATED @15 :Float32; } struct DrivingModelData { diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index 25a7defbbd..3984b66c63 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -5,7 +5,7 @@ from opendbc.car.interfaces import MAX_CTRL_SPEED, CarStateBase, CarControllerBa from opendbc.car.volkswagen.values import CarControllerParams as VWCarControllerParams from opendbc.car.hyundai.interface import ENABLE_BUTTONS as HYUNDAI_ENABLE_BUTTONS -from openpilot.selfdrive.controls.lib.events import Events +from openpilot.selfdrive.selfdrived.events import Events ButtonType = structs.CarState.ButtonEvent.Type GearShifter = structs.CarState.GearShifter diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 6494074458..62569f64f5 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -22,7 +22,7 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car.cruise import VCruiseHelper from openpilot.selfdrive.car.car_specific import CarSpecificEvents, MockCarState from openpilot.selfdrive.car.helpers import convert_carControl, convert_to_capnp -from openpilot.selfdrive.controls.lib.events import Events, ET +from openpilot.selfdrive.selfdrived.events import Events, ET REPLAY = "REPLAY" in os.environ @@ -266,7 +266,7 @@ class Car: self.state_publish(CS, RD) - initialized = (not any(e.name == EventName.controlsInitializing for e in self.sm['onroadEvents']) and + initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and self.sm.seen['onroadEvents']) if not self.CP.passive and initialized: self.controls_update(CS, self.sm['carControl']) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a7fac2a1ff..4492b21b47 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,132 +1,53 @@ #!/usr/bin/env python3 -import os import math -import time -import threading from typing import SupportsFloat -import cereal.messaging as messaging - from cereal import car, log -from msgq.visionipc import VisionIpcClient, VisionStreamType - - +import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV -from openpilot.common.git import get_short_branch -from openpilot.common.numpy_fast import clip from openpilot.common.params import Params -from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL +from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper from openpilot.common.swaglog import cloudlog -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.events import Events, ET +from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel -from openpilot.selfdrive.controls.lib.selfdrive import StateMachine from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose -from openpilot.system.hardware import HARDWARE - -JOYSTICK_MAX_LAT_ACCEL = 2.5 # m/s^2 -REPLAY = "REPLAY" in os.environ -SIMULATION = "SIMULATION" in os.environ -TESTING_CLOSET = "TESTING_CLOSET" in os.environ -IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"} - -ThermalStatus = log.DeviceState.ThermalStatus State = log.SelfdriveState.OpenpilotState -PandaType = log.PandaState.PandaType LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection -EventName = car.OnroadEvent.EventName -ButtonType = car.CarState.ButtonEvent.Type -SafetyModel = car.CarParams.SafetyModel -IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) -CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError} ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) - class Controls: - def __init__(self, CI=None): + def __init__(self) -> None: self.params = Params() + cloudlog.info("controlsd is waiting for CarParams") + self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams) + cloudlog.info("controlsd got CarParams") - if CI is None: - cloudlog.info("controlsd is waiting for CarParams") - self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams) - cloudlog.info("controlsd got CarParams") - - # Uses car interface helper functions, altering state won't be considered by card for actuation - self.CI = get_car_interface(self.CP) - else: - self.CI, self.CP = CI, CI.CP - - # Ensure the current branch is cached, otherwise the first iteration of controlsd lags - self.branch = get_short_branch() - - # Setup sockets - self.pm = messaging.PubMaster(['selfdriveState', 'controlsState', 'carControl', 'onroadEvents']) + self.CI = get_car_interface(self.CP) - self.gps_location_service = get_gps_location_service(self.params) - self.gps_packets = [self.gps_location_service] - self.sensor_packets = ["accelerometer", "gyroscope"] - self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] + self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', + 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', + 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='carState') + self.pm = messaging.PubMaster(['carControl', 'controlsState']) - self.log_sock = messaging.sub_sock('androidLog') - - # TODO: de-couple controlsd 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 + ['testJoystick'] - if SIMULATION: - ignore += ['driverCameraState', 'managerState'] - if REPLAY: - # no vipc in replay will make them ignored anyways - ignore += ['roadCameraState', 'wideRoadCameraState'] - self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', - 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - '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)) - - self.joystick_mode = self.params.get_bool("JoystickDebugMode") - - # read params - self.is_metric = self.params.get_bool("IsMetric") - self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") - - # detect sound card presence and ensure successful init - sounds_available = HARDWARE.get_sound_card_online() - - car_recognized = self.CP.carName != 'mock' - - # cleanup old params - if not self.CP.experimentalLongitudinalAvailable: - self.params.remove("ExperimentalLongitudinalEnabled") - if not self.CP.openpilotLongitudinalControl: - self.params.remove("ExperimentalMode") - - self.CS_prev = car.CarState.new_message() - self.CC_prev = car.CarControl.new_message() - self.lac_log_prev = None - self.AM = AlertManager() - self.events = Events() + self.steer_limited = False + self.desired_curvature = 0.0 self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose|None = None self.LoC = LongControl(self.CP) self.VM = VehicleModel(self.CP) - self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.LaC = LatControlAngle(self.CP, self.CI) @@ -135,341 +56,16 @@ class Controls: elif self.CP.lateralTuning.which() == 'torque': self.LaC = LatControlTorque(self.CP, self.CI) - self.initialized = False - self.enabled = False - self.active = False - self.mismatch_counter = 0 - self.cruise_mismatch_counter = 0 - self.last_steering_pressed_frame = 0 - self.distance_traveled = 0 - self.last_functional_fan_frame = 0 - self.events_prev = [] - self.logged_comm_issue = None - self.not_running_prev = None - self.steer_limited = False - self.desired_curvature = 0.0 - self.experimental_mode = False - self.personality = self.read_personality_param() - self.recalibrating_seen = False - self.state_machine = StateMachine() - - self.can_log_mono_time = 0 - - self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0) - - if not sounds_available: - self.events.add(EventName.soundsUnavailable, static=True) - if not car_recognized: - self.events.add(EventName.carUnrecognized, static=True) - if len(self.CP.carFw) > 0: - set_offroad_alert("Offroad_CarUnrecognized", True) - else: - set_offroad_alert("Offroad_NoFirmware", True) - elif self.CP.passive: - self.events.add(EventName.dashcamMode, static=True) - - # controlsd is driven by carState, expected at 100Hz - self.rk = Ratekeeper(100, print_delay_threshold=None) - - def set_initial_state(self): - if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']): - self.state_machine.state = State.enabled - - def update_events(self, CS): - """Compute onroadEvents from carState""" - - self.events.clear() - - # Add joystick event, static on cars, dynamic on nonCars - if self.joystick_mode: - self.events.add(EventName.joystickDebug) - self.startup_event = None - - # Add startup event - if self.startup_event is not None: - self.events.add(self.startup_event) - self.startup_event = None - - # Don't add any more events if not initialized - if not self.initialized: - self.events.add(EventName.controlsInitializing) - return - - # no more events while in dashcam mode - if self.CP.passive: - return - - # Block resume if cruise never previously enabled - resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) - if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed: - self.events.add(EventName.resumeBlocked) - - if not self.CP.notCar: - self.events.add_from_msg(self.sm['driverMonitoringState'].events) - - # Add car events, ignore if CAN isn't valid - if CS.canValid: - self.events.add_from_msg(CS.events) - - # Create events for temperature, disk space, and memory - if self.sm['deviceState'].thermalStatus >= ThermalStatus.red: - self.events.add(EventName.overheat) - if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION: - self.events.add(EventName.outOfSpace) - if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION: - self.events.add(EventName.lowMemory) - - # Alert if fan isn't spinning for 5 seconds - if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown: - if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50: - # allow enough time for the fan controller in the panda to recover from stalls - if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0: - self.events.add(EventName.fanMalfunction) - else: - self.last_functional_fan_frame = self.sm.frame - - # Handle calibration status - cal_status = self.sm['liveCalibration'].calStatus - if cal_status != log.LiveCalibrationData.Status.calibrated: - if cal_status == log.LiveCalibrationData.Status.uncalibrated: - self.events.add(EventName.calibrationIncomplete) - elif cal_status == log.LiveCalibrationData.Status.recalibrating: - if not self.recalibrating_seen: - set_offroad_alert("Offroad_Recalibration", True) - self.recalibrating_seen = True - self.events.add(EventName.calibrationRecalibrating) - else: - self.events.add(EventName.calibrationInvalid) - - # Lane departure warning - 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: - direction = self.sm['modelV2'].meta.laneChangeDirection - if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ - (CS.rightBlindspot and direction == LaneChangeDirection.right): - self.events.add(EventName.laneChangeBlocked) - else: - if direction == LaneChangeDirection.left: - self.events.add(EventName.preLaneChangeLeft) - else: - self.events.add(EventName.preLaneChangeRight) - 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 - if i < len(self.CP.safetyConfigs): - safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \ - pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \ - pandaState.alternativeExperience != self.CP.alternativeExperience - else: - safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES - - # safety mismatch allows some time for pandad to set the safety mode and publish it back from panda - if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200: - self.events.add(EventName.controlsMismatch) - - if log.PandaState.FaultType.relayMalfunction in pandaState.faults: - self.events.add(EventName.relayMalfunction) - - # Handle HW and system malfunctions - # Order is very intentional here. Be careful when modifying this. - # All events here should at least have NO_ENTRY and SOFT_DISABLE. - num_events = len(self.events) - - not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning} - if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES): - self.events.add(EventName.processNotRunning) - if not_running != self.not_running_prev: - cloudlog.event("process_not_running", not_running=not_running, error=True) - self.not_running_prev = not_running - else: - if not SIMULATION and not self.rk.lagging: - if not self.sm.all_alive(self.camera_packets): - self.events.add(EventName.cameraMalfunction) - elif not self.sm.all_freq_ok(self.camera_packets): - self.events.add(EventName.cameraFrameRate) - if not REPLAY and self.rk.lagging: - self.events.add(EventName.controlsdLagging) - if not self.sm.valid['radarState']: - self.events.add(EventName.radarFault) - if not self.sm.valid['pandaStates']: - self.events.add(EventName.usbError) - if CS.canTimeout: - self.events.add(EventName.canBusMissing) - elif not CS.canValid: - self.events.add(EventName.canError) - - # generic catch-all. ideally, a more specific event should be added above instead - has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE)) - no_system_errors = (not has_disable_events) or (len(self.events) == num_events) - if not self.sm.all_checks() and no_system_errors: - if not self.sm.all_alive(): - self.events.add(EventName.commIssue) - elif not self.sm.all_freq_ok(): - self.events.add(EventName.commIssueAvgFreq) - else: - self.events.add(EventName.commIssue) - - logs = { - 'invalid': [s for s, valid in self.sm.valid.items() if not valid], - 'not_alive': [s for s, alive in self.sm.alive.items() if not alive], - 'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], - } - if logs != self.logged_comm_issue: - cloudlog.event("commIssue", error=True, **logs) - self.logged_comm_issue = logs - else: - self.logged_comm_issue = None - - if not (self.CP.notCar and self.joystick_mode): - if not self.sm['livePose'].posenetOK: - self.events.add(EventName.posenetInvalid) - if not self.sm['livePose'].inputsOK: - self.events.add(EventName.locationdTemporaryError) - if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY): - self.events.add(EventName.paramsdTemporaryError) - - # conservative HW alert. if the data or frequency are off, locationd will throw an error - if any((self.sm.frame - self.sm.recv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets): - self.events.add(EventName.sensorDataInvalid) - - if not REPLAY: - # Check for mismatch between openpilot and car's PCM - cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) - self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0 - if self.cruise_mismatch_counter > int(6. / DT_CTRL): - self.events.add(EventName.cruiseMismatch) - - # Send a "steering required alert" if saturation count has reached the limit - lac_log = self.lac_log_prev - if CS.steeringPressed: - self.last_steering_pressed_frame = self.sm.frame - recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0 - if self.lac_log_prev is not None and self.lac_log_prev.active and not recent_steer_pressed and not self.CP.notCar: - lac_log = self.lac_log_prev - if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode: - undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 - turning = abs(lac_log.desiredLateralAccel) > 1.0 - good_speed = CS.vEgo > 5 - max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99 - if undershooting and turning and good_speed and max_torque: - self.events.add(EventName.steerSaturated) - elif lac_log.saturated: - # TODO probably should not use dpath_points but curvature - dpath_points = self.sm['modelV2'].position.y - if len(dpath_points): - # Check if we deviated from the path - # TODO use desired vs actual curvature - if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - steering_value = self.CC_prev.actuators.steeringAngleDeg - else: - steering_value = self.CC_prev.actuators.steer - - left_deviation = steering_value > 0 and dpath_points[0] < -0.20 - right_deviation = steering_value < 0 and dpath_points[0] > 0.20 - - if left_deviation or right_deviation: - self.events.add(EventName.steerSaturated) - - # Check for FCW - stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25 - model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking - planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled - if (planner_fcw or model_fcw) and not (self.CP.notCar and self.joystick_mode): - self.events.add(EventName.fcw) - - for m in messaging.drain_sock(self.log_sock, wait_for_one=False): - try: - msg = m.androidLog.message - if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")): - csid = msg.split("CSID:")[-1].split(" ")[0] - evt = CSID_MAP.get(csid, None) - if evt is not None: - self.events.add(evt) - except UnicodeDecodeError: - pass - - # TODO: fix simulator - if not SIMULATION or REPLAY: - # Not show in first 1.5 km to allow for driving out of garage. This event shows after 5 minutes - gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0 - if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500): - self.events.add(EventName.noGps) - if gps_ok: - self.distance_traveled = 0 - self.distance_traveled += CS.vEgo * DT_CTRL - - if self.sm['modelV2'].frameDropPerc > 20: - self.events.add(EventName.modeldLagging) - - # decrement personality on distance button press - if self.CP.openpilotLongitudinalControl: - if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents): - self.personality = (self.personality - 1) % 3 - self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) - self.events.add(EventName.personalityChanged) - - def data_sample(self): - """Receive data from sockets""" - - car_state = messaging.recv_one(self.car_state_sock) - CS = car_state.carState if car_state else self.CS_prev - - self.sm.update(0) - - if not self.initialized: - all_valid = CS.canValid and self.sm.all_checks() - timed_out = self.sm.frame * DT_CTRL > 6. - if all_valid or timed_out or (SIMULATION and not REPLAY): - available_streams = VisionIpcClient.available_streams("camerad", block=False) - if VisionStreamType.VISION_STREAM_ROAD not in available_streams: - self.sm.ignore_alive.append('roadCameraState') - if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams: - self.sm.ignore_alive.append('wideRoadCameraState') - - self.initialized = True - self.set_initial_state() - - cloudlog.event( - "controlsd.initialized", - dt=self.sm.frame*DT_CTRL, - timeout=timed_out, - canValid=CS.canValid, - invalid=[s for s, valid in self.sm.valid.items() if not valid], - not_alive=[s for s, alive in self.sm.alive.items() if not alive], - not_freq_ok=[s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], - error=True, - ) - - # When the panda and controlsd do not agree on controls_allowed - # we want to disengage openpilot. However the status from the panda goes through - # another socket other than the CAN messages and one can arrive earlier than the other. - # Therefore we allow a mismatch for two samples, then we trigger the disengagement. - if not self.enabled: - self.mismatch_counter = 0 - - # All pandas not in silent mode must have controlsAllowed when openpilot is enabled - if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates'] - if ps.safetyModel not in IGNORED_SAFETY_MODES): - self.mismatch_counter += 1 - - # calibrate the live pose and save it for later use + def update(self): + self.sm.update(15) if self.sm.updated["liveCalibration"]: self.pose_calibrator.feed_live_calib(self.sm['liveCalibration']) if self.sm.updated["livePose"]: device_pose = Pose.from_live_pose(self.sm['livePose']) self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose) - return CS - - def state_control(self, CS): - """Given the state, this function returns a CarControl packet""" + def state_control(self): + CS = self.sm['carState'] # Update VehicleModel lp = self.sm['liveParameters'] @@ -488,13 +84,12 @@ class Controls: model_v2 = self.sm['modelV2'] CC = car.CarControl.new_message() - CC.enabled = self.enabled + CC.enabled = self.sm['selfdriveState'].enabled # Check which actuators can be enabled standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill - CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ - (not standstill or self.joystick_mode) - CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl + CC.latActive = self.sm['selfdriveState'].active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and not standstill + CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state @@ -504,51 +99,21 @@ class Controls: CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right - if CS.leftBlinker or CS.rightBlinker: - self.last_blinker_frame = self.sm.frame - - # State specific actions - if not CC.latActive: self.LaC.reset() if not CC.longActive: self.LoC.reset() - if not self.joystick_mode: - # accel PID loop - pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS) - actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits) - - # Steering PID loop and lateral MPC - self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) - actuators.curvature = self.desired_curvature - actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited, self.desired_curvature, - self.calibrated_pose) # TODO what if not available - else: - lac_log = log.ControlsState.LateralDebugState.new_message() - if self.sm.recv_frame['testJoystick'] > 0: - # reset joystick if it hasn't been received in a while - should_reset_joystick = (self.sm.frame - self.sm.recv_frame['testJoystick'])*DT_CTRL > 0.2 - if not should_reset_joystick: - joystick_axes = self.sm['testJoystick'].axes - else: - joystick_axes = [0.0, 0.0] - - if CC.longActive: - actuators.accel = 4.0*clip(joystick_axes[0], -1, 1) - - if CC.latActive: - max_curvature = JOYSTICK_MAX_LAT_ACCEL / max(CS.vEgo ** 2, 1) - max_angle = math.degrees(self.VM.get_steer_from_curvature(max_curvature, CS.vEgo, lp.roll)) - - steer = clip(joystick_axes[1], -1, 1) - actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer * max_angle, steer * -max_curvature - - lac_log.active = self.active - lac_log.steeringAngleDeg = CS.steeringAngleDeg - lac_log.output = actuators.steer - lac_log.saturated = abs(actuators.steer) >= 0.9 + # accel PID loop + pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS) + actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits) + + # Steering PID loop and lateral MPC + self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) + actuators.curvature = self.desired_curvature + actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, + self.steer_limited, self.desired_curvature, + self.calibrated_pose) # TODO what if not available # Ensure no NaNs/Infs for p in ACTUATOR_FIELDS: @@ -562,53 +127,37 @@ class Controls: return CC, lac_log - def update_alerts(self, CS): - clear_event_types = set() - if ET.WARNING not in self.state_machine.current_alert_types: - clear_event_types.add(ET.WARNING) - if self.enabled: - clear_event_types.add(ET.NO_ENTRY) + def publish(self, CC, lac_log): + CS = self.sm['carState'] - pers = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}[self.personality] - alerts = self.events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric, - self.state_machine.soft_disable_timer, pers]) - self.AM.add_many(self.sm.frame, alerts) - self.AM.process_alerts(self.sm.frame, clear_event_types) - - def publish_carControl(self, CS, CC, lac_log): # Orientation and angle rates can be useful for carcontroller # Only calibrated (car) frame is relevant for the carcontroller if self.calibrated_pose is not None: CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() - CC.cruiseControl.override = self.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl - CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) - if self.joystick_mode and self.sm.recv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]: - CC.cruiseControl.cancel = True + CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl + CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise) speeds = self.sm['longitudinalPlan'].speeds if len(speeds): - CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1 + CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1 hudControl = CC.hudControl hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS) - hudControl.speedVisible = self.enabled - hudControl.lanesVisible = self.enabled + hudControl.speedVisible = CC.enabled + hudControl.lanesVisible = CC.enabled hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead - hudControl.leadDistanceBars = self.personality + 1 + hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1 + hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual hudControl.rightLaneVisible = True hudControl.leftLaneVisible = True - - if self.is_ldw_enabled and self.sm.valid['driverAssistance']: + if 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 - - if not self.CP.passive and self.initialized: + if self.sm['selfdriveState'].active: CO = self.sm['carOutput'] if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ @@ -616,6 +165,8 @@ class Controls: else: self.steer_limited = abs(CC.actuators.steer - CO.actuatorsOutput.steer) > 1e-2 + # TODO: both controlsState and carControl valids should be set by + # sm.all_checks(), but this creates a circular dependency # controlsState dat = messaging.new_message('controlsState') @@ -633,14 +184,11 @@ class Controls: cs.upAccelCmd = float(self.LoC.pid.p) cs.uiAccelCmd = float(self.LoC.pid.i) cs.ufAccelCmd = float(self.LoC.pid.f) - cs.cumLagMs = -self.rk.remaining * 1000. cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or - (self.state_machine.state == State.softDisabling)) + (self.sm['selfdriveState'].state == State.softDisabling)) lat_tuning = self.CP.lateralTuning.which() - if self.joystick_mode: - cs.lateralControlState.debugState = lac_log - elif self.CP.steerControlType == car.CarParams.SteerControlType.angle: + if self.CP.steerControlType == car.CarParams.SteerControlType.angle: cs.lateralControlState.angleState = lac_log elif lat_tuning == 'pid': cs.lateralControlState.pidState = lac_log @@ -655,84 +203,18 @@ class Controls: cc_send.carControl = CC self.pm.send('carControl', cc_send) - def publish_selfdriveState(self, CS): - # selfdriveState - ss_msg = messaging.new_message('selfdriveState') - ss_msg.valid = CS.canValid - ss = ss_msg.selfdriveState - if self.AM.current_alert: - ss.alertText1 = self.AM.current_alert.alert_text_1 - ss.alertText2 = self.AM.current_alert.alert_text_2 - ss.alertSize = self.AM.current_alert.alert_size - ss.alertStatus = self.AM.current_alert.alert_status - ss.alertType = self.AM.current_alert.alert_type - ss.alertSound = self.AM.current_alert.audible_alert - ss.enabled = self.enabled - ss.active = self.active - ss.state = self.state_machine.state - ss.engageable = not self.events.contains(ET.NO_ENTRY) - ss.experimentalMode = self.experimental_mode - ss.personality = self.personality - self.pm.send('selfdriveState', ss_msg) - - # onroadEvents - logged every second or on change - if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): - ce_send = messaging.new_message('onroadEvents', len(self.events)) - ce_send.valid = True - ce_send.onroadEvents = self.events.to_msg() - self.pm.send('onroadEvents', ce_send) - self.events_prev = self.events.names.copy() - - def step(self): - CS = self.data_sample() - self.update_events(CS) - if not self.CP.passive and self.initialized: - self.enabled, self.active = self.state_machine.update(self.events) - self.update_alerts(CS) - - # Compute actuators (runs PID loops and lateral MPC) - CC, lac_log = self.state_control(CS) - - # Publish data - self.publish_carControl(CS, CC, lac_log) - self.publish_selfdriveState(CS) - - self.CS_prev = CS - self.CC_prev = CC - self.lac_log_prev = lac_log - - def read_personality_param(self): - try: - return int(self.params.get('LongitudinalPersonality')) - except (ValueError, TypeError): - return log.LongitudinalPersonality.standard - - def params_thread(self, evt): - while not evt.is_set(): - self.is_metric = self.params.get_bool("IsMetric") - self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl - self.personality = self.read_personality_param() - if self.CP.notCar: - self.joystick_mode = self.params.get_bool("JoystickDebugMode") - time.sleep(0.1) - - def controlsd_thread(self): - e = threading.Event() - t = threading.Thread(target=self.params_thread, args=(e, )) - try: - t.start() - while True: - self.step() - self.rk.monitor_time() - finally: - e.set() - t.join() - + def run(self): + rk = Ratekeeper(100, print_delay_threshold=None) + while True: + self.update() + CC, lac_log = self.state_control() + self.publish(CC, lac_log) + rk.keep_time() def main(): config_realtime_process(4, Priority.CTRL_HIGH) controls = Controls() - controls.controlsd_thread() + controls.run() if __name__ == "__main__": diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 35810c4832..64cbf473d6 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,9 +1,6 @@ -from cereal import car, log +from cereal import log from openpilot.common.numpy_fast import clip from openpilot.common.realtime import DT_CTRL -from openpilot.system.version import get_build_metadata - -EventName = car.OnroadEvent.EventName MIN_SPEED = 1.0 CONTROL_N = 17 @@ -29,20 +26,3 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) return float(vel_err) return 0.0 - - -def get_startup_event(car_recognized, controller_available, fw_seen): - build_metadata = get_build_metadata() - if build_metadata.openpilot.comma_remote and build_metadata.tested_channel: - event = EventName.startup - else: - event = EventName.startupMaster - - if not car_recognized: - if fw_seen: - event = EventName.startupNoCar - else: - event = EventName.startupNoFw - elif car_recognized and not controller_available: - event = EventName.startupNoControl - return event diff --git a/selfdrive/controls/tests/test_startup.py b/selfdrive/controls/tests/test_startup.py deleted file mode 100644 index 077d4ae93a..0000000000 --- a/selfdrive/controls/tests/test_startup.py +++ /dev/null @@ -1,121 +0,0 @@ -import os -from parameterized import parameterized - -from cereal import log, car -import cereal.messaging as messaging -from opendbc.car.fingerprints import _FINGERPRINTS -from opendbc.car.toyota.values import CAR as TOYOTA -from opendbc.car.mazda.values import CAR as MAZDA -from openpilot.common.params import Params -from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp -from openpilot.selfdrive.controls.lib.events import EVENT_NAME -from openpilot.system.manager.process_config import managed_processes - -EventName = car.OnroadEvent.EventName -Ecu = car.CarParams.Ecu - -COROLLA_FW_VERSIONS = [ - (Ecu.engine, 0x7e0, None, b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.abs, 0x7b0, None, b'F152602190\x00\x00\x00\x00\x00\x00'), - (Ecu.eps, 0x7a1, None, b'8965B02181\x00\x00\x00\x00\x00\x00'), - (Ecu.fwdRadar, 0x750, 0xf, b'8821F4702100\x00\x00\x00\x00'), - (Ecu.fwdCamera, 0x750, 0x6d, b'8646F0201101\x00\x00\x00\x00'), - (Ecu.dsu, 0x791, None, b'881510201100\x00\x00\x00\x00'), -] -COROLLA_FW_VERSIONS_FUZZY = COROLLA_FW_VERSIONS[:-1] + [(Ecu.dsu, 0x791, None, b'xxxxxx')] -COROLLA_FW_VERSIONS_NO_DSU = COROLLA_FW_VERSIONS[:-1] - -CX5_FW_VERSIONS = [ - (Ecu.engine, 0x7e0, None, b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.abs, 0x760, None, b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.eps, 0x730, None, b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.fwdRadar, 0x764, None, b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.fwdCamera, 0x706, None, b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.transmission, 0x7e1, None, b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), -] - - -@parameterized.expand([ - # TODO: test EventName.startup for release branches - - # officially supported car - (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"), - (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"), - - # dashcamOnly car - (EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"), - (EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"), - - # unrecognized car with no fw - (EventName.startupNoFw, None, None, ""), - (EventName.startupNoFw, None, None, ""), - - # unrecognized car - (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), - (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), - - # fuzzy match - (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), - (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), -]) -def test_startup_alert(expected_event, car_model, fw_versions, brand): - controls_sock = messaging.sub_sock("selfdriveState") - pm = messaging.PubMaster(['can', 'pandaStates']) - - params = Params() - params.put_bool("OpenpilotEnabledToggle", True) - - # Build capnn version of FW array - if fw_versions is not None: - car_fw = [] - cp = car.CarParams.new_message() - for ecu, addr, subaddress, version in fw_versions: - f = car.CarParams.CarFw.new_message() - f.ecu = ecu - f.address = addr - f.fwVersion = version - f.brand = brand - - if subaddress is not None: - f.subAddress = subaddress - - car_fw.append(f) - cp.carVin = "1" * 17 - cp.carFw = car_fw - params.put("CarParamsCache", cp.to_bytes()) - else: - os.environ['SKIP_FW_QUERY'] = '1' - - managed_processes['controlsd'].start() - managed_processes['card'].start() - - assert pm.wait_for_readers_to_update('can', 5) - pm.send('can', can_list_to_can_capnp([[0, b"", 0]])) - - assert pm.wait_for_readers_to_update('pandaStates', 5) - msg = messaging.new_message('pandaStates', 1) - msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno - pm.send('pandaStates', msg) - - # fingerprint - if (car_model is None) or (fw_versions is not None): - finger = {addr: 1 for addr in range(1, 100)} - else: - finger = _FINGERPRINTS[car_model][0] - - msgs = [[addr, b'\x00'*length, 0] for addr, length in finger.items()] - for _ in range(1000): - # card waits for pandad to echo back that it has changed the multiplexing mode - if not params.get_bool("ObdMultiplexingChanged"): - params.put_bool("ObdMultiplexingChanged", True) - - pm.send('can', can_list_to_can_capnp(msgs)) - assert pm.wait_for_readers_to_update('can', 5, dt=0.001), f"step: {_}" - - ctrls = messaging.drain_sock(controls_sock) - if len(ctrls): - event_name = ctrls[0].selfdriveState.alertType.split("/")[0] - assert EVENT_NAME[expected_event] == event_name, f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}" - break - else: - raise Exception(f"failed to fingerprint {car_model}") diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index fbac65cac1..ec32306436 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -6,8 +6,8 @@ from cereal import car, log import cereal.messaging as messaging from opendbc.car.honda.interface import CarInterface from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.controls.lib.events import ET, Events -from openpilot.selfdrive.controls.lib.alertmanager import AlertManager +from openpilot.selfdrive.selfdrived.events import ET, Events +from openpilot.selfdrive.selfdrived.alertmanager import AlertManager from openpilot.system.manager.process_config import managed_processes EventName = car.OnroadEvent.EventName diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index f3dbbd5306..2a7979221a 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -2,7 +2,7 @@ from math import atan2 from cereal import car import cereal.messaging as messaging -from openpilot.selfdrive.controls.lib.events import Events +from openpilot.selfdrive.selfdrived.events import Events from openpilot.common.numpy_fast import interp from openpilot.common.realtime import DT_DMON from openpilot.common.filter_simple import FirstOrderFilter diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/selfdrived/alertmanager.py similarity index 91% rename from selfdrive/controls/lib/alertmanager.py rename to selfdrive/selfdrived/alertmanager.py index a3b8684871..47d7c29420 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/selfdrived/alertmanager.py @@ -6,10 +6,10 @@ from dataclasses import dataclass from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params -from openpilot.selfdrive.controls.lib.events import Alert +from openpilot.selfdrive.selfdrived.events import Alert -with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f: +with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f: OFFROAD_ALERTS = json.load(f) diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/selfdrived/alerts_offroad.json similarity index 100% rename from selfdrive/controls/lib/alerts_offroad.json rename to selfdrive/selfdrived/alerts_offroad.json diff --git a/selfdrive/controls/lib/events.py b/selfdrive/selfdrived/events.py similarity index 98% rename from selfdrive/controls/lib/events.py rename to selfdrive/selfdrived/events.py index d27f02794b..b79a716097 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/selfdrived/events.py @@ -318,10 +318,11 @@ def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - axes = sm['testJoystick'].axes - gb, steer = list(axes)[:2] if len(axes) else (0., 0.) - vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" - return NormalPermanentAlert("Joystick Mode", vals) + # TODO: add some info back + #axes = sm['testJoystick'].axes + #gb, steer = list(axes)[:2] if len(axes) else (0., 0.) + #vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" + return NormalPermanentAlert("Joystick Mode", "") def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: personality = str(personality).title() @@ -342,7 +343,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.PERMANENT: NormalPermanentAlert("Joystick Mode"), }, - EventName.controlsInitializing: { + EventName.selfdriveInitializing: { ET.NO_ENTRY: NoEntryAlert("System Initializing"), }, @@ -773,9 +774,9 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.NO_ENTRY: NoEntryAlert("Low Communication Rate Between Processes"), }, - EventName.controlsdLagging: { - ET.SOFT_DISABLE: soft_disable_alert("Controls Lagging"), - ET.NO_ENTRY: NoEntryAlert("Controls Process Lagging: Reboot Your Device"), + EventName.selfdrivedLagging: { + ET.SOFT_DISABLE: soft_disable_alert("System Lagging"), + ET.NO_ENTRY: NoEntryAlert("Selfdrive Process Lagging: Reboot Your Device"), }, # Thrown when manager detects a service exited unexpectedly while driving diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py new file mode 100755 index 0000000000..2abc87ceb9 --- /dev/null +++ b/selfdrive/selfdrived/selfdrived.py @@ -0,0 +1,518 @@ +#!/usr/bin/env python3 +import os +import time +import threading + +import cereal.messaging as messaging + +from cereal import car, log +from msgq.visionipc import VisionIpcClient, VisionStreamType + + +from openpilot.common.git import get_short_branch +from openpilot.common.params import Params +from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL +from openpilot.common.swaglog import cloudlog +from openpilot.common.gps import get_gps_location_service + +from openpilot.selfdrive.selfdrived.events import Events, ET +from openpilot.selfdrive.selfdrived.state import StateMachine +from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert + +from openpilot.system.hardware import HARDWARE +from openpilot.system.version import get_build_metadata + +REPLAY = "REPLAY" in os.environ +SIMULATION = "SIMULATION" in os.environ +TESTING_CLOSET = "TESTING_CLOSET" in os.environ +IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"} + +ThermalStatus = log.DeviceState.ThermalStatus +State = log.SelfdriveState.OpenpilotState +PandaType = log.PandaState.PandaType +LaneChangeState = log.LaneChangeState +LaneChangeDirection = log.LaneChangeDirection +EventName = car.OnroadEvent.EventName +ButtonType = car.CarState.ButtonEvent.Type +SafetyModel = car.CarParams.SafetyModel + +IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) +CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError} + +def get_startup_event(car_recognized, controller_available, fw_seen): + build_metadata = get_build_metadata() + if build_metadata.openpilot.comma_remote and build_metadata.tested_channel: + event = EventName.startup + else: + event = EventName.startupMaster + + if not car_recognized: + if fw_seen: + event = EventName.startupNoCar + else: + event = EventName.startupNoFw + elif car_recognized and not controller_available: + event = EventName.startupNoControl + return event + + +class SelfdriveD: + def __init__(self): + self.params = Params() + + # Ensure the current branch is cached, otherwise the first cycle lags + self.branch = get_short_branch() + + cloudlog.info("selfdrived is waiting for CarParams") + self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams) + cloudlog.info("selfdrived got CarParams") + + # Setup sockets + self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents']) + + self.gps_location_service = get_gps_location_service(self.params) + self.gps_packets = [self.gps_location_service] + self.sensor_packets = ["accelerometer", "gyroscope"] + self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] + + self.log_sock = messaging.sub_sock('androidLog') + + # 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 + if SIMULATION: + ignore += ['driverCameraState', 'managerState'] + if REPLAY: + # no vipc in replay will make them ignored anyways + ignore += ['roadCameraState', 'wideRoadCameraState'] + self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', + 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', + 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', + 'controlsState', 'carControl', 'driverAssistance'] + \ + self.camera_packets + self.sensor_packets + self.gps_packets, + ignore_alive=ignore, ignore_avg_freq=ignore+['radarState',], + frequency=int(1/DT_CTRL)) + + # read params + self.is_metric = self.params.get_bool("IsMetric") + self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") + + # detect sound card presence and ensure successful init + sounds_available = HARDWARE.get_sound_card_online() + + car_recognized = self.CP.carName != 'mock' + + # cleanup old params + if not self.CP.experimentalLongitudinalAvailable: + self.params.remove("ExperimentalLongitudinalEnabled") + if not self.CP.openpilotLongitudinalControl: + self.params.remove("ExperimentalMode") + + self.CS_prev = car.CarState.new_message() + self.AM = AlertManager() + self.events = Events() + + self.initialized = False + self.enabled = False + self.active = False + self.mismatch_counter = 0 + self.cruise_mismatch_counter = 0 + self.last_steering_pressed_frame = 0 + self.distance_traveled = 0 + self.last_functional_fan_frame = 0 + self.events_prev = [] + self.logged_comm_issue = None + self.not_running_prev = None + self.experimental_mode = False + self.personality = self.read_personality_param() + self.recalibrating_seen = False + self.state_machine = StateMachine() + + self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0) + + if not sounds_available: + self.events.add(EventName.soundsUnavailable, static=True) + if not car_recognized: + self.events.add(EventName.carUnrecognized, static=True) + if len(self.CP.carFw) > 0: + set_offroad_alert("Offroad_CarUnrecognized", True) + else: + set_offroad_alert("Offroad_NoFirmware", True) + elif self.CP.passive: + self.events.add(EventName.dashcamMode, static=True) + + # selfdrived is driven by carState, expected at 100Hz + self.rk = Ratekeeper(100, print_delay_threshold=None) + + def set_initial_state(self): + if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']): + self.state_machine.state = State.enabled + + def update_events(self, CS): + """Compute onroadEvents from carState""" + + self.events.clear() + + if self.sm['controlsState'].lateralControlState.which() == 'debugState': + self.events.add(EventName.joystickDebug) + self.startup_event = None + + # Add startup event + if self.startup_event is not None: + self.events.add(self.startup_event) + self.startup_event = None + + # Don't add any more events if not initialized + if not self.initialized: + self.events.add(EventName.selfdriveInitializing) + return + + # no more events while in dashcam mode + if self.CP.passive: + return + + # Block resume if cruise never previously enabled + resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) + if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed: + self.events.add(EventName.resumeBlocked) + + if not self.CP.notCar: + self.events.add_from_msg(self.sm['driverMonitoringState'].events) + + # Add car events, ignore if CAN isn't valid + if CS.canValid: + self.events.add_from_msg(CS.events) + + # Create events for temperature, disk space, and memory + if self.sm['deviceState'].thermalStatus >= ThermalStatus.red: + self.events.add(EventName.overheat) + if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION: + self.events.add(EventName.outOfSpace) + if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION: + self.events.add(EventName.lowMemory) + + # Alert if fan isn't spinning for 5 seconds + if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown: + if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50: + # allow enough time for the fan controller in the panda to recover from stalls + if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0: + self.events.add(EventName.fanMalfunction) + else: + self.last_functional_fan_frame = self.sm.frame + + # Handle calibration status + cal_status = self.sm['liveCalibration'].calStatus + if cal_status != log.LiveCalibrationData.Status.calibrated: + if cal_status == log.LiveCalibrationData.Status.uncalibrated: + self.events.add(EventName.calibrationIncomplete) + elif cal_status == log.LiveCalibrationData.Status.recalibrating: + if not self.recalibrating_seen: + set_offroad_alert("Offroad_Recalibration", True) + self.recalibrating_seen = True + self.events.add(EventName.calibrationRecalibrating) + else: + self.events.add(EventName.calibrationInvalid) + + # Lane departure warning + 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: + direction = self.sm['modelV2'].meta.laneChangeDirection + if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ + (CS.rightBlindspot and direction == LaneChangeDirection.right): + self.events.add(EventName.laneChangeBlocked) + else: + if direction == LaneChangeDirection.left: + self.events.add(EventName.preLaneChangeLeft) + else: + self.events.add(EventName.preLaneChangeRight) + 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 + if i < len(self.CP.safetyConfigs): + safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \ + pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \ + pandaState.alternativeExperience != self.CP.alternativeExperience + else: + safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES + + # safety mismatch allows some time for pandad to set the safety mode and publish it back from panda + if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200: + self.events.add(EventName.controlsMismatch) + + if log.PandaState.FaultType.relayMalfunction in pandaState.faults: + self.events.add(EventName.relayMalfunction) + + # Handle HW and system malfunctions + # Order is very intentional here. Be careful when modifying this. + # All events here should at least have NO_ENTRY and SOFT_DISABLE. + num_events = len(self.events) + + not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning} + if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES): + self.events.add(EventName.processNotRunning) + if not_running != self.not_running_prev: + cloudlog.event("process_not_running", not_running=not_running, error=True) + self.not_running_prev = not_running + else: + if not SIMULATION and not self.rk.lagging: + if not self.sm.all_alive(self.camera_packets): + self.events.add(EventName.cameraMalfunction) + elif not self.sm.all_freq_ok(self.camera_packets): + self.events.add(EventName.cameraFrameRate) + if not REPLAY and self.rk.lagging: + self.events.add(EventName.selfdrivedLagging) + if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) and not self.sm.all_checks(['radarState'])): + self.events.add(EventName.radarFault) + if not self.sm.valid['pandaStates']: + self.events.add(EventName.usbError) + if CS.canTimeout: + self.events.add(EventName.canBusMissing) + elif not CS.canValid: + self.events.add(EventName.canError) + + # generic catch-all. ideally, a more specific event should be added above instead + has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE)) + no_system_errors = (not has_disable_events) or (len(self.events) == num_events) + if not self.sm.all_checks() and no_system_errors: + if not self.sm.all_alive(): + self.events.add(EventName.commIssue) + elif not self.sm.all_freq_ok(): + self.events.add(EventName.commIssueAvgFreq) + else: + self.events.add(EventName.commIssue) + + logs = { + 'invalid': [s for s, valid in self.sm.valid.items() if not valid], + 'not_alive': [s for s, alive in self.sm.alive.items() if not alive], + 'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], + } + if logs != self.logged_comm_issue: + cloudlog.event("commIssue", error=True, **logs) + self.logged_comm_issue = logs + else: + self.logged_comm_issue = None + + if not self.CP.notCar: + if not self.sm['livePose'].posenetOK: + self.events.add(EventName.posenetInvalid) + if not self.sm['livePose'].inputsOK: + self.events.add(EventName.locationdTemporaryError) + if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY): + self.events.add(EventName.paramsdTemporaryError) + + # conservative HW alert. if the data or frequency are off, locationd will throw an error + if any((self.sm.frame - self.sm.recv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets): + self.events.add(EventName.sensorDataInvalid) + + if not REPLAY: + # Check for mismatch between openpilot and car's PCM + cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) + self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0 + if self.cruise_mismatch_counter > int(6. / DT_CTRL): + self.events.add(EventName.cruiseMismatch) + + # Send a "steering required alert" if saturation count has reached the limit + if CS.steeringPressed: + self.last_steering_pressed_frame = self.sm.frame + recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0 + lac = getattr(self.sm['controlsState'].lateralControlState, self.sm['controlsState'].lateralControlState.which()) + if lac.active and not recent_steer_pressed and not self.CP.notCar: + if self.CP.lateralTuning.which() == 'torque': + undershooting = abs(lac.desiredLateralAccel) / abs(1e-3 + lac.actualLateralAccel) > 1.2 + turning = abs(lac.desiredLateralAccel) > 1.0 + good_speed = CS.vEgo > 5 + max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99 + if undershooting and turning and good_speed and max_torque: + self.events.add(EventName.steerSaturated) + elif lac.saturated: + # TODO probably should not use dpath_points but curvature + dpath_points = self.sm['modelV2'].position.y + if len(dpath_points): + # Check if we deviated from the path + # TODO use desired vs actual curvature + if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + steering_value = self.sm['carControl'].actuators.steeringAngleDeg + else: + steering_value = self.sm['carControl'].actuators.steer + + left_deviation = steering_value > 0 and dpath_points[0] < -0.20 + right_deviation = steering_value < 0 and dpath_points[0] > 0.20 + if left_deviation or right_deviation: + self.events.add(EventName.steerSaturated) + + # Check for FCW + stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25 + model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking + planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled + if (planner_fcw or model_fcw) and not self.CP.notCar: + self.events.add(EventName.fcw) + + # Camera errors from the kernel + for m in messaging.drain_sock(self.log_sock, wait_for_one=False): + try: + msg = m.androidLog.message + if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")): + csid = msg.split("CSID:")[-1].split(" ")[0] + evt = CSID_MAP.get(csid, None) + if evt is not None: + self.events.add(evt) + except UnicodeDecodeError: + pass + + # TODO: fix simulator + if not SIMULATION or REPLAY: + # Not show in first 1.5 km to allow for driving out of garage. This event shows after 5 minutes + gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0 + if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500): + self.events.add(EventName.noGps) + if gps_ok: + self.distance_traveled = 0 + self.distance_traveled += CS.vEgo * DT_CTRL + + if self.sm['modelV2'].frameDropPerc > 20: + self.events.add(EventName.modeldLagging) + + # decrement personality on distance button press + if self.CP.openpilotLongitudinalControl: + if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents): + self.personality = (self.personality - 1) % 3 + self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) + self.events.add(EventName.personalityChanged) + + def data_sample(self): + car_state = messaging.recv_one(self.car_state_sock) + CS = car_state.carState if car_state else self.CS_prev + + self.sm.update(0) + + if not self.initialized: + all_valid = CS.canValid and self.sm.all_checks() + timed_out = self.sm.frame * DT_CTRL > 6. + if all_valid or timed_out or (SIMULATION and not REPLAY): + available_streams = VisionIpcClient.available_streams("camerad", block=False) + if VisionStreamType.VISION_STREAM_ROAD not in available_streams: + self.sm.ignore_alive.append('roadCameraState') + if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams: + self.sm.ignore_alive.append('wideRoadCameraState') + + self.initialized = True + self.set_initial_state() + + cloudlog.event( + "selfdrived.initialized", + dt=self.sm.frame*DT_CTRL, + timeout=timed_out, + canValid=CS.canValid, + invalid=[s for s, valid in self.sm.valid.items() if not valid], + not_alive=[s for s, alive in self.sm.alive.items() if not alive], + not_freq_ok=[s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], + error=True, + ) + + # When the panda and selfdrived do not agree on controls_allowed + # we want to disengage openpilot. However the status from the panda goes through + # another socket other than the CAN messages and one can arrive earlier than the other. + # Therefore we allow a mismatch for two samples, then we trigger the disengagement. + if not self.enabled: + self.mismatch_counter = 0 + + # All pandas not in silent mode must have controlsAllowed when openpilot is enabled + if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates'] + if ps.safetyModel not in IGNORED_SAFETY_MODES): + self.mismatch_counter += 1 + + return CS + + def update_alerts(self, CS): + clear_event_types = set() + if ET.WARNING not in self.state_machine.current_alert_types: + clear_event_types.add(ET.WARNING) + if self.enabled: + clear_event_types.add(ET.NO_ENTRY) + + pers = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}[self.personality] + alerts = self.events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric, + self.state_machine.soft_disable_timer, pers]) + self.AM.add_many(self.sm.frame, alerts) + self.AM.process_alerts(self.sm.frame, clear_event_types) + + def publish_selfdriveState(self, CS): + # selfdriveState + ss_msg = messaging.new_message('selfdriveState') + ss_msg.valid = True + ss = ss_msg.selfdriveState + if self.AM.current_alert: + ss.alertText1 = self.AM.current_alert.alert_text_1 + ss.alertText2 = self.AM.current_alert.alert_text_2 + ss.alertSize = self.AM.current_alert.alert_size + ss.alertStatus = self.AM.current_alert.alert_status + ss.alertType = self.AM.current_alert.alert_type + ss.alertSound = self.AM.current_alert.audible_alert + ss.enabled = self.enabled + ss.active = self.active + ss.state = self.state_machine.state + ss.engageable = not self.events.contains(ET.NO_ENTRY) + ss.experimentalMode = self.experimental_mode + ss.personality = self.personality + self.pm.send('selfdriveState', ss_msg) + + # onroadEvents - logged every second or on change + if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): + ce_send = messaging.new_message('onroadEvents', len(self.events)) + ce_send.valid = True + ce_send.onroadEvents = self.events.to_msg() + self.pm.send('onroadEvents', ce_send) + self.events_prev = self.events.names.copy() + + def step(self): + CS = self.data_sample() + self.update_events(CS) + if not self.CP.passive and self.initialized: + self.enabled, self.active = self.state_machine.update(self.events) + self.update_alerts(CS) + + self.publish_selfdriveState(CS) + + self.CS_prev = CS + + def read_personality_param(self): + try: + return int(self.params.get('LongitudinalPersonality')) + except (ValueError, TypeError): + return log.LongitudinalPersonality.standard + + def params_thread(self, evt): + while not evt.is_set(): + self.is_metric = self.params.get_bool("IsMetric") + self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + self.personality = self.read_personality_param() + time.sleep(0.1) + + def run(self): + e = threading.Event() + t = threading.Thread(target=self.params_thread, args=(e, )) + try: + t.start() + while True: + self.step() + self.rk.monitor_time() + finally: + e.set() + t.join() + + +def main(): + config_realtime_process(4, Priority.CTRL_HIGH) + s = SelfdriveD() + s.run() + +if __name__ == "__main__": + main() diff --git a/selfdrive/controls/lib/selfdrive.py b/selfdrive/selfdrived/state.py similarity index 98% rename from selfdrive/controls/lib/selfdrive.py rename to selfdrive/selfdrived/state.py index adbcd1d926..073ddb56eb 100644 --- a/selfdrive/controls/lib/selfdrive.py +++ b/selfdrive/selfdrived/state.py @@ -1,5 +1,5 @@ from cereal import log -from openpilot.selfdrive.controls.lib.events import Events, ET +from openpilot.selfdrive.selfdrived.events import Events, ET from openpilot.common.realtime import DT_CTRL State = log.SelfdriveState.OpenpilotState diff --git a/selfdrive/controls/lib/tests/test_alertmanager.py b/selfdrive/selfdrived/tests/test_alertmanager.py similarity index 89% rename from selfdrive/controls/lib/tests/test_alertmanager.py rename to selfdrive/selfdrived/tests/test_alertmanager.py index bc0da0da8c..20bf4e8903 100644 --- a/selfdrive/controls/lib/tests/test_alertmanager.py +++ b/selfdrive/selfdrived/tests/test_alertmanager.py @@ -1,7 +1,7 @@ import random -from openpilot.selfdrive.controls.lib.events import Alert, EVENTS -from openpilot.selfdrive.controls.lib.alertmanager import AlertManager +from openpilot.selfdrive.selfdrived.events import Alert, EVENTS +from openpilot.selfdrive.selfdrived.alertmanager import AlertManager class TestAlertManager: diff --git a/selfdrive/controls/tests/test_alerts.py b/selfdrive/selfdrived/tests/test_alerts.py similarity index 94% rename from selfdrive/controls/tests/test_alerts.py rename to selfdrive/selfdrived/tests/test_alerts.py index 52d71dc22d..a5249cc03f 100644 --- a/selfdrive/controls/tests/test_alerts.py +++ b/selfdrive/selfdrived/tests/test_alerts.py @@ -8,13 +8,13 @@ from cereal import log, car from cereal.messaging import SubMaster from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params -from openpilot.selfdrive.controls.lib.events import Alert, EVENTS, ET -from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert +from openpilot.selfdrive.selfdrived.events import Alert, EVENTS, ET +from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS AlertSize = log.SelfdriveState.AlertSize -OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json") +OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json") # TODO: add callback alerts ALERTS = [] diff --git a/selfdrive/controls/tests/test_state_machine.py b/selfdrive/selfdrived/tests/test_state_machine.py similarity index 97% rename from selfdrive/controls/tests/test_state_machine.py rename to selfdrive/selfdrived/tests/test_state_machine.py index d2117b3342..4274ba9f67 100644 --- a/selfdrive/controls/tests/test_state_machine.py +++ b/selfdrive/selfdrived/tests/test_state_machine.py @@ -1,7 +1,7 @@ from cereal import log from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.selfdrive import StateMachine, SOFT_DISABLE_TIME -from openpilot.selfdrive.controls.lib.events import Events, ET, EVENTS, NormalPermanentAlert +from openpilot.selfdrive.selfdrived.events import Events, ET, EVENTS, NormalPermanentAlert State = log.SelfdriveState.OpenpilotState diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index 14af9672d0..13d51a636f 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -141,7 +141,7 @@ def format_diff(results, log_paths, ref_commit): if __name__ == "__main__": log1 = list(LogReader(sys.argv[1])) log2 = list(LogReader(sys.argv[2])) - ignore_fields = sys.argv[3:] or ["logMonoTime", "controlsState.cumLagMs"] + ignore_fields = sys.argv[3:] or ["logMonoTime"] results = {"segment": {"proc": compare_logs(log1, log2, ignore_fields)}} log_paths = {"segment": {"proc": {"ref": sys.argv[1], "new": sys.argv[2]}}} diff_short, diff_long, failed = format_diff(results, log_paths, None) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 39fec7b669..a2ba4a5787 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -373,7 +373,7 @@ def get_car_params_callback(rc, pm, msgs, fingerprint): params.put("CarParams", convert_to_capnp(CP).to_bytes()) -def controlsd_rcv_callback(msg, cfg, frame): +def selfdrived_rcv_callback(msg, cfg, frame): return (frame - 1) == 0 or msg.which() == 'carState' @@ -452,7 +452,7 @@ class FrequencyBasedRcvCallback: return bool(len(resp_sockets)) -def controlsd_config_callback(params, cfg, lr): +def selfdrived_config_callback(params, cfg, lr): ublox = params.get_bool("UbloxAvailable") sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", }) @@ -461,22 +461,33 @@ def controlsd_config_callback(params, cfg, lr): CONFIGS = [ ProcessConfig( - proc_name="controlsd", + proc_name="selfdrived", pubs=[ "carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState", "longitudinalPlan", "livePose", "liveParameters", "radarState", "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", - "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput", - "gpsLocationExternal", "gpsLocation", "driverAssistance" + "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput", + "gpsLocationExternal", "gpsLocation", "controlsState", "carControl", "driverAssistance", ], - subs=["selfdriveState", "controlsState", "carControl", "onroadEvents"], - ignore=["logMonoTime", "controlsState.cumLagMs"], - config_callback=controlsd_config_callback, + subs=["selfdriveState", "onroadEvents"], + ignore=["logMonoTime"], + config_callback=selfdrived_config_callback, init_callback=get_car_params_callback, - should_recv_callback=controlsd_rcv_callback, + should_recv_callback=selfdrived_rcv_callback, tolerance=NUMPY_TOLERANCE, processing_time=0.004, ), + ProcessConfig( + proc_name="controlsd", + pubs=["liveParameters", "liveTorqueParameters", "modelV2", "selfdriveState", + "liveCalibration", "livePose", "longitudinalPlan", "carState", "carOutput", + "driverMonitoringState", "onroadEvents", "driverAssistance"], + subs=["carControl", "controlsState"], + ignore=["logMonoTime", ], + init_callback=get_car_params_callback, + should_recv_callback=MessageBasedRcvCallback("carState"), + tolerance=NUMPY_TOLERANCE, + ), ProcessConfig( proc_name="card", pubs=["pandaStates", "carControl", "onroadEvents", "can"], diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 9ff5403d3c..b866e015ac 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -fc1344b16b802cdb4ec542791f3b4d76a82da68b \ No newline at end of file +eac137f456f25bf138677315b7c4907e2fe9971b \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_fuzzy.py b/selfdrive/test/process_replay/test_fuzzy.py index d938839039..39e3bfb94a 100644 --- a/selfdrive/test/process_replay/test_fuzzy.py +++ b/selfdrive/test/process_replay/test_fuzzy.py @@ -11,7 +11,7 @@ import openpilot.selfdrive.test.process_replay.process_replay as pr # These processes currently fail because of unrealistic data breaking assumptions # that openpilot makes causing error with NaN, inf, int size, array indexing ... # TODO: Make each one testable -NOT_TESTED = ['controlsd', 'card', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld'] +NOT_TESTED = ['selfdrived', 'controlsd', 'card', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld'] TEST_CASES = [(cfg.proc_name, copy.deepcopy(cfg)) for cfg in pr.CONFIGS if cfg.proc_name not in NOT_TESTED] diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 702de4f7d1..50c34a190f 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -12,7 +12,7 @@ from openpilot.common.git import get_commit from openpilot.tools.lib.openpilotci import get_url, upload_file from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, PROC_REPLAY_DIR, FAKEDATA, replay_process, \ - check_openpilot_enabled, check_most_messages_valid + check_most_messages_valid from openpilot.tools.lib.filereader import FileReader from openpilot.tools.lib.logreader import LogReader, save_log @@ -103,10 +103,6 @@ def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=Non except Exception as e: raise Exception("failed on segment: " + segment) from e - # check to make sure openpilot is engaged in the route - if cfg.proc_name == "controlsd": - if not check_openpilot_enabled(log_msgs): - return f"Route did not enable at all or for long enough: {new_log_path}", log_msgs if not check_most_messages_valid(log_msgs): return f"Route did not have enough valid messages: {new_log_path}", log_msgs diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 7fe26b739f..d4ec3c2fff 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -19,7 +19,7 @@ from cereal.services import SERVICE_LIST from openpilot.common.basedir import BASEDIR from openpilot.common.timeout import Timeout from openpilot.common.params import Params -from openpilot.selfdrive.controls.lib.events import EVENTS, ET +from openpilot.selfdrive.selfdrived.events import EVENTS, ET from openpilot.selfdrive.test.helpers import set_params_enabled, release_only from openpilot.system.hardware import HARDWARE from openpilot.system.hardware.hw import Paths @@ -35,7 +35,8 @@ CPU usage budget MAX_TOTAL_CPU = 260. # total for all 8 cores PROCS = { # Baseline CPU usage by process - "selfdrive.controls.controlsd": 32.0, + "selfdrive.controls.controlsd": 18.0, + "selfdrive.selfdrived.selfdrived": 21.0, "selfdrive.car.card": 30.0, "./loggerd": 14.0, "./encoderd": 17.0, @@ -87,6 +88,7 @@ TIMINGS = { "carControl": [2.5, 0.35], "controlsState": [2.5, 0.35], "longitudinalPlan": [2.5, 0.5], + "driverAssistance": [2.5, 0.5], "roadCameraState": [2.5, 0.35], "driverCameraState": [2.5, 0.35], "modelV2": [2.5, 0.35], diff --git a/selfdrive/test/test_time_to_onroad.py b/selfdrive/test/test_time_to_onroad.py index 58e6cc3ccf..d3692eabe2 100644 --- a/selfdrive/test/test_time_to_onroad.py +++ b/selfdrive/test/test_time_to_onroad.py @@ -20,7 +20,7 @@ def test_time_to_onroad(): proc = subprocess.Popen(["python", manager_path]) start_time = time.monotonic() - sm = messaging.SubMaster(['selfdriveState', 'controlsState', 'deviceState', 'onroadEvents']) + sm = messaging.SubMaster(['selfdriveState', 'deviceState', 'onroadEvents']) try: # wait for onroad. timeout assumes panda is up to date with Timeout(10, "timed out waiting to go onroad"): @@ -34,7 +34,7 @@ def test_time_to_onroad(): while True: sm.update(100) - if sm.seen['onroadEvents'] and not any(EventName.controlsInitializing == e.name for e in sm['onroadEvents']): + if sm.seen['onroadEvents'] and not any(EventName.selfdriveInitializing == e.name for e in sm['onroadEvents']): initialized = True if initialized: @@ -51,7 +51,6 @@ def test_time_to_onroad(): sm.update(100) assert sm.all_alive(), sm.alive assert sm['selfdriveState'].engageable, f"events: {sm['onroadEvents']}" - assert sm['controlsState'].cumLagMs < 10. finally: proc.terminate() if proc.wait(20) is None: diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.cc b/selfdrive/ui/qt/widgets/offroad_alerts.cc index 74ece36d15..f630875978 100644 --- a/selfdrive/ui/qt/widgets/offroad_alerts.cc +++ b/selfdrive/ui/qt/widgets/offroad_alerts.cc @@ -70,7 +70,7 @@ AbstractAlert::AbstractAlert(bool hasRebootBtn, QWidget *parent) : QFrame(parent int OffroadAlert::refresh() { // build widgets for each offroad alert on first refresh if (alerts.empty()) { - QString json = util::read_file("../controls/lib/alerts_offroad.json").c_str(); + QString json = util::read_file("../selfdrived/alerts_offroad.json").c_str(); QJsonObject obj = QJsonDocument::fromJson(json.toUtf8()).object(); // descending sort labels by severity diff --git a/selfdrive/ui/tests/cycle_offroad_alerts.py b/selfdrive/ui/tests/cycle_offroad_alerts.py index 75b19ceb90..e468d88e0e 100755 --- a/selfdrive/ui/tests/cycle_offroad_alerts.py +++ b/selfdrive/ui/tests/cycle_offroad_alerts.py @@ -6,12 +6,12 @@ import json from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params -from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert +from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert if __name__ == "__main__": params = Params() - with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f: + with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f: offroad_alerts = json.load(f) t = 10 if len(sys.argv) < 2 else int(sys.argv[1]) diff --git a/selfdrive/ui/tests/test_ui/run.py b/selfdrive/ui/tests/test_ui/run.py index 2bf30d7b90..5050adfd98 100644 --- a/selfdrive/ui/tests/test_ui/run.py +++ b/selfdrive/ui/tests/test_ui/run.py @@ -17,7 +17,7 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params from openpilot.common.prefix import OpenpilotPrefix from openpilot.common.transformations.camera import CameraConfig, DEVICE_CAMERAS -from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert +from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from openpilot.selfdrive.test.helpers import with_processes from openpilot.selfdrive.test.process_replay.migration import migrate_controlsState from openpilot.tools.lib.logreader import LogReader diff --git a/selfdrive/ui/update_translations.py b/selfdrive/ui/update_translations.py index f13c65fdae..65880bdad9 100755 --- a/selfdrive/ui/update_translations.py +++ b/selfdrive/ui/update_translations.py @@ -16,7 +16,7 @@ def generate_translations_include(): # offroad alerts # TODO translate events from openpilot.selfdrive/controls/lib/events.py content = "// THIS IS AN AUTOGENERATED FILE, PLEASE EDIT alerts_offroad.json\n" - with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f: + with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f: for alert in json.load(f).values(): content += f'QT_TRANSLATE_NOOP("OffroadAlert", R"({alert["text"]})");\n' diff --git a/system/athena/registration.py b/system/athena/registration.py index 06ecd0b9d4..a1e8605a9d 100755 --- a/system/athena/registration.py +++ b/system/athena/registration.py @@ -8,7 +8,7 @@ from datetime import datetime, timedelta, UTC from openpilot.common.api import api_get from openpilot.common.params import Params from openpilot.common.spinner import Spinner -from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert +from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from openpilot.system.hardware import HARDWARE, PC from openpilot.system.hardware.hw import Paths from openpilot.common.swaglog import cloudlog diff --git a/system/camerad/snapshot/snapshot.py b/system/camerad/snapshot/snapshot.py index 4ba38c1df4..b3369891d7 100755 --- a/system/camerad/snapshot/snapshot.py +++ b/system/camerad/snapshot/snapshot.py @@ -10,7 +10,7 @@ from msgq.visionipc import VisionIpcClient, VisionStreamType from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.system.hardware import PC -from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert +from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from openpilot.system.manager.process_config import managed_processes diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index 34e58315d0..9ebb9c9d54 100755 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -16,7 +16,7 @@ from openpilot.common.dict_helpers import strip_deprecated_keys from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params from openpilot.common.realtime import DT_HW -from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert +from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from openpilot.system.hardware import HARDWARE, TICI, AGNOS from openpilot.system.loggerd.config import get_available_percent from openpilot.system.statsd import statlog diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 4688951620..8af376c624 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -64,6 +64,7 @@ procs = [ PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad), + PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad), PythonProcess("card", "selfdrive.car.card", only_onroad), PythonProcess("deleter", "system.loggerd.deleter", always_run), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)), diff --git a/system/updated/updated.py b/system/updated/updated.py index 005c52bc8b..6f9fe173d8 100755 --- a/system/updated/updated.py +++ b/system/updated/updated.py @@ -17,7 +17,7 @@ from openpilot.common.params import Params from openpilot.common.time import system_time_valid from openpilot.common.markdown import parse_markdown from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert +from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from openpilot.system.hardware import AGNOS, HARDWARE from openpilot.system.version import get_build_metadata