![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
a
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_12 by
Diff: main.cpp
- Revision:
- 2:23daa5fa28b4
- Parent:
- 1:f383708a5a52
- Child:
- 3:206b17251f5b
--- a/main.cpp Fri Sep 07 09:47:37 2018 +0000 +++ b/main.cpp Sat Sep 08 05:32:55 2018 +0000 @@ -26,12 +26,11 @@ #define KP_RUD 3.0 #define KI_RUD 0.0 #define KD_RUD 0.0 -#define KP_AIL 3.0 -#define KI_AIL 0.0 -#define KD_AIL 0.0 +#define KP_AIL 0.1 +#define KI_AIL 0.2 +#define KD_AIL 0.2 - - +//#define g_AIL_L_Ratio_rightloop 0.5 #define GAIN_CONTROLVALUE_TO_PWM 3.0 @@ -44,6 +43,8 @@ #define TAKEOFF_THR 0.8 #define LOOP_THR 0.6 +#define g_rightloopRUD 1500 + #define RIGHT_ROLL_SHORT -12.0 #define RIGHT_PITCH_SHORT -5.0 #define LEFT_ROLL_SHORT 12.0 @@ -130,6 +131,9 @@ int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180}; int16_t oldTHR = 1000; +int16_t g_AIL_L_Ratio_rightloop = 0.5; + + static float nowAngle[3] = {0,0,0}; const float trimAngle[3] = {0.0, 0.0, 0.0}; const float maxAngle[2] = {90, 90}; @@ -316,7 +320,7 @@ //Rotate(targetAngle, 30.0); CalculateControlValue(targetAngle, controlValue); UpdateAutoPWM(controlValue); - wait_ms(23); + wait_ms(30); #if DEBUG_PRINT_INLOOP DebugPrint(); #endif @@ -366,7 +370,7 @@ } void Init_PWM(){ - for (uint8_t i = 0; i < 4; ++i){ + for (uint8_t i = 0; i < 6; ++i){ trimpwm[i] = 1500 + (int16_t)(lengthdivpwm * (trim[i]/100)); maxpwm[i] = 1500 + (int16_t)(lengthdivpwm * (expMax[i]/100)); minpwm[i] = 1500 - (int16_t)(lengthdivpwm * (expMin[i]/100)); @@ -725,12 +729,13 @@ addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ:機首下げ正 レバー:機首上げ正 addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ:右回転正(8月13日時点;左回転が正!) レバー:右回転正 - //autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH]; + autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH]; //rewrite autopwm[AIL_R] = trimpwm[AIL_R] + reverce[AIL_R] * addpwm[AIL_R]; //autopwm[THR] = oldTHR; autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]); autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]); + } inline float CalcRatio(float value, float trim, float limit){ @@ -755,6 +760,8 @@ return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio); } + + /*---SBUS割り込み処理---*/ //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード) @@ -769,8 +776,8 @@ for(uint8_t i=0;i<6;i++){ pwm[i] = sbus.manualpwm[i]; } - pc.printf("%d ,",pwm[0]);//R - pc.printf("%d ,\r\n",pwm[5]);//L + /*pc.printf("%d ,",pwm[0]);//R + pc.printf("%d ,\r\n",pwm[5]);//L*/ oldTHR = sbus.manualpwm[THR]; //pc.printf("update_manual\r\n"); break; @@ -782,7 +789,10 @@ pwm[RUD] = autopwm[RUD]; pwm[DROP] = autopwm[DROP]; pwm[AIL_L] = autopwm[AIL_L]; + pc.printf("%d ,",pwm[AIL_R]);//R + pc.printf("%d ,\r\n",pwm[AIL_L]);//L //pc.printf("update_auto\r\n"); + break; default: @@ -1176,9 +1186,12 @@ //右旋回 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回 + autopwm[RUD]=g_rightloopRUD; targetAngle[ROLL] = g_rightloopROLL; targetAngle[PITCH] = g_rightloopPITCH ; - autopwm[THR] = SetTHRinRatio(g_loopTHR); + autopwm[THR] = oldTHR;//SetTHRinRatio(g_loopTHR); + autopwm[AIL_L]=trimpwm[AIL_L]+(autopwm[AIL_R]-trimpwm[AIL_R])*0.5;//*inverse;//g_AIL_L_Ratio_rightloop; + pc.printf("",) //checkHeight(targetAngle); //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); } @@ -1201,7 +1214,7 @@ autopwm[THR] = SetTHRinRatio(g_loopTHR); //checkHeight(targetAngle); - //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); + //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]); } void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){ @@ -1236,7 +1249,7 @@ //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]); //for(uint8_t i=1; i<4; i++) pc.printf("%d ",sbus.manualpwm[i]); //pc.printf("\r\n"); - for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); + //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ 小数点以下2ケタ //pc.printf("%d\t",autopwm[ELE]); pc.printf("%d\t",autopwm[RUD]); //pc.printf("%d",g_distance);