航空研究会
a
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of
Autoflight2018_4
by 
航空研究会
Diff: main.cpp
- Revision:
- 3:954228fd64b3
- Parent:
- 2:23daa5fa28b4
- Child:
- 4:b66ab0bee119
--- a/main.cpp Sat Sep 08 05:32:55 2018 +0000
+++ b/main.cpp Wed Sep 12 05:13:58 2018 +0000
@@ -26,11 +26,10 @@
#define KP_RUD 3.0
#define KI_RUD 0.0
#define KD_RUD 0.0
-#define KP_AIL 0.1
+#define KP_AIL 2.0
#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
@@ -43,7 +42,18 @@
#define TAKEOFF_THR 0.8
#define LOOP_THR 0.6
-#define g_rightloopRUD 1500
+#define rightloopRUD 1250
+#define AIL_L_correctionrightloop 0
+#define rightloopshortRUD 1250
+#define AIL_L_correctionrightloopshort 0
+#define leftloopRUD 1500
+#define AIL_L_correctionleftloop -0
+#define leftloopshortRUD 1500
+#define AIL_L_correctionleftloopshort 0
+#define glideloopRUD 1300
+
+#define AIL_L_RatioRising 0.5
+#define AIL_L_RatioDescent 2
#define RIGHT_ROLL_SHORT -12.0
#define RIGHT_PITCH_SHORT -5.0
@@ -86,7 +96,7 @@
const float trim[4] = {Trim_Falfalla[0],Trim_Falfalla[1],Trim_Falfalla[2],Trim_Falfalla[3]};
const float expMax[4] = {ExpMax_Falfalla[0],ExpMax_Falfalla[1],ExpMax_Falfalla[2],ExpMax_Falfalla[3]};
const float expMin[4] = {ExpMin_Falfalla[0],ExpMin_Falfalla[1],ExpMin_Falfalla[2],ExpMin_Falfalla[3]};
-const int16_t reverce[4] = {ExpMin_Falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
+const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
SBUS sbus(PA_9, PA_10); //SBUS
@@ -126,12 +136,11 @@
OperationMode operation_mode = StartUp;
BombingMode bombing_mode = Takeoff;
static int16_t autopwm[8] = {1500,1500,1180,1500,1392,1500};
-static int16_t trimpwm[6] = {1500,1500,1180,1500,1392,1500};
+static int16_t trimpwm[6] = {1428,1500,1180,1500,1392,1629};
int16_t maxpwm[6] = {1820,1820,1820,1820,1820,1820};
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};
@@ -722,6 +731,9 @@
//controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);
controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); //エルロンでロール制御
controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt);
+
+ //pc.printf("%f,%f",controlValue[ROLL],controlValue[PITCH]);
+
}
void UpdateAutoPWM(float controlValue[3]){
@@ -730,7 +742,7 @@
addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ：右回転正（8月13日時点；左回転が正！） レバー:右回転正
autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH]; //rewrite
- autopwm[AIL_R] = trimpwm[AIL_R] + reverce[AIL_R] * addpwm[AIL_R];
+ autopwm[AIL_R] = trimpwm[AIL_R] - reverce[AIL_R] * addpwm[ROLL];
//autopwm[THR] = oldTHR;
autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
@@ -776,6 +788,7 @@
for(uint8_t i=0;i<6;i++){
pwm[i] = sbus.manualpwm[i];
}
+ ///pc.printf("%d ,\r\n",pwm[AIL_R]);//L
/*pc.printf("%d ,",pwm[0]);//R
pc.printf("%d ,\r\n",pwm[5]);//L*/
oldTHR = sbus.manualpwm[THR];
@@ -789,8 +802,9 @@
pwm[RUD] = autopwm[RUD];
pwm[DROP] = autopwm[DROP];
pwm[AIL_L] = autopwm[AIL_L];
+ //pc.printf("%d ,",pwm[ELE]);//ELE
pc.printf("%d ,",pwm[AIL_R]);//R
- pc.printf("%d ,\r\n",pwm[AIL_L]);//L
+ pc.printf("%d ,\r\n",pwm[AIL_R]);//L
//pc.printf("update_auto\r\n");
break;
@@ -923,6 +937,8 @@
static bool flg_setInStartAuto = false;
static float FirstYAW_Moebius = 0.0;
float newYaw_Moebius;
+
+
if(!flg_setInStartAuto && CheckSW_Up(Ch7)){
FirstYAW_Moebius = nowAngle[YAW];
@@ -933,12 +949,14 @@
led2 = 0;
}
+ autopwm[THR]=oldTHR;
+
newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius);
if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0) {RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n");}
if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0) {RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n");}
- if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius < 10.0f) {RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n");}
- if(RotateCounter == 3 && newYaw_Moebius >20.0 && newYaw_Moebius < 180.0f) {RotateCounter++; led2 = 0; pc.printf("Change Rotate direction\r\n");}
+ if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius < -10.0) {RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n");}
+ if(RotateCounter == 3 && newYaw_Moebius >0.0 && newYaw_Moebius < 90.0) {RotateCounter++; led2 = 0; pc.printf("Change Rotate direction\r\n");}
if(RotateCounter <= 3) UpdateTargetAngle_Rightloop_short(targetAngle);
@@ -956,12 +974,13 @@
targetAngle[ROLL] = g_glideloopROLL;
targetAngle[PITCH] = g_glideloopPITCH;
+ autopwm[RUD]=glideloopRUD;
- autopwm[THR]=oldTHR;
+ //autopwm[THR]=oldTHR;
//シリアル通信受信の割り込みイベント登録
- pc.attach(ISR_Serial_Rx, Serial::RxIrq);
+ //pc.attach(ISR_Serial_Rx, Serial::RxIrq);
-/*
+
//時間計測開始設定
if(!flg_tstart && CheckSW_Up(Ch7)){
t_start = t.read();
@@ -979,7 +998,7 @@
//一定高度or15秒でled点灯
if((groundcount>5 && g_distance>0) || t_diff > 15){
led2 = 1;
- //pc.printf("Call [Stop!] calling!\r\n");
+ pc.printf("Call [Stop!] calling!\r\n");
}
if(g_distance<180 && g_distance > 0) groundcount++;
}else{
@@ -995,13 +1014,13 @@
THRcount++;
if(THRcount>5){
autopwm[THR] = SetTHRinRatio(0.6);
- //pc.printf("throttle ON\r\n");
+ pc.printf("throttle ON\r\n");
}
}else{
autopwm[THR] = 1180;
THRcount = 0;
}
- }*/
+ }
}
//離陸-投下-着陸一連
void Take_off_and_landing(float targetAngle[3]){
@@ -1186,12 +1205,15 @@
//右旋回
void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回
- autopwm[RUD]=g_rightloopRUD;
targetAngle[ROLL] = g_rightloopROLL;
targetAngle[PITCH] = g_rightloopPITCH ;
+ autopwm[RUD]=rightloopRUD;
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("",)
+ if(autopwm[AIL_R]>trimpwm[AIL_R]){
+ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+ }
+ else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
+
//checkHeight(targetAngle);
//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
}
@@ -1200,8 +1222,12 @@
targetAngle[ROLL] = g_rightloopROLLshort;
targetAngle[PITCH] = g_rightloopPITCHshort;
- autopwm[THR] = SetTHRinRatio(g_loopTHR);
-
+ autopwm[RUD]=rightloopshortRUD;
+ autopwm[THR] = oldTHR;
+ if(autopwm[AIL_R]>trimpwm[AIL_R]){
+ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+ }
+ else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
}
@@ -1211,7 +1237,13 @@
targetAngle[ROLL] = g_leftloopROLL;
targetAngle[PITCH] = g_leftloopPITCH;
- autopwm[THR] = SetTHRinRatio(g_loopTHR);
+ autopwm[RUD]=leftloopRUD;
+ autopwm[THR] = oldTHR;
+ if(autopwm[AIL_R]>trimpwm[AIL_R]){
+ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+ }
+ else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
+ //autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_Ratio_leftloop;
//checkHeight(targetAngle);
//printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
@@ -1221,8 +1253,13 @@
targetAngle[ROLL] = g_leftloopROLLshort;
targetAngle[PITCH] = g_leftloopPITCHshort;
- autopwm[THR] = SetTHRinRatio(g_loopTHR);
-
+ autopwm[RUD]=leftloopRUD;
+ autopwm[THR] = oldTHR;
+ if(autopwm[AIL_R]>trimpwm[AIL_R]){
+ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+ }
+ else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
+
//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
}
void ISR_Serial_Rx()
@@ -1253,8 +1290,8 @@
//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);
-
//pc.printf("Mode: %c: ",g_buf[0]);
//if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW);
pc.printf("\r\n");
+ //pc.printf("%d",usensor.get_dist_cm()
}
\ No newline at end of file