Quadcopter Attitude Control(Yaw-Pitch-Roll)

Dependencies:   mbed

Revision:
0:e63996fd7d3e
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Counter.cpp	Fri Jul 03 11:16:02 2015 +0000
@@ -0,0 +1,66 @@
+#include "Counter.h"
+
+
+
+Counter::Counter(PinName channel,
+                 float inc_enc_constant) : _interrupt(channel)          // create the InterruptIn on the pin specified to Counter
+{
+    _interrupt.fall(this, &Counter::increment); // attach increment function of this counter instance
+    delta_time = 0;
+    rpm = 0;
+    t.start();
+    _inc_enc_constant=inc_enc_constant;
+}
+
+void Counter::increment()
+{
+    delta_time = t.read_us();
+    t.reset();
+    rpm = ( _inc_enc_constant  / (delta_time/1000000) ) *60 ; // rev / m
+
+    _count++;
+
+}
+
+int Counter::read_count()
+{
+    return _count;
+}
+float Counter::read_rpm()
+{
+    return rpm;
+}
+void Counter::reset()
+{
+    rpm=0;
+    _count=0;
+}
+
+float Counter::medianFilter(int Window_Size)
+{
+     float *array;
+    float return_val;
+    int i,j,return_idx;
+    array=(float*)malloc(sizeof(float)*Window_Size);
+    for (i=0; i<Window_Size; i++) {
+        array[i]=read_rpm();
+        if(i>0) {
+            for (j=0; j<i; j++) {
+                if(array[j]<array[i]) {
+                    swap(&array[j],&array[i]);
+                }
+            }
+        }
+
+    }
+    delete array;
+    return_idx=Window_Size/2;
+    return_val=array[return_idx];
+    return return_val;
+}
+void Counter::swap(float *x,float *y){
+    float z;
+    z=*x;
+    *x=*y;
+    *y=z;    
+}
\ No newline at end of file