Merge pull request #3 from mike-matera/performance-updates

Performance updates

README.md

@@ -1,34 +1,50 @@
 # FastPID
-A fast fixed-point PID controller for Arduino 
+A fast 32-bit fixed-point PID controller for Arduino 
 
 ## About 
 
-This PID controller is faster than alternatives for Arduino becuase it avoids expensive floating point operations. The PID controller is configured with floating point coefficients and translates them to fixed point internally. This imposes limitations on the domain of the coefficients. Setting the I and D terms to zero makes the controller run faster. 
+This PID controller is faster than alternatives for Arduino becuase it avoids expensive floating point operations. The PID controller is configured with floating point coefficients and translates them to fixed point internally. This imposes limitations on the domain of the coefficients. Setting the I and D terms to zero makes the controller run faster. The controller is configured to run at a fixed frequency and calling code is responsible for running at that frequency. The ```Ki``` and ```Kd``` parameters are scaled by the frequency to save time during the ```step()``` operation. 
 
 ## Description of Coefficients 
 
   * ```Kp``` - P term of the PID controller. 
   * ```Ki``` - I term of the PID controller. 
   * ```Kd``` - D term of the PID controller. 
+  * ```Hz``` - The execution frequency of the controller. 
 
 ### Coefficient Domain 
 
-The computation pipeline expects 25 bit coefficients. This is controlled by ``PARAM_BITS`` and cannot be changed without breaking the controller. The number of bits before and after the decimal place is controlled by ``PARAM_SHIFT`` in FastPID.h. The default value for ``PARAM_SHIFT`` is 15.
+The computation pipeline expects 16 bit coefficients. This is controlled by ``PARAM_BITS`` and should not be changed or caluclations may overflow. The number of bits before and after the decimal place is controlled by ``PARAM_SHIFT`` in FastPID.h. The default value for ``PARAM_SHIFT`` is 8 and can be changed to suit your application.
+
+  * **The parameter P domain is [0.00390625 to 255] inclusive.** 
+  * **The parameter I domain is P / Hz** 
+  * **The parameter D domain is P * Hz** 
+
+The controller checks for parameter domain violations and won't operate if a coefficient is outside of the range. All of the configuration operations return ```bool``` to alert the user of an error. The ```err()``` function checks the error condition. Errors can be cleared with the ```clear()``` function.
+
+## Execution Frequency
+
+**The execution frequency is not automatically detected as of version v1.1.0** This greatly improves the controller performance. Instead the '''Ki''' and '''Kd''' terms are scaled in the configuration step. It's essential to call '''step()''' at the rate that you specify. 
 
-  **The parameter domain is [1023 to 0.00006103515625] inclsive** 
-
 ## Input and Output 
 
 The input and the setpoint are an ```int16_t``` this matches the width of Analog pins and accomodate negative readings and setpoints. The output of the PID is an ```int16_t```. The actual bit-width and signedness of the output can be configured. 
 
-  * ```bits``` - The output width will be limited to values inside of this bit range. Valid values are 16 through 1 
-  * ```sign``` If ```true``` the output range is [-2^(bits-1), -2^(bits-1)-1]. If ```false``` output range is [0, 2^bits-1]
+  * ```bits``` - The output width will be limited to values inside of this bit range. Valid values are 1 through 16 
+  * ```sign``` If ```true``` the output range is [-2^(bits-1), -2^(bits-1)-1]. If ```false``` output range is [0, 2^(bits-1)-1]. **The maximum output value of the controller is 32767 (even in 16 bit unsigned mode)** 
+
+## Performance 
 
-If you're using an unsigned type as an output be sure to cast the output so you don't inadvertantly get a negative value. 
+FastPID performance varies depending on the coefficients. When a coefficient is zero less calculation is done. The controller was benchmarked using an Arduino UNO and the code below. 
 
-## Time Calculation 
+| Kp | Ki | Kd | Step Time (uS) | 
+| -- | -- | -- | -------------- |
+| 0.1 | 0.5 | 0.1 | ~64 | 
+| 0.1 | 0.5 | 0 | ~56 | 
+| 0.1 | 0 | 0 | ~28 | 
+| 0 | 0 | 0 | ~28 | 
 
-Time is computed automatically each time ```step()``` is called based on ```millis()```. Only PID controllers that use a ```Ki``` and ```Kd``` term rely on time information. You must control the rate at which the controller is called. Unlike ArduinoPID calling ``step()`` will always do a calculation, no matter how much time has passed. 
+For comparison the excellent [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library takes an average of about 90-100 uS per step with all non-zero coefficients. 
 
 ## Sample Code 
 
@@ -39,11 +55,11 @@ Time is computed automatically each time ```step()``` is called based on ```mill
 #define PIN_SETPOINT  A1
 #define PIN_OUTPUT    9
 
-float Kp=0.1, Ki=0.5, Kd=0;
+float Kp=0.1, Ki=0.5, Kd=0.1, Hz=10;
 int output_bits = 8;
 bool output_signed = false;
 
-FastPID myPID(Kp, Ki, Kd, output_bits, output_signed);
+FastPID myPID(Kp, Ki, Kd, Hz, output_bits, output_signed);
 
 void setup()
 {
@@ -54,9 +70,13 @@ void loop()
 {
   int setpoint = analogRead(PIN_SETPOINT) / 2; 
   int feedback = analogRead(PIN_INPUT);
+  int ts = micros();
   uint8_t output = myPID.step(setpoint, feedback);
+  int tss = micros();
   analogWrite(PIN_OUTPUT, output);
-  Serial.print("sp: "); 
+  Serial.print("(Fast) micros: "); 
+  Serial.print(tss - ts);
+  Serial.print(" sp: "); 
   Serial.print(setpoint); 
   Serial.print(" fb: "); 
   Serial.print(feedback);

examples/VoltageRegulator/VoltageRegulator.ino

@@ -18,11 +18,11 @@
 #define PIN_SETPOINT  A1
 #define PIN_OUTPUT    9
 
-float Kp=0.1, Ki=0.5, Kd=0;
+float Kp=0.1, Ki=0.5, Kd=0, Hz=10;
 int output_bits = 8;
 bool output_signed = false;
 
-FastPID myPID(Kp, Ki, Kd, output_bits, output_signed);
+FastPID myPID(Kp, Ki, Kd, Hz, output_bits, output_signed);
 
 void setup()
 {
@@ -33,14 +33,19 @@ void loop()
 {
   int setpoint = analogRead(PIN_SETPOINT) / 2; 
   int feedback = analogRead(PIN_INPUT);
+  uint32_t before, after;
+  before = micros();
   uint8_t output = myPID.step(setpoint, feedback);
+  after = micros();
+
   analogWrite(PIN_OUTPUT, output);
-  Serial.print("sp: "); 
+  Serial.print("runtime: "); 
+  Serial.print(after - before);
+  Serial.print(" sp: "); 
   Serial.print(setpoint); 
   Serial.print(" fb: "); 
   Serial.print(feedback);
   Serial.print(" out: ");
   Serial.println(output);
   delay(100);
 }
-

library.properties

@@ -1,5 +1,5 @@
 name=FastPID
-version=1.0.0
+version=1.2.0
 author=Mike Matera <[email protected]>
 maintainer=Mike Matera <[email protected]>
 sentence=A PID controlled implemented using fixed-point arithmetic. 

src/FastPID.cpp

@@ -10,40 +10,42 @@ void FastPID::clear() {
   _last_out = 0;
   _sum = 0; 
   _last_err = 0;
-  _last_run = 0;
   _cfg_err = false;
 } 
 
-bool FastPID::setCoefficients(float kp, float ki, float kd) {
+bool FastPID::setCoefficients(float kp, float ki, float kd, float hz) {
   _p = floatToParam(kp);
-  _i = floatToParam(ki);
-  _d = floatToParam(kd);
+  _i = floatToParam(ki / hz);
+  _d = floatToParam(kd * hz);
   return ! _cfg_err;
 }
 
-bool FastPID::setOutputConfig(int bits, bool sign, bool differential) {
+bool FastPID::setOutputConfig(int bits, bool sign) {
   // Set output bits
   if (bits > 16 || bits < 1) {
     _cfg_err = true; 
   }
   else {
+    if (bits == 16) {
+      _outmax = (0xFFFFULL >> (17 - bits)) * PARAM_MULT;
+    }
+    else{
+      _outmax = (0xFFFFULL >> (16 - bits)) * PARAM_MULT;
+    }
     if (sign) {
-      _outmax = ((0x1ULL << (bits - 1)) - 1) * PARAM_MULT;
-      _outmin = -((0x1ULL << (bits - 1))) * PARAM_MULT;
+      _outmin = -((0xFFFFULL >> (17 - bits)) + 1) * PARAM_MULT;
     }
     else {
-      _outmax = ((0x1ULL << bits) - 1) * PARAM_MULT;
       _outmin = 0;
     }
   }
-  _differential = differential;
   return ! _cfg_err;
 }
 
-bool FastPID::configure(float kp, float ki, float kd, int bits, bool sign, bool diff) {
+bool FastPID::configure(float kp, float ki, float kd, float hz, int bits, bool sign) {
   clear();
-  setCoefficients(kp, ki, kd);
-  setOutputConfig(bits, sign, diff);
+  setCoefficients(kp, ki, kd, hz);
+  setOutputConfig(bits, sign);
   return ! _cfg_err; 
 }
 
@@ -63,76 +65,51 @@ uint32_t FastPID::floatToParam(float in) {
   return param;
 }
 
-int16_t FastPID::step(int16_t sp, int16_t fb, uint32_t timestamp) {
-
-  // Calculate delta T
-  // millis(): Frequencies less than 1Hz become 1Hz. 
-  //   max freqency 1 kHz (XXX: is this too low?)
-  uint32_t now;
-  if (timestamp != 0) {
-    // Let the user specify the sample time. 
-    now = timestamp;
-  }
-  else {
-    // Otherwise use the clock
-    now = millis();
-  }
-  uint32_t hz = 0;
-  if (_last_run == 0) {
-    // Ignore I and D on the first step. They will be 
-    // unreliable because no time has really passed.
-    hz = 0;
-  }
-  else {
-    if (now < _last_run) {
-      // 47-day timebomb
-      hz = uint32_t(1000) / (now + (~_last_run));
-    }
-    else {
-      hz = uint32_t(1000) / (now - _last_run); 
-    }
-    if (hz == 0) 
-      hz = 1;
-  }
-
-  _last_run = now;
+int16_t FastPID::step(int16_t sp, int16_t fb) {
 
   // int16 + int16 = int17
-  int32_t err = int32_t(sp) - int32_t(fb); 
-  int64_t P = 0, I = 0, D = 0;
+  int32_t err = int32_t(sp) - int32_t(fb);
+  int32_t P = 0, I = 0;
+  int32_t D = 0;
 
   if (_p) {
-    // uint23 * int16 = int39
-    P = int64_t(_p) * int64_t(err);
+    // uint16 * int16 = int32
+    P = int32_t(_p) * int32_t(err);
   }
 
-  if (_i && hz) {
-    // int31 + ( int25 *  int17) / int10  = int43
-    _sum += (int64_t(_i) * int32_t(err)) / int32_t(hz); 
+  if (_i) {
+    // int17 * int16 = int33
+    _sum += int64_t(err) * int64_t(_i);
 
-    // Limit sum to 31-bit signed value so that it saturates, never overflows.
+    // Limit sum to 32-bit signed value so that it saturates, never overflows.
     if (_sum > INTEG_MAX)
       _sum = INTEG_MAX;
     else if (_sum < INTEG_MIN)
       _sum = INTEG_MIN;
 
-    // int43
-    I = int64_t(_sum);
+    // int32
+    I = _sum;
   }
 
-  if (_d && hz) {
-    // int17 - (int16 - int16) = int19
-    int32_t deriv = (err - _last_err) - (sp - _last_sp);
+  if (_d) {
+    // (int17 - int16) - (int16 - int16) = int19
+    int32_t deriv = (err - _last_err) - int32_t(sp - _last_sp);
     _last_sp = sp; 
     _last_err = err; 
 
-    // uint23 * int19 * uint16 = int58
-    D = int64_t(_d) * int64_t(deriv) * int64_t(hz);
+    // Limit the derivative to 16-bit signed value.
+    if (deriv > DERIV_MAX)
+      deriv = DERIV_MAX;
+    else if (deriv < DERIV_MIN)
+      deriv = DERIV_MIN;
+
+    // int16 * int16 = int32
+    D = int32_t(_d) * int32_t(deriv);
   }
 
-  // int39 (P) + int43 (I) + int58 (D) = int61
-  int64_t out = P + I + D;
-  
+  // int32 (P) + int32 (I) + int32 (D) = int34
+  int64_t out = int64_t(P) + int64_t(I) + int64_t(D);
+
   // Make the output saturate
   if (out > _outmax) 
     out = _outmax;

src/FastPID.h

@@ -3,16 +3,18 @@
 
 #include <stdint.h>
 
-#define INTEG_MAX    (INT64_MAX >> 1)
-#define INTEG_MIN    (INT64_MIN >> 1)
+#define INTEG_MAX    (INT32_MAX)
+#define INTEG_MIN    (INT32_MIN)
+#define DERIV_MAX    (INT16_MAX)
+#define DERIV_MIN    (INT16_MIN)
 
-#define PARAM_SHIFT  15
-#define PARAM_BITS   25
+#define PARAM_SHIFT  8
+#define PARAM_BITS   16
 #define PARAM_MAX    (((0x1ULL << PARAM_BITS)-1) >> PARAM_SHIFT) 
-#define PARAM_MULT   (((0x1ULL << PARAM_BITS)-1) >> (PARAM_BITS - PARAM_SHIFT)) 
+#define PARAM_MULT   (((0x1ULL << PARAM_BITS)) >> (PARAM_BITS - PARAM_SHIFT)) 
 
 /*
-  A fixed point PID controller with a 64-bit internal calculation pipeline.
+  A fixed point PID controller with a 32-bit internal calculation pipeline.
 */
 class FastPID {
 
@@ -22,18 +24,18 @@ class FastPID {
     clear();
   }
 
-  FastPID(float kp, float ki, float kd, int bits=16, bool sign=false, bool diff=false)
+  FastPID(float kp, float ki, float kd, float hz, int bits=16, bool sign=false)
   {
-    configure(kp, ki, kd, bits, sign, diff);
+    configure(kp, ki, kd, hz, bits, sign);
   }
 
   ~FastPID();
 
-  bool setCoefficients(float kp, float ki, float kd);
-  bool setOutputConfig(int bits, bool sign, bool differential=false);
+  bool setCoefficients(float kp, float ki, float kd, float hz);
+  bool setOutputConfig(int bits, bool sign);
   void clear();
-  bool configure(float kp, float ki, float kd, int bits=16, bool sign=false, bool diff=false);
-  int16_t step(int16_t sp, int16_t fb, uint32_t timestamp=0);
+  bool configure(float kp, float ki, float kd, float hz, int bits=16, bool sign=false);
+  int16_t step(int16_t sp, int16_t fb);
 
   bool err() {
     return _cfg_err;
@@ -49,14 +51,12 @@ class FastPID {
   // Configuration
   uint32_t _p, _i, _d;
   int64_t _outmax, _outmin; 
-  bool _differential;
   bool _cfg_err; 
 
   // State
   int16_t _last_sp, _last_out;
   int64_t _sum;
   int32_t _last_err;
-  uint32_t _last_run; 
 };
 
 #endif

test/.gitignore

@@ -4,4 +4,5 @@ AutoPID-1.0.egg-info
 *.so
 build/
 __pycache__
+randomtest-*
 

test/fastpid_wrapper.cpp

@@ -14,7 +14,7 @@ configure(PyObject *self, PyObject *args) {
   if (!PyArg_ParseTuple(args, "fffib", &kp, &ki, &kd, &bits, &sign))
     return NULL;
 
-  return PyBool_FromLong(pid.configure(kp, ki, kd, bits, sign, false));
+  return PyBool_FromLong(pid.configure(kp, ki, kd, 1, bits, sign));
 }
 
 static PyObject *
@@ -23,7 +23,7 @@ step(PyObject *self, PyObject *args) {
   int err;
   if (!PyArg_ParseTuple(args, "ii", &sp, &err))
     return NULL;
-  
+
   return PyLong_FromLong(pid.step(sp, err));
 }