From d7425d2c096ced300d96e4f1fa0b394d3f0e15a7 Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Thu, 22 Aug 2024 14:24:52 +0200 Subject: [PATCH 1/3] Enabling tumble-check settings to be platform-specific --- src/modules/src/supervisor.c | 8 +++---- src/platform/interface/platform_defaults.h | 23 +++++++++++++++++-- .../interface/platform_defaults_flapper.h | 10 +++++--- 3 files changed, 32 insertions(+), 9 deletions(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 66649ca353..9940f5d77a 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) { 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 9c5548c70a..840d869544 100644 --- a/src/platform/interface/platform_defaults_flapper.h +++ b/src/platform/interface/platform_defaults_flapper.h @@ -144,8 +144,12 @@ #define IMU_PSI 180.0f #endif -// Disable tumble check // -////////////////////////// -#define SUPERVISOR_TUMBLE_CHECK_ENABLE false +// Tumble check settings // +/////////////////////////// +#define SUPERVISOR_TUMBLE_CHECK_ENABLE true +#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 From ed55d649fc3efc281f27bdab37e5d072d78f4336 Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Tue, 27 Aug 2024 14:46:11 +0200 Subject: [PATCH 2/3] Tumble check can be disabled via parameter --- src/modules/src/supervisor.c | 12 +++++++++++- src/modules/src/supervisor_state_machine.c | 18 ++++++------------ src/platform/interface/platform_defaults.h | 4 ---- .../interface/platform_defaults_flapper.h | 1 - 4 files changed, 17 insertions(+), 18 deletions(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 9940f5d77a..80089dfb84 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -297,6 +297,8 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t } } +uint8_t tumbleCheckEnabled = 1; + 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 3accbfaa6e..72d2e58c4b 100644 --- a/src/platform/interface/platform_defaults.h +++ b/src/platform/interface/platform_defaults.h @@ -124,10 +124,6 @@ #endif // 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 diff --git a/src/platform/interface/platform_defaults_flapper.h b/src/platform/interface/platform_defaults_flapper.h index 840d869544..1ea5597d98 100644 --- a/src/platform/interface/platform_defaults_flapper.h +++ b/src/platform/interface/platform_defaults_flapper.h @@ -146,7 +146,6 @@ // Tumble check settings // /////////////////////////// -#define SUPERVISOR_TUMBLE_CHECK_ENABLE true #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 From 2c74eb9e1d3ee8004cff88a8515c673ce6a5d7c3 Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Mon, 9 Sep 2024 17:18:53 +0200 Subject: [PATCH 3/3] returning SUPERVISOR_TUMBLE_CHECK_ENABLE --- src/modules/src/supervisor.c | 2 +- src/platform/interface/platform_defaults.h | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c index 80089dfb84..d9e6ae3e73 100644 --- a/src/modules/src/supervisor.c +++ b/src/modules/src/supervisor.c @@ -297,7 +297,7 @@ static void postTransitionActions(SupervisorMem_t* this, const supervisorState_t } } -uint8_t tumbleCheckEnabled = 1; +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; diff --git a/src/platform/interface/platform_defaults.h b/src/platform/interface/platform_defaults.h index 72d2e58c4b..3accbfaa6e 100644 --- a/src/platform/interface/platform_defaults.h +++ b/src/platform/interface/platform_defaults.h @@ -124,6 +124,10 @@ #endif // 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