import math import numpy as np import time import wave from pathlib import Path from cereal import car, custom, log, messaging from openpilot.common.basedir import BASEDIR from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params 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.system.hardware import HARDWARE from openpilot.starpilot.common.starpilot_variables import ACTIVE_THEME_PATH, ERROR_LOGS_PATH, RANDOM_EVENTS_PATH, get_starpilot_toggles SAMPLE_RATE = 48000 SAMPLE_BUFFER = 4096 # (approx 100ms) MAX_VOLUME = 1.0 MIN_VOLUME = 0.1 SELFDRIVE_STATE_TIMEOUT = 5 # 5 seconds FILTER_DT = 1. / (micd.SAMPLE_RATE / micd.FFT_SAMPLES) AMBIENT_DB = 30 # 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"): VOLUME_BASE = 10 AudibleAlert = car.CarControl.HUDControl.AudibleAlert StarPilotAudibleAlert = custom.StarPilotCarControl.HUDControl.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: ("prompt.wav", 1, MAX_VOLUME), AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME), AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME), AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME), AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME), StarPilotAudibleAlert.angry: ("angry.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.continued: ("continued.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.dejaVu: ("dejaVu.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.doc: ("doc.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.fart: ("fart.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.firefox: ("firefox.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.goat: ("goat.wav", None, MAX_VOLUME), StarPilotAudibleAlert.hal9000: ("hal9000.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.mail: ("mail.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.nessie: ("nessie.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.noice: ("noice.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.startup: ("startup.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.thisIsFine: ("this_is_fine.wav", 1, MAX_VOLUME), StarPilotAudibleAlert.uwu: ("uwu.wav", 1, MAX_VOLUME), } if HARDWARE.get_device_type() in ("tici", "tizi"): sound_list.update({ AudibleAlert.engage: ("engage_tizi.wav", 1, MAX_VOLUME), AudibleAlert.disengage: ("disengage_tizi.wav", 1, MAX_VOLUME), }) 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): self.current_alert = AudibleAlert.none self.current_alert_type = "" self.current_volume = MIN_VOLUME self.current_sound_frame = 0 self.selfdrive_timeout_alert = False self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) self.params_memory = Params(memory=True) self.starpilot_toggles = get_starpilot_toggles() self.openpilot_crashed_played = False self.auto_volume = 0 self.previous_sound_pack = None self.previous_sound_source_signature = None self.error_log = ERROR_LOGS_PATH / "error.txt" self.random_events_directory = RANDOM_EVENTS_PATH / "sounds" self.update_starpilot_sounds() def get_sound_source_signature(self): try: resolved_path = self.sound_directory.resolve() except OSError: resolved_path = self.sound_directory sound_files = () try: if self.sound_directory.exists(): sound_files = tuple(sorted( (entry.name, entry.stat().st_mtime_ns) for entry in self.sound_directory.iterdir() if entry.is_file() )) except OSError: sound_files = () return str(resolved_path), sound_files def load_sounds(self): self.loaded_sounds: dict[int, np.ndarray] = {} # Load all sounds for sound in sound_list: filename, play_count, volume = sound_list[sound] random_events_path = self.random_events_directory / filename sounds_path = self.sound_directory / filename if not sounds_path.exists() and "_tizi" in filename: standard_path = self.sound_directory / filename.replace("_tizi", "") if standard_path.exists(): sounds_path = standard_path if random_events_path.exists(): wavefile = wave.open(str(random_events_path), 'r') elif sounds_path.exists(): wavefile = wave.open(str(sounds_path), 'r') else: if filename == "startup.wav": filename = "engage.wav" wavefile = wave.open(BASEDIR + "/selfdrive/assets/sounds/" + filename, 'r') assert wavefile.getnchannels() == 1 assert wavefile.getsampwidth() == 2 assert wavefile.getframerate() == SAMPLE_RATE length = wavefile.getnframes() self.loaded_sounds[sound] = np.frombuffer(wavefile.readframes(length), dtype=np.int16).astype(np.float32) / (2**16/2) 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) if self.current_alert != AudibleAlert.none: num_loops = sound_list[self.current_alert][1] sound_data = self.loaded_sounds[self.current_alert] 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 return ret * self.current_volume def callback(self, data_out: np.ndarray, frames: int, time, status) -> None: if status: cloudlog.warning(f"soundd stream over/underflow: {status}") data_out[:frames, 0] = self.get_sound_data(frames) 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_alert]) if self.current_alert != new_alert and (new_alert != AudibleAlert.none or current_alert_played_once): self.current_alert = new_alert self.current_sound_frame = 0 def get_audible_alert(self, sm): if self.params_memory.get("TestAlert"): test_alert = self.params_memory.get("TestAlert") if isinstance(test_alert, bytes): test_alert = test_alert.decode("utf-8", "ignore") if test_alert == "belowSteerSpeed": self.current_alert_type = "belowSteerSpeed/warning" self.update_alert(AudibleAlert.prompt) else: self.current_alert_type = "" self.update_alert(getattr(AudibleAlert, test_alert)) self.params_memory.remove("TestAlert") elif not self.openpilot_crashed_played and self.error_log.is_file(): self.current_alert_type = "" self.update_alert(AudibleAlert.prompt) self.openpilot_crashed_played = True elif sm.updated['selfdriveState']: new_alert = sm['selfdriveState'].alertSound.raw new_alert_type = sm['selfdriveState'].alertType critical_full_alert = sm['selfdriveState'].alertStatus == log.SelfdriveState.AlertStatus.critical critical_full_alert &= sm['selfdriveState'].alertSize == log.SelfdriveState.AlertSize.full if self.starpilot_toggles.goat_scream_critical_alerts and critical_full_alert: new_alert = StarPilotAudibleAlert.goat new_starpilot_alert = sm['starpilotSelfdriveState'].alertSound.raw if new_alert == AudibleAlert.none and new_starpilot_alert != StarPilotAudibleAlert.none: new_alert = new_starpilot_alert new_alert_type = sm['starpilotSelfdriveState'].alertType self.current_alert_type = new_alert_type self.update_alert(new_alert) elif check_selfdrive_timeout_alert(sm): self.current_alert_type = "" self.update_alert(AudibleAlert.warningImmediate) self.selfdrive_timeout_alert = True elif self.selfdrive_timeout_alert: self.current_alert_type = "" self.update_alert(AudibleAlert.none) self.selfdrive_timeout_alert = False def get_volume_override(self): if self.current_alert_type.startswith("belowSteerSpeed/"): return self.starpilot_toggles.below_steer_speed_volume / 100.0 return self.volume_map.get(self.current_alert, 1.01) 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): # reload sounddevice to reinitialize portaudio sd._terminate() sd._initialize() return sd.OutputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback, blocksize=SAMPLE_BUFFER) def start_stream(self, sd): stream = self.get_stream(sd) stream.start() cloudlog.info(f"soundd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}, {stream.blocksize=}") return stream def describe_stream(self, stream) -> str: attrs = { "active": getattr(stream, "active", None), "stopped": getattr(stream, "stopped", None), "closed": getattr(stream, "closed", None), "samplerate": getattr(stream, "samplerate", None), "channels": getattr(stream, "channels", None), "dtype": getattr(stream, "dtype", None), "device": getattr(stream, "device", None), "blocksize": getattr(stream, "blocksize", None), } return " ".join(f"{k}={v!r}" for k, v in attrs.items()) def soundd_thread(self): # sounddevice must be imported after forking processes import sounddevice as sd sm = messaging.SubMaster(['selfdriveState', 'soundPressure']) sm = sm.extend(['starpilotSelfdriveState', 'starpilotPlan']) while True: stream = None try: stream = self.start_stream(sd) rk = Ratekeeper(20) while True: sm.update(0) if sm.updated['soundPressure'] and self.current_alert == AudibleAlert.none: # only update volume filter when not playing alert self.spl_filter_weighted.update(sm["soundPressure"].soundPressureWeightedDb) self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x)) if self.starpilot_toggles.alert_volume_controller: self.auto_volume = self.current_volume self.current_volume = 0.0 elif self.current_alert != AudibleAlert.none and self.starpilot_toggles.alert_volume_controller: self.current_volume = self.get_volume_override() if self.current_volume == 1.01: self.current_volume = self.auto_volume self.get_audible_alert(sm) rk.keep_time() if not stream.active: raise RuntimeError(f"soundd stream inactive: {self.describe_stream(stream)}") starpilot_toggles = get_starpilot_toggles(sm) if starpilot_toggles != self.starpilot_toggles: self.starpilot_toggles = starpilot_toggles stream = self.update_starpilot_sounds(sd, stream) elif rk.frame % 5 == 0: stream = self.update_starpilot_sounds(sd, stream) except Exception: cloudlog.exception("soundd: stream failed, restarting") time.sleep(1) finally: if stream is not None: try: stream.close() except Exception: cloudlog.exception("soundd: failed to close stream") def update_starpilot_sounds(self, sd=None, stream=None): self.volume_map = { AudibleAlert.engage: self.starpilot_toggles.engage_volume / 100.0, AudibleAlert.disengage: self.starpilot_toggles.disengage_volume / 100.0, AudibleAlert.refuse: self.starpilot_toggles.refuse_volume / 100.0, AudibleAlert.prompt: self.starpilot_toggles.prompt_volume / 100.0, AudibleAlert.promptRepeat: self.starpilot_toggles.prompt_volume / 100.0, AudibleAlert.promptDistracted: self.starpilot_toggles.promptDistracted_volume / 100.0, AudibleAlert.warningSoft: self.starpilot_toggles.warningSoft_volume / 100.0, AudibleAlert.warningImmediate: self.starpilot_toggles.warningImmediate_volume / 100.0, StarPilotAudibleAlert.goat: self.starpilot_toggles.prompt_volume / 100.0, StarPilotAudibleAlert.startup: self.starpilot_toggles.engage_volume / 100.0 } for sound in sound_list: if sound not in self.volume_map: self.volume_map[sound] = 1.01 if self.starpilot_toggles.sound_pack != "stock": self.sound_directory = ACTIVE_THEME_PATH / "sounds" else: self.sound_directory = Path(BASEDIR) / "selfdrive" / "assets" / "sounds" sound_source_signature = self.get_sound_source_signature() should_reload = self.starpilot_toggles.sound_pack != self.previous_sound_pack should_reload |= sound_source_signature != self.previous_sound_source_signature if should_reload: self.load_sounds() self.previous_sound_pack = self.starpilot_toggles.sound_pack self.previous_sound_source_signature = sound_source_signature if stream is not None: stream.close() stream = self.get_stream(sd) stream.start() return stream def main(): s = Soundd() s.soundd_thread() if __name__ == "__main__": main()