From d843555a06247db8d2c751ffa90440b6d51649de Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 29 Sep 2023 10:38:22 +0200
Subject: [PATCH] Added locked info to supervisor

---
 src/modules/interface/supervisor.h | 8 ++++++++
 src/modules/src/supervisor.c       | 8 ++++++++
 2 files changed, 16 insertions(+)

diff --git a/src/modules/interface/supervisor.h b/src/modules/interface/supervisor.h
index 0eb4671672..48de77d8ed 100644
--- a/src/modules/interface/supervisor.h
+++ b/src/modules/interface/supervisor.h
@@ -108,3 +108,11 @@ bool supervisorCanArm();
  * @return false
  */
 bool supervisorIsArmed();
+
+/**
+ * @brief Query if the system is locked (due to a crash)
+ *
+ * @return true
+ * @return false
+ */
+bool supervisorIsLocked();
diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c
index ab7c50df17..2e209829ab 100644
--- a/src/modules/src/supervisor.c
+++ b/src/modules/src/supervisor.c
@@ -111,6 +111,10 @@ bool supervisorIsArmed() {
   return supervisorMem.isArmingActivated || supervisorMem.deprecatedArmParam;
 }
 
+bool supervisorIsLocked() {
+  return supervisorStateLocked == supervisorMem.state;
+}
+
 bool supervisorRequestArming(const bool doArm) {
   if (doArm == supervisorMem.isArmingActivated) {
     return true;
@@ -305,6 +309,9 @@ static void updateLogData(SupervisorMem_t* this, const supervisorConditionBits_t
   if (this->isTumbled) {
     this->infoBitfield |= 0x0020;
   }
+  if (supervisorStateLocked == this->state) {
+    this->infoBitfield |= 0x0040;
+  }
 }
 
 void supervisorUpdate(const sensorData_t *sensors, const setpoint_t* setpoint, stabilizerStep_t stabilizerStep) {
@@ -440,6 +447,7 @@ LOG_GROUP_START(supervisor)
  * Bit 3 = can fly - the Crazyflie is ready to fly
  * Bit 4 = is flying - the Crazyflie is flying.
  * Bit 5 = is tumbled - the Crazyflie is up side down.
+ * Bit 6 = is locked - the Crazyflie is in the locked state and must be restarted.
  */
 LOG_ADD(LOG_UINT16, info, &supervisorMem.infoBitfield)
 LOG_GROUP_STOP(supervisor)