Diff: main.cpp

Revision:

2:ad66e39cb4a1

Parent:

1:c91577293e84

--- a/main.cpp	Thu Feb 07 06:14:43 2019 +0000
+++ b/main.cpp	Fri Feb 08 07:10:06 2019 +0000
@@ -1,17 +1,29 @@
 #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(){ 
-   int16_t a[3];
-   double b;
+   
+   
+   setup();
   
    while(1){
-       compass.init();
-       b= compass.getHeadingXYDeg(20,50);
-       printf("%lf\r\n",b);
-       
+       SensingHMC();
+       pc.printf("%3.2f\t\r\n",nowAngle_HMC);
       // wait(0.);
        }
        
@@ -24,3 +36,100 @@
   
     }*/
 }
+
+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;
+    
+}