Skip to content

Commit

Permalink
Improved Tumble Detection and Handling
Browse files Browse the repository at this point in the history 
This change improves several issues with the current tumble
detector:
  1. The old detector was only enabled if the Kalman filter
     was initially enabled. If one switched dynamically to a
     different state estimator, this important safety feature did
     not engage.
  2. The old detector triggered by angle, which is an estimated
     value. If the state estimator diverged, no tumble was detected.
     The new solution relies on the accelerometer instead, a direct
     sensor measurement.
  3. The old detector was not sticky, i.e., a user might pick
     up a CF and it starts spinning again, see issue bitcraze#258. The new
     version triggers an emergencyStop instead. The emergency stop
     can be reset and read using the new parameter stabilizer.stop.
  4. Smaller fixes:
     a) Add more #ifdef guards to only compile actually enabled code.
     b) Interface clean-ups

This replaces the PR bitcraze#470. This change has been tested on a bench
test and with the Crazyswarm.
  • Loading branch information
@whoenig
whoenig committed Oct 4, 2019
1 parent 90c8b53 commit c78289f
Show file tree
Hide file tree
Showing 3 changed files with 81 additions and 76 deletions.
21 changes: 13 additions & 8 deletions src/modules/interface/sitaw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,12 +32,6 @@

#include "stabilizer_types.h"

bool sitAwFFTest(float accWZ, float accMag);
bool sitAwFFDetected(void);
bool sitAwARTest(float accX, float accY, float accZ);
bool sitAwARDetected(void);
bool sitAwTuTest(float eulerRollActual, float eulerPitchActual);
bool sitAwTuDetected(void);
void sitAwInit(void);
void sitAwUpdateSetpoint(setpoint_t *setpoint, const sensorData_t *sensorData,
const state_t *state);
Expand All @@ -57,8 +51,9 @@ void sitAwUpdateSetpoint(setpoint_t *setpoint, const sensorData_t *sensorData,
#define SITAW_AR_TRIGGER_COUNT 500 /* The number of consecutive tests for At Rest to be detected. Configured for 250Hz testing. */

/* Configuration options for the 'Tumbled' detection. */
#define SITAW_TU_THRESHOLD 60 /* The minimum roll angle indicating a Tumbled situation. */
#define SITAW_TU_TRIGGER_COUNT 15 /* The number of consecutive tests for Tumbled to be detected. Configured for 250Hz testing. */
#define SITAW_TU_ACC_THRESHOLD (-0.5f) /* The maximum acc.z value indicating a Tumbled situation. */
#define SITAW_TU_ACC_TRIGGER_COUNT 15 /* The number of consecutive tests for Tumbled to be detected. Configured for 250Hz testing. */
#define SITAW_TU_IN_FLIGHT_THRESHOLD 1000 /* Minimum summed motor PWM that means we are flying */

/* LOG configurations. Enable these to be able to log detection in the cfclient. */
#define SITAW_LOG_ENABLED /* Uncomment to enable LOG framework. */
Expand All @@ -73,4 +68,14 @@ void sitAwUpdateSetpoint(setpoint_t *setpoint, const sensorData_t *sensorData,
//#define SITAW_AR_PARAM_ENABLED /* Uncomment to enable PARAM framework for the At Rest detection. */
//#define SITAW_TU_PARAM_ENABLED /* Uncomment to enable PARAM framework for the Tumbled detection. */

#ifdef SITAW_FF_ENABLED
bool sitAwFFDetected(void);
#endif
#ifdef SITAW_AR_ENABLED
bool sitAwARDetected(void);
#endif
#ifdef SITAW_TU_ENABLED
bool sitAwTuDetected(void);
#endif

#endif
130 changes: 66 additions & 64 deletions src/modules/src/sitaw.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@
#include "trigger.h"
#include "sitaw.h"
#include "commander.h"
#include "stabilizer.h"
#include "motors.h"

/* Trigger object used to detect Free Fall situation. */
static trigger_t sitAwFFAccWZ;
Expand All @@ -41,7 +43,7 @@ static trigger_t sitAwFFAccWZ;
static trigger_t sitAwARAccZ;

/* Trigger object used to detect Tumbled situation. */
static trigger_t sitAwTuAngle;
static trigger_t sitAwTuAcc;

#if defined(SITAW_ENABLED)

Expand All @@ -56,13 +58,13 @@ LOG_ADD(LOG_UINT32, ARTestCounter, &sitAwARAccZ.testCounter)
LOG_ADD(LOG_UINT8, ARDetected, &sitAwARAccZ.released)
#endif
#if defined(SITAW_TU_LOG_ENABLED) /* Log trigger variables for Tumbled detection. */
LOG_ADD(LOG_UINT32, TuTestCounter, &sitAwTuAngle.testCounter)
LOG_ADD(LOG_UINT8, TuDetected, &sitAwTuAngle.released)
LOG_ADD(LOG_UINT32, TuTestCounter, &sitAwTuAcc.testCounter)
LOG_ADD(LOG_UINT8, TuDetected, &sitAwTuAcc.released)
#endif
#if defined(SITAW_LOG_ALL_DETECT_ENABLED) /* Log all the 'Detected' flags. */
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, TuDetected, &sitAwTuAcc.released)
#endif
LOG_GROUP_STOP(sitAw)
#endif /* SITAW_LOG_ENABLED */
Expand All @@ -80,26 +82,25 @@ PARAM_ADD(PARAM_UINT32, ARTriggerCount, &sitAwARAccZ.triggerCount)
PARAM_ADD(PARAM_FLOAT, ARaccZ, &sitAwARAccZ.threshold)
#endif
#if defined(SITAW_TU_PARAM_ENABLED) /* Param variables for Tumbled detection. */
PARAM_ADD(PARAM_UINT8, TuActive, &sitAwTuAngle.active)
PARAM_ADD(PARAM_UINT32, TuTriggerCount, &sitAwTuAngle.triggerCount)
PARAM_ADD(PARAM_FLOAT, TuAngle, &sitAwTuAngle.threshold)
PARAM_ADD(PARAM_UINT8, TuActive, &sitAwTuAcc.active)
PARAM_ADD(PARAM_UINT32, TuTriggerCount, &sitAwTuAcc.triggerCount)
PARAM_ADD(PARAM_FLOAT, TuAcc, &sitAwTuAcc.threshold)
#endif
PARAM_GROUP_STOP(sitAw)
#endif /* SITAW_PARAM_ENABLED */

#endif /* SITAW_ENABLED */

/**
* Initialize the Free Fall detection.
*
* See the sitAwFFTest() function for details.
*/
void sitAwFFInit(void)
{
triggerInit(&sitAwFFAccWZ, triggerFuncIsLE, SITAW_FF_THRESHOLD, SITAW_FF_TRIGGER_COUNT);
triggerActivate(&sitAwFFAccWZ, true);
}

// forward declaration of private functions
#ifdef SITAW_FF_ENABLED
static void sitAwFFTest(float accWZ, float accMag);
#endif
#ifdef SITAW_AR_ENABLED
static void sitAwARTest(float accX, float accY, float accZ);
#endif
#ifdef SITAW_TU_ENABLED
static void sitAwTuTest(float accz);
#endif

static void sitAwPostStateUpdateCallOut(const sensorData_t *sensorData,
const state_t *state)
Expand All @@ -116,8 +117,16 @@ static void sitAwPostStateUpdateCallOut(const sensorData_t *sensorData,
sitAwFFTest(state->acc.z, accMAG);
#endif
#ifdef SITAW_TU_ENABLED
/* Test values for Tumbled detection. */
sitAwTuTest(state->attitude.roll, state->attitude.pitch);
/* check if we actually fly */
int sumRatio = 0;
for (int i = 0; i < NBR_OF_MOTORS; ++i) {
sumRatio += motorsGetRatio(i);
}
bool isFlying = sumRatio > SITAW_TU_IN_FLIGHT_THRESHOLD;
if (isFlying) {
/* Test values for Tumbled detection. */
sitAwTuTest(sensorData->acc.z);
}
#endif
#ifdef SITAW_AR_ENABLED
/* Test values for At Rest detection. */
Expand All @@ -134,10 +143,7 @@ static void sitAwPreThrustUpdateCallOut(setpoint_t *setpoint)
#ifdef SITAW_TU_ENABLED
if(sitAwTuDetected()) {
/* 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;
stabilizerSetEmergencyStop();
}
#endif

Expand Down Expand Up @@ -167,6 +173,18 @@ void sitAwUpdateSetpoint(setpoint_t *setpoint, const sensorData_t *sensorData,
sitAwPreThrustUpdateCallOut(setpoint);
}

#ifdef SITAW_FF_ENABLED
/**
* Initialize the Free Fall detection.
*
* See the sitAwFFTest() function for details.
*/
void sitAwFFInit(void)
{
triggerInit(&sitAwFFAccWZ, triggerFuncIsLE, SITAW_FF_THRESHOLD, SITAW_FF_TRIGGER_COUNT);
triggerActivate(&sitAwFFAccWZ, true);
}

/**
* Test values for a Free Fall situation.
*
Expand All @@ -185,23 +203,21 @@ void sitAwUpdateSetpoint(setpoint_t *setpoint, const sensorData_t *sensorData,
*
* @param accWZ Vertical acceleration (regardless of orientation)
* @param accMAG All experienced accelerations.
*
* @return True if the situation has been detected, otherwise false.
*/
bool sitAwFFTest(float accWZ, float accMAG)
void sitAwFFTest(float accWZ, float accMAG)
{
/* Check that the total acceleration is close to zero. */
if(fabs(accMAG) > SITAW_FF_THRESHOLD) {
/* If the total acceleration deviates from 0, this is not a free fall situation. */
triggerReset(&sitAwFFAccWZ);
return false;
}
} else {

/**
* AccWZ approaches -1 in free fall. Check that the value stays within
* SITAW_FF_THRESHOLD of -1 for the triggerCount specified.
*/
return(triggerTestValue(&sitAwFFAccWZ, fabs(accWZ + 1)));
/**
* AccWZ approaches -1 in free fall. Check that the value stays within
* SITAW_FF_THRESHOLD of -1 for the triggerCount specified.
*/
triggerTestValue(&sitAwFFAccWZ, fabs(accWZ + 1));
}
}

/**
Expand All @@ -213,7 +229,9 @@ bool sitAwFFDetected(void)
{
return sitAwFFAccWZ.released;
}
#endif

#ifdef SITAW_AR_ENABLED
/**
* Initialize the At Rest detection.
*
Expand Down Expand Up @@ -241,16 +259,13 @@ void sitAwARInit(void)
* @param accX Horizontal X acceleration (when crazyflie is placed on its feet)
* @param accY Horizontal Y acceleration (when crazyflie is placed on its feet)
* @param accZ Vertical Z acceleration (when crazyflie is placed on its feet)
*
* @return True if the situation has been detected, otherwise false.
*/
bool sitAwARTest(float accX, float accY, float accZ)
void sitAwARTest(float accX, float accY, float accZ)
{
/* Check that there are no horizontal accelerations. At rest, these are 0. */
if((fabs(accX) > SITAW_AR_THRESHOLD) || (fabs(accY) > SITAW_AR_THRESHOLD)) {
/* If the X or Y accelerations are different than 0, the crazyflie is not at rest. */
triggerReset(&sitAwARAccZ);
return(false);
}

/**
Expand All @@ -260,7 +275,7 @@ bool sitAwARTest(float accX, float accY, float accZ)
* The vertical acceleration must be close to 1, but is allowed to oscillate slightly
* around 1. Testing that the deviation from 1 stays within SITAW_AR_THRESHOLD.
*/
return(triggerTestValue(&sitAwARAccZ, fabs(accZ - 1)));
triggerTestValue(&sitAwARAccZ, fabs(accZ - 1));
}

/**
Expand All @@ -272,50 +287,36 @@ bool sitAwARDetected(void)
{
return sitAwARAccZ.released;
}
#endif

#ifdef SITAW_TU_ENABLED
/**
* Initialize the Tumbled detection.
*
* See the sitAwTuTest() function for details.
*/
void sitAwTuInit(void)
{
triggerInit(&sitAwTuAngle, triggerFuncIsGE, SITAW_TU_THRESHOLD, SITAW_TU_TRIGGER_COUNT);
triggerActivate(&sitAwTuAngle, true);
triggerInit(&sitAwTuAcc, triggerFuncIsLE, SITAW_TU_ACC_THRESHOLD, SITAW_TU_ACC_TRIGGER_COUNT);
triggerActivate(&sitAwTuAcc, true);
}

/**
* Test values for a Tumbled situation.
*
* A tumbled situation is considered identified when the roll or pitch has
* exceeded +/- SITAW_TU_THRESHOLD degrees.
*
* For thresholds beyond +/- 90 degrees, this is only reported by the roll
* value. The roll value is thus the only one of roll, pitch and yaw values
* which can detect upside down situations.
* A tumbled situation is considered identified when accelerometer reports
* a negative value.
*
* Once a tumbled situation is identified, this can be used for instance to
* cut the thrust to the motors, avoiding the crazyflie from running
* propellers at significant thrust when accidentially crashing into walls
* propellers at significant thrust when accidentally 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.
*
* @return True if the situation has been detected, otherwise false.
* @param The current accelerometer reading in z direction
*/
bool sitAwTuTest(float eulerRollActual, float eulerPitchActual)
void sitAwTuTest(float accz)
{
/*
* It is sufficient to use a single trigger object, we simply pass the
* greatest of the roll and pitch absolute values to the trigger object
* at any given time.
*/
float fAbsRoll = fabs(eulerRollActual);
float fAbsPitch = fabs(eulerPitchActual);

/* Only the roll value will report if the crazyflie is turning upside down. */
return(triggerTestValue(&sitAwTuAngle, fAbsRoll >= fAbsPitch ? fAbsRoll : fAbsPitch));
triggerTestValue(&sitAwTuAcc, accz);
}

/**
Expand All @@ -325,8 +326,9 @@ bool sitAwTuTest(float eulerRollActual, float eulerPitchActual)
*/
bool sitAwTuDetected(void)
{
return sitAwTuAngle.released;
return sitAwTuAcc.released;
}
#endif

/**
* Initialize the situation awareness subsystem.
Expand Down
6 changes: 2 additions & 4 deletions src/modules/src/stabilizer.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,10 +183,7 @@ void stabilizerInit(StateEstimatorType estimator)
stateEstimatorInit(estimator);
controllerInit(ControllerTypeAny);
powerDistributionInit();
if (estimator == kalmanEstimator)
{
sitAwInit();
}
sitAwInit();
estimatorType = getStateEstimator();
controllerType = getControllerType();

Expand Down Expand Up @@ -530,6 +527,7 @@ PARAM_GROUP_STOP(health)
PARAM_GROUP_START(stabilizer)
PARAM_ADD(PARAM_UINT8, estimator, &estimatorType)
PARAM_ADD(PARAM_UINT8, controller, &controllerType)
PARAM_ADD(PARAM_UINT8, stop, &emergencyStop)
PARAM_GROUP_STOP(stabilizer)

LOG_GROUP_START(health)
Expand Down

0 comments on commit c78289f

Please sign in to comment.