Sentry mode: automatically records and plays noise when bumped

selfdrive/boardd/boardd.cc

@@ -452,6 +452,8 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
 
     for (const auto &panda : pandas) {
       panda->send_heartbeat(engaged);
+      // TODO do something cool with led
+//      panda->set_red_led(sm["deviceState"].getDeviceState().getStartedSentry());  // spams red led
     }
     util::sleep_for(500);
   }

selfdrive/common/params.cc

@@ -147,6 +147,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"RecordFrontLock", PERSISTENT},  // for the internal fleet
     {"ReleaseNotes", PERSISTENT},
     {"ShouldDoUpdate", CLEAR_ON_MANAGER_START},
+    {"SentryMode", PERSISTENT},
     {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
     {"SshEnabled", PERSISTENT},
     {"SubscriberInfo", PERSISTENT},

selfdrive/manager/manager.py

@@ -145,9 +145,9 @@ def manager_thread() -> None:
     if sm['deviceState'].freeSpacePercent < 5:
       not_run.append("loggerd")
 
-    started = sm['deviceState'].started
+    started = sm['deviceState'].started or sm['deviceState'].startedSentry
     driverview = params.get_bool("IsDriverViewEnabled")
-    ensure_running(managed_processes.values(), started, driverview, not_run)
+    ensure_running(managed_processes.values(), started, driverview, sm['deviceState'].startedSentry, not_run)
 
     # trigger an update after going offroad
     if started_prev and not started and 'updated' in managed_processes:

selfdrive/manager/process.py

@@ -67,6 +67,7 @@ class ManagerProcess(ABC):
   daemon = False
   sigkill = False
   persistent = False
+  sentry = False
   proc: Optional[Process] = None
   enabled = True
   name = ""
@@ -178,7 +179,7 @@ def get_process_state_msg(self):
 
 
 class NativeProcess(ManagerProcess):
-  def __init__(self, name, cwd, cmdline, enabled=True, persistent=False, driverview=False, unkillable=False, sigkill=False, watchdog_max_dt=None):
+  def __init__(self, name, cwd, cmdline, enabled=True, persistent=False, sentry=False, driverview=False, unkillable=False, sigkill=False, watchdog_max_dt=None):
     self.name = name
     self.cwd = cwd
     self.cmdline = cmdline
@@ -188,6 +189,7 @@ def __init__(self, name, cwd, cmdline, enabled=True, persistent=False, drivervie
     self.unkillable = unkillable
     self.sigkill = sigkill
     self.watchdog_max_dt = watchdog_max_dt
+    self.sentry = sentry
 
   def prepare(self) -> None:
     pass
@@ -209,7 +211,7 @@ def start(self) -> None:
 
 
 class PythonProcess(ManagerProcess):
-  def __init__(self, name, module, enabled=True, persistent=False, driverview=False, unkillable=False, sigkill=False, watchdog_max_dt=None):
+  def __init__(self, name, module, enabled=True, persistent=False, sentry=False, driverview=False, unkillable=False, sigkill=False, watchdog_max_dt=None):
     self.name = name
     self.module = module
     self.enabled = enabled
@@ -218,6 +220,7 @@ def __init__(self, name, module, enabled=True, persistent=False, driverview=Fals
     self.unkillable = unkillable
     self.sigkill = sigkill
     self.watchdog_max_dt = watchdog_max_dt
+    self.sentry = sentry
 
   def prepare(self) -> None:
     if self.enabled:
@@ -280,7 +283,7 @@ def stop(self, retry=True, block=True) -> None:
     pass
 
 
-def ensure_running(procs: ValuesView[ManagerProcess], started: bool, driverview: bool=False, not_run: Optional[List[str]]=None) -> None:
+def ensure_running(procs: ValuesView[ManagerProcess], started: bool, driverview: bool=False, started_sentry: bool=False, not_run: Optional[List[str]]=None) -> None:
   if not_run is None:
     not_run = []
 
@@ -294,6 +297,9 @@ def ensure_running(procs: ValuesView[ManagerProcess], started: bool, driverview:
     elif getattr(p, 'driverview', False) and driverview:
       # TODO: why is driverview an argument here? can this be done with the name?
       p.start()
+    # stop processes normally only starting when onroad from starting if not a sentry process
+    elif not p.sentry and started_sentry:
+      p.stop(block=False)
     elif started:
       p.start()
     else:

selfdrive/manager/process_config.py

@@ -9,15 +9,15 @@
 procs = [
   DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"),
   # due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption
-  NativeProcess("camerad", "selfdrive/camerad", ["./camerad"], unkillable=True, driverview=True),
+  NativeProcess("camerad", "selfdrive/camerad", ["./camerad"], unkillable=True, driverview=True, sentry=True),
   NativeProcess("clocksd", "selfdrive/clocksd", ["./clocksd"]),
   NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], enabled=(not PC or WEBCAM), driverview=True),
   NativeProcess("logcatd", "selfdrive/logcatd", ["./logcatd"]),
-  NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"]),
+  NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], sentry=True),
   NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]),
   NativeProcess("navd", "selfdrive/ui/navd", ["./navd"], enabled=(PC or TICI), persistent=True),
   NativeProcess("proclogd", "selfdrive/proclogd", ["./proclogd"]),
-  NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC, persistent=EON, sigkill=EON),
+  NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC, persistent=True, sigkill=EON),
   NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)),
   NativeProcess("ui", "selfdrive/ui", ["./ui"], persistent=True, watchdog_max_dt=(5 if TICI else None)),
   NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], persistent=True),
@@ -27,12 +27,13 @@
   PythonProcess("controlsd", "selfdrive.controls.controlsd"),
   PythonProcess("deleter", "selfdrive.loggerd.deleter", persistent=True),
   PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), driverview=True),
+  PythonProcess("lanespeedd", "selfdrive.controls.lane_speed"),
   PythonProcess("logmessaged", "selfdrive.logmessaged", persistent=True),
   PythonProcess("pandad", "selfdrive.pandad", persistent=True),
   PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
   PythonProcess("plannerd", "selfdrive.controls.plannerd"),
   PythonProcess("radard", "selfdrive.controls.radard"),
-  PythonProcess("lanespeedd", "selfdrive.controls.lane_speed"),
+  PythonProcess("sentryd", "selfdrive.sentryd", persistent=True),
   PythonProcess("thermald", "selfdrive.thermald.thermald", persistent=True),
   PythonProcess("timezoned", "selfdrive.timezoned", enabled=TICI, persistent=True),
   PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, persistent=True),

selfdrive/sentryd.py

@@ -0,0 +1,153 @@
+#!/usr/bin/env python3
+import numpy as np
+
+from cereal import messaging
+from common.filter_simple import FirstOrderFilter
+from common.params import Params
+from common.realtime import sec_since_boot, DT_CTRL
+from opendbc.can.parser import CANParser
+
+# ****** Sentry mode states ******
+# Enabled: parameter is set allowing operation
+# Armed: watching for car movement
+# Tripped: movement tripped sentry mode, recording and alarming
+# Car active: any action that signifies a user is present and interacting with their car
+
+# ****** Sentry mode behavior ******
+# Sentry arms immediately when car is locked with keyfob
+# - Normal inactive timeout proceeds if driver locks car internally
+# If car is left unlocked (or falsely detected so), sentry becomes armed after INACTIVE_TIME
+# Locking, unlocking, or starting your car will disarm sentry mode and reset timer
+
+
+MAX_TIME_ONROAD = 5 * 60.  # after this is reached, car stops recording, disregarding movement
+MOVEMENT_TIME = 60.  # each movement resets onroad timer to this
+MIN_TIME_ONROAD = MOVEMENT_TIME + 5.
+INACTIVE_TIME = 2. * 60.  # car needs to be inactive for this time before sentry mode is enabled
+
+DEBUG = False
+
+signals = [
+  ("LOCK_STATUS_CHANGED", "DOOR_LOCKS", 0),
+  ("LOCK_STATUS", "DOOR_LOCKS", 1),  # 1 is unlocked
+  ("LOCKED_VIA_KEYFOB", "DOOR_LOCKS", 0),
+]
+
+
+class SentryMode:
+  def __init__(self):
+    self.sm = messaging.SubMaster(['deviceState', 'sensorEvents'], poll=['sensorEvents'])
+    self.pm = messaging.PubMaster(['sentryState'])
+
+    # TODO: Only toyota supported for now. detect car type and switch DBC/signals
+    self.cp = CANParser("toyota_nodsu_pt_generated", signals, bus=0, enforce_checks=False)
+    self.can_sock = messaging.sub_sock('can', timeout=100)
+
+    self.prev_accel = np.zeros(3)
+    self.initialized = False
+
+    self.params = Params()
+    self.sentry_enabled = self.params.get_bool("SentryMode")
+    self.last_read_ts = sec_since_boot()
+
+    self.car_locked = False
+    self.sentry_tripped = False
+    self.sentry_armed = False
+    self.sentry_tripped_ts = 0.
+    self.car_active_ts = sec_since_boot()  # start at active
+    self.movement_ts = 0.
+    self.accel_filters = [FirstOrderFilter(0, 0.5, DT_CTRL) for _ in range(3)]
+
+  def sprint(self, *args, **kwargs):  # slow print
+    if DEBUG:
+      if self.sm.frame % (100 / 20.) == 0:  # 20 hz
+        print(*args, **kwargs)
+
+  def update(self):
+    # Update CAN
+    can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
+    self.cp.update_strings(can_strs)
+
+    # Update car locked status
+    self.car_locked = self.cp.vl["DOOR_LOCKS"]["LOCK_STATUS"] == 0
+    self.sprint("car locked: {}".format(self.car_locked))
+
+    # Update parameter
+    now_ts = sec_since_boot()
+    if now_ts - self.last_read_ts > 15.:
+      self.sentry_enabled = self.params.get_bool("SentryMode")
+      self.last_read_ts = float(now_ts)
+
+    # Handle sensors
+    for sensor in self.sm['sensorEvents']:
+      if sensor.which() == 'acceleration':
+        accels = sensor.acceleration.v
+        if len(accels) == 3:  # sometimes is empty, in that case don't update
+          if self.initialized:  # prevent initial jump # TODO: can remove since we start at an active car state?
+            for idx, v in enumerate(accels):
+              self.accel_filters[idx].update(accels[idx] - self.prev_accel[idx])
+          self.initialized = True
+          self.prev_accel = list(accels)
+
+    self.update_sentry_tripped(now_ts)
+    # self.sprint(f"{self.sentry_tripped=}")
+
+  def is_sentry_armed(self, now_ts):
+    """Returns if sentry is actively monitoring for movements/can be alarmed"""
+    # Handle car interaction, reset interaction timeout
+    car_active = self.sm['deviceState'].started
+    car_active |= bool(self.cp.vl["DOOR_LOCKS"]["LOCK_STATUS_CHANGED"])
+    if bool(self.cp.vl["DOOR_LOCKS"]["LOCK_STATUS_CHANGED"]):
+      self.sprint('lock status changed!')
+    if car_active:
+      self.car_active_ts = float(now_ts)
+
+    car_inactive_long_enough = now_ts - self.car_active_ts > INACTIVE_TIME
+    car_locked_with_fob = self.car_locked and bool(self.cp.vl["DOOR_LOCKS"]["LOCKED_VIA_KEYFOB"])
+    return self.sentry_enabled and (car_inactive_long_enough or car_locked_with_fob)
+
+  def update_sentry_tripped(self, now_ts):
+    movement = any([abs(a_filter.x) > .01 for a_filter in self.accel_filters])
+    if movement:
+      self.movement_ts = float(now_ts)
+
+    # For as long as we see movement, extend timer by MOVEMENT_TIME.
+    tripped_long_enough = now_ts - self.movement_ts > MOVEMENT_TIME
+    tripped_long_enough &= now_ts - self.sentry_tripped_ts > MIN_TIME_ONROAD  # minimum of
+    tripped_long_enough |= now_ts - self.sentry_tripped_ts > MAX_TIME_ONROAD  # maximum of
+
+    sentry_tripped = False
+    self.sentry_armed = self.is_sentry_armed(now_ts)
+    self.sprint(f"{now_ts - self.sentry_tripped_ts=} > {MIN_TIME_ONROAD=}")
+    if self.sentry_armed:
+      if movement:  # trip if armed and there's movement
+        sentry_tripped = True
+      elif self.sentry_tripped and not tripped_long_enough:  # trip for long enough
+        sentry_tripped = True
+
+    # set when we first tripped
+    if sentry_tripped and not self.sentry_tripped:
+      self.sentry_tripped_ts = sec_since_boot()
+    self.sentry_tripped = sentry_tripped
+
+  def publish(self):
+    sentry_state = messaging.new_message('sentryState')
+    sentry_state.sentryState.started = bool(self.sentry_tripped)
+    sentry_state.sentryState.armed = bool(self.sentry_armed)
+
+    self.pm.send('sentryState', sentry_state)
+
+  def start(self):
+    while 1:
+      self.sm.update()
+      self.update()
+      self.publish()
+
+
+def main():
+  sentry_mode = SentryMode()
+  sentry_mode.start()
+
+
+if __name__ == "__main__":
+  main()

selfdrive/thermald/thermald.py

@@ -158,7 +158,7 @@ def thermald_thread() -> NoReturn:
 
   pandaState_timeout = int(1000 * 2.5 * DT_TRML)  # 2.5x the expected pandaState frequency
   pandaState_sock = messaging.sub_sock('pandaStates', timeout=pandaState_timeout)
-  sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "managerState"])
+  sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "managerState", "sentryState"])
 
   fan_speed = 0
   count = 0
@@ -347,10 +347,11 @@ def thermald_thread() -> NoReturn:
       set_offroad_alert_if_changed("Offroad_StorageMissing", (not Path("/data/media").is_mount()))
 
     # Handle offroad/onroad transition
-    should_start = all(onroad_conditions.values())
+    should_start = all(onroad_conditions.values()) or sm["sentryState"].started
     if started_ts is None:
       should_start = should_start and all(startup_conditions.values())
 
+    # tie sentry started in with ignition started, differentiate when we publish
     if should_start != should_start_prev or (count == 0):
       params.put_bool("IsOnroad", should_start)
       params.put_bool("IsOffroad", not should_start)
@@ -393,7 +394,8 @@ def thermald_thread() -> NoReturn:
     ui_running_prev = ui_running
 
     msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90  # if current is positive, then battery is being discharged
-    msg.deviceState.started = started_ts is not None
+    msg.deviceState.started = started_ts is not None and onroad_conditions["ignition"]
+    msg.deviceState.startedSentry = started_ts is not None and not onroad_conditions["ignition"]
     msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0))
 
     last_ping = params.get("LastAthenaPingTime")

selfdrive/ui/qt/onroad.cc

@@ -55,7 +55,7 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
 
 void OnroadWindow::updateState(const UIState &s) {
   QColor bgColor = bg_colors[s.status];
-  Alert alert = Alert::get(*(s.sm), s.scene.started_frame);
+  Alert alert = Alert::get(*(s.sm), s.scene.started_frame, s.scene.started_sentry);
   if (s.sm->updated("controlsState") || !alert.equal({})) {
     if (alert.type == "controlsUnresponsive") {
       bgColor = bg_colors[STATUS_ALERT];

selfdrive/ui/qt/widgets/controls.h

@@ -101,13 +101,11 @@ class ToggleControl : public AbstractControl {
     QObject::connect(&toggle, &Toggle::stateChanged, this, &ToggleControl::toggleFlipped);
   }
 
+  Toggle toggle;
   void setEnabled(bool enabled) { toggle.setEnabled(enabled); }
 
 signals:
   void toggleFlipped(bool state);
-
-protected:
-  Toggle toggle;
 };
 
 // widget to toggle params