diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d9e258f07f07e0..1a2136aa7a38ff 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -117,6 +117,8 @@ def __init__(self, CI=None): 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() @@ -216,16 +218,10 @@ def update_events(self, CS): if self.sm['deviceState'].thermalStatus >= ThermalStatus.red: self.events.add(EventName.overheat) if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION: - # under 7% of space free no enable allowed self.events.add(EventName.outOfSpace) if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION: self.events.add(EventName.lowMemory) - # TODO: enable this once loggerd CPU usage is more reasonable - #cpus = list(self.sm['deviceState'].cpuUsagePercent) - #if max(cpus, default=0) > 95 and not SIMULATION: - # self.events.add(EventName.highCpuUsage) - # 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: @@ -248,6 +244,13 @@ def update_events(self, CS): else: 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) + # Handle lane change if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange: direction = self.sm['modelV2'].meta.laneChangeDirection @@ -348,6 +351,37 @@ def update_events(self, CS): 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 @@ -521,36 +555,6 @@ def state_control(self, CS): lac_log.output = actuators.steer lac_log.saturated = abs(actuators.steer) >= 0.9 - 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 - - # Send a "steering required alert" if saturation count has reached the limit - if lac_log.active and not recent_steer_pressed and not self.CP.notCar: - 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: - lac_log.active and self.events.add(EventName.steerSaturated) - elif lac_log.saturated: - # TODO probably should not use dpath_points but curvature - dpath_points = model_v2.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 = actuators.steeringAngleDeg - else: - steering_value = 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) - # Ensure no NaNs/Infs for p in ACTUATOR_FIELDS: attr = getattr(actuators, p) @@ -563,9 +567,20 @@ def state_control(self, CS): return CC, lac_log - def publish_logs(self, CS, CC, lac_log): - """Send actuators and hud commands to the car, send controlsstate and MPC logging""" + 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_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: @@ -591,26 +606,12 @@ def publish_logs(self, CS, CC, lac_log): hudControl.rightLaneVisible = True hudControl.leftLaneVisible = True - self.ldw.update(self.sm.frame, self.sm['modelV2'], CS, CC) - if self.is_ldw_enabled and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated: + 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.ldw.warning: - self.events.add(EventName.ldw) - 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) - current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types) - if current_alert: - hudControl.visualAlert = current_alert.visual_alert + if self.AM.current_alert: + hudControl.visualAlert = self.AM.current_alert.visual_alert if not self.CP.passive and self.initialized: CO = self.sm['carOutput'] @@ -620,53 +621,57 @@ def publish_logs(self, CS, CC, lac_log): else: self.steer_limited = abs(CC.actuators.steer - CO.actuatorsOutput.steer) > 1e-2 - force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ - (self.state_machine.state == State.softDisabling) - - # Curvature & Steering angle - lp = self.sm['liveParameters'] - - steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) - curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) # controlsState dat = messaging.new_message('controlsState') dat.valid = CS.canValid - controlsState = dat.controlsState - controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] - controlsState.lateralPlanMonoTime = self.sm.logMonoTime['modelV2'] - controlsState.curvature = curvature - controlsState.desiredCurvature = self.desired_curvature - controlsState.longControlState = self.LoC.long_control_state - controlsState.upAccelCmd = float(self.LoC.pid.p) - controlsState.uiAccelCmd = float(self.LoC.pid.i) - controlsState.ufAccelCmd = float(self.LoC.pid.f) - controlsState.cumLagMs = -self.rk.remaining * 1000. - controlsState.forceDecel = bool(force_decel) + cs = dat.controlsState + + lp = self.sm['liveParameters'] + steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) + cs.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) + + cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] + cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2'] + cs.desiredCurvature = self.desired_curvature + cs.longControlState = self.LoC.long_control_state + 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)) lat_tuning = self.CP.lateralTuning.which() if self.joystick_mode: - controlsState.lateralControlState.debugState = lac_log + cs.lateralControlState.debugState = lac_log elif self.CP.steerControlType == car.CarParams.SteerControlType.angle: - controlsState.lateralControlState.angleState = lac_log + cs.lateralControlState.angleState = lac_log elif lat_tuning == 'pid': - controlsState.lateralControlState.pidState = lac_log + cs.lateralControlState.pidState = lac_log elif lat_tuning == 'torque': - controlsState.lateralControlState.torqueState = lac_log + cs.lateralControlState.torqueState = lac_log self.pm.send('controlsState', dat) + # carControl + cc_send = messaging.new_message('carControl') + cc_send.valid = CS.canValid + 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 current_alert: - ss.alertText1 = current_alert.alert_text_1 - ss.alertText2 = current_alert.alert_text_2 - ss.alertSize = current_alert.alert_size - ss.alertStatus = current_alert.alert_status - ss.alertType = current_alert.alert_type - ss.alertSound = current_alert.audible_alert + 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 @@ -683,30 +688,23 @@ def publish_logs(self, CS, CC, lac_log): self.pm.send('onroadEvents', ce_send) self.events_prev = self.events.names.copy() - # carControl - cc_send = messaging.new_message('carControl') - cc_send.valid = CS.canValid - cc_send.carControl = CC - self.pm.send('carControl', cc_send) - def step(self): - # Sample data from sockets and get a carState CS = self.data_sample() - cloudlog.timestamp("Data sampled") - self.update_events(CS) - cloudlog.timestamp("Events updated") - 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_logs(CS, CC, lac_log) + 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: diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index f67e269fa97854..a3b8684871272c 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -34,6 +34,7 @@ def active(self, frame: int) -> bool: class AlertManager: def __init__(self): self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry) + self.current_alert: Alert | None = None def add_many(self, frame: int, alerts: list[Alert]) -> None: for alert in alerts: @@ -44,8 +45,8 @@ def add_many(self, frame: int, alerts: list[Alert]) -> None: min_end_frame = entry.start_frame + alert.duration entry.end_frame = max(frame + 1, min_end_frame) - def process_alerts(self, frame: int, clear_event_types: set) -> Alert | None: - current_alert = AlertEntry() + def process_alerts(self, frame: int, clear_event_types: set): + ae = AlertEntry() for v in self.alerts.values(): if not v.alert: continue @@ -54,8 +55,8 @@ def process_alerts(self, frame: int, clear_event_types: set) -> Alert | None: v.end_frame = -1 # sort by priority first and then by start_frame - greater = current_alert.alert is None or (v.alert.priority, v.start_frame) > (current_alert.alert.priority, current_alert.start_frame) + greater = ae.alert is None or (v.alert.priority, v.start_frame) > (ae.alert.priority, ae.start_frame) if v.active(frame) and greater: - current_alert = v + ae = v - return current_alert.alert + self.current_alert = ae.alert diff --git a/selfdrive/controls/lib/tests/test_alertmanager.py b/selfdrive/controls/lib/tests/test_alertmanager.py index 8b9c18a9d48206..bc0da0da8cc2e7 100644 --- a/selfdrive/controls/lib/tests/test_alertmanager.py +++ b/selfdrive/controls/lib/tests/test_alertmanager.py @@ -32,8 +32,8 @@ def test_duration(self): for frame in range(duration+10): if frame < add_duration: AM.add_many(frame, [alert, ]) - current_alert = AM.process_alerts(frame, {}) + AM.process_alerts(frame, {}) - shown = current_alert is not None + shown = AM.current_alert is not None should_show = frame <= show_duration assert shown == should_show, f"{frame=} {add_duration=} {duration=}" diff --git a/selfdrive/debug/count_events.py b/selfdrive/debug/count_events.py index 59420547570b0e..4095ae3fc1694e 100755 --- a/selfdrive/debug/count_events.py +++ b/selfdrive/debug/count_events.py @@ -33,8 +33,8 @@ if len(events) == 0 or ae != events[-1][1]: events.append((t, ae)) - elif msg.which() == 'controlsState': - at = msg.controlsState.alertType + elif msg.which() == 'selfdriveState': + at = msg.selfdriveState.alertType if "/override" not in at or "lanechange" in at.lower(): if len(alerts) == 0 or alerts[-1][1] != at: alerts.append((t, at)) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 4942a6920ae2d3..90d6c85cb94604 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c2022c388da6eb2e26bb23ad6009be9d5314c0be +fb0827a00b21b97733f8385cf7ce87a21a526f4c \ No newline at end of file