sdカード完成
Dependencies: mbed MPU6050_2 HMC5883L_2 SDFileSystem3
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