File content as of revision 0:5bbdcb797a66:

#include "mbed.h"
#include "sub.hpp"
#include "FIR_LPF.hpp"

float mean_sum, ad_data[101];
int average_mode, max_count, ad_pointer[5];

void sum_clear(){
    int i;
    for (i=0; i<5; i++) ad_pointer[i] = 0;
    for (i=0; i<101; i++) ad_data[i] = 0;
    mean_sum = 0;
}

float move_mean_calc(float data){
    mean_sum = mean_sum - ad_data[ad_pointer[0]] + data;
    ad_data[ad_pointer[0]] = data;
    ad_pointer[0]++;
    if(ad_pointer[0] == max_count) ad_pointer[0] = 0;
    return(mean_sum / (float)max_count);
}

int main(){
    int i, detection, sum, gain;
    i = 0;
    detection = 0;
    gain = 1;
    for (i=0; i<5; i++) timer_value[i] = 0;
    for (i=0; i<=order; i++) xn[i] = 0.0;
    sum_clear();
    xbee.baud(38400);
    xbee.attach(&rx_fifoset, xbee.RxIrq);
    timer_setup.attach_us(&timer_interrupt, 5); // 5usec
    while(1){
        if(timer_value[1] > 19){ // 0.1msec sampling
            timer_value[1] = 0;
            float data = (float)gain * (analog_value3.read() - 0.5f);
            if(detection == 1){
                if (data < 0) data = -data;
            }
            data = FIR_calc(data);      // FIR calc call
            if(timer_value[2] > 1999){  // 10msec
                timer_value[2] = 0;
                if(average_mode != 0) data = move_mean_calc(data);
                tx_message((uint16_t)((data + 1.0f) * 2047)<<4);
            }
        }
        if(timer_value[0] > 99999){ // 500msec
            timer_value[0] = 0;
            myled = !myled;
        }
        tx_fifo_check();
        if(rx_fifo_check() == 1){
            sum = 0;
            for (i=0; i<6; i++) sum += conv_hex(raw_data[i])<<(4*(5-i));
            tx_message(sum); // Echo Back 
            if(sum>>16 == 0x80){
                switch((sum & 0xff00)>>8){
                    case 0x00:
                        detection = sum & 0x01;
                        break;
                    case 0x01:
                        gain = sum & 0x0f;
                        break;
                    case 0x02:
                        average_mode = sum & 0x07;
                        max_count = 5;
                        switch(average_mode){
                            case(2):
                                max_count = 10;
                                break;
                            case(3):
                                max_count = 20;
                                break;
                            case(4):
                                max_count = 50;
                                break;
                            case(5):
                                max_count = 100;
                                break;
                            }
                        sum_clear();
                        break;
                }
            }
        }        
    }
}