diff --git a/cereal/log.capnp b/cereal/log.capnp index 9ffcbf72ed..ad2ecc6049 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -128,6 +128,7 @@ struct OnroadEvent @0xc4fa6047f024e718 { personalityChanged @91; aeb @92; userFlag @95; + excessiveActuation @96; soundsUnavailableDEPRECATED @47; } diff --git a/common/params_keys.h b/common/params_keys.h index d3b75e4abd..a08d69f0fc 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -87,6 +87,7 @@ inline static std::unordered_map 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}}, diff --git a/selfdrive/selfdrived/alerts_offroad.json b/selfdrive/selfdrived/alerts_offroad.json index b7fcce5c44..7c97f5a1c2 100644 --- a/selfdrive/selfdrived/alerts_offroad.json +++ b/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." } } diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 369685f398..fe4c1a6820 100755 --- a/selfdrive/selfdrived/events.py +++ b/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"), diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 6e3e622430..63ce471126 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/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 diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.cc b/selfdrive/ui/qt/widgets/offroad_alerts.cc index f630875978..3a4828a2a8 100644 --- a/selfdrive/ui/qt/widgets/offroad_alerts.cc +++ b/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")); - snooze_btn->setVisible(false); - snooze_btn->setFixedSize(550, 125); - footer_layout->addWidget(snooze_btn, 0, Qt::AlignBottom | Qt::AlignRight); - QObject::connect(snooze_btn, &QPushButton::clicked, [=]() { - params.putBool("SnoozeUpdate", true); + action_btn = new QPushButton(); + action_btn->setVisible(false); + action_btn->setFixedHeight(125); + footer_layout->addWidget(action_btn, 0, Qt::AlignBottom | Qt::AlignRight); + QObject::connect(action_btn, &QPushButton::clicked, [=]() { + if (!alerts["Offroad_ExcessiveActuation"]->text().isEmpty()) { + params.remove("Offroad_ExcessiveActuation"); + } else { + params.putBool("SnoozeUpdate", true); + } }); - QObject::connect(snooze_btn, &QPushButton::clicked, this, &AbstractAlert::dismiss); - snooze_btn->setStyleSheet(R"(color: white; background-color: #4F4F4F;)"); + QObject::connect(action_btn, &QPushButton::clicked, this, &AbstractAlert::dismiss); + 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; } diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.h b/selfdrive/ui/qt/widgets/offroad_alerts.h index ace2e75456..2dcf4f9d8c 100644 --- a/selfdrive/ui/qt/widgets/offroad_alerts.h +++ b/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 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 alerts; }; diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index d5b8d6397b..17ac4d1618 100755 --- a/system/hardware/hardwared.py +++ b/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