feat: speed limit ding sound when cruise warning sign appears
Plays a ding via soundd when the cruise warning sign becomes visible (cruise set speed out of range vs speed limit) or when the speed limit changes while the warning sign is already showing. Max 1 ding per 30s. Ding is mixed independently into soundd output at max volume without interrupting alert sounds. bench_cmd ding available for manual trigger. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
@@ -260,6 +260,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||||||
{"ClearpilotSpeedUnit", PERSISTENT},
|
{"ClearpilotSpeedUnit", PERSISTENT},
|
||||||
{"ClearpilotCruiseWarning", PERSISTENT},
|
{"ClearpilotCruiseWarning", PERSISTENT},
|
||||||
{"ClearpilotCruiseWarningSpeed", PERSISTENT},
|
{"ClearpilotCruiseWarningSpeed", PERSISTENT},
|
||||||
|
{"ClearpilotPlayDing", PERSISTENT},
|
||||||
|
|
||||||
// {"SpeedLimitLatDesired", PERSISTENT},
|
// {"SpeedLimitLatDesired", PERSISTENT},
|
||||||
// {"SpeedLimitVTSC", PERSISTENT},
|
// {"SpeedLimitVTSC", PERSISTENT},
|
||||||
|
|||||||
@@ -9,6 +9,7 @@ Usage:
|
|||||||
python3 -m selfdrive.clearpilot.bench_cmd cruise 55
|
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 cruiseactive 0|1|2 (0=disabled, 1=active, 2=paused)
|
||||||
python3 -m selfdrive.clearpilot.bench_cmd engaged 1
|
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 debugbutton (simulate LKAS debug button press)
|
||||||
python3 -m selfdrive.clearpilot.bench_cmd dump
|
python3 -m selfdrive.clearpilot.bench_cmd dump
|
||||||
python3 -m selfdrive.clearpilot.bench_cmd wait_ready
|
python3 -m selfdrive.clearpilot.bench_cmd wait_ready
|
||||||
@@ -108,6 +109,10 @@ def main():
|
|||||||
elif cmd == "wait_ready":
|
elif cmd == "wait_ready":
|
||||||
wait_ready()
|
wait_ready()
|
||||||
|
|
||||||
|
elif cmd == "ding":
|
||||||
|
params.put("ClearpilotPlayDing", "1")
|
||||||
|
print("Ding triggered")
|
||||||
|
|
||||||
elif cmd == "debugbutton":
|
elif cmd == "debugbutton":
|
||||||
# Simulate LKAS debug button — same state machine as controlsd.clearpilot_state_control()
|
# Simulate LKAS debug button — same state machine as controlsd.clearpilot_state_control()
|
||||||
current = params.get_int("ScreenDisplayMode")
|
current = params.get_int("ScreenDisplayMode")
|
||||||
|
|||||||
BIN
selfdrive/clearpilot/sounds/ding.wav
Normal file
BIN
selfdrive/clearpilot/sounds/ding.wav
Normal file
Binary file not shown.
@@ -9,6 +9,7 @@ unit setting), detects speed limit changes, and writes results to params_memory
|
|||||||
for the onroad UI to read.
|
for the onroad UI to read.
|
||||||
"""
|
"""
|
||||||
import math
|
import math
|
||||||
|
import time
|
||||||
from openpilot.common.params import Params
|
from openpilot.common.params import Params
|
||||||
from openpilot.common.conversions import Conversions as CV
|
from openpilot.common.conversions import Conversions as CV
|
||||||
|
|
||||||
@@ -18,20 +19,18 @@ class SpeedState:
|
|||||||
self.params_memory = Params("/dev/shm/params")
|
self.params_memory = Params("/dev/shm/params")
|
||||||
self.prev_speed_limit = 0
|
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,
|
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):
|
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.
|
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:
|
if is_metric:
|
||||||
speed_display = speed_ms * CV.MS_TO_KPH
|
speed_display = speed_ms * CV.MS_TO_KPH
|
||||||
speed_limit_display = speed_limit_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))
|
speed_limit_int = int(math.floor(speed_limit_display))
|
||||||
cruise_int = int(round(cruise_display))
|
cruise_int = int(round(cruise_display))
|
||||||
|
|
||||||
# Detect speed limit changes (groundwork for future chime)
|
self.prev_speed_limit = speed_limit_int
|
||||||
if speed_limit_int != self.prev_speed_limit:
|
|
||||||
self.prev_speed_limit = speed_limit_int
|
|
||||||
|
|
||||||
# Write display-ready values to params_memory
|
# Write display-ready values to params_memory
|
||||||
self.params_memory.put("ClearpilotHasSpeed", "1" if has_speed and speed_int > 0 else "0")
|
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")
|
self.params_memory.put("ClearpilotIsMetric", "1" if is_metric else "0")
|
||||||
|
|
||||||
# Cruise warning logic
|
# Cruise warning logic
|
||||||
# Only evaluate when speed limit >= 20 and cruise is active (not paused, not disabled)
|
|
||||||
warning = ""
|
warning = ""
|
||||||
warning_speed = ""
|
warning_speed = ""
|
||||||
cruise_engaged = cruise_active and not cruise_standstill
|
cruise_engaged = cruise_active and not cruise_standstill
|
||||||
@@ -75,3 +71,20 @@ class SpeedState:
|
|||||||
|
|
||||||
self.params_memory.put("ClearpilotCruiseWarning", warning)
|
self.params_memory.put("ClearpilotCruiseWarning", warning)
|
||||||
self.params_memory.put("ClearpilotCruiseWarningSpeed", warning_speed)
|
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
|
||||||
|
|||||||
@@ -97,6 +97,7 @@ def manager_init(frogpilot_functions) -> None:
|
|||||||
params_memory.put("ClearpilotSpeedUnit", "mph")
|
params_memory.put("ClearpilotSpeedUnit", "mph")
|
||||||
params_memory.put("ClearpilotCruiseWarning", "")
|
params_memory.put("ClearpilotCruiseWarning", "")
|
||||||
params_memory.put("ClearpilotCruiseWarningSpeed", "")
|
params_memory.put("ClearpilotCruiseWarningSpeed", "")
|
||||||
|
params_memory.put("ClearpilotPlayDing", "0")
|
||||||
params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION)
|
params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION)
|
||||||
params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
|
params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
|
||||||
if is_release_branch():
|
if is_release_branch():
|
||||||
|
|||||||
@@ -93,6 +93,27 @@ class Soundd:
|
|||||||
|
|
||||||
self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
|
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):
|
def load_sounds(self):
|
||||||
self.loaded_sounds: dict[int, np.ndarray] = {}
|
self.loaded_sounds: dict[int, np.ndarray] = {}
|
||||||
|
|
||||||
@@ -137,7 +158,20 @@ class Soundd:
|
|||||||
written_frames += frames_to_write
|
written_frames += frames_to_write
|
||||||
self.current_sound_frame += 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:
|
def callback(self, data_out: np.ndarray, frames: int, time, status) -> None:
|
||||||
if status:
|
if status:
|
||||||
@@ -197,6 +231,14 @@ class Soundd:
|
|||||||
|
|
||||||
self.get_audible_alert(sm)
|
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()
|
rk.keep_time()
|
||||||
|
|
||||||
assert stream.active
|
assert stream.active
|
||||||
|
|||||||
Reference in New Issue
Block a user