🔉 Set Sound Volume Based on Ambient Noise Level

This is a squash commit of 7 commits. soundd: change system sound mixer volume (commaai#26633) * test changing sound volume * create system/hardware/pc/hardware.h * soundd: use Hardware::set_volume * implement Hardware::set_volume using pactl * Revert "test changing sound volume" This reverts commit 4bbd870. * don't run command in background * pactl: use default sink micd: scale sound volume with ambient noise level (commaai#26399) * test changing sound volume * test changing sound volume * create system/hardware/pc/hardware.h * implement Hardware::set_volume using pactl * soundd: use Hardware::set_volume * add sounddevice dependency * sounddevice example * simple micd * cleanup * remove this * fix process config * add to release files * hardware: get sound input device * no more offroad * debug * calculate volume from all measurements since last update * use microphone noise level to update sound volume * fix scale * mute microphone during alerts * log raw noise level * hardware: reduce tici min volume * improve scale * add package * clear measurements on muted * change default to min volume and respond quicker * fixes Co-authored-by: Shane Smiskol <shane@smiskol.com> * logarithmic scaling * fix * respond quicker * fixes * tweak scaling * specify default device * Revert "hardware: get sound input device" This reverts commit 50f594f. * tuning * forgot to update submaster * tuning * don't mute microphone, and clip measurement * remove submaster * fixes * tuning * implement Hardware::set_volume using pactl * Revert "test changing sound volume" This reverts commit 4bbd870. * draft * draft * calculate sound pressure level in dB * fix setting * faster filter * start at initial value * don't run command in background * pactl: use default sink * use sound pressure db * tuning * bump up max volume threshold * update filter slower * fix divide by zero * bump cereal Co-authored-by: Shane Smiskol <shane@smiskol.com> micd: don't update filtered sound level if playing sound (commaai#26652) * add is_sound_playing to hardware.py * micd: don't update filtered sound level if playing sound micd: apply A-weighting to the sound pressure level (commaai#26668) * record * record * draft * some clean up * some clean up * wishful tuning * log pressure level (db) for debugging * fix * tuning * ignore complex to real warning * remove this * Update selfdrive/ui/soundd/sound.cc * Update system/micd.py * remove warning supp * bump cereal to master Co-authored-by: Cameron Clough <cameronjclough@gmail.com> micd: revert check playing sound (high cpu usage) (commaai#26672) * don't use hardware * check micd proc * use pactl package * cleanup * Revert "cleanup" This reverts commit baf9887. * Revert "use pactl package" This reverts commit 0c1f3a4. * Revert "micd: don't update filtered sound level if playing sound (commaai#26652)" This reverts commit 86cd919. * Revert "check micd proc" This reverts commit 9ebbe2a. Co-authored-by: Cameron Clough <cameronjclough@gmail.com> Micd: update sound levels in callback (commaai#26674) * update once reached 4096 * update once reached 4096 * reduce * debug & cmt * fix * fifo again * fix * clean that up * update filter on demand Co-authored-by: Cameron Clough <cameronjclough@gmail.com> Merge in Cereal Changes from Upstream Add Fields for Ambient Noise Level Detection This is a squash commit of 3 commits. add microphone (commaai#382) * add microphone socket * increase freq * add raw noise level * rename to ambient * switch Micd fields (commaai#391) * add new field * uncalib Co-authored-by: Shane Smiskol <shane@smiskol.com> Micd: add A-weighted sound level fields (commaai#392) * new fields * add temp field * Revert "add temp field" This reverts commit 54b597470f1da13246599502d91dd4e18f4aa11f. * move Co-authored-by: Cameron Clough <cameronjclough@gmail.com>
krkeegan · Jan 9, 2023 · ab450f7 · ab450f7
1 parent e43a1ba
commit ab450f7
Show file tree

Hide file tree

Showing 10 changed files with 158 additions and 20 deletions.
diff --git a/cereal/log.capnp b/cereal/log.capnp
@@ -1961,6 +1961,15 @@ struct EncodeData {
 struct UserFlag {
 }
 
+struct Microphone {
+  soundPressure @0 :Float32;
+
+  # uncalibrated, A-weighted
+  soundPressureWeighted @3 :Float32;
+  soundPressureWeightedDb @1 :Float32;
+  filteredSoundPressureWeightedDb @2 :Float32;
+}
+
 struct Event {
   logMonoTime @0 :UInt64;  # nanoseconds
   valid @67 :Bool = true;
@@ -2020,6 +2029,9 @@ struct Event {
     wideRoadEncodeIdx @77 :EncodeIndex;
     qRoadEncodeIdx @90 :EncodeIndex;
 
+    # microphone data
+    microphone @103 :Microphone;
+
     # systems stuff
     androidLog @20 :AndroidLogEntry;
     managerState @78 :ManagerState;

diff --git a/cereal/services.py b/cereal/services.py
@@ -76,6 +76,7 @@ def __init__(self, port: int, should_log: bool, frequency: float, decimation: Op
   "navThumbnail": (True, 0.),
   "qRoadEncodeIdx": (False, 20.),
   "userFlag": (True, 0., 1),
+  "microphone": (True, 10., 10),
 
   # debug
   "uiDebug": (True, 0., 1),

diff --git a/release/files_common b/release/files_common
@@ -71,6 +71,7 @@ selfdrive/rtshield.py
 selfdrive/statsd.py
 
 system/logmessaged.py
+system/micd.py
 system/swaglog.py
 system/version.py
 
@@ -216,6 +217,7 @@ system/hardware/tici/amplifier.py
 system/hardware/tici/updater
 system/hardware/tici/iwlist.py
 system/hardware/pc/__init__.py
+system/hardware/pc/hardware.h
 system/hardware/pc/hardware.py
 
 selfdrive/locationd/__init__.py

diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py
@@ -24,6 +24,7 @@ def logging(started, params, CP: car.CarParams) -> bool:
   NativeProcess("logcatd", "system/logcatd", ["./logcatd"]),
   NativeProcess("proclogd", "system/proclogd", ["./proclogd"]),
   PythonProcess("logmessaged", "system.logmessaged", offroad=True),
+  PythonProcess("micd", "system.micd"),
   PythonProcess("timezoned", "system.timezoned", enabled=not PC, offroad=True),
 
   DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"),

diff --git a/selfdrive/ui/soundd/sound.cc b/selfdrive/ui/soundd/sound.cc
@@ -12,15 +12,14 @@
 // TODO: detect when we can't play sounds
 // TODO: detect when we can't display the UI
 
-Sound::Sound(QObject *parent) : sm({"carState", "controlsState", "deviceState"}) {
+Sound::Sound(QObject *parent) : sm({"controlsState", "deviceState", "microphone"}) {
   qInfo() << "default audio device: " << QAudioDeviceInfo::defaultOutputDevice().deviceName();
 
   for (auto &[alert, fn, loops] : sound_list) {
     QSoundEffect *s = new QSoundEffect(this);
     QObject::connect(s, &QSoundEffect::statusChanged, [=]() {
       assert(s->status() != QSoundEffect::Error);
     });
-    s->setVolume(Hardware::MIN_VOLUME);
     s->setSource(QUrl::fromLocalFile("../../assets/sounds/" + fn));
     sounds[alert] = {s, loops};
   }
@@ -48,15 +47,10 @@ void Sound::update() {
   }
 
   // scale volume with speed
-  if (sm.updated("carState")) {
-    float volume = util::map_val(sm["carState"].getCarState().getVEgo(), 11.f, 30.f, 0.f, 1.0f);  // KRKeegan max volume at ~65mph
+  if (sm.updated("microphone")) {
+    float volume = util::map_val(sm["microphone"].getMicrophone().getFilteredSoundPressureWeightedDb(), 30.f, 55.f, 0.f, 1.f);
     volume = QAudio::convertVolume(volume, QAudio::LogarithmicVolumeScale, QAudio::LinearVolumeScale);
-    float max_volume_scale = 0.7;  // KRKeegan percentage of default max volume
-    float min_volume_scale = 0.3;  // KRKeegan percentage of default min volume
-    volume = util::map_val(volume, 0.f, 1.f, Hardware::MIN_VOLUME * min_volume_scale, Hardware::MAX_VOLUME * max_volume_scale);
-    for (auto &[s, loops] : sounds) {
-      s->setVolume(std::round(100 * volume) / 100);
-    }
+    Hardware::set_volume(volume);
   }
 
   setAlert(Alert::get(sm, started_frame));

diff --git a/system/hardware/base.h b/system/hardware/base.h
@@ -20,6 +20,7 @@ class HardwareNone {
   static void poweroff() {}
   static void set_brightness(int percent) {}
   static void set_display_power(bool on) {}
+  static void set_volume(float volume) {}
 
   static bool get_ssh_enabled() { return false; }
   static void set_ssh_enabled(bool enabled) {}

diff --git a/system/hardware/hw.h b/system/hardware/hw.h
@@ -7,15 +7,7 @@
 #include "system/hardware/tici/hardware.h"
 #define Hardware HardwareTici
 #else
-class HardwarePC : public HardwareNone {
-public:
-  static std::string get_os_version() { return "openpilot for PC"; }
-  static std::string get_name() { return "pc"; };
-  static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; };
-  static bool PC() { return true; }
-  static bool TICI() { return util::getenv("TICI", 0) == 1; }
-  static bool AGNOS() { return util::getenv("TICI", 0) == 1; }
-};
+#include "system/hardware/pc/hardware.h"
 #define Hardware HardwarePC
 #endif
 

diff --git a/system/hardware/pc/hardware.h b/system/hardware/pc/hardware.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "system/hardware/base.h"
+
+class HardwarePC : public HardwareNone {
+public:
+  static std::string get_os_version() { return "openpilot for PC"; }
+  static std::string get_name() { return "pc"; };
+  static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; };
+  static bool PC() { return true; }
+  static bool TICI() { return util::getenv("TICI", 0) == 1; }
+  static bool AGNOS() { return util::getenv("TICI", 0) == 1; }
+
+  static void set_volume(float volume) {
+    volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME);
+
+    char volume_str[6];
+    snprintf(volume_str, sizeof(volume_str), "%.3f", volume);
+    std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str());
+  }
+};
diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h
@@ -10,7 +10,7 @@
 class HardwareTici : public HardwareNone {
 public:
   static constexpr float MAX_VOLUME = 0.9;
-  static constexpr float MIN_VOLUME = 0.2;
+  static constexpr float MIN_VOLUME = 0.1;
   static bool TICI() { return true; }
   static bool AGNOS() { return true; }
   static std::string get_os_version() {
@@ -38,6 +38,13 @@ class HardwareTici : public HardwareNone {
       bl_power_control.close();
     }
   };
+  static void set_volume(float volume) {
+    volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME);
+
+    char volume_str[6];
+    snprintf(volume_str, sizeof(volume_str), "%.3f", volume);
+    std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str());
+  }
 
   static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); };
   static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); };

diff --git a/system/micd.py b/system/micd.py
@@ -0,0 +1,107 @@
+#!/usr/bin/env python3
+import sounddevice as sd
+import numpy as np
+
+from cereal import messaging
+from common.filter_simple import FirstOrderFilter
+from common.realtime import Ratekeeper
+from system.swaglog import cloudlog
+
+RATE = 10
+FFT_SAMPLES = 4096
+REFERENCE_SPL = 2e-5  # newtons/m^2
+SAMPLE_RATE = 44100
+FILTER_DT = 1. / (SAMPLE_RATE / FFT_SAMPLES)
+
+
+def calculate_spl(measurements):
+  # https://www.engineeringtoolbox.com/sound-pressure-d_711.html
+  sound_pressure = np.sqrt(np.mean(measurements ** 2))  # RMS of amplitudes
+  if sound_pressure > 0:
+    sound_pressure_level = 20 * np.log10(sound_pressure / REFERENCE_SPL)  # dB
+  else:
+    sound_pressure_level = 0
+  return sound_pressure, sound_pressure_level
+
+
+def apply_a_weighting(measurements: np.ndarray) -> np.ndarray:
+  # Generate a Hanning window of the same length as the audio measurements
+  hanning_window = np.hanning(len(measurements))
+  measurements_windowed = measurements * hanning_window
+
+  # Calculate the frequency axis for the signal
+  freqs = np.fft.fftfreq(measurements_windowed.size, d=1 / SAMPLE_RATE)
+
+  # Calculate the A-weighting filter
+  # https://en.wikipedia.org/wiki/A-weighting
+  A = 12194 ** 2 * freqs ** 4 / ((freqs ** 2 + 20.6 ** 2) * (freqs ** 2 + 12194 ** 2) * np.sqrt((freqs ** 2 + 107.7 ** 2) * (freqs ** 2 + 737.9 ** 2)))
+  A /= np.max(A)  # Normalize the filter
+
+  # Apply the A-weighting filter to the signal
+  return np.abs(np.fft.ifft(np.fft.fft(measurements_windowed) * A))
+
+
+class Mic:
+  def __init__(self, pm):
+    self.pm = pm
+    self.rk = Ratekeeper(RATE)
+
+    self.measurements = np.empty(0)
+
+    self.sound_pressure = 0
+    self.sound_pressure_weighted = 0
+    self.sound_pressure_level_weighted = 0
+
+    self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
+
+  def update(self):
+    msg = messaging.new_message('microphone')
+    msg.microphone.soundPressure = float(self.sound_pressure)
+    msg.microphone.soundPressureWeighted = float(self.sound_pressure_weighted)
+
+    msg.microphone.soundPressureWeightedDb = float(self.sound_pressure_level_weighted)
+    msg.microphone.filteredSoundPressureWeightedDb = float(self.spl_filter_weighted.x)
+
+    self.pm.send('microphone', msg)
+    self.rk.keep_time()
+
+  def callback(self, indata, frames, time, status):
+    """
+    Using amplitude measurements, calculate an uncalibrated sound pressure and sound pressure level.
+    Then apply A-weighting to the raw amplitudes and run the same calculations again.
+
+    Logged A-weighted equivalents are rough approximations of the human-perceived loudness.
+    """
+
+    self.measurements = np.concatenate((self.measurements, indata[:, 0]))
+
+    while self.measurements.size >= FFT_SAMPLES:
+      measurements = self.measurements[:FFT_SAMPLES]
+
+      self.sound_pressure, _ = calculate_spl(measurements)
+      measurements_weighted = apply_a_weighting(measurements)
+      self.sound_pressure_weighted, self.sound_pressure_level_weighted = calculate_spl(measurements_weighted)
+      self.spl_filter_weighted.update(self.sound_pressure_level_weighted)
+
+      self.measurements = self.measurements[FFT_SAMPLES:]
+
+  def micd_thread(self, device=None):
+    if device is None:
+      device = "sysdefault"
+
+    with sd.InputStream(device=device, channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream:
+      cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}")
+      while True:
+        self.update()
+
+
+def main(pm=None):
+  if pm is None:
+    pm = messaging.PubMaster(['microphone'])
+
+  mic = Mic(pm)
+  mic.micd_thread()
+
+
+if __name__ == "__main__":
+  main()