SDの設定が反映されるように修正
Dependencies: mbed MPU6050_2 HMC5883L_2 SDFileSystem3
Diff: main.cpp
- Revision:
- 4:67f705d42f1e
- Parent:
- 3:c18342e4fddd
- Child:
- 5:8bfe95431ec0
diff -r c18342e4fddd -r 67f705d42f1e main.cpp --- a/main.cpp Wed Feb 06 11:49:05 2019 +0000 +++ b/main.cpp Fri Feb 08 07:28:48 2019 +0000 @@ -4,8 +4,8 @@ //MPU_check用 #define PI 3.14159265358979 -#define servo_NEUTRAL_R 1460 -#define servo_NEUTRAL_L 1460 +#define servo_NEUTRAL_R 1616 +#define servo_NEUTRAL_L 1616 #define servo_FORWARD_R 1860 #define servo_FORWARD_L 1860 #define servo_back_R 1060 @@ -36,10 +36,13 @@ void setup(); void Init_sensors(); void DisplayClock(); +void DebugPrint(); static float nowAngle[3] = {0,0,0}; float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0; +bool setupFlag=false; + enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度 Timer t; @@ -80,9 +83,9 @@ MoveCansat(g_landingcommand); SensingMPU(); + DebugPrint(); + wait_ms(23); - //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用 - //pc.printf("\r\n"); } } @@ -270,6 +273,7 @@ wait(0.2); pc.printf("All initialized\r\n"); + setupFlag=true; } @@ -299,7 +303,16 @@ for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI; rpy[ROLL] -= FirstROLL; rpy[PITCH] -= FirstPITCH; - rpy[YAW] -= FirstYAW; + if(!setupFlag){ + rpy[YAW] -= FirstYAW; + }else{ + if(rpy[YAW] >= FirstYAW){ + rpy[YAW] -= FirstYAW; + }else{ + rpy[YAW] += 360.0f; + 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){ @@ -326,4 +339,11 @@ 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 +} + +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("\r\n"); + } + \ No newline at end of file