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