Yoichi Nagashima
/
CQ_nagasm_08_notch_test
add 60Hz/50Hz notch filter
sub.hpp@2:048d8bddb100, 2014-12-20 (annotated)
- Committer:
- nagasm
- Date:
- Sat Dec 20 02:08:03 2014 +0000
- Revision:
- 2:048d8bddb100
- Parent:
- 0:5ec74240d7f1
revised
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nagasm | 0:5ec74240d7f1 | 1 | unsigned char rxFIFO[256], txFIFO[256], raw_data[6]; |
nagasm | 0:5ec74240d7f1 | 2 | unsigned char rx_top, rx_end, tx_top, tx_end, phase; |
nagasm | 0:5ec74240d7f1 | 3 | float mean_sum1, mean_sum2, ad_data1[101], ad_data2[101]; |
nagasm | 0:5ec74240d7f1 | 4 | int timer_value[6], max_count, fir_lpf, gain, ad_pointer1, ad_pointer2; |
nagasm | 0:5ec74240d7f1 | 5 | const int order = 200; |
nagasm | 0:5ec74240d7f1 | 6 | float xn1[order+1], xn2[order+1]; |
nagasm | 0:5ec74240d7f1 | 7 | |
nagasm | 0:5ec74240d7f1 | 8 | RawSerial xbee(PA_2, PA_3); |
nagasm | 0:5ec74240d7f1 | 9 | Ticker timer_setup; |
nagasm | 0:5ec74240d7f1 | 10 | AnalogIn analog_value0(A0); |
nagasm | 0:5ec74240d7f1 | 11 | AnalogIn analog_value1(A1); |
nagasm | 0:5ec74240d7f1 | 12 | AnalogIn analog_value2(A2); |
nagasm | 0:5ec74240d7f1 | 13 | AnalogIn analog_value3(A3); |
nagasm | 0:5ec74240d7f1 | 14 | DigitalOut myled(LED1); |
nagasm | 0:5ec74240d7f1 | 15 | |
nagasm | 0:5ec74240d7f1 | 16 | void sum_clear(){ |
nagasm | 0:5ec74240d7f1 | 17 | int i; |
nagasm | 0:5ec74240d7f1 | 18 | for (i=0; i<101; i++) ad_data1[i] = ad_data2[i] = 0; |
nagasm | 0:5ec74240d7f1 | 19 | ad_pointer1 = ad_pointer2 = mean_sum1 = mean_sum2 = 0; |
nagasm | 0:5ec74240d7f1 | 20 | } |
nagasm | 0:5ec74240d7f1 | 21 | |
nagasm | 0:5ec74240d7f1 | 22 | void common_setup(){ |
nagasm | 0:5ec74240d7f1 | 23 | int i; |
nagasm | 0:5ec74240d7f1 | 24 | rx_top = rx_end = tx_top = tx_end = phase = 0; |
nagasm | 0:5ec74240d7f1 | 25 | max_count = 100; |
nagasm | 0:5ec74240d7f1 | 26 | fir_lpf = 0; |
nagasm | 0:5ec74240d7f1 | 27 | gain = 5; |
nagasm | 0:5ec74240d7f1 | 28 | for (i=0; i<3; i++) timer_value[i] = 0; |
nagasm | 0:5ec74240d7f1 | 29 | timer_value[3] = 10; |
nagasm | 0:5ec74240d7f1 | 30 | timer_value[4] = 1500; |
nagasm | 0:5ec74240d7f1 | 31 | for (i=0; i<=order; i++) xn1[i] = xn2[i] = 0.0; |
nagasm | 0:5ec74240d7f1 | 32 | sum_clear(); |
nagasm | 0:5ec74240d7f1 | 33 | } |
nagasm | 0:5ec74240d7f1 | 34 | |
nagasm | 0:5ec74240d7f1 | 35 | void timer_interrupt(){ |
nagasm | 0:5ec74240d7f1 | 36 | int i; |
nagasm | 0:5ec74240d7f1 | 37 | for (i=0; i<6; i++) timer_value[i]++; |
nagasm | 0:5ec74240d7f1 | 38 | } |
nagasm | 0:5ec74240d7f1 | 39 | |
nagasm | 0:5ec74240d7f1 | 40 | void tx_fifo_check(){ |
nagasm | 0:5ec74240d7f1 | 41 | if(xbee.writeable() == 1){ |
nagasm | 0:5ec74240d7f1 | 42 | if(tx_top != tx_end){ |
nagasm | 0:5ec74240d7f1 | 43 | xbee.putc(txFIFO[tx_end]); |
nagasm | 0:5ec74240d7f1 | 44 | ++tx_end &= 255; |
nagasm | 0:5ec74240d7f1 | 45 | } |
nagasm | 0:5ec74240d7f1 | 46 | } |
nagasm | 0:5ec74240d7f1 | 47 | } |
nagasm | 0:5ec74240d7f1 | 48 | |
nagasm | 0:5ec74240d7f1 | 49 | int rx_fifo_check(){ |
nagasm | 0:5ec74240d7f1 | 50 | unsigned char data; |
nagasm | 0:5ec74240d7f1 | 51 | if(rx_top != rx_end){ |
nagasm | 0:5ec74240d7f1 | 52 | data = rxFIFO[rx_end]; |
nagasm | 0:5ec74240d7f1 | 53 | ++rx_end &= 255; |
nagasm | 0:5ec74240d7f1 | 54 | if (data < 33){ |
nagasm | 0:5ec74240d7f1 | 55 | phase = 0; |
nagasm | 0:5ec74240d7f1 | 56 | return(1); |
nagasm | 0:5ec74240d7f1 | 57 | } |
nagasm | 0:5ec74240d7f1 | 58 | raw_data[phase] = data; |
nagasm | 0:5ec74240d7f1 | 59 | if(++phase > 5) phase = 0; |
nagasm | 0:5ec74240d7f1 | 60 | return(0); |
nagasm | 0:5ec74240d7f1 | 61 | } |
nagasm | 0:5ec74240d7f1 | 62 | return(0); |
nagasm | 0:5ec74240d7f1 | 63 | } |
nagasm | 0:5ec74240d7f1 | 64 | |
nagasm | 0:5ec74240d7f1 | 65 | void rx_fifoset(void){ |
nagasm | 0:5ec74240d7f1 | 66 | rxFIFO[rx_top] = xbee.getc(); |
nagasm | 0:5ec74240d7f1 | 67 | ++rx_top &= 255; |
nagasm | 0:5ec74240d7f1 | 68 | } |
nagasm | 0:5ec74240d7f1 | 69 | |
nagasm | 0:5ec74240d7f1 | 70 | void tx_fifoset(unsigned char data){ |
nagasm | 0:5ec74240d7f1 | 71 | txFIFO[tx_top] = data; |
nagasm | 0:5ec74240d7f1 | 72 | ++tx_top &= 255; |
nagasm | 0:5ec74240d7f1 | 73 | } |
nagasm | 0:5ec74240d7f1 | 74 | |
nagasm | 0:5ec74240d7f1 | 75 | unsigned char hex_conv(unsigned char data){ |
nagasm | 0:5ec74240d7f1 | 76 | data &= 15; |
nagasm | 0:5ec74240d7f1 | 77 | if(data < 10) return(data+48); |
nagasm | 0:5ec74240d7f1 | 78 | else return(data+55); |
nagasm | 0:5ec74240d7f1 | 79 | } |
nagasm | 0:5ec74240d7f1 | 80 | |
nagasm | 0:5ec74240d7f1 | 81 | unsigned char conv_hex(unsigned char data){ |
nagasm | 0:5ec74240d7f1 | 82 | if((data > 47) && (data < 58)) return(data-48); |
nagasm | 0:5ec74240d7f1 | 83 | else if((data > 64) && (data < 71)) return(data-55); |
nagasm | 0:5ec74240d7f1 | 84 | return(0); |
nagasm | 0:5ec74240d7f1 | 85 | } |
nagasm | 0:5ec74240d7f1 | 86 | |
nagasm | 0:5ec74240d7f1 | 87 | void tx_message(int data){ |
nagasm | 0:5ec74240d7f1 | 88 | int i; |
nagasm | 0:5ec74240d7f1 | 89 | for (i=0; i<6; i++) tx_fifoset(hex_conv((data>>(4*(5-i))) & 15)); |
nagasm | 0:5ec74240d7f1 | 90 | tx_fifoset(13); |
nagasm | 0:5ec74240d7f1 | 91 | } |
nagasm | 0:5ec74240d7f1 | 92 | |
nagasm | 0:5ec74240d7f1 | 93 | float move_mean_calc1(float data){ |
nagasm | 0:5ec74240d7f1 | 94 | mean_sum1 = mean_sum1 - ad_data1[ad_pointer1] + data; |
nagasm | 0:5ec74240d7f1 | 95 | ad_data1[ad_pointer1] = data; |
nagasm | 0:5ec74240d7f1 | 96 | ad_pointer1++; |
nagasm | 0:5ec74240d7f1 | 97 | if(ad_pointer1 == max_count) ad_pointer1 = 0; |
nagasm | 0:5ec74240d7f1 | 98 | return(mean_sum1 / (float)max_count); |
nagasm | 0:5ec74240d7f1 | 99 | } |
nagasm | 0:5ec74240d7f1 | 100 | |
nagasm | 0:5ec74240d7f1 | 101 | float move_mean_calc2(float data){ |
nagasm | 0:5ec74240d7f1 | 102 | mean_sum2 = mean_sum2 - ad_data2[ad_pointer2] + data; |
nagasm | 0:5ec74240d7f1 | 103 | ad_data2[ad_pointer2] = data; |
nagasm | 0:5ec74240d7f1 | 104 | ad_pointer2++; |
nagasm | 0:5ec74240d7f1 | 105 | if(ad_pointer2 == max_count) ad_pointer2 = 0; |
nagasm | 0:5ec74240d7f1 | 106 | return(mean_sum2 / (float)max_count); |
nagasm | 0:5ec74240d7f1 | 107 | } |