Diff: main.cpp

Revision:

0:eae101ae93c0

Child:

1:bbbebd8be115

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 06 10:54:28 2019 +0000
@@ -0,0 +1,123 @@
+#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;
+
+
+
+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;
+    
+    wait(0.2);
+    
+    
+    pc.printf("All initialized\r\n");
+}
+
+void SensingMPU(){
+    //static int16_t deltaT = 0, t_start = 0;
+    //t_start = t.read_us();
+    
+    float rpy[3] = {0}, oldrpy[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;
+    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");
+}
\ No newline at end of file