File content as of revision 1:bbbebd8be115:

#include "mbed.h"
#include "MPU6050_DMP6.h"

#define PI 3.14159265358979

RawSerial pc(PA_2,PA_3, 115200);
MPU6050DMP6 mpu6050(PC_0,&pc);

void SensingMPU();
void setup();
void Init_sensors();
void DisplayClock();  

static float nowAngle[3] = {0,0,0};
float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;

enum Angle{ROLL, PITCH, YAW};   //yaw:北を0とした絶対角度

Timer t;

bool setupFlag=false;



int main() {
    setup();
    while(1){
        SensingMPU();
        wait_ms(23);
        for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
        pc.printf("\r\n");
        }

}

void setup(){

    
        
  
     
    Init_sensors();
    //switch2.rise(ResetTrim);
    
    NVIC_SetPriority(USART1_IRQn,0);
    NVIC_SetPriority(EXTI0_IRQn,1);
    NVIC_SetPriority(TIM5_IRQn,2);
    NVIC_SetPriority(EXTI9_5_IRQn,3);
    NVIC_SetPriority(USART2_IRQn,4);
    DisplayClock();
    t.start();
    
    
    pc.printf("MPU calibration start\r\n");
    
    float offsetstart = t.read();
    while(t.read() - offsetstart < 26){
        SensingMPU();
        for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
        pc.printf("\r\n");
    }
    
    FirstROLL = nowAngle[ROLL];
    FirstPITCH = nowAngle[PITCH];
    nowAngle[ROLL] -=FirstROLL;
    nowAngle[PITCH] -=FirstPITCH;
    FirstYAW = nowAngle[YAW];
    nowAngle[YAW] -= FirstYAW;
    
    wait(0.2);
    
    
    pc.printf("All initialized\r\n");
    setupFlag=true;
}

void SensingMPU(){
    //static int16_t deltaT = 0, t_start = 0;
    //t_start = t.read_us();
    
    float rpy[3] = {0};
    static uint16_t count_changeRPY = 0;
    static bool flg_checkoutlier = false;
    NVIC_DisableIRQ(USART1_IRQn);
    NVIC_DisableIRQ(USART2_IRQn);
    NVIC_DisableIRQ(TIM5_IRQn);
    NVIC_DisableIRQ(EXTI0_IRQn);
    NVIC_DisableIRQ(EXTI9_5_IRQn);

    mpu6050.getRollPitchYaw_Skipper(rpy);
 
    NVIC_EnableIRQ(USART1_IRQn);
    NVIC_EnableIRQ(USART2_IRQn);
    NVIC_EnableIRQ(TIM5_IRQn);
    NVIC_EnableIRQ(EXTI0_IRQn);
    NVIC_EnableIRQ(EXTI9_5_IRQn);
    
    
    //外れ値対策
    for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
    rpy[ROLL] -= FirstROLL;
    rpy[PITCH] -= FirstPITCH;
    if(!setupFlag){
        rpy[YAW] -= FirstYAW;
        }else{
        if(rpy[YAW] >= FirstYAW){
            rpy[YAW] -= FirstYAW;
            }else{
            rpy[YAW] += 360.0f;
            rpy[YAW] -= FirstYAW;
        }
    }
    
    for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
    if(!flg_checkoutlier || count_changeRPY >= 2){
        for(uint8_t i=0; i<3; i++){
            nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f;  //2つの移動平均
        }
        count_changeRPY = 0;
    }else   count_changeRPY++;
    flg_checkoutlier = false;
    
}

void Init_sensors(){
    if(mpu6050.setup() == -1){
        pc.printf("failed initialize\r\n");
    }
}

void DisplayClock(){
    pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
    pc.printf("HCLK Clock   = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
    pc.printf("PCLK1 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
    pc.printf("PCLK2 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
    pc.printf("\r\n");
}