航空研究会 / Mbed 2 deprecated MPU_check_kai

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

Dependencies:   mbed MPU6050_2

main.cpp@0:eae101ae93c0, 2019-02-06 (annotated)

Committer:
taknokolat
Date:
Wed Feb 06 10:54:28 2019 +0000
Revision:
0:eae101ae93c0
Child:
1:bbbebd8be115
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 0:eae101ae93c0 21
taknokolat 0:eae101ae93c0 22
taknokolat 0:eae101ae93c0 23 int main() {
taknokolat 0:eae101ae93c0 24 setup();
taknokolat 0:eae101ae93c0 25 while(1){
taknokolat 0:eae101ae93c0 26 SensingMPU();
taknokolat 0:eae101ae93c0 27 wait_ms(23);
taknokolat 0:eae101ae93c0 28 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
taknokolat 0:eae101ae93c0 29 pc.printf("\r\n");
taknokolat 0:eae101ae93c0 30 }
taknokolat 0:eae101ae93c0 31
taknokolat 0:eae101ae93c0 32 }
taknokolat 0:eae101ae93c0 33
taknokolat 0:eae101ae93c0 34 void setup(){
taknokolat 0:eae101ae93c0 35
taknokolat 0:eae101ae93c0 36
taknokolat 0:eae101ae93c0 37
taknokolat 0:eae101ae93c0 38
taknokolat 0:eae101ae93c0 39
taknokolat 0:eae101ae93c0 40 Init_sensors();
taknokolat 0:eae101ae93c0 41 //switch2.rise(ResetTrim);
taknokolat 0:eae101ae93c0 42
taknokolat 0:eae101ae93c0 43 NVIC_SetPriority(USART1_IRQn,0);
taknokolat 0:eae101ae93c0 44 NVIC_SetPriority(EXTI0_IRQn,1);
taknokolat 0:eae101ae93c0 45 NVIC_SetPriority(TIM5_IRQn,2);
taknokolat 0:eae101ae93c0 46 NVIC_SetPriority(EXTI9_5_IRQn,3);
taknokolat 0:eae101ae93c0 47 NVIC_SetPriority(USART2_IRQn,4);
taknokolat 0:eae101ae93c0 48 DisplayClock();
taknokolat 0:eae101ae93c0 49 t.start();
taknokolat 0:eae101ae93c0 50
taknokolat 0:eae101ae93c0 51
taknokolat 0:eae101ae93c0 52 pc.printf("MPU calibration start\r\n");
taknokolat 0:eae101ae93c0 53
taknokolat 0:eae101ae93c0 54 float offsetstart = t.read();
taknokolat 0:eae101ae93c0 55 while(t.read() - offsetstart < 26){
taknokolat 0:eae101ae93c0 56 SensingMPU();
taknokolat 0:eae101ae93c0 57 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
taknokolat 0:eae101ae93c0 58 pc.printf("\r\n");
taknokolat 0:eae101ae93c0 59 }
taknokolat 0:eae101ae93c0 60
taknokolat 0:eae101ae93c0 61 FirstROLL = nowAngle[ROLL];
taknokolat 0:eae101ae93c0 62 FirstPITCH = nowAngle[PITCH];
taknokolat 0:eae101ae93c0 63 nowAngle[ROLL] -=FirstROLL;
taknokolat 0:eae101ae93c0 64 nowAngle[PITCH] -=FirstPITCH;
taknokolat 0:eae101ae93c0 65
taknokolat 0:eae101ae93c0 66 wait(0.2);
taknokolat 0:eae101ae93c0 67
taknokolat 0:eae101ae93c0 68
taknokolat 0:eae101ae93c0 69 pc.printf("All initialized\r\n");
taknokolat 0:eae101ae93c0 70 }
taknokolat 0:eae101ae93c0 71
taknokolat 0:eae101ae93c0 72 void SensingMPU(){
taknokolat 0:eae101ae93c0 73 //static int16_t deltaT = 0, t_start = 0;
taknokolat 0:eae101ae93c0 74 //t_start = t.read_us();
taknokolat 0:eae101ae93c0 75
taknokolat 0:eae101ae93c0 76 float rpy[3] = {0}, oldrpy[3] = {0};
taknokolat 0:eae101ae93c0 77 static uint16_t count_changeRPY = 0;
taknokolat 0:eae101ae93c0 78 static bool flg_checkoutlier = false;
taknokolat 0:eae101ae93c0 79 NVIC_DisableIRQ(USART1_IRQn);
taknokolat 0:eae101ae93c0 80 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 0:eae101ae93c0 81 NVIC_DisableIRQ(TIM5_IRQn);
taknokolat 0:eae101ae93c0 82 NVIC_DisableIRQ(EXTI0_IRQn);
taknokolat 0:eae101ae93c0 83 NVIC_DisableIRQ(EXTI9_5_IRQn);
taknokolat 0:eae101ae93c0 84
taknokolat 0:eae101ae93c0 85 mpu6050.getRollPitchYaw_Skipper(rpy);
taknokolat 0:eae101ae93c0 86
taknokolat 0:eae101ae93c0 87 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 0:eae101ae93c0 88 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 0:eae101ae93c0 89 NVIC_EnableIRQ(TIM5_IRQn);
taknokolat 0:eae101ae93c0 90 NVIC_EnableIRQ(EXTI0_IRQn);
taknokolat 0:eae101ae93c0 91 NVIC_EnableIRQ(EXTI9_5_IRQn);
taknokolat 0:eae101ae93c0 92
taknokolat 0:eae101ae93c0 93
taknokolat 0:eae101ae93c0 94 //外れ値対策
taknokolat 0:eae101ae93c0 95 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
taknokolat 0:eae101ae93c0 96 rpy[ROLL] -= FirstROLL;
taknokolat 0:eae101ae93c0 97 rpy[PITCH] -= FirstPITCH;
taknokolat 0:eae101ae93c0 98 rpy[YAW] -= FirstYAW;
taknokolat 0:eae101ae93c0 99
taknokolat 0:eae101ae93c0 100 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 101 if(!flg_checkoutlier || count_changeRPY >= 2){
taknokolat 0:eae101ae93c0 102 for(uint8_t i=0; i<3; i++){
taknokolat 0:eae101ae93c0 103 nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
taknokolat 0:eae101ae93c0 104 }
taknokolat 0:eae101ae93c0 105 count_changeRPY = 0;
taknokolat 0:eae101ae93c0 106 }else count_changeRPY++;
taknokolat 0:eae101ae93c0 107 flg_checkoutlier = false;
taknokolat 0:eae101ae93c0 108
taknokolat 0:eae101ae93c0 109 }
taknokolat 0:eae101ae93c0 110
taknokolat 0:eae101ae93c0 111 void Init_sensors(){
taknokolat 0:eae101ae93c0 112 if(mpu6050.setup() == -1){
taknokolat 0:eae101ae93c0 113 pc.printf("failed initialize\r\n");
taknokolat 0:eae101ae93c0 114 }
taknokolat 0:eae101ae93c0 115 }
taknokolat 0:eae101ae93c0 116
taknokolat 0:eae101ae93c0 117 void DisplayClock(){
taknokolat 0:eae101ae93c0 118 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
taknokolat 0:eae101ae93c0 119 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
taknokolat 0:eae101ae93c0 120 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
taknokolat 0:eae101ae93c0 121 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
taknokolat 0:eae101ae93c0 122 pc.printf("\r\n");
taknokolat 0:eae101ae93c0 123 }