Yoichi Nagashima
/
CQ_nagasm_06_LPF
add - CQpub0 Mikami / FIR_LPF_Direct
main.cpp@0:5bbdcb797a66, 2014-12-15 (annotated)
- Committer:
- nagasm
- Date:
- Mon Dec 15 10:33:45 2014 +0000
- Revision:
- 0:5bbdcb797a66
add - CQpub0 Mikami / FIR_LPF_Direct
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nagasm | 0:5bbdcb797a66 | 1 | #include "mbed.h" |
nagasm | 0:5bbdcb797a66 | 2 | #include "sub.hpp" |
nagasm | 0:5bbdcb797a66 | 3 | #include "FIR_LPF.hpp" |
nagasm | 0:5bbdcb797a66 | 4 | |
nagasm | 0:5bbdcb797a66 | 5 | float mean_sum, ad_data[101]; |
nagasm | 0:5bbdcb797a66 | 6 | int average_mode, max_count, ad_pointer[5]; |
nagasm | 0:5bbdcb797a66 | 7 | |
nagasm | 0:5bbdcb797a66 | 8 | void sum_clear(){ |
nagasm | 0:5bbdcb797a66 | 9 | int i; |
nagasm | 0:5bbdcb797a66 | 10 | for (i=0; i<5; i++) ad_pointer[i] = 0; |
nagasm | 0:5bbdcb797a66 | 11 | for (i=0; i<101; i++) ad_data[i] = 0; |
nagasm | 0:5bbdcb797a66 | 12 | mean_sum = 0; |
nagasm | 0:5bbdcb797a66 | 13 | } |
nagasm | 0:5bbdcb797a66 | 14 | |
nagasm | 0:5bbdcb797a66 | 15 | float move_mean_calc(float data){ |
nagasm | 0:5bbdcb797a66 | 16 | mean_sum = mean_sum - ad_data[ad_pointer[0]] + data; |
nagasm | 0:5bbdcb797a66 | 17 | ad_data[ad_pointer[0]] = data; |
nagasm | 0:5bbdcb797a66 | 18 | ad_pointer[0]++; |
nagasm | 0:5bbdcb797a66 | 19 | if(ad_pointer[0] == max_count) ad_pointer[0] = 0; |
nagasm | 0:5bbdcb797a66 | 20 | return(mean_sum / (float)max_count); |
nagasm | 0:5bbdcb797a66 | 21 | } |
nagasm | 0:5bbdcb797a66 | 22 | |
nagasm | 0:5bbdcb797a66 | 23 | int main(){ |
nagasm | 0:5bbdcb797a66 | 24 | int i, detection, sum, gain; |
nagasm | 0:5bbdcb797a66 | 25 | i = 0; |
nagasm | 0:5bbdcb797a66 | 26 | detection = 0; |
nagasm | 0:5bbdcb797a66 | 27 | gain = 1; |
nagasm | 0:5bbdcb797a66 | 28 | for (i=0; i<5; i++) timer_value[i] = 0; |
nagasm | 0:5bbdcb797a66 | 29 | for (i=0; i<=order; i++) xn[i] = 0.0; |
nagasm | 0:5bbdcb797a66 | 30 | sum_clear(); |
nagasm | 0:5bbdcb797a66 | 31 | xbee.baud(38400); |
nagasm | 0:5bbdcb797a66 | 32 | xbee.attach(&rx_fifoset, xbee.RxIrq); |
nagasm | 0:5bbdcb797a66 | 33 | timer_setup.attach_us(&timer_interrupt, 5); // 5usec |
nagasm | 0:5bbdcb797a66 | 34 | while(1){ |
nagasm | 0:5bbdcb797a66 | 35 | if(timer_value[1] > 19){ // 0.1msec sampling |
nagasm | 0:5bbdcb797a66 | 36 | timer_value[1] = 0; |
nagasm | 0:5bbdcb797a66 | 37 | float data = (float)gain * (analog_value3.read() - 0.5f); |
nagasm | 0:5bbdcb797a66 | 38 | if(detection == 1){ |
nagasm | 0:5bbdcb797a66 | 39 | if (data < 0) data = -data; |
nagasm | 0:5bbdcb797a66 | 40 | } |
nagasm | 0:5bbdcb797a66 | 41 | data = FIR_calc(data); // FIR calc call |
nagasm | 0:5bbdcb797a66 | 42 | if(timer_value[2] > 1999){ // 10msec |
nagasm | 0:5bbdcb797a66 | 43 | timer_value[2] = 0; |
nagasm | 0:5bbdcb797a66 | 44 | if(average_mode != 0) data = move_mean_calc(data); |
nagasm | 0:5bbdcb797a66 | 45 | tx_message((uint16_t)((data + 1.0f) * 2047)<<4); |
nagasm | 0:5bbdcb797a66 | 46 | } |
nagasm | 0:5bbdcb797a66 | 47 | } |
nagasm | 0:5bbdcb797a66 | 48 | if(timer_value[0] > 99999){ // 500msec |
nagasm | 0:5bbdcb797a66 | 49 | timer_value[0] = 0; |
nagasm | 0:5bbdcb797a66 | 50 | myled = !myled; |
nagasm | 0:5bbdcb797a66 | 51 | } |
nagasm | 0:5bbdcb797a66 | 52 | tx_fifo_check(); |
nagasm | 0:5bbdcb797a66 | 53 | if(rx_fifo_check() == 1){ |
nagasm | 0:5bbdcb797a66 | 54 | sum = 0; |
nagasm | 0:5bbdcb797a66 | 55 | for (i=0; i<6; i++) sum += conv_hex(raw_data[i])<<(4*(5-i)); |
nagasm | 0:5bbdcb797a66 | 56 | tx_message(sum); // Echo Back |
nagasm | 0:5bbdcb797a66 | 57 | if(sum>>16 == 0x80){ |
nagasm | 0:5bbdcb797a66 | 58 | switch((sum & 0xff00)>>8){ |
nagasm | 0:5bbdcb797a66 | 59 | case 0x00: |
nagasm | 0:5bbdcb797a66 | 60 | detection = sum & 0x01; |
nagasm | 0:5bbdcb797a66 | 61 | break; |
nagasm | 0:5bbdcb797a66 | 62 | case 0x01: |
nagasm | 0:5bbdcb797a66 | 63 | gain = sum & 0x0f; |
nagasm | 0:5bbdcb797a66 | 64 | break; |
nagasm | 0:5bbdcb797a66 | 65 | case 0x02: |
nagasm | 0:5bbdcb797a66 | 66 | average_mode = sum & 0x07; |
nagasm | 0:5bbdcb797a66 | 67 | max_count = 5; |
nagasm | 0:5bbdcb797a66 | 68 | switch(average_mode){ |
nagasm | 0:5bbdcb797a66 | 69 | case(2): |
nagasm | 0:5bbdcb797a66 | 70 | max_count = 10; |
nagasm | 0:5bbdcb797a66 | 71 | break; |
nagasm | 0:5bbdcb797a66 | 72 | case(3): |
nagasm | 0:5bbdcb797a66 | 73 | max_count = 20; |
nagasm | 0:5bbdcb797a66 | 74 | break; |
nagasm | 0:5bbdcb797a66 | 75 | case(4): |
nagasm | 0:5bbdcb797a66 | 76 | max_count = 50; |
nagasm | 0:5bbdcb797a66 | 77 | break; |
nagasm | 0:5bbdcb797a66 | 78 | case(5): |
nagasm | 0:5bbdcb797a66 | 79 | max_count = 100; |
nagasm | 0:5bbdcb797a66 | 80 | break; |
nagasm | 0:5bbdcb797a66 | 81 | } |
nagasm | 0:5bbdcb797a66 | 82 | sum_clear(); |
nagasm | 0:5bbdcb797a66 | 83 | break; |
nagasm | 0:5bbdcb797a66 | 84 | } |
nagasm | 0:5bbdcb797a66 | 85 | } |
nagasm | 0:5bbdcb797a66 | 86 | } |
nagasm | 0:5bbdcb797a66 | 87 | } |
nagasm | 0:5bbdcb797a66 | 88 | } |