diff --git a/common/params.cc b/common/params.cc index e692693..9da6281 100755 --- a/common/params.cc +++ b/common/params.cc @@ -260,6 +260,7 @@ std::unordered_map keys = { {"ClearpilotSpeedUnit", PERSISTENT}, {"ClearpilotCruiseWarning", PERSISTENT}, {"ClearpilotCruiseWarningSpeed", PERSISTENT}, + {"ClearpilotPlayDing", PERSISTENT}, // {"SpeedLimitLatDesired", PERSISTENT}, // {"SpeedLimitVTSC", PERSISTENT}, diff --git a/selfdrive/clearpilot/bench_cmd.py b/selfdrive/clearpilot/bench_cmd.py index 839ad03..cb90f6c 100644 --- a/selfdrive/clearpilot/bench_cmd.py +++ b/selfdrive/clearpilot/bench_cmd.py @@ -9,6 +9,7 @@ Usage: python3 -m selfdrive.clearpilot.bench_cmd cruise 55 python3 -m selfdrive.clearpilot.bench_cmd cruiseactive 0|1|2 (0=disabled, 1=active, 2=paused) python3 -m selfdrive.clearpilot.bench_cmd engaged 1 + python3 -m selfdrive.clearpilot.bench_cmd ding (trigger speed limit ding sound) python3 -m selfdrive.clearpilot.bench_cmd debugbutton (simulate LKAS debug button press) python3 -m selfdrive.clearpilot.bench_cmd dump python3 -m selfdrive.clearpilot.bench_cmd wait_ready @@ -108,6 +109,10 @@ def main(): elif cmd == "wait_ready": wait_ready() + elif cmd == "ding": + params.put("ClearpilotPlayDing", "1") + print("Ding triggered") + elif cmd == "debugbutton": # Simulate LKAS debug button — same state machine as controlsd.clearpilot_state_control() current = params.get_int("ScreenDisplayMode") diff --git a/selfdrive/clearpilot/sounds/ding.wav b/selfdrive/clearpilot/sounds/ding.wav new file mode 100644 index 0000000..374f2d4 Binary files /dev/null and b/selfdrive/clearpilot/sounds/ding.wav differ diff --git a/selfdrive/clearpilot/speed_logic.py b/selfdrive/clearpilot/speed_logic.py index d4d869f..3a5b61d 100644 --- a/selfdrive/clearpilot/speed_logic.py +++ b/selfdrive/clearpilot/speed_logic.py @@ -9,6 +9,7 @@ unit setting), detects speed limit changes, and writes results to params_memory for the onroad UI to read. """ import math +import time from openpilot.common.params import Params from openpilot.common.conversions import Conversions as CV @@ -18,20 +19,18 @@ class SpeedState: self.params_memory = Params("/dev/shm/params") self.prev_speed_limit = 0 + # Ding state tracking + self.last_ding_time = 0.0 + self.prev_warning = "" + self.prev_warning_speed_limit = 0 + def update(self, speed_ms: float, has_speed: bool, speed_limit_ms: float, is_metric: bool, cruise_speed_ms: float = 0.0, cruise_active: bool = False, cruise_standstill: bool = False): """ Convert raw m/s values to display-ready strings and write to params_memory. - - Args: - speed_ms: current vehicle speed in m/s (from GPS or bench) - has_speed: whether we have a valid speed source - speed_limit_ms: current speed limit in m/s (from CAN or bench) - is_metric: True if car's CAN reports metric units (e.g. Canada) - cruise_speed_ms: cruise control set speed in m/s - cruise_active: True if cruise is engaged and not paused - cruise_standstill: True if cruise is paused at standstill """ + now = time.monotonic() + if is_metric: speed_display = speed_ms * CV.MS_TO_KPH speed_limit_display = speed_limit_ms * CV.MS_TO_KPH @@ -47,9 +46,7 @@ class SpeedState: speed_limit_int = int(math.floor(speed_limit_display)) cruise_int = int(round(cruise_display)) - # Detect speed limit changes (groundwork for future chime) - if speed_limit_int != self.prev_speed_limit: - self.prev_speed_limit = speed_limit_int + self.prev_speed_limit = speed_limit_int # Write display-ready values to params_memory self.params_memory.put("ClearpilotHasSpeed", "1" if has_speed and speed_int > 0 else "0") @@ -59,7 +56,6 @@ class SpeedState: self.params_memory.put("ClearpilotIsMetric", "1" if is_metric else "0") # Cruise warning logic - # Only evaluate when speed limit >= 20 and cruise is active (not paused, not disabled) warning = "" warning_speed = "" cruise_engaged = cruise_active and not cruise_standstill @@ -75,3 +71,20 @@ class SpeedState: self.params_memory.put("ClearpilotCruiseWarning", warning) self.params_memory.put("ClearpilotCruiseWarningSpeed", warning_speed) + + # Ding logic: play when warning sign appears or speed limit changes while visible + should_ding = False + if warning: + if not self.prev_warning: + # Warning sign just appeared + should_ding = True + elif speed_limit_int != self.prev_warning_speed_limit: + # Speed limit changed while warning sign is visible + should_ding = True + + if should_ding and now - self.last_ding_time >= 30: + self.params_memory.put("ClearpilotPlayDing", "1") + self.last_ding_time = now + + self.prev_warning = warning + self.prev_warning_speed_limit = speed_limit_int if warning else 0 diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 5646886..359c9b5 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -97,6 +97,7 @@ def manager_init(frogpilot_functions) -> None: params_memory.put("ClearpilotSpeedUnit", "mph") params_memory.put("ClearpilotCruiseWarning", "") params_memory.put("ClearpilotCruiseWarningSpeed", "") + params_memory.put("ClearpilotPlayDing", "0") params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) if is_release_branch(): diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 69263b7..e8768f8 100755 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -93,6 +93,27 @@ class Soundd: self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) + # ClearPilot ding (plays independently of alerts) + self.ding_sound = None + self.ding_frame = 0 + self.ding_playing = False + self.ding_check_counter = 0 + self._load_ding() + + def _load_ding(self): + ding_path = BASEDIR + "/selfdrive/clearpilot/sounds/ding.wav" + try: + wavefile = wave.open(ding_path, 'r') + assert wavefile.getnchannels() == 1 + assert wavefile.getsampwidth() == 2 + assert wavefile.getframerate() == SAMPLE_RATE + length = wavefile.getnframes() + self.ding_sound = np.frombuffer(wavefile.readframes(length), dtype=np.int16).astype(np.float32) / (2**16/2) + cloudlog.info(f"ClearPilot ding loaded: {length} frames") + except Exception as e: + cloudlog.error(f"Failed to load ding sound: {e}") + self.ding_sound = None + def load_sounds(self): self.loaded_sounds: dict[int, np.ndarray] = {} @@ -137,7 +158,20 @@ class Soundd: written_frames += frames_to_write self.current_sound_frame += frames_to_write - return ret * self.current_volume + ret = ret * self.current_volume + + # Mix in ClearPilot ding (independent of alerts, always max volume) + if self.ding_playing and self.ding_sound is not None: + ding_remaining = len(self.ding_sound) - self.ding_frame + if ding_remaining > 0: + frames_to_write = min(ding_remaining, frames) + ret[:frames_to_write] += self.ding_sound[self.ding_frame:self.ding_frame + frames_to_write] * MAX_VOLUME + self.ding_frame += frames_to_write + else: + self.ding_playing = False + self.ding_frame = 0 + + return ret def callback(self, data_out: np.ndarray, frames: int, time, status) -> None: if status: @@ -197,6 +231,14 @@ class Soundd: self.get_audible_alert(sm) + # ClearPilot: check for ding trigger at ~2Hz + self.ding_check_counter += 1 + if self.ding_check_counter % 10 == 0 and self.ding_sound is not None: + if self.params_memory.get("ClearpilotPlayDing") == b"1": + self.params_memory.put("ClearpilotPlayDing", "0") + self.ding_playing = True + self.ding_frame = 0 + rk.keep_time() assert stream.active