Diff: main.cpp

Revision:

6:166746820555

Parent:

5:8bfe95431ec0

Child:

7:8989a4b84695

--- a/main.cpp	Fri Feb 08 09:11:27 2019 +0000
+++ b/main.cpp	Fri Feb 08 10:39:17 2019 +0000
@@ -1,11 +1,12 @@
 #include "mbed.h"
 #include "MPU6050_DMP6.h"
+#include "HMC5883L.h"
 
 //MPU_check用
 #define PI 3.14159265358979
 
-#define servo_NEUTRAL_R    1616
-#define servo_NEUTRAL_L    1616
+#define servo_NEUTRAL_R    1900
+#define servo_NEUTRAL_L    1900
 #define servo_FORWARD_R    1860
 #define servo_FORWARD_L    1860
 #define servo_back_R       1060
@@ -37,9 +38,10 @@
 void Init_sensors();
 void DisplayClock();
 void DebugPrint();
+void SensingHMC();
 
-static float nowAngle[3] = {0,0,0};
-float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;
+static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
+float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
 
 bool setupFlag=false;
 
@@ -48,8 +50,8 @@
 Timer t;
 
 //PWM pin宣言
-PwmOut servoR(PC_6);           //TIM3_CH1
-PwmOut servoL(PC_7);           //TIM3_CH2
+PwmOut servoR(PC_6);           //TIM3_CH1 車輪右
+PwmOut servoL(PC_7);           //TIM3_CH2 車輪左
 PwmOut servoTurnTable(PB_0);   //TIM3_CH3 カメラ台回転Servo
 PwmOut servoCameradeg(PB_1);   //TIM3_CH4 カメラ角度調節Servo
 PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
@@ -66,7 +68,7 @@
 /*超音波はRaspberryPiに積む*/
 
 //外付けコンパス
-//HMC5983 compass(PB_8, PB_9);    //コンパスセンサー TIM4_CH3とCH4
+HMC5883L compass(PB_9, PB_8);    //コンパスセンサー TIM4_CH3とCH4
 
 RawSerial pc(PA_2,PA_3,115200); //uart2
 //pa2:UART2_TX,pa3:UART2_RX
@@ -98,6 +100,7 @@
         MoveCansat(g_landingcommand);
         
         SensingMPU();
+        SensingHMC();
         DebugPrint();
         
         wait_ms(23);
@@ -272,11 +275,14 @@
     t.start();
     
     pc.printf("MPU calibration start\r\n");
+    pc.printf("HMC calibration start\r\n");
     
     float offsetstart = t.read();
     while(t.read() - offsetstart < 26){
         SensingMPU();
+        SensingHMC();
         for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
+        pc.printf("%3.2f\t",nowAngle_HMC);
         pc.printf("\r\n");
     }
     
@@ -284,6 +290,12 @@
     FirstPITCH = nowAngle[PITCH];
     nowAngle[ROLL] -=FirstROLL;
     nowAngle[PITCH] -=FirstPITCH;
+    FirstYAW = nowAngle[YAW];
+    nowAngle[YAW] -= FirstYAW;
+    
+    g_FirstYAW_HMC = nowAngle_HMC;
+    nowAngle_HMC -=g_FirstYAW_HMC;
+    if(nowAngle_HMC<0)nowAngle_HMC+=360;
     
     wait(0.2);
     
@@ -355,10 +367,56 @@
     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;
+    
+}
+    
 void DebugPrint(){
     //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
     pc.printf("%3.2f\t",nowAngle[2]);
+    pc.printf("%3.2f\t",nowAngle_HMC);
     pc.printf("\r\n");
-    }
-    
\ No newline at end of file
+    }
\ No newline at end of file