From 02adda07b575d53c9ef272c0235f0d65fee06586 Mon Sep 17 00:00:00 2001
From: Tamas Nepusz <ntamas@gmail.com>
Date: Fri, 13 Sep 2019 09:29:48 +0200
Subject: [PATCH] cut motor thrust permanently after a tumble

---
 src/modules/src/sitaw.c | 28 +++++++++++++++++++++++++---
 1 file changed, 25 insertions(+), 3 deletions(-)

diff --git a/src/modules/src/sitaw.c b/src/modules/src/sitaw.c
index 87a9707ec2..42e3029760 100644
--- a/src/modules/src/sitaw.c
+++ b/src/modules/src/sitaw.c
@@ -32,7 +32,7 @@
 #include "param.h"
 #include "trigger.h"
 #include "sitaw.h"
-#include "commander.h"
+#include "crtp_commander_high_level.h"
 
 /* Trigger object used to detect Free Fall situation. */
 static trigger_t sitAwFFAccWZ;
@@ -43,6 +43,12 @@ static trigger_t sitAwARAccZ;
 /* Trigger object used to detect Tumbled situation. */
 static trigger_t sitAwTuAngle;
 
+/* Flag to denote whether we have cut the motor thrust because of a tumbled situation earlier. */
+static bool sitAwMotorThrustCut;
+
+/* Flag that the user can set to 1 to mark a tumbled situation as resolved. */
+static bool sitAwResetRequested;
+
 #if defined(SITAW_ENABLED)
 
 #if defined(SITAW_LOG_ENABLED) /* Enable the log group. */
@@ -63,6 +69,7 @@ LOG_ADD(LOG_UINT8, TuDetected, &sitAwTuAngle.released)
 LOG_ADD(LOG_UINT8, FFAccWZDetected, &sitAwFFAccWZ.released)
 LOG_ADD(LOG_UINT8, ARDetected, &sitAwARAccZ.released)
 LOG_ADD(LOG_UINT8, TuDetected, &sitAwTuAngle.released)
+LOG_ADD(LOG_UINT8, MotorThrustCut, &sitAwMotorThrustCut)
 #endif
 LOG_GROUP_STOP(sitAw)
 #endif /* SITAW_LOG_ENABLED */
@@ -84,6 +91,7 @@ PARAM_ADD(PARAM_UINT8, TuActive, &sitAwTuAngle.active)
 PARAM_ADD(PARAM_UINT32, TuTriggerCount, &sitAwTuAngle.triggerCount)
 PARAM_ADD(PARAM_FLOAT, TuAngle, &sitAwTuAngle.threshold)
 #endif
+PARAM_ADD(PARAM_UINT8, resetDetector, &sitAwResetRequested);
 PARAM_GROUP_STOP(sitAw)
 #endif /* SITAW_PARAM_ENABLED */
 
@@ -132,12 +140,26 @@ static void sitAwPreThrustUpdateCallOut(setpoint_t *setpoint)
 
 #if defined(SITAW_ENABLED)
 #ifdef SITAW_TU_ENABLED
-      if(sitAwTuDetected()) {
+      if(sitAwResetRequested) {
+        /* Reset the "tumbled earlier" flag if the operator marks the situation as resolved */
+        sitAwMotorThrustCut = 0;
+        sitAwResetRequested = 0;
+      }
+
+      if(sitAwTuDetected() || sitAwMotorThrustCut) {
         /* Kill the thrust to the motors if a Tumbled situation is detected. */
         setpoint->mode.x = modeDisable;
         setpoint->mode.y = modeDisable;
         setpoint->mode.z = modeDisable;
         setpoint->thrust = 0;
+
+        /* Also stop the high-level commander */
+        crtpCommanderHighLevelStop();
+
+        /* And set a flag to mark that we have cut the motor thrust and we should not
+         * re-enable the motors until the operator marks the situation as
+         * resolved */
+        sitAwMotorThrustCut = 1;
       }
 #endif
 
@@ -298,7 +320,7 @@ void sitAwTuInit(void)
  * cut the thrust to the motors, avoiding the crazyflie from running
  * propellers at significant thrust when accidentially crashing into walls
  * or the ground.
-
+ *
  * @param The actual roll in degrees. +180/-180 degrees means upside down.
  * @param The actual pitch in degrees. 0 degrees means horizontal.
  *