Matthias Grob / Mbed 2 deprecated FlyBed3

Successful acro and level mode now! Relying on MPU9250 as base sensor. I'm working continuously on tuning and features :) NEWEST VERSION ON: https://github.com/MaEtUgR/FlyBed (CODE 100% compatible/copyable)

Dependencies:   mbed

Diff: PID/PID.cpp

Revision:
8:609a2ad4c30e
Parent:
5:8ea99e98de73
--- a/PID/PID.cpp	Mon Sep 14 12:49:08 2015 +0000
+++ b/PID/PID.cpp	Thu Nov 19 18:47:27 2015 +0000
@@ -31,6 +31,9 @@
         
     // Derivative
     RollBuffer[RollBufferIndex] = (Error - PreviousError) / dt;
+    RollBufferIndex++;
+    if (RollBufferIndex == BUFFERSIZE)
+        RollBufferIndex = 0;
     float Derivative = 0;
     for(int i=0; i<BUFFERSIZE ;i++)
         Derivative += RollBuffer[i];