import math import numpy as np import time from pathlib import Path from openpilot.cereal import log, messaging from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import Ratekeeper from openpilot.common.utils import retry from openpilot.common.swaglog import cloudlog from openpilot.system import micd from openpilot.common.hardware import HARDWARE, PC from openpilot.common.params import Params from openpilot.starpilot.aol.wire import INTENT_SERVICE from openpilot.starpilot.audio.alert_volume import AUTO, VOLUMES, effective_volume, read_volume from openpilot.starpilot.audio.axis_alerts import AxisAlerts from openpilot.starpilot.audio.sound_pack import PACK_ROOT, SoundPackLoader, migrate_removed_selection SAMPLE_RATE = 48000 SAMPLE_BUFFER = 4096 # (approx 100ms) MAX_VOLUME = 1.0 MIN_VOLUME = 0.1 STARPILOT_AUTO_MIN = 0.50 ALERT_RAMP_TIME = 4 # seconds to ramp critical alerts to max volume ALERT_MAX_TIME = 8 # seconds before critical alerts switch to the max sound SELFDRIVE_STATE_TIMEOUT = 5 # 5 seconds STREAM_REPORT_INTERVAL = 5.0 FILTER_DT = 1. / (micd.SAMPLE_RATE / micd.FFT_SAMPLES) AMBIENT_DB = 22 # DB where MIN_VOLUME is applied DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied VOLUME_BASE = 20 if HARDWARE.get_device_type() in ("tici", "tizi"): AMBIENT_DB = 30 VOLUME_BASE = 10 AudibleAlert = log.SelfdriveState.AudibleAlert CRITICAL_MAX = -1 # internal sound key, not an AudibleAlert sound_list: dict[int, tuple[str, int | None, float]] = { # AudibleAlert, file name, play count (none for infinite) AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME), AudibleAlert.prompt: ("warning.wav", 1, MAX_VOLUME), AudibleAlert.promptRepeat: ("warning.wav", None, MAX_VOLUME), AudibleAlert.promptDistracted: ("dm_warning.wav", None, MAX_VOLUME), AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME), AudibleAlert.warningSoft: ("critical.wav", None, MAX_VOLUME), AudibleAlert.warningImmediate: ("dm_critical.wav", None, MAX_VOLUME), CRITICAL_MAX: ("dm_critical_max.wav", None, MAX_VOLUME), } ALERT_VOLUME_KEYS = { AudibleAlert.engage: "EngageVolume", AudibleAlert.disengage: "DisengageVolume", AudibleAlert.refuse: "RefuseVolume", AudibleAlert.prompt: "PromptVolume", AudibleAlert.promptRepeat: "PromptVolume", AudibleAlert.preAlert: "PromptDistractedVolume", AudibleAlert.promptDistracted: "PromptDistractedVolume", AudibleAlert.warningSoft: "WarningSoftVolume", AudibleAlert.warningImmediate: "WarningImmediateVolume", } def check_selfdrive_timeout_alert(sm): ss_missing = time.monotonic() - sm.recv_time['selfdriveState'] if ss_missing > SELFDRIVE_STATE_TIMEOUT: if sm['selfdriveState'].enabled and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < 10: return True return False class Soundd: def __init__(self, params=None, pack_root=PACK_ROOT): self.volume_params = params if params is not None else Params() migrate_removed_selection(self.volume_params, pack_root) self.pack_loader = SoundPackLoader(self.volume_params, Path(__file__).resolve().parents[1] / "assets/sounds", pack_root, device_type=HARDWARE.get_device_type()) self.load_sounds() self.saved_volumes = {key: AUTO for key, _, _ in VOLUMES} self.volume_read_at = 0.0 self.current_alert = AudibleAlert.none self.current_sound = AudibleAlert.none self.current_volume = MIN_VOLUME self.current_sound_frame = 0 self.ramp_start_volume = MIN_VOLUME self.ramp_start_time = 0. self.selfdrive_timeout_alert = False self.pending_stop = False self.pending_stream_status = None self.stream_status_count = 0 self.output_underflow_count = 0 self.reported_output_underflows = 0 self.stream_report_at = None self.native_audio = None self.axis_alerts = AxisAlerts() self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) def load_sounds(self): loaded = self.pack_loader.refresh(tuple(dict.fromkeys(spec[0] for spec in sound_list.values())), time.monotonic()) if loaded is not None: self.loaded_sounds = {sound: loaded[spec[0]] for sound, spec in sound_list.items()} def get_sound_data(self, frames): # get "frames" worth of data from the current alert sound, looping when required ret = np.zeros(frames, dtype=np.float32) producing_alert = self.current_alert loaded_sounds = self.loaded_sounds sound_data = None if self.current_alert != AudibleAlert.none: num_loops = sound_list[self.current_alert][1] sound_data = loaded_sounds[self.current_sound] written_frames = 0 current_sound_frame = self.current_sound_frame % len(sound_data) loops = self.current_sound_frame // len(sound_data) while written_frames < frames and (num_loops is None or loops < num_loops): available_frames = sound_data.shape[0] - current_sound_frame frames_to_write = min(available_frames, frames - written_frames) ret[written_frames:written_frames+frames_to_write] = sound_data[current_sound_frame:current_sound_frame+frames_to_write] written_frames += frames_to_write self.current_sound_frame += frames_to_write current_sound_frame = self.current_sound_frame % len(sound_data) loops = self.current_sound_frame // len(sound_data) if self.pending_stop and current_sound_frame == 0: self.current_alert = AudibleAlert.none self.pending_stop = False break key = ALERT_VOLUME_KEYS.get(producing_alert) ambient = self.current_volume if sound_data is not None and self.pack_loader.is_builtin(sound_data): ambient = max(ambient, STARPILOT_AUTO_MIN) gain = ambient if key is None else effective_volume( key, self.saved_volumes.get(key), ambient, immediate_ramp=self.current_volume if producing_alert in (AudibleAlert.warningSoft, AudibleAlert.warningImmediate) else None) return ret * gain def refresh_saved_volumes(self, now: float) -> None: """Read slow Params state from the service loop, never the audio callback.""" if now - self.volume_read_at < 0.25: return self.volume_read_at = now self.saved_volumes = {key: read_volume(self.volume_params, key).value for key, _, _ in VOLUMES} def callback(self, data_out: np.ndarray, frames: int, time, status) -> None: data_out[:frames, 0] = self.get_sound_data(frames) if status: self.pending_stream_status = status self.stream_status_count += 1 self.output_underflow_count += int(bool(getattr(status, "output_underflow", False))) def log_pending_stream_status(self, stream=None) -> None: status = self.pending_stream_status if status is None and self.output_underflow_count == self.reported_output_underflows: return now = time.monotonic() if self.stream_report_at is not None and now - self.stream_report_at < STREAM_REPORT_INTERVAL: return self.pending_stream_status = None self.stream_report_at = now underflows = self.output_underflow_count message = (f"soundd stream diagnostics: status={status} status_callbacks={self.stream_status_count} " + f"output_underflows={underflows}") # Output dropouts must reach errorLogMessage, which is retained in qlog. if underflows > self.reported_output_underflows: cloudlog.error(message) else: cloudlog.warning(message) self.reported_output_underflows = underflows def update_alert(self, new_alert): current_alert_played_once = self.current_alert == AudibleAlert.none or self.current_sound_frame >= len(self.loaded_sounds[self.current_sound]) # let looping sounds finish the current loop instead of cutting off mid tone if new_alert == AudibleAlert.none and self.current_alert != AudibleAlert.none and sound_list[self.current_alert][1] is None: # Complete the current loop even when dismissal arrives during the first # pass. Cutting that pass early creates a step from its current sample to # silence. get_sound_data stops at the next loop endpoint; repeated None # updates leave that endpoint pending rather than restarting playback. completed_boundary = self.current_sound_frame % len(self.loaded_sounds[self.current_sound]) == 0 if completed_boundary: self.current_alert = AudibleAlert.none self.pending_stop = False else: self.pending_stop = True return self.pending_stop = False if self.current_alert != new_alert and (new_alert != AudibleAlert.none or current_alert_played_once): if new_alert in (AudibleAlert.warningSoft, AudibleAlert.warningImmediate): self.ramp_start_volume = self.current_volume self.ramp_start_time = time.monotonic() self.current_alert = new_alert self.current_sound = new_alert self.current_sound_frame = 0 def get_audible_alert(self, sm): if sm.updated['selfdriveState']: new_alert = sm['selfdriveState'].alertSound.raw now_ns = time.monotonic_ns() new_alert = self.axis_alerts.update(sm, new_alert, now_ns) self.update_alert(new_alert) elif check_selfdrive_timeout_alert(sm): self.update_alert(AudibleAlert.warningImmediate) self.selfdrive_timeout_alert = True elif self.selfdrive_timeout_alert: self.update_alert(AudibleAlert.none) self.selfdrive_timeout_alert = False def update_critical_sound(self, now: float) -> None: if self.current_alert in (AudibleAlert.warningSoft, AudibleAlert.warningImmediate): elapsed = now - self.ramp_start_time ramp_vol = float(np.interp(elapsed, [0, ALERT_RAMP_TIME], [self.ramp_start_volume, MAX_VOLUME])) self.current_volume = max(self.current_volume, ramp_vol) # Dismissal owns the current waveform's remaining tail. Escalating here # would reset its cursor and start an additional critical-max loop. if elapsed >= ALERT_MAX_TIME and self.current_sound != CRITICAL_MAX and not self.pending_stop: self.current_sound = CRITICAL_MAX self.current_sound_frame = 0 def calculate_volume(self, weighted_db): volume = ((weighted_db - AMBIENT_DB) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME return math.pow(VOLUME_BASE, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1)) @retry(attempts=10, delay=3) def get_stream(self, sd): self.audio_backend = "python" self.audio_fallback_reason = "host" if PC else None # reload sounddevice to reinitialize portaudio sd._terminate() sd._initialize() if not PC: from openpilot.starpilot.audio.native_playback import NativePlayback try: playback = NativePlayback(self, sd) except (OSError, AttributeError, ValueError, MemoryError) as error: self.audio_fallback_reason = "initialization:" + type(error).__name__ else: try: stream = playback.stream() except (AttributeError, TypeError, ValueError) as error: self.audio_fallback_reason = "stream:" + type(error).__name__ playback.close() except BaseException: playback.close() raise else: self.native_audio = playback self.audio_backend = "native" return stream return sd.OutputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback, blocksize=SAMPLE_BUFFER) def soundd_thread(self): # sounddevice must be imported after forking processes import sounddevice as sd micd.patch_sounddevice(sd) sm = messaging.SubMaster(['selfdriveState', 'soundPressure', 'aolAxisState', INTENT_SERVICE]) with self.get_stream(sd) as stream: rk = Ratekeeper(20) cloudlog.info(f"soundd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}, " + f"{stream.blocksize=}, {stream.latency=}, backend={self.audio_backend}, fallback={self.audio_fallback_reason}") while True: sm.update(0) if self.native_audio is not None: self.native_audio.sync_owner() self.native_audio.report_underflow_timing() self.log_pending_stream_status(stream) self.refresh_saved_volumes(time.monotonic()) self.load_sounds() # freeze volume during alerts to avoid mic feedback increasing volume if sm.updated['soundPressure']: self.spl_filter_weighted.update(sm["soundPressure"].soundPressureWeightedDb) if self.current_alert == AudibleAlert.none: self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x)) self.get_audible_alert(sm) self.update_critical_sound(time.monotonic()) if self.native_audio is not None: self.native_audio.publish() rk.keep_time() active = stream.active if not active and self.native_audio is not None: self.native_audio.report_underflow_timing(closing=True) assert active def main(): s = Soundd() s.soundd_thread() if __name__ == "__main__": main()