Permalink
Browse files

Fix issue where Crazyflie spins uncontrollably if you bump it so it y…

…aws left while it's hovering.

The problem was, the int16_t yawOutput value computed in controllerCorrectRatePID() will be negated in stabilizerTask().  However if it saturates to -32768, the +32768 can't fit in an int16_t, so it will roll over back to -32768, causing it to spiral out of control. The fix is to not allow that value.

However, there's another issue: TRUNCATE_SINT16() is a macro, and will evaluate its second argument three times, thus calling pidUpdate() three times as often as necessary.  This only affects CPU load, and may require modifying the PID constants to fix, so that will be in a later snapshot.
  • Loading branch information...
tomfelker committed Mar 25, 2015
1 parent c0abdf0 commit 7c1fec91b1ee7c8aba28aababf53d30f5b49c6b7
Showing with 6 additions and 3 deletions.
  1. +6 −3 modules/src/controller.c
@@ -32,20 +32,23 @@
#include "pid.h"
#include "param.h"
#include "imu.h"

// don't use INT16_MIN, because later we may negate it, which won't work for that value.

/*
#define TRUNCATE_SINT16(out, in) \
{\
if (in > INT16_MAX) out = (int16_t)INT16_MAX;\
else if (in < INT16_MIN) out = (int16_t)INT16_MIN;\
else if (in < -INT16_MAX) out = (int16_t)-INT16_MAX;\
else out = (int16_t)in;\
}
*/

//Fancier version
#define TRUNCATE_SINT16(out, in) (out = (in<INT16_MIN)?INT16_MIN:((in>INT16_MAX)?INT16_MAX:in) )
#define TRUNCATE_SINT16(out, in) (out = (in<-INT16_MAX)?-INT16_MAX:((in>INT16_MAX)?INT16_MAX:in) )

//Better semantic
#define SATURATE_SINT16(in) ( (in<INT16_MIN)?INT16_MIN:((in>INT16_MAX)?INT16_MAX:in) )
#define SATURATE_SINT16(in) ( (in<-INT16_MAX)?-INT16_MAX:((in>INT16_MAX)?INT16_MAX:in) )

PidObject pidRollRate;
PidObject pidPitchRate;

0 comments on commit 7c1fec9

Please sign in to comment.