Merge pull request bitcraze#45 from tomfelker/crazyflie2

Fix for spinning out of control when yawing hard

.gitignore

@@ -7,3 +7,4 @@ bin/*
 *~
 config.mk
 cflie.*
+version.c

hal/src/syslink.c

@@ -38,6 +38,7 @@
 #include "config.h"
 #include "debug.h"
 #include "syslink.h"
+#include "radiolink.h"
 #include "uart_syslink.h"
 #include "configblock.h"
 #include "pm.h"

modules/interface/pid.h

@@ -32,17 +32,17 @@
 #define PID_ROLL_RATE_KP  70.0
 #define PID_ROLL_RATE_KI  0.0
 #define PID_ROLL_RATE_KD  0.0
-#define PID_ROLL_RATE_INTEGRATION_LIMIT    100.0
+#define PID_ROLL_RATE_INTEGRATION_LIMIT    33.3
 
 #define PID_PITCH_RATE_KP  70.0
 #define PID_PITCH_RATE_KI  0.0
 #define PID_PITCH_RATE_KD  0.0
-#define PID_PITCH_RATE_INTEGRATION_LIMIT   100.0
+#define PID_PITCH_RATE_INTEGRATION_LIMIT   33.3
 
 #define PID_YAW_RATE_KP  70.0
-#define PID_YAW_RATE_KI  50.0
+#define PID_YAW_RATE_KI  16.7
 #define PID_YAW_RATE_KD  0.0
-#define PID_YAW_RATE_INTEGRATION_LIMIT     500.0
+#define PID_YAW_RATE_INTEGRATION_LIMIT     166.7
 
 #define PID_ROLL_KP  3.5
 #define PID_ROLL_KI  2.0

modules/src/controller.c

@@ -32,20 +32,17 @@
 #include "pid.h"
 #include "param.h"
 #include "imu.h"
-/*
-#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 out = (int16_t)in;\
-  }
-*/
 
-//Fancier version
-#define TRUNCATE_SINT16(out, in) (out = (in<INT16_MIN)?INT16_MIN:((in>INT16_MAX)?INT16_MAX:in) )
-
-//Better semantic
-#define SATURATE_SINT16(in) ( (in<INT16_MIN)?INT16_MIN:((in>INT16_MAX)?INT16_MAX:in) )
+static inline int16_t saturateSignedInt16(float in)
+{
+  // don't use INT16_MIN, because later we may negate it, which won't work for that value.
+  if (in > INT16_MAX)
+    return INT16_MAX;
+  else if (in < -INT16_MAX)
+    return -INT16_MAX;
+  else
+    return (int16_t)in;
+}
 
 PidObject pidRollRate;
 PidObject pidPitchRate;
@@ -93,13 +90,13 @@ void controllerCorrectRatePID(
        float rollRateDesired, float pitchRateDesired, float yawRateDesired)
 {
   pidSetDesired(&pidRollRate, rollRateDesired);
-  TRUNCATE_SINT16(rollOutput, pidUpdate(&pidRollRate, rollRateActual, true));
+  rollOutput = saturateSignedInt16(pidUpdate(&pidRollRate, rollRateActual, true));
 
   pidSetDesired(&pidPitchRate, pitchRateDesired);
-  TRUNCATE_SINT16(pitchOutput, pidUpdate(&pidPitchRate, pitchRateActual, true));
+  pitchOutput = saturateSignedInt16(pidUpdate(&pidPitchRate, pitchRateActual, true));
 
   pidSetDesired(&pidYawRate, yawRateDesired);
-  TRUNCATE_SINT16(yawOutput, pidUpdate(&pidYawRate, yawRateActual, true));
+  yawOutput = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, true));
 }
 
 void controllerCorrectAttitudePID(