航空研究会 / Mbed 2 deprecated MPU_check_kai

－の値が出ないように修正

Dependencies:   mbed MPU6050_2

main.cpp@1:bbbebd8be115, 2019-02-08 (annotated)

Committer:
taknokolat
Date:
Fri Feb 08 06:54:54 2019 +0000
Revision:
1:bbbebd8be115
Parent:
0:eae101ae93c0 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taknokolat 0:eae101ae93c0 1 #include "mbed.h"
taknokolat 0:eae101ae93c0 2 #include "MPU6050_DMP6.h"
taknokolat 0:eae101ae93c0 3
taknokolat 0:eae101ae93c0 4 #define PI 3.14159265358979
taknokolat 0:eae101ae93c0 5
taknokolat 0:eae101ae93c0 6 RawSerial pc(PA_2,PA_3, 115200);
taknokolat 0:eae101ae93c0 7 MPU6050DMP6 mpu6050(PC_0,&pc);
taknokolat 0:eae101ae93c0 8
taknokolat 0:eae101ae93c0 9 void SensingMPU();
taknokolat 0:eae101ae93c0 10 void setup();
taknokolat 0:eae101ae93c0 11 void Init_sensors();
taknokolat 0:eae101ae93c0 12 void DisplayClock();
taknokolat 0:eae101ae93c0 13
taknokolat 0:eae101ae93c0 14 static float nowAngle[3] = {0,0,0};
taknokolat 0:eae101ae93c0 15 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;
taknokolat 0:eae101ae93c0 16
taknokolat 0:eae101ae93c0 17 enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
taknokolat 0:eae101ae93c0 18
taknokolat 0:eae101ae93c0 19 Timer t;
taknokolat 0:eae101ae93c0 20
taknokolat 1:bbbebd8be115 21 bool setupFlag=false;
taknokolat 1:bbbebd8be115 22
taknokolat 0:eae101ae93c0 23
taknokolat 0:eae101ae93c0 24
taknokolat 0:eae101ae93c0 25 int main() {
taknokolat 0:eae101ae93c0 26 setup();
taknokolat 0:eae101ae93c0 27 while(1){
taknokolat 0:eae101ae93c0 28 SensingMPU();
taknokolat 0:eae101ae93c0 29 wait_ms(23);
taknokolat 0:eae101ae93c0 30 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
taknokolat 0:eae101ae93c0 31 pc.printf("\r\n");
taknokolat 0:eae101ae93c0 32 }
taknokolat 0:eae101ae93c0 33
taknokolat 0:eae101ae93c0 34 }
taknokolat 0:eae101ae93c0 35
taknokolat 0:eae101ae93c0 36 void setup(){
taknokolat 0:eae101ae93c0 37
taknokolat 0:eae101ae93c0 38
taknokolat 0:eae101ae93c0 39
taknokolat 0:eae101ae93c0 40
taknokolat 0:eae101ae93c0 41
taknokolat 0:eae101ae93c0 42 Init_sensors();
taknokolat 0:eae101ae93c0 43 //switch2.rise(ResetTrim);
taknokolat 0:eae101ae93c0 44
taknokolat 0:eae101ae93c0 45 NVIC_SetPriority(USART1_IRQn,0);
taknokolat 0:eae101ae93c0 46 NVIC_SetPriority(EXTI0_IRQn,1);
taknokolat 0:eae101ae93c0 47 NVIC_SetPriority(TIM5_IRQn,2);
taknokolat 0:eae101ae93c0 48 NVIC_SetPriority(EXTI9_5_IRQn,3);
taknokolat 0:eae101ae93c0 49 NVIC_SetPriority(USART2_IRQn,4);
taknokolat 0:eae101ae93c0 50 DisplayClock();
taknokolat 0:eae101ae93c0 51 t.start();
taknokolat 0:eae101ae93c0 52
taknokolat 0:eae101ae93c0 53
taknokolat 0:eae101ae93c0 54 pc.printf("MPU calibration start\r\n");
taknokolat 0:eae101ae93c0 55
taknokolat 0:eae101ae93c0 56 float offsetstart = t.read();
taknokolat 0:eae101ae93c0 57 while(t.read() - offsetstart < 26){
taknokolat 0:eae101ae93c0 58 SensingMPU();
taknokolat 0:eae101ae93c0 59 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
taknokolat 0:eae101ae93c0 60 pc.printf("\r\n");
taknokolat 0:eae101ae93c0 61 }
taknokolat 0:eae101ae93c0 62
taknokolat 0:eae101ae93c0 63 FirstROLL = nowAngle[ROLL];
taknokolat 0:eae101ae93c0 64 FirstPITCH = nowAngle[PITCH];
taknokolat 0:eae101ae93c0 65 nowAngle[ROLL] -=FirstROLL;
taknokolat 0:eae101ae93c0 66 nowAngle[PITCH] -=FirstPITCH;
taknokolat 1:bbbebd8be115 67 FirstYAW = nowAngle[YAW];
taknokolat 1:bbbebd8be115 68 nowAngle[YAW] -= FirstYAW;
taknokolat 0:eae101ae93c0 69
taknokolat 0:eae101ae93c0 70 wait(0.2);
taknokolat 0:eae101ae93c0 71
taknokolat 0:eae101ae93c0 72
taknokolat 0:eae101ae93c0 73 pc.printf("All initialized\r\n");
taknokolat 1:bbbebd8be115 74 setupFlag=true;
taknokolat 0:eae101ae93c0 75 }
taknokolat 0:eae101ae93c0 76
taknokolat 0:eae101ae93c0 77 void SensingMPU(){
taknokolat 0:eae101ae93c0 78 //static int16_t deltaT = 0, t_start = 0;
taknokolat 0:eae101ae93c0 79 //t_start = t.read_us();
taknokolat 0:eae101ae93c0 80
taknokolat 1:bbbebd8be115 81 float rpy[3] = {0};
taknokolat 0:eae101ae93c0 82 static uint16_t count_changeRPY = 0;
taknokolat 0:eae101ae93c0 83 static bool flg_checkoutlier = false;
taknokolat 0:eae101ae93c0 84 NVIC_DisableIRQ(USART1_IRQn);
taknokolat 0:eae101ae93c0 85 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 0:eae101ae93c0 86 NVIC_DisableIRQ(TIM5_IRQn);
taknokolat 0:eae101ae93c0 87 NVIC_DisableIRQ(EXTI0_IRQn);
taknokolat 0:eae101ae93c0 88 NVIC_DisableIRQ(EXTI9_5_IRQn);
taknokolat 0:eae101ae93c0 89
taknokolat 0:eae101ae93c0 90 mpu6050.getRollPitchYaw_Skipper(rpy);
taknokolat 0:eae101ae93c0 91
taknokolat 0:eae101ae93c0 92 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 0:eae101ae93c0 93 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 0:eae101ae93c0 94 NVIC_EnableIRQ(TIM5_IRQn);
taknokolat 0:eae101ae93c0 95 NVIC_EnableIRQ(EXTI0_IRQn);
taknokolat 0:eae101ae93c0 96 NVIC_EnableIRQ(EXTI9_5_IRQn);
taknokolat 0:eae101ae93c0 97
taknokolat 0:eae101ae93c0 98
taknokolat 0:eae101ae93c0 99 //外れ値対策
taknokolat 0:eae101ae93c0 100 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
taknokolat 0:eae101ae93c0 101 rpy[ROLL] -= FirstROLL;
taknokolat 0:eae101ae93c0 102 rpy[PITCH] -= FirstPITCH;
taknokolat 1:bbbebd8be115 103 if(!setupFlag){
taknokolat 1:bbbebd8be115 104 rpy[YAW] -= FirstYAW;
taknokolat 1:bbbebd8be115 105 }else{
taknokolat 1:bbbebd8be115 106 if(rpy[YAW] >= FirstYAW){
taknokolat 1:bbbebd8be115 107 rpy[YAW] -= FirstYAW;
taknokolat 1:bbbebd8be115 108 }else{
taknokolat 1:bbbebd8be115 109 rpy[YAW] += 360.0f;
taknokolat 1:bbbebd8be115 110 rpy[YAW] -= FirstYAW;
taknokolat 1:bbbebd8be115 111 }
taknokolat 1:bbbebd8be115 112 }
taknokolat 0:eae101ae93c0 113
taknokolat 0:eae101ae93c0 114 for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
taknokolat 0:eae101ae93c0 115 if(!flg_checkoutlier || count_changeRPY >= 2){
taknokolat 0:eae101ae93c0 116 for(uint8_t i=0; i<3; i++){
taknokolat 0:eae101ae93c0 117 nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
taknokolat 0:eae101ae93c0 118 }
taknokolat 0:eae101ae93c0 119 count_changeRPY = 0;
taknokolat 0:eae101ae93c0 120 }else count_changeRPY++;
taknokolat 0:eae101ae93c0 121 flg_checkoutlier = false;
taknokolat 0:eae101ae93c0 122
taknokolat 0:eae101ae93c0 123 }
taknokolat 0:eae101ae93c0 124
taknokolat 0:eae101ae93c0 125 void Init_sensors(){
taknokolat 0:eae101ae93c0 126 if(mpu6050.setup() == -1){
taknokolat 0:eae101ae93c0 127 pc.printf("failed initialize\r\n");
taknokolat 0:eae101ae93c0 128 }
taknokolat 0:eae101ae93c0 129 }
taknokolat 0:eae101ae93c0 130
taknokolat 0:eae101ae93c0 131 void DisplayClock(){
taknokolat 0:eae101ae93c0 132 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
taknokolat 0:eae101ae93c0 133 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
taknokolat 0:eae101ae93c0 134 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
taknokolat 0:eae101ae93c0 135 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
taknokolat 0:eae101ae93c0 136 pc.printf("\r\n");
taknokolat 0:eae101ae93c0 137 }