Yoichi Nagashima
/
CQ_nagasm_08_notch_test
add 60Hz/50Hz notch filter
main.cpp@2:048d8bddb100, 2014-12-20 (annotated)
- Committer:
- nagasm
- Date:
- Sat Dec 20 02:08:03 2014 +0000
- Revision:
- 2:048d8bddb100
- Parent:
- 1:8fa2f521009a
revised
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nagasm | 0:5ec74240d7f1 | 1 | #include "mbed.h" |
nagasm | 0:5ec74240d7f1 | 2 | #include "sub.hpp" |
nagasm | 0:5ec74240d7f1 | 3 | #include "FIR_LPF.hpp" |
nagasm | 0:5ec74240d7f1 | 4 | #include "notch.hpp" |
nagasm | 0:5ec74240d7f1 | 5 | |
nagasm | 0:5ec74240d7f1 | 6 | int main(){ |
nagasm | 0:5ec74240d7f1 | 7 | common_setup(); |
nagasm | 0:5ec74240d7f1 | 8 | xbee.baud(38400); |
nagasm | 0:5ec74240d7f1 | 9 | xbee.attach(&rx_fifoset, xbee.RxIrq); |
nagasm | 0:5ec74240d7f1 | 10 | timer_setup.attach_us(&timer_interrupt, 5); // 5usec |
nagasm | 2:048d8bddb100 | 11 | notch_1 = notch_2 = 1; |
nagasm | 0:5ec74240d7f1 | 12 | coef_set(60); // Ham notch filter set |
nagasm | 0:5ec74240d7f1 | 13 | while(1){ |
nagasm | 0:5ec74240d7f1 | 14 | tx_fifo_check(); |
nagasm | 0:5ec74240d7f1 | 15 | if(timer_value[1] > 19){ // 0.1msec sampling |
nagasm | 0:5ec74240d7f1 | 16 | timer_value[1] = 0; |
nagasm | 0:5ec74240d7f1 | 17 | float data1 = (float)gain * (analog_value2.read() - 0.5f); // A/D in (3) |
nagasm | 0:5ec74240d7f1 | 18 | if (data1 < 0) data1 = -data1; // always detection ON |
nagasm | 0:5ec74240d7f1 | 19 | if (fir_lpf != 0) data1 = FIR_calc1(data1); // FIR calc (1) call |
nagasm | 1:8fa2f521009a | 20 | if (notch_1 != 0) data1 = notch_filter1(data1); |
nagasm | 0:5ec74240d7f1 | 21 | if(timer_value[2] > 2999){ // 15msec |
nagasm | 0:5ec74240d7f1 | 22 | timer_value[2] = 0; |
nagasm | 0:5ec74240d7f1 | 23 | if(max_count != 0) data1 = move_mean_calc1(data1); |
nagasm | 0:5ec74240d7f1 | 24 | tx_message((uint16_t)((data1 + 1.0f) * 2047)<<4); |
nagasm | 0:5ec74240d7f1 | 25 | } |
nagasm | 0:5ec74240d7f1 | 26 | } |
nagasm | 0:5ec74240d7f1 | 27 | if(timer_value[3] > 19){ // 0.1msec sampling |
nagasm | 0:5ec74240d7f1 | 28 | timer_value[3] = 0; |
nagasm | 0:5ec74240d7f1 | 29 | float data2 = (float)gain * (analog_value3.read() - 0.5f); // A/D in (4) |
nagasm | 0:5ec74240d7f1 | 30 | if (data2 < 0) data2 = -data2; // always detection ON |
nagasm | 0:5ec74240d7f1 | 31 | if (fir_lpf != 0) data2 = FIR_calc2(data2); // FIR calc (1) call |
nagasm | 1:8fa2f521009a | 32 | if (notch_2 != 0) data2 = notch_filter2(data2); |
nagasm | 0:5ec74240d7f1 | 33 | if(timer_value[4] > 2999){ // 15msec |
nagasm | 0:5ec74240d7f1 | 34 | timer_value[4] = 0; |
nagasm | 0:5ec74240d7f1 | 35 | if(max_count != 0) data2 = move_mean_calc2(data2); |
nagasm | 0:5ec74240d7f1 | 36 | tx_message(0x400000 + ((uint16_t)((data2 + 1.0f) * 2047)<<4)); |
nagasm | 0:5ec74240d7f1 | 37 | } |
nagasm | 0:5ec74240d7f1 | 38 | } |
nagasm | 0:5ec74240d7f1 | 39 | if(timer_value[0] > 199999){ // 1000msec |
nagasm | 0:5ec74240d7f1 | 40 | timer_value[0] = 0; |
nagasm | 0:5ec74240d7f1 | 41 | myled = !myled; |
nagasm | 0:5ec74240d7f1 | 42 | } |
nagasm | 0:5ec74240d7f1 | 43 | if(rx_fifo_check() == 1){ |
nagasm | 0:5ec74240d7f1 | 44 | int sum = 0; |
nagasm | 0:5ec74240d7f1 | 45 | for (int i=0; i<6; i++) sum += conv_hex(raw_data[i])<<(4*(5-i)); |
nagasm | 0:5ec74240d7f1 | 46 | tx_message(sum); // Echo Back |
nagasm | 0:5ec74240d7f1 | 47 | if(sum>>16 == 0x80){ |
nagasm | 0:5ec74240d7f1 | 48 | switch((sum & 0xff00)>>8){ |
nagasm | 0:5ec74240d7f1 | 49 | case 0x00: |
nagasm | 0:5ec74240d7f1 | 50 | fir_lpf = sum & 0x01; |
nagasm | 0:5ec74240d7f1 | 51 | break; |
nagasm | 0:5ec74240d7f1 | 52 | case 0x01: |
nagasm | 0:5ec74240d7f1 | 53 | gain = sum & 0x0f; |
nagasm | 0:5ec74240d7f1 | 54 | break; |
nagasm | 0:5ec74240d7f1 | 55 | case 0x02: |
nagasm | 0:5ec74240d7f1 | 56 | max_count = sum & 0x7f; |
nagasm | 0:5ec74240d7f1 | 57 | if (max_count>100) max_count = 100; |
nagasm | 0:5ec74240d7f1 | 58 | sum_clear(); |
nagasm | 0:5ec74240d7f1 | 59 | break; |
nagasm | 0:5ec74240d7f1 | 60 | case 0x03: |
nagasm | 1:8fa2f521009a | 61 | notch_1 = sum & 0x01; |
nagasm | 1:8fa2f521009a | 62 | break; |
nagasm | 1:8fa2f521009a | 63 | case 0x04: |
nagasm | 1:8fa2f521009a | 64 | notch_2 = sum & 0x01; |
nagasm | 1:8fa2f521009a | 65 | break; |
nagasm | 1:8fa2f521009a | 66 | case 0x05: |
nagasm | 0:5ec74240d7f1 | 67 | coef_set(sum & 0x3f); |
nagasm | 0:5ec74240d7f1 | 68 | break; |
nagasm | 0:5ec74240d7f1 | 69 | } |
nagasm | 0:5ec74240d7f1 | 70 | } |
nagasm | 0:5ec74240d7f1 | 71 | } |
nagasm | 0:5ec74240d7f1 | 72 | } |
nagasm | 0:5ec74240d7f1 | 73 | } |