From a793b949bea7308051448924e0f3a97c396e39e8 Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Wed, 15 Dec 2021 17:58:16 -0800
Subject: [PATCH] boardd: add controls heartbeat (#23185)

* boardd: add controls heartbeat

* bump panda

* bump again

* bump panda
---
 panda                      | 2 +-
 selfdrive/boardd/boardd.cc | 7 ++++++-
 selfdrive/boardd/panda.cc  | 4 ++--
 selfdrive/boardd/panda.h   | 2 +-
 4 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/panda b/panda
index 652367d2e82f21..6f95a096e6beb2 160000
--- a/panda
+++ b/panda
@@ -1 +1 @@
-Subproject commit 652367d2e82f21f996c2217857d20ea05567ad62
+Subproject commit 6f95a096e6beb254786759003a38e6e5c4f2c10e
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index d09738a709de82..0ca943de2e84f7 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -411,6 +411,8 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
   util::set_thread_name("boardd_panda_state");
 
   Params params;
+  SubMaster sm({"controlsState"});
+
   Panda *peripheral_panda = pandas[0];
   bool ignition_last = false;
   std::future<bool> safety_future;
@@ -445,8 +447,11 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
 
     ignition_last = ignition;
 
+    sm.update(0);
+    const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled();
+
     for (const auto &panda : pandas) {
-      panda->send_heartbeat();
+      panda->send_heartbeat(engaged);
     }
     util::sleep_for(500);
   }
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index 751f735ca589c4..6850ad41b26522 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -340,8 +340,8 @@ void Panda::set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode)
   usb_write(0xe6, (uint16_t)power_mode, 0);
 }
 
-void Panda::send_heartbeat() {
-  usb_write(0xf3, 1, 0);
+void Panda::send_heartbeat(bool engaged) {
+  usb_write(0xf3, engaged, 0);
 }
 
 void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) {
diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h
index fe69189489e4fe..1a18a7f15a4a5e 100644
--- a/selfdrive/boardd/panda.h
+++ b/selfdrive/boardd/panda.h
@@ -108,7 +108,7 @@ class Panda {
   std::optional<std::string> get_serial();
   void set_power_saving(bool power_saving);
   void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
-  void send_heartbeat();
+  void send_heartbeat(bool engaged);
   void set_can_speed_kbps(uint16_t bus, uint16_t speed);
   void set_data_speed_kbps(uint16_t bus, uint16_t speed);
   void can_send(capnp::List<cereal::CanData>::Reader can_data_list);