diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 66649ca353..d9e6ae3e73 100644 --- a/src/modules/src/supervisor.c +++ b/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) diff --git a/src/modules/src/supervisor_state_machine.c b/src/modules/src/supervisor_state_machine.c index 2ccb2c7e65..636b6e378b 100644 --- a/src/modules/src/supervisor_state_machine.c +++ b/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, diff --git a/src/platform/interface/platform_defaults.h b/src/platform/interface/platform_defaults.h index 6798e10dcd..3accbfaa6e 100644 --- a/src/platform/interface/platform_defaults.h +++ b/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 diff --git a/src/platform/interface/platform_defaults_flapper.h b/src/platform/interface/platform_defaults_flapper.h index a4f9061696..20fed7bb49 100644 --- a/src/platform/interface/platform_defaults_flapper.h +++ b/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