Merge pull request #1408 from flapper-drones/PR_tumbleCheckPlatformSp…

…ecific

Tumble check: platform-specific settings

src/modules/src/supervisor.c

@@ -205,11 +205,11 @@ static bool isFlyingCheck(SupervisorMem_t* this, const uint32_t tick) {
 static bool isTumbledCheck(SupervisorMem_t* this, const sensorData_t *data, const uint32_t tick) {
   const float freeFallThreshold = 0.1;
 
-  const float acceptedTiltAccZ = 0.5;  // 60 degrees tilt (when stationary)
-  const uint32_t maxTiltTime = M2T(1000);
+  const float acceptedTiltAccZ = SUPERVISOR_TUMBLE_CHECK_ACCEPTED_TILT_ACCZ;  // 60 degrees tilt (when stationary)
+  const uint32_t maxTiltTime = M2T(SUPERVISOR_TUMBLE_CHECK_ACCEPTED_TILT_TIME);
 
-  const float acceptedUpsideDownAccZ = -0.2;
-  const uint32_t maxUpsideDownTime = M2T(100);
+  const float acceptedUpsideDownAccZ = SUPERVISOR_TUMBLE_CHECK_ACCEPTED_UPSIDEDOWN_ACCZ;
+  const uint32_t maxUpsideDownTime = M2T(SUPERVISOR_TUMBLE_CHECK_ACCEPTED_UPSIDEDOWN_TIME);
 
   const bool isFreeFalling = (fabsf(data->acc.z) < freeFallThreshold && fabsf(data->acc.y) < freeFallThreshold && fabsf(data->acc.x) < freeFallThreshold);
   if (isFreeFalling) {
@@ -297,6 +297,8 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t
   }
 }
 
+uint8_t tumbleCheckEnabled = SUPERVISOR_TUMBLE_CHECK_ENABLE;
+
 static supervisorConditionBits_t updateAndPopulateConditions(SupervisorMem_t* this, const sensorData_t *sensors, const setpoint_t* setpoint, const uint32_t currentTick) {
   supervisorConditionBits_t conditions = 0;
 
@@ -311,7 +313,10 @@ static supervisorConditionBits_t updateAndPopulateConditions(SupervisorMem_t* th
 
   const bool isTumbled = isTumbledCheck(this, sensors, currentTick);
   if (isTumbled) {
-    conditions |= SUPERVISOR_CB_IS_TUMBLED;
+    if (tumbleCheckEnabled)
+    {
+      conditions |= SUPERVISOR_CB_IS_TUMBLED;
+    }
   }
 
   const uint32_t setpointAge = currentTick - setpoint->timestamp;
@@ -534,4 +539,9 @@ PARAM_ADD(PARAM_UINT8, infdmp, &supervisorMem.doinfodump)
  */
 PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, landedTimeout, &landingTimeoutDuration)
 
+/**
+ * @brief Set to zero to disable tumble check
+ */
+PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, tmblChckEn, &tumbleCheckEnabled)
+
 PARAM_GROUP_STOP(supervisor)

src/modules/src/supervisor_state_machine.c

@@ -63,12 +63,6 @@ static const char* const conditionNames[] = {
 };
 static_assert(sizeof(conditionNames) / sizeof(conditionNames[0]) == supervisorCondition_NrOfConditions);
 
-#if SUPERVISOR_TUMBLE_CHECK_ENABLE
-  #define SUPERVISOR_CB_CONF_IS_TUMBLED (SUPERVISOR_CB_IS_TUMBLED)
-#else
-  #define SUPERVISOR_CB_CONF_IS_TUMBLED (SUPERVISOR_CB_NONE)
-#endif
-
 // State transition definitions
 static SupervisorStateTransition_t transitionsNotInitialized[] = {
   {
@@ -93,7 +87,7 @@ static SupervisorStateTransition_t transitionsPreFlChecksNotPassed[] = {
 
     .triggerCombiner = supervisorAlways,
 
-    .blockers = SUPERVISOR_CB_CONF_IS_TUMBLED,
+    .blockers = SUPERVISOR_CB_IS_TUMBLED,
     .negatedBlockers = SUPERVISOR_CB_NONE,
     .blockerCombiner = supervisorAny,
   }
@@ -112,7 +106,7 @@ static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = {
   {
     .newState = supervisorStatePreFlChecksNotPassed,
 
-    .triggers = SUPERVISOR_CB_CONF_IS_TUMBLED,
+    .triggers = SUPERVISOR_CB_IS_TUMBLED,
     .negatedTriggers = SUPERVISOR_CB_NONE,
     .triggerCombiner = supervisorAny,
 
@@ -125,7 +119,7 @@ static SupervisorStateTransition_t transitionsPreFlChecksPassed[] = {
     .negatedTriggers = SUPERVISOR_CB_NONE,
     .triggerCombiner = supervisorAll,
 
-    .blockers = SUPERVISOR_CB_CONF_IS_TUMBLED,
+    .blockers = SUPERVISOR_CB_IS_TUMBLED,
     .negatedBlockers = SUPERVISOR_CB_NONE,
     .blockerCombiner = supervisorAny,
   },
@@ -144,7 +138,7 @@ static SupervisorStateTransition_t transitionsReadyToFly[] = {
   {
     .newState = supervisorStatePreFlChecksNotPassed,
 
-    .triggers = SUPERVISOR_CB_CONF_IS_TUMBLED,
+    .triggers = SUPERVISOR_CB_IS_TUMBLED,
     .negatedTriggers = SUPERVISOR_CB_ARMED,
     .triggerCombiner = supervisorAny,
 
@@ -174,7 +168,7 @@ static SupervisorStateTransition_t transitionsFlying[] = {
   {
     .newState = supervisorStateCrashed,
 
-    .triggers = SUPERVISOR_CB_CONF_IS_TUMBLED,
+    .triggers = SUPERVISOR_CB_IS_TUMBLED,
     .negatedTriggers = SUPERVISOR_CB_ARMED,
     .triggerCombiner = supervisorAny,
 
@@ -235,7 +229,7 @@ static SupervisorStateTransition_t transitionsWarningLevelOut[] = {
   {
     .newState = supervisorStateExceptFreeFall,
 
-    .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_CONF_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
+    .triggers = SUPERVISOR_CB_COMMANDER_WDT_TIMEOUT | SUPERVISOR_CB_IS_TUMBLED | SUPERVISOR_CB_EMERGENCY_STOP,
     .negatedTriggers = SUPERVISOR_CB_ARMED,
     .triggerCombiner = supervisorAny,
 

src/platform/interface/platform_defaults.h

@@ -120,14 +120,33 @@
     #define PID_VEL_Z_FILT_CUTOFF 20.0f
 #endif
 #ifndef PID_VEL_Z_FILT_CUTOFF_BARO_Z_HOLD
-    #define PID_VEL_Z_FILT_CUTOFF_BARO_Z_HOLD 0.7 f
+    #define PID_VEL_Z_FILT_CUTOFF_BARO_Z_HOLD 0.7f
 #endif
 
-// Tumble detection enabled by default
+// Tumble detection settings
 #ifndef SUPERVISOR_TUMBLE_CHECK_ENABLE
     #define SUPERVISOR_TUMBLE_CHECK_ENABLE true
 #endif
 
+// 60 degrees tilt (when stationary)
+#ifndef SUPERVISOR_TUMBLE_CHECK_ACCEPTED_TILT_ACCZ
+    #define SUPERVISOR_TUMBLE_CHECK_ACCEPTED_TILT_ACCZ 0.5f
+#endif
+
+
+#ifndef SUPERVISOR_TUMBLE_CHECK_ACCEPTED_TILT_TIME
+    #define SUPERVISOR_TUMBLE_CHECK_ACCEPTED_TILT_TIME 1000
+#endif
+
+#ifndef SUPERVISOR_TUMBLE_CHECK_ACCEPTED_UPSIDEDOWN_ACCZ
+    #define SUPERVISOR_TUMBLE_CHECK_ACCEPTED_UPSIDEDOWN_ACCZ -0.2f
+#endif
+
+
+#ifndef SUPERVISOR_TUMBLE_CHECK_ACCEPTED_UPSIDEDOWN_TIME
+    #define SUPERVISOR_TUMBLE_CHECK_ACCEPTED_UPSIDEDOWN_TIME 100
+#endif
+
 // Landing timeout before disarming
 #ifndef LANDING_TIMEOUT_MS
     #define LANDING_TIMEOUT_MS 3000

src/platform/interface/platform_defaults_flapper.h

@@ -144,8 +144,11 @@
     #define IMU_PSI                   180.0f
 #endif
 
-// Disable tumble check //
-//////////////////////////
-#define SUPERVISOR_TUMBLE_CHECK_ENABLE false
+// Tumble check settings //
+///////////////////////////
+#define SUPERVISOR_TUMBLE_CHECK_ACCEPTED_TILT_ACCZ 0.0f
+#define SUPERVISOR_TUMBLE_CHECK_ACCEPTED_TILT_TIME 2000
+#define SUPERVISOR_TUMBLE_CHECK_ACCEPTED_UPSIDEDOWN_ACCZ -0.5f
+#define SUPERVISOR_TUMBLE_CHECK_ACCEPTED_UPSIDEDOWN_TIME 200
 
 #define YAW_MAX_DELTA 30.0