File content as of revision 2:ad66e39cb4a1:

#include "mbed.h"
#include "HMC5883L.h"

#define PI 3.14159265358979

void setup();
void DisplayClock();
void SensingHMC();

HMC5883L compass(PB_9, PB_8); 
RawSerial pc(PA_2,PA_3, 115200);

Timer t;
double g_HMC;
static float nowAngle_HMC=0;
float g_FirstYAW_HMC;
bool setupFlag=false;

int main(){ 
   
   
   setup();
  
   while(1){
       SensingHMC();
       pc.printf("%3.2f\t\r\n",nowAngle_HMC);
      // wait(0.);
       }
       
   /*while(1){
   compass.getXYZ(a);
   //printf("%d, %d, %d,\r\n",a[0],a[2]-150,a[1]);
   printf("%d, %d\r\n",a[0]+20,a[2]+50);
   //wait(1);}

  
    }*/
}

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("HMC calibration start\r\n");
    
    float offsetstart = t.read();
    while(t.read() - offsetstart < 26){
        SensingHMC();
        pc.printf("%3.2f\t",nowAngle_HMC);
        pc.printf("\r\n");
    }
    
    g_FirstYAW_HMC = nowAngle_HMC;
    nowAngle_HMC -=g_FirstYAW_HMC;
    if(nowAngle_HMC<0)nowAngle_HMC+=360;
    wait(0.2);
    
    
    pc.printf("All initialized\r\n");
    setupFlag=true;
}

/*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");
}

void SensingHMC(){
    //static int16_t deltaT = 0, t_start = 0;
    //t_start = t.read_us();
    
    float rpy=0, oldrpy=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);

    rpy= compass.getHeadingXYDeg(20,50);
 
    NVIC_EnableIRQ(USART1_IRQn);
    NVIC_EnableIRQ(USART2_IRQn);
    NVIC_EnableIRQ(TIM5_IRQn);
    NVIC_EnableIRQ(EXTI0_IRQn);
    NVIC_EnableIRQ(EXTI9_5_IRQn);
    
    
    //外れ値対策
    //rpy*= 180.0f/PI;
    if(!setupFlag){
        rpy -= g_FirstYAW_HMC;
        }else{
        if(rpy >= g_FirstYAW_HMC){
            rpy -= g_FirstYAW_HMC;
            }else{
            rpy += 360.0f;
            rpy -= g_FirstYAW_HMC;
        }
    }
    
    if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;}
    if(!flg_checkoutlier || count_changeRPY >= 2){
        
        nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f;  //2つの移動平均
        
        count_changeRPY = 0;
    }else   count_changeRPY++;
    flg_checkoutlier = false;
    
}