From a5288dc6bea196adf555982b599d87570886020b Mon Sep 17 00:00:00 2001 From: Tom Felker Date: Thu, 19 Mar 2015 06:20:57 +0100 Subject: [PATCH 1/4] fix implicit declaration warning --- hal/src/syslink.c | 1 + 1 file changed, 1 insertion(+) diff --git a/hal/src/syslink.c b/hal/src/syslink.c index 92c8ea1a86..ea3a8da54c 100644 --- a/hal/src/syslink.c +++ b/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" From c0abdf03c148d3394b9da40c32f28468b6f5c600 Mon Sep 17 00:00:00 2001 From: Tom Felker Date: Sun, 22 Mar 2015 01:09:46 +0100 Subject: [PATCH 2/4] Add version.c to .gitignore to avoid accidental commits. --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 6b5258732c..4a068513ab 100644 --- a/.gitignore +++ b/.gitignore @@ -7,3 +7,4 @@ bin/* *~ config.mk cflie.* +version.c From 7c1fec91b1ee7c8aba28aababf53d30f5b49c6b7 Mon Sep 17 00:00:00 2001 From: Tom Felker Date: Wed, 25 Mar 2015 06:07:53 +0100 Subject: [PATCH 3/4] Fix issue where Crazyflie spins uncontrollably if you bump it so it yaws 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. --- modules/src/controller.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/modules/src/controller.c b/modules/src/controller.c index 8ed317cbfa..fdb58e2cc9 100644 --- a/modules/src/controller.c +++ b/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 = (inINT16_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) ( (inINT16_MAX)?INT16_MAX:in) ) +#define SATURATE_SINT16(in) ( (in<-INT16_MAX)?-INT16_MAX:((in>INT16_MAX)?INT16_MAX:in) ) PidObject pidRollRate; PidObject pidPitchRate; From 8694d2080342e6ef437db21778a354f4226c25c7 Mon Sep 17 00:00:00 2001 From: Tom Felker Date: Wed, 25 Mar 2015 07:10:18 +0100 Subject: [PATCH 4/4] Fix pidUpdate() being called 3x more often than necessary in controllerCorrectRatePID(). TRUNCATE_SINT16() was a preprocessor macro that evaluates its input three times in the common case. Where it was used, the input is a call to pidUpdate() with updateError true, so in addition to the performance impact, there are unintended side effects. Specifically, the integral would wind up 3x more then intended, and the derivative constant would have no effect at all, other than to potentially break the TRUNCATE_SINT16() logic. Luckily the three PID loops affected by this did not use the derivative constant. I've replaced the macro with an inline function, removed an unused version of the macro, and divided the integral and integral windup terms of the three affected PID loops by 3 to compensate. The resulting behavior should be unchanged. --- modules/interface/pid.h | 8 ++++---- modules/src/controller.c | 32 +++++++++++++------------------- 2 files changed, 17 insertions(+), 23 deletions(-) diff --git a/modules/interface/pid.h b/modules/interface/pid.h index aa87b5b256..9e82fc5461 100644 --- a/modules/interface/pid.h +++ b/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 diff --git a/modules/src/controller.c b/modules/src/controller.c index fdb58e2cc9..b01bea56a4 100644 --- a/modules/src/controller.c +++ b/modules/src/controller.c @@ -33,22 +33,16 @@ #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_MAX) out = (int16_t)-INT16_MAX;\ - else out = (int16_t)in;\ - } -*/ - -//Fancier version -#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_MAX)?-INT16_MAX:((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; @@ -96,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(