sub.hpp

00001 unsigned char rxFIFO[256], txFIFO[256], raw_data[6];
00002 unsigned char rx_top, rx_end, tx_top, tx_end, phase;
00003 float mean_sum1, mean_sum2, ad_data1[101], ad_data2[101];
00004 int timer_value[6], max_count, fir_lpf, gain, ad_pointer1, ad_pointer2;
00005 const int order = 200;
00006 float xn1[order+1], xn2[order+1];
00007 float a1, a2, b1, c0, y1[3], y2[3], old_1, old_2;
00008 int notch_1, notch_2, emergence, em_count1, em_count2;
00009 const float _60Hz_a1 = 1.936768, _60Hz_a2 = -0.939101, _60Hz_b1 = -1.998579, _60Hz_c0 = 1.642174;
00010 const float _50Hz_a1 = 1.937188, _50Hz_a2 = -0.939101, _50Hz_b1 = -1.999013, _50Hz_c0 = 1.938304;
00011 
00012 RawSerial xbee(PA_2, PA_3);
00013 Ticker timer_setup;
00014 AnalogIn analog_value0(A0);
00015 AnalogIn analog_value1(A1);
00016 AnalogIn analog_value2(A2);
00017 AnalogIn analog_value3(A3);
00018 DigitalOut myled(LED1);
00019 
00020 void sum_clear(){
00021     int i;
00022     for (i=0; i<101; i++) ad_data1[i] = ad_data2[i] = 0;
00023     ad_pointer1 = ad_pointer2 = mean_sum1 = mean_sum2 = 0;
00024 }
00025 
00026 void coef_set(int herz){
00027     if(herz < 55){
00028         a1 = _50Hz_a1;
00029         a2 = _50Hz_a2;
00030         b1 = _50Hz_b1;
00031         c0 = _50Hz_c0;
00032     }
00033     else{
00034         a1 = _60Hz_a1;
00035         a2 = _60Hz_a2;
00036         b1 = _60Hz_b1;
00037         c0 = _60Hz_c0;
00038     }
00039     for (int i=0; i<3; i++) y1[i] = y2[i] = 0;
00040 }
00041 void common_setup(){
00042     int i;
00043     rx_top = rx_end = tx_top = tx_end = phase = 0;
00044     em_count1 = em_count2 = 0;
00045     max_count = 100;
00046     fir_lpf = emergence = old_1 = old_2 = 0;
00047     gain = 5;
00048     for (i=0; i<3; i++) timer_value[i] = 0;
00049     timer_value[3] = 10;
00050     timer_value[4] = 1500;
00051     for (i=0; i<=order; i++) xn1[i] = xn2[i] = 0.0;
00052     sum_clear();
00053     notch_1 = notch_2 = 1;
00054     coef_set(60);
00055 }
00056 
00057 void timer_interrupt(){
00058     int i;
00059     for (i=0; i<6; i++) timer_value[i]++;
00060 }
00061 
00062 void tx_fifo_check(){
00063     if(xbee.writeable() == 1){
00064         if(tx_top != tx_end){
00065             xbee.putc(txFIFO[tx_end]);
00066             ++tx_end &= 255;
00067         }
00068     }
00069 }
00070 
00071 int rx_fifo_check(){
00072     unsigned char data;
00073     if(rx_top != rx_end){
00074         data = rxFIFO[rx_end];
00075         ++rx_end &= 255;
00076         if (data < 33){
00077             phase = 0;
00078             return(1);
00079         }
00080         raw_data[phase] = data;
00081         if(++phase > 5) phase = 0;
00082         return(0);
00083     }
00084     return(0);
00085 }
00086 
00087 void rx_fifoset(void){
00088     rxFIFO[rx_top] = xbee.getc();
00089     ++rx_top &= 255;
00090 }
00091 
00092 void tx_fifoset(unsigned char data){
00093     txFIFO[tx_top] = data;
00094     ++tx_top &= 255;
00095 }
00096 
00097 unsigned char hex_conv(unsigned char data){
00098     data &= 15;
00099     if(data < 10) return(data+48);
00100     else return(data+55);
00101 }
00102 
00103 unsigned char conv_hex(unsigned char data){
00104     if((data > 47) && (data < 58)) return(data-48);
00105     else if((data > 64) && (data < 71)) return(data-55);
00106     return(0);
00107 }
00108 
00109 void tx_message(int data){
00110     int i;
00111     for (i=0; i<6; i++) tx_fifoset(hex_conv((data>>(4*(5-i))) & 15));
00112     tx_fifoset(13);
00113 }
00114 
00115 float move_mean_calc1(float data){
00116     mean_sum1 = mean_sum1 - ad_data1[ad_pointer1] + data;
00117     ad_data1[ad_pointer1] = data;
00118     ad_pointer1++;
00119     if(ad_pointer1 == max_count) ad_pointer1 = 0;
00120     return(mean_sum1 / (float)max_count);
00121 }
00122 
00123 float move_mean_calc2(float data){
00124     mean_sum2 = mean_sum2 - ad_data2[ad_pointer2] + data;
00125     ad_data2[ad_pointer2] = data;
00126     ad_pointer2++;
00127     if(ad_pointer2 == max_count) ad_pointer2 = 0;
00128     return(mean_sum2 / (float)max_count);
00129 }