Emergency stop disable

crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/supervisor.c

@@ -29,6 +29,7 @@
 #include <stdlib.h>
 
 #include "log.h"
+#include "param.h"
 #include "motors.h"
 #include "pm.h"
 #include "stabilizer.h"
@@ -43,6 +44,7 @@
 static bool canFly;
 static bool isFlying;
 static bool isTumbled;
+static bool emergencyStopEnable = 1;
 
 bool supervisorCanFly()
 {
@@ -118,7 +120,9 @@ void supervisorUpdate(const sensorData_t *data)
 
   isTumbled = isTumbledCheck(data);
   if (isTumbled && isFlying) {
-    stabilizerSetEmergencyStop();
+    if(emergencyStopEnable){
+      stabilizerSetEmergencyStop();
+    }
   }
 
   canFly = canFlyCheck();
@@ -141,3 +145,14 @@ LOG_ADD_CORE(LOG_UINT8, isFlying, &isFlying)
  */
 LOG_ADD_CORE(LOG_UINT8, isTumbled, &isTumbled)
 LOG_GROUP_STOP(sys)
+
+PARAM_GROUP_START(sys)
+
+/**
+ * @brief If 1 (default) enables the emergency stop when the crazyflie is tumbling,
+ *  if 0 disables emergency stop (used for testing purposes)
+ * 
+ */
+PARAM_ADD(PARAM_TYPE_INT, e_stop, &emergencyStopEnable)
+
+PARAM_GROUP_STOP(sys)