File content as of revision 0:2245e978868c:

#include "mbed.h"
#include "FXOS8700Q.h"
#include "FXAS21000.h"
#include "rtos.h"
#include "kalman.c"
#include "arm_math.h" 

#define TRUE            1
#define FALSE           0
#define RAW             0
#define RECTIFIED       1
#define SMOOTH          2        
#define THRESHOLDF      0.25f
#define THRESHOLDE      0.25f
#define FLEXION2        0.0f
#define Rad2Dree        57.295779513082320876798154814105f

Serial pc(USBTX, USBRX);
AnalogIn   Ref(A0);
AnalogIn   E1(A1);
AnalogIn   E2(A2);
AnalogIn   E3(A3);
FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR1);
FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR1);
FXAS21000 gyro(D14, D15);
DigitalOut led1(LED1);
InterruptIn sw2(SW2);
Timer GlobalTime;
Timer ProgramTimer;

//Threadas
uint32_t button_pressed;
Thread *thread2;
Thread *thread3;
Thread *thread4;

//EMG samples and DSP variables
float32_t EMG1, EMG2, EMG3;
float32_t samples[25];
float32_t samples2[25];
float32_t samples3[25];
float32_t abs_output[25];
float32_t abs_output2[25];
float32_t abs_output3[25];
float32_t mean = 0.0f;
float32_t mean2 = 0.0f; 
float32_t mean3 = 0.0f;

//IMU
float gyro_data[3];
MotionSensorDataUnits adata;
MotionSensorDataUnits mdata;
float R;
float angle[3];
float roll;
float pitch;
kalman filter_pitch; 
kalman filter_roll;

//OS    
osMutexId el_mutex;
osMutexDef(el_mutex);                                          
 
//time 
unsigned long timer;
long loopStartTime;

void sw2_press(void);
void button_thread(void const *argument);
void imu_thread(void const *argument);
void sampler_thread(void const *argument);
void logger_thread(void const *argument);

void Serial_Oscilloscope(uint8_t activation, uint8_t type);

int main(void)
{
    pc.baud(115200);                                  //Serial com at 115200 bauds
    Thread thread(imu_thread);
    thread2 = new Thread(button_thread);
    thread3 = new Thread(sampler_thread);
    thread4 = new Thread(logger_thread);
    el_mutex = osMutexCreate(osMutex(el_mutex));
    GlobalTime.start();
    
    combo_acc.enable();
    combo_mag.enable();
    pc.printf("FXOS8700 Combo = %X\r\n", combo_acc.whoAmI());
    pc.printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI());
    
    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
    
    button_pressed = 0;
    sw2.fall(&sw2_press);
    
    ProgramTimer.start();
    loopStartTime = ProgramTimer.read_us();
    timer = loopStartTime;
    
    while (true) {
        Thread::wait(5000);
        led1 = !led1;
        pc.printf("SW2 was pressed (last 5 seconds): %d \n\r", button_pressed);
        pc.printf("Roll Angle X: %.6f   Pitch Angle Y: %.6f \r\n", Rad2Dree * angle[1], Rad2Dree * angle[0]);
        fflush(stdout);
        button_pressed = 0;
    }
}

void sw2_press(void){
    thread2->signal_set(0x1);
}

void button_thread(void const *argument){
    while (true) {
        Thread::signal_wait(0x1);
        button_pressed++;
    }
}

void imu_thread(void const *argument){
    while (true) {
        
        combo_acc.getAxis(adata); //pc.printf("FXOS8700 Acc:   X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
        combo_mag.getAxis(mdata); //pc.printf("FXOS8700 Mag:   X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
        gyro.ReadXYZ(gyro_data);  //pc.printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
        R = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
        
        kalman_predict(&filter_pitch, gyro_data[0],  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_pitch, acos(adata.x/R));
        kalman_predict(&filter_roll, gyro_data[1],  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_roll, acos(adata.y/R));
        
        angle[0] = kalman_get_angle(&filter_pitch);
        angle[1] = kalman_get_angle(&filter_roll);
        Thread::wait(10);
    }
}

void sampler_thread(void const *argument){
    while (true) {
        EMG1 = (E1.read()-Ref.read())*3.3f;
        EMG2 = (E2.read()-Ref.read())*3.3f;
        EMG3 = (E3.read()-Ref.read())*3.3f;
        //osMutexWait(el_mutex, osWaitForever);        
        for(int j=24;j>0;j--) {
            samples[j]=samples[j-1];                    //Fill Array
            samples2[j]=samples2[j-1];                  //Fill Array
            samples3[j]=samples3[j-1];                  //Fill Array
        }
        samples[0]=EMG1;
        samples2[0]=EMG2;
        samples3[0]=EMG3;       
        //osMutexRelease(el_mutex);
        Thread::wait(1);
    }
}


void logger_thread(void const *argument){
    while (true) {
        Thread::wait(50);
    }
}

void Serial_Oscilloscope(uint8_t activation, uint8_t type){
    if (activation){
        switch(type){
            case RAW: pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3); break; 
            case RECTIFIED: pc.printf("%.10f,%.10f,%.10f\n\r",abs_output[0],abs_output2[0],abs_output3[0]);break;
            case SMOOTH: pc.printf("%.10f,%.10f,%.10f\n\r",mean,mean2,mean3); break;
            default: pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3); 
        } 
    }    
}