Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improved Tumble Detection and Handling #481

Merged
merged 1 commit into from
Oct 4, 2019
Merged

Improved Tumble Detection and Handling #481

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 13 additions & 8 deletions src/modules/interface/sitaw.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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