Yoichi Nagashima
/
CQ_nagasm_11_FFT1
NucleoF401RE EMG sensor with 2channel 16band FFT, mixed 7band XBee send, only test
sub.hpp@0:5e2a4b964485, 2014-12-20 (annotated)
- Committer:
- nagasm
- Date:
- Sat Dec 20 05:22:00 2014 +0000
- Revision:
- 0:5e2a4b964485
NucleoF401RE EMG sensor with 2channel 16band FFT, mixed 7band XBee send, only test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nagasm | 0:5e2a4b964485 | 1 | unsigned char rxFIFO[256], txFIFO[256], raw_data[6]; |
nagasm | 0:5e2a4b964485 | 2 | unsigned char rx_top, rx_end, tx_top, tx_end, phase; |
nagasm | 0:5e2a4b964485 | 3 | float mean_sum1, mean_sum2, ad_data1[101], ad_data2[101]; |
nagasm | 0:5e2a4b964485 | 4 | int timer_value[6], max_count, gain, ad_pointer1, ad_pointer2; |
nagasm | 0:5e2a4b964485 | 5 | float a1, a2, b1, c0, y1[3], y2[3]; |
nagasm | 0:5e2a4b964485 | 6 | int notch_1, notch_2; |
nagasm | 0:5e2a4b964485 | 7 | float data1, data2, data3; |
nagasm | 0:5e2a4b964485 | 8 | const float _60Hz_a1 = 1.936768, _60Hz_a2 = -0.939101, _60Hz_b1 = -1.998579, _60Hz_c0 = 1.642174; |
nagasm | 0:5e2a4b964485 | 9 | const float _50Hz_a1 = 1.937188, _50Hz_a2 = -0.939101, _50Hz_b1 = -1.999013, _50Hz_c0 = 1.938304; |
nagasm | 0:5e2a4b964485 | 10 | |
nagasm | 0:5e2a4b964485 | 11 | RawSerial xbee(PA_2, PA_3); |
nagasm | 0:5e2a4b964485 | 12 | Ticker timer_setup; |
nagasm | 0:5e2a4b964485 | 13 | AnalogIn analog_value0(A0); |
nagasm | 0:5e2a4b964485 | 14 | AnalogIn analog_value1(A1); |
nagasm | 0:5e2a4b964485 | 15 | AnalogIn analog_value2(A2); |
nagasm | 0:5e2a4b964485 | 16 | AnalogIn analog_value3(A3); |
nagasm | 0:5e2a4b964485 | 17 | DigitalOut myled(LED1); |
nagasm | 0:5e2a4b964485 | 18 | |
nagasm | 0:5e2a4b964485 | 19 | void sum_clear(){ |
nagasm | 0:5e2a4b964485 | 20 | for (int i=0; i<101; i++) ad_data1[i] = ad_data2[i] = 0; |
nagasm | 0:5e2a4b964485 | 21 | ad_pointer1 = ad_pointer2 = mean_sum1 = mean_sum2 = 0; |
nagasm | 0:5e2a4b964485 | 22 | } |
nagasm | 0:5e2a4b964485 | 23 | |
nagasm | 0:5e2a4b964485 | 24 | void coef_set(int herz){ |
nagasm | 0:5e2a4b964485 | 25 | if(herz < 55){ |
nagasm | 0:5e2a4b964485 | 26 | a1 = _50Hz_a1; |
nagasm | 0:5e2a4b964485 | 27 | a2 = _50Hz_a2; |
nagasm | 0:5e2a4b964485 | 28 | b1 = _50Hz_b1; |
nagasm | 0:5e2a4b964485 | 29 | c0 = _50Hz_c0; |
nagasm | 0:5e2a4b964485 | 30 | } |
nagasm | 0:5e2a4b964485 | 31 | else{ |
nagasm | 0:5e2a4b964485 | 32 | a1 = _60Hz_a1; |
nagasm | 0:5e2a4b964485 | 33 | a2 = _60Hz_a2; |
nagasm | 0:5e2a4b964485 | 34 | b1 = _60Hz_b1; |
nagasm | 0:5e2a4b964485 | 35 | c0 = _60Hz_c0; |
nagasm | 0:5e2a4b964485 | 36 | } |
nagasm | 0:5e2a4b964485 | 37 | for (int i=0; i<3; i++) y1[i] = y2[i] = 0; |
nagasm | 0:5e2a4b964485 | 38 | } |
nagasm | 0:5e2a4b964485 | 39 | void common_setup(){ |
nagasm | 0:5e2a4b964485 | 40 | rx_top = rx_end = tx_top = tx_end = phase = 0; |
nagasm | 0:5e2a4b964485 | 41 | max_count = 100; |
nagasm | 0:5e2a4b964485 | 42 | gain = 7; |
nagasm | 0:5e2a4b964485 | 43 | sum_clear(); |
nagasm | 0:5e2a4b964485 | 44 | notch_1 = notch_2 = 1; |
nagasm | 0:5e2a4b964485 | 45 | coef_set(60); |
nagasm | 0:5e2a4b964485 | 46 | for(int i=0; i<6; i++) timer_value[i] = 0; |
nagasm | 0:5e2a4b964485 | 47 | timer_value[3] = 100; |
nagasm | 0:5e2a4b964485 | 48 | } |
nagasm | 0:5e2a4b964485 | 49 | |
nagasm | 0:5e2a4b964485 | 50 | void timer_interrupt(){ |
nagasm | 0:5e2a4b964485 | 51 | for(int i=0; i<6; i++) timer_value[i]++; |
nagasm | 0:5e2a4b964485 | 52 | } |
nagasm | 0:5e2a4b964485 | 53 | |
nagasm | 0:5e2a4b964485 | 54 | void tx_fifo_check(){ |
nagasm | 0:5e2a4b964485 | 55 | if(xbee.writeable() == 1){ |
nagasm | 0:5e2a4b964485 | 56 | if(tx_top != tx_end){ |
nagasm | 0:5e2a4b964485 | 57 | xbee.putc(txFIFO[tx_end]); |
nagasm | 0:5e2a4b964485 | 58 | ++tx_end &= 255; |
nagasm | 0:5e2a4b964485 | 59 | } |
nagasm | 0:5e2a4b964485 | 60 | } |
nagasm | 0:5e2a4b964485 | 61 | } |
nagasm | 0:5e2a4b964485 | 62 | |
nagasm | 0:5e2a4b964485 | 63 | int rx_fifo_check(){ |
nagasm | 0:5e2a4b964485 | 64 | if(rx_top != rx_end){ |
nagasm | 0:5e2a4b964485 | 65 | unsigned char data = rxFIFO[rx_end]; |
nagasm | 0:5e2a4b964485 | 66 | ++rx_end &= 255; |
nagasm | 0:5e2a4b964485 | 67 | if (data < 33){ |
nagasm | 0:5e2a4b964485 | 68 | phase = 0; |
nagasm | 0:5e2a4b964485 | 69 | return(1); |
nagasm | 0:5e2a4b964485 | 70 | } |
nagasm | 0:5e2a4b964485 | 71 | raw_data[phase] = data; |
nagasm | 0:5e2a4b964485 | 72 | if(++phase > 5) phase = 0; |
nagasm | 0:5e2a4b964485 | 73 | return(0); |
nagasm | 0:5e2a4b964485 | 74 | } |
nagasm | 0:5e2a4b964485 | 75 | return(0); |
nagasm | 0:5e2a4b964485 | 76 | } |
nagasm | 0:5e2a4b964485 | 77 | |
nagasm | 0:5e2a4b964485 | 78 | void rx_fifoset(void){ |
nagasm | 0:5e2a4b964485 | 79 | rxFIFO[rx_top] = xbee.getc(); |
nagasm | 0:5e2a4b964485 | 80 | ++rx_top &= 255; |
nagasm | 0:5e2a4b964485 | 81 | } |
nagasm | 0:5e2a4b964485 | 82 | |
nagasm | 0:5e2a4b964485 | 83 | void tx_fifoset(unsigned char data){ |
nagasm | 0:5e2a4b964485 | 84 | txFIFO[tx_top] = data; |
nagasm | 0:5e2a4b964485 | 85 | ++tx_top &= 255; |
nagasm | 0:5e2a4b964485 | 86 | } |
nagasm | 0:5e2a4b964485 | 87 | |
nagasm | 0:5e2a4b964485 | 88 | unsigned char hex_conv(unsigned char data){ |
nagasm | 0:5e2a4b964485 | 89 | data &= 15; |
nagasm | 0:5e2a4b964485 | 90 | if(data < 10) return(data+48); |
nagasm | 0:5e2a4b964485 | 91 | else return(data+55); |
nagasm | 0:5e2a4b964485 | 92 | } |
nagasm | 0:5e2a4b964485 | 93 | |
nagasm | 0:5e2a4b964485 | 94 | unsigned char conv_hex(unsigned char data){ |
nagasm | 0:5e2a4b964485 | 95 | if((data > 47) && (data < 58)) return(data-48); |
nagasm | 0:5e2a4b964485 | 96 | else if((data > 64) && (data < 71)) return(data-55); |
nagasm | 0:5e2a4b964485 | 97 | return(0); |
nagasm | 0:5e2a4b964485 | 98 | } |
nagasm | 0:5e2a4b964485 | 99 | |
nagasm | 0:5e2a4b964485 | 100 | void tx_message(int data){ |
nagasm | 0:5e2a4b964485 | 101 | for (int i=0; i<6; i++) tx_fifoset(hex_conv((data>>(4*(5-i))) & 15)); |
nagasm | 0:5e2a4b964485 | 102 | tx_fifoset(13); |
nagasm | 0:5e2a4b964485 | 103 | } |
nagasm | 0:5e2a4b964485 | 104 | |
nagasm | 0:5e2a4b964485 | 105 | float move_mean_calc1(float data){ |
nagasm | 0:5e2a4b964485 | 106 | mean_sum1 = mean_sum1 - ad_data1[ad_pointer1] + data; |
nagasm | 0:5e2a4b964485 | 107 | ad_data1[ad_pointer1] = data; |
nagasm | 0:5e2a4b964485 | 108 | ad_pointer1++; |
nagasm | 0:5e2a4b964485 | 109 | if(ad_pointer1 == max_count) ad_pointer1 = 0; |
nagasm | 0:5e2a4b964485 | 110 | return(mean_sum1 / (float)max_count); |
nagasm | 0:5e2a4b964485 | 111 | } |
nagasm | 0:5e2a4b964485 | 112 | |
nagasm | 0:5e2a4b964485 | 113 | float move_mean_calc2(float data){ |
nagasm | 0:5e2a4b964485 | 114 | mean_sum2 = mean_sum2 - ad_data2[ad_pointer2] + data; |
nagasm | 0:5e2a4b964485 | 115 | ad_data2[ad_pointer2] = data; |
nagasm | 0:5e2a4b964485 | 116 | ad_pointer2++; |
nagasm | 0:5e2a4b964485 | 117 | if(ad_pointer2 == max_count) ad_pointer2 = 0; |
nagasm | 0:5e2a4b964485 | 118 | return(mean_sum2 / (float)max_count); |
nagasm | 0:5e2a4b964485 | 119 | } |