Lock out for excessive actuation (#35792)

* excessive actuation

* text

* software

* check

* logic

* whoops

* dont want to lose alert unless user interacts with it

* implement

* try x2

* counter

* try to false trigger

* use livepose

* need to check for livePose noise

* cmt

* nl

* add back

* organization

* setVisible and isVisible consecutively don't work

* style

* cant do this sadly

* actually we can!

* clean up

* clean up

* clean up

* need to match torqued, paramsd, lagd, etc. (fix op sim)

cereal/log.capnp

@@ -128,6 +128,7 @@ struct OnroadEvent @0xc4fa6047f024e718 {
     personalityChanged @91;
     aeb @92;
     userFlag @95;
+    excessiveActuation @96;
 
     soundsUnavailableDEPRECATED @47;
   }

common/params_keys.h

@@ -87,6 +87,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"Offroad_CarUnrecognized", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
     {"Offroad_ConnectivityNeeded", {CLEAR_ON_MANAGER_START, JSON}},
     {"Offroad_ConnectivityNeededPrompt", {CLEAR_ON_MANAGER_START, JSON}},
+    {"Offroad_ExcessiveActuation", {PERSISTENT, JSON}},
     {"Offroad_IsTakingSnapshot", {CLEAR_ON_MANAGER_START, JSON}},
     {"Offroad_NeosUpdate", {CLEAR_ON_MANAGER_START, JSON}},
     {"Offroad_NoFirmware", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},

selfdrive/selfdrived/alerts_offroad.json

@@ -40,5 +40,10 @@
   "Offroad_Recalibration": {
     "text": "openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.",
     "severity": 0
+  },
+  "Offroad_ExcessiveActuation": {
+    "text": "openpilot has detected excessive %1 actuation. This may be due to a software bug. Please contact support at https://comma.ai/support.",
+    "severity": 1,
+    "_comment": "Set extra field to lateral or longitudinal."
   }
 }

selfdrive/selfdrived/events.py

@@ -758,6 +758,11 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"),
   },
 
+  EventName.excessiveActuation: {
+    ET.SOFT_DISABLE: soft_disable_alert("Excessive Actuation"),
+    ET.NO_ENTRY: NoEntryAlert("Excessive Actuation"),
+  },
+
   EventName.overheat: {
     ET.PERMANENT: overheat_alert,
     ET.SOFT_DISABLE: soft_disable_alert("System Overheated"),

selfdrive/selfdrived/selfdrived.py

@@ -7,6 +7,7 @@ import cereal.messaging as messaging
 
 from cereal import car, log
 from msgq.visionipc import VisionIpcClient, VisionStreamType
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 
 
 from openpilot.common.params import Params
@@ -15,6 +16,7 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.gps import get_gps_location_service
 
 from openpilot.selfdrive.car.car_specific import CarSpecificEvents
+from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from openpilot.selfdrive.selfdrived.events import Events, ET
 from openpilot.selfdrive.selfdrived.state import StateMachine
 from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert
@@ -25,7 +27,9 @@ from openpilot.system.version import get_build_metadata
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
 TESTING_CLOSET = "TESTING_CLOSET" in os.environ
+
 LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}
+MIN_EXCESSIVE_ACTUATION_COUNT = int(0.25 / DT_CTRL)
 
 ThermalStatus = log.DeviceState.ThermalStatus
 State = log.SelfdriveState.OpenpilotState
@@ -39,6 +43,21 @@ SafetyModel = car.CarParams.SafetyModel
 IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
 
 
+def check_excessive_actuation(sm: messaging.SubMaster, CS: car.CarState, calibrator: PoseCalibrator, counter: int) -> tuple[int, bool]:
+  # CS.aEgo can be noisy to bumps in the road, transitioning from standstill, losing traction, etc.
+  device_pose = Pose.from_live_pose(sm['livePose'])
+  calibrated_pose = calibrator.build_calibrated_pose(device_pose)
+  accel_calibrated = calibrated_pose.acceleration.x
+
+  # livePose acceleration can be noisy due to bad mounting or aliased livePose measurements
+  accel_valid = abs(CS.aEgo - accel_calibrated) < 2
+
+  excessive_actuation = accel_calibrated > ACCEL_MAX * 2 or accel_calibrated < ACCEL_MIN * 2
+  counter = counter + 1 if sm['carControl'].longActive and excessive_actuation and accel_valid else 0
+
+  return counter, counter > MIN_EXCESSIVE_ACTUATION_COUNT
+
+
 class SelfdriveD:
   def __init__(self, CP=None):
     self.params = Params()
@@ -54,6 +73,7 @@ class SelfdriveD:
       self.CP = CP
 
     self.car_events = CarSpecificEvents(self.CP)
+    self.calibrator = PoseCalibrator()
 
     # Setup sockets
     self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'])
@@ -111,6 +131,8 @@ class SelfdriveD:
     self.experimental_mode = False
     self.personality = self.params.get("LongitudinalPersonality", return_default=True)
     self.recalibrating_seen = False
+    self.excessive_actuation = self.params.get("Offroad_ExcessiveActuation") is not None
+    self.excessive_actuation_counter = 0
     self.state_machine = StateMachine()
     self.rk = Ratekeeper(100, print_delay_threshold=None)
 
@@ -227,6 +249,18 @@ class SelfdriveD:
       if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture:
         self.events.add(EventName.ldw)
 
+    # Check for excessive (longitudinal) actuation
+    if self.sm.updated['liveCalibration']:
+      self.calibrator.feed_live_calib(self.sm['liveCalibration'])
+
+    self.excessive_actuation_counter, excessive_actuation = check_excessive_actuation(self.sm, CS, self.calibrator, self.excessive_actuation_counter)
+    if not self.excessive_actuation and excessive_actuation:
+      set_offroad_alert("Offroad_ExcessiveActuation", True, extra_text="longitudinal")
+      self.excessive_actuation = True
+
+    if self.excessive_actuation:
+      self.events.add(EventName.excessiveActuation)
+
     # Handle lane change
     if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
       direction = self.sm['modelV2'].meta.laneChangeDirection

selfdrive/ui/qt/widgets/offroad_alerts.cc

@@ -32,15 +32,19 @@ AbstractAlert::AbstractAlert(bool hasRebootBtn, QWidget *parent) : QFrame(parent
   footer_layout->addWidget(dismiss_btn, 0, Qt::AlignBottom | Qt::AlignLeft);
   QObject::connect(dismiss_btn, &QPushButton::clicked, this, &AbstractAlert::dismiss);
 
-  snooze_btn = new QPushButton(tr("Snooze Update"));
+  action_btn = new QPushButton();
-  snooze_btn->setVisible(false);
+  action_btn->setVisible(false);
-  snooze_btn->setFixedSize(550, 125);
+  action_btn->setFixedHeight(125);
-  footer_layout->addWidget(snooze_btn, 0, Qt::AlignBottom | Qt::AlignRight);
+  footer_layout->addWidget(action_btn, 0, Qt::AlignBottom | Qt::AlignRight);
-  QObject::connect(snooze_btn, &QPushButton::clicked, [=]() {
+  QObject::connect(action_btn, &QPushButton::clicked, [=]() {
-    params.putBool("SnoozeUpdate", true);
+    if (!alerts["Offroad_ExcessiveActuation"]->text().isEmpty()) {
+      params.remove("Offroad_ExcessiveActuation");
+    } else {
+      params.putBool("SnoozeUpdate", true);
+    }
   });
-  QObject::connect(snooze_btn, &QPushButton::clicked, this, &AbstractAlert::dismiss);
+  QObject::connect(action_btn, &QPushButton::clicked, this, &AbstractAlert::dismiss);
-  snooze_btn->setStyleSheet(R"(color: white; background-color: #4F4F4F;)");
+  action_btn->setStyleSheet("color: white; background-color: #4F4F4F; padding-left: 60px; padding-right: 60px;");
 
   if (hasRebootBtn) {
     QPushButton *rebootBtn = new QPushButton(tr("Reboot and Update"));
@@ -107,7 +111,14 @@ int OffroadAlert::refresh() {
     label->setVisible(!text.isEmpty());
     alertCount += !text.isEmpty();
   }
-  snooze_btn->setVisible(!alerts["Offroad_ConnectivityNeeded"]->text().isEmpty());
+
+  action_btn->setVisible(!alerts["Offroad_ExcessiveActuation"]->text().isEmpty() || !alerts["Offroad_ConnectivityNeeded"]->text().isEmpty());
+  if (!alerts["Offroad_ExcessiveActuation"]->text().isEmpty()) {
+    action_btn->setText(tr("Acknowledge Excessive Actuation"));
+  } else {
+    action_btn->setText(tr("Snooze Update"));
+  }
+
   return alertCount;
 }
 

selfdrive/ui/qt/widgets/offroad_alerts.h

@@ -15,9 +15,10 @@ class AbstractAlert : public QFrame {
 protected:
   AbstractAlert(bool hasRebootBtn, QWidget *parent = nullptr);
 
-  QPushButton *snooze_btn;
+  QPushButton *action_btn;
   QVBoxLayout *scrollable_layout;
   Params params;
+  std::map<std::string, QLabel*> alerts;
 
 signals:
   void dismiss();
@@ -40,7 +41,4 @@ class OffroadAlert : public AbstractAlert {
 public:
   explicit OffroadAlert(QWidget *parent = 0) : AbstractAlert(false, parent) {}
   int refresh();
-
-private:
-  std::map<std::string, QLabel*> alerts;
 };

system/hardware/hardwared.py

@@ -303,6 +303,7 @@ def hardware_thread(end_event, hw_queue) -> None:
     # **** starting logic ****
 
     startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate")
+    startup_conditions["no_excessive_actuation"] = params.get("Offroad_ExcessiveActuation") is None
     startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
     startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version