quadcopter cufe / Mbed 2 deprecated 2006_Theta_Control_yaw

Quadcopter Attitude Control(Yaw-Pitch-Roll)

Dependencies:   mbed

Counter.cpp@0:e63996fd7d3e, 2015-07-03 (annotated)

Committer:
khaledelmadawi
Date:
Fri Jul 03 11:16:02 2015 +0000
Revision:
0:e63996fd7d3e
Quadcopter Attitude Control(Yaw-Pitch-Roll)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
khaledelmadawi 0:e63996fd7d3e 1 #include "Counter.h"
khaledelmadawi 0:e63996fd7d3e 2
khaledelmadawi 0:e63996fd7d3e 3
khaledelmadawi 0:e63996fd7d3e 4
khaledelmadawi 0:e63996fd7d3e 5 Counter::Counter(PinName channel,
khaledelmadawi 0:e63996fd7d3e 6 float inc_enc_constant) : _interrupt(channel) // create the InterruptIn on the pin specified to Counter
khaledelmadawi 0:e63996fd7d3e 7 {
khaledelmadawi 0:e63996fd7d3e 8 _interrupt.fall(this, &Counter::increment); // attach increment function of this counter instance
khaledelmadawi 0:e63996fd7d3e 9 delta_time = 0;
khaledelmadawi 0:e63996fd7d3e 10 rpm = 0;
khaledelmadawi 0:e63996fd7d3e 11 t.start();
khaledelmadawi 0:e63996fd7d3e 12 _inc_enc_constant=inc_enc_constant;
khaledelmadawi 0:e63996fd7d3e 13 }
khaledelmadawi 0:e63996fd7d3e 14
khaledelmadawi 0:e63996fd7d3e 15 void Counter::increment()
khaledelmadawi 0:e63996fd7d3e 16 {
khaledelmadawi 0:e63996fd7d3e 17 delta_time = t.read_us();
khaledelmadawi 0:e63996fd7d3e 18 t.reset();
khaledelmadawi 0:e63996fd7d3e 19 rpm = ( _inc_enc_constant / (delta_time/1000000) ) *60 ; // rev / m
khaledelmadawi 0:e63996fd7d3e 20
khaledelmadawi 0:e63996fd7d3e 21 _count++;
khaledelmadawi 0:e63996fd7d3e 22
khaledelmadawi 0:e63996fd7d3e 23 }
khaledelmadawi 0:e63996fd7d3e 24
khaledelmadawi 0:e63996fd7d3e 25 int Counter::read_count()
khaledelmadawi 0:e63996fd7d3e 26 {
khaledelmadawi 0:e63996fd7d3e 27 return _count;
khaledelmadawi 0:e63996fd7d3e 28 }
khaledelmadawi 0:e63996fd7d3e 29 float Counter::read_rpm()
khaledelmadawi 0:e63996fd7d3e 30 {
khaledelmadawi 0:e63996fd7d3e 31 return rpm;
khaledelmadawi 0:e63996fd7d3e 32 }
khaledelmadawi 0:e63996fd7d3e 33 void Counter::reset()
khaledelmadawi 0:e63996fd7d3e 34 {
khaledelmadawi 0:e63996fd7d3e 35 rpm=0;
khaledelmadawi 0:e63996fd7d3e 36 _count=0;
khaledelmadawi 0:e63996fd7d3e 37 }
khaledelmadawi 0:e63996fd7d3e 38
khaledelmadawi 0:e63996fd7d3e 39 float Counter::medianFilter(int Window_Size)
khaledelmadawi 0:e63996fd7d3e 40 {
khaledelmadawi 0:e63996fd7d3e 41 float *array;
khaledelmadawi 0:e63996fd7d3e 42 float return_val;
khaledelmadawi 0:e63996fd7d3e 43 int i,j,return_idx;
khaledelmadawi 0:e63996fd7d3e 44 array=(float*)malloc(sizeof(float)*Window_Size);
khaledelmadawi 0:e63996fd7d3e 45 for (i=0; i<Window_Size; i++) {
khaledelmadawi 0:e63996fd7d3e 46 array[i]=read_rpm();
khaledelmadawi 0:e63996fd7d3e 47 if(i>0) {
khaledelmadawi 0:e63996fd7d3e 48 for (j=0; j<i; j++) {
khaledelmadawi 0:e63996fd7d3e 49 if(array[j]<array[i]) {
khaledelmadawi 0:e63996fd7d3e 50 swap(&array[j],&array[i]);
khaledelmadawi 0:e63996fd7d3e 51 }
khaledelmadawi 0:e63996fd7d3e 52 }
khaledelmadawi 0:e63996fd7d3e 53 }
khaledelmadawi 0:e63996fd7d3e 54
khaledelmadawi 0:e63996fd7d3e 55 }
khaledelmadawi 0:e63996fd7d3e 56 delete array;
khaledelmadawi 0:e63996fd7d3e 57 return_idx=Window_Size/2;
khaledelmadawi 0:e63996fd7d3e 58 return_val=array[return_idx];
khaledelmadawi 0:e63996fd7d3e 59 return return_val;
khaledelmadawi 0:e63996fd7d3e 60 }
khaledelmadawi 0:e63996fd7d3e 61 void Counter::swap(float *x,float *y){
khaledelmadawi 0:e63996fd7d3e 62 float z;
khaledelmadawi 0:e63996fd7d3e 63 z=*x;
khaledelmadawi 0:e63996fd7d3e 64 *x=*y;
khaledelmadawi 0:e63996fd7d3e 65 *y=z;
khaledelmadawi 0:e63996fd7d3e 66 }