a

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_4 by 航空研究会

Files at this revision

API Documentation at this revision

Comitter:
taknokolat
Date:
Thu Sep 13 06:16:12 2018 +0000
Parent:
3:954228fd64b3
Commit message:
a

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 954228fd64b3 -r b66ab0bee119 main.cpp
--- a/main.cpp	Wed Sep 12 05:13:58 2018 +0000
+++ b/main.cpp	Thu Sep 13 06:16:12 2018 +0000
@@ -110,7 +110,7 @@
 //PwmOut servo8(PB_9); // TIM4_CH4   //new trigger
 
 RawSerial pc(PA_2,PA_3, 115200);    //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX
-//RawSerial pc2(PB_6,PB_7, 115200);   //sbus確認用
+//RawSerial pc2(PB_6,PB_7, 115200);   //確認用
 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
 
 DigitalOut led1(PA_0);  //黄色のコネクタ
@@ -141,6 +141,8 @@
 int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180};
 int16_t oldTHR = 1000;
 
+int zeroTHR=1;//着陸時にスロットルが0かどうかの判断に使用
+
 
 
 static float nowAngle[3] = {0,0,0};
@@ -166,7 +168,7 @@
 
 void Init_PWM();
 void Init_servo();              //サーボ初期化
-void Init_sbus();               //SBUS初期化
+void Init_();               //初期化
 void Init_sensors();
 void DisplayClock();            //クロック状態確認
 
@@ -186,7 +188,7 @@
 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min);
 inline int16_t SetTHRinRatio(float ratio);
 
-//sbus割り込み
+//割り込み
 void Update_PWM();              //マニュアル・自動モードのpwmデータを整形しpwm変数に入力
 void Output_PWM(int16_t pwm[6]);    //pwmをサーボへ出力
 
@@ -213,10 +215,13 @@
 
 //自動操縦
 void UpdateTargetAngle_GoStraight(float targetAngle[3]);
+void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]);       //着陸時にスロットルが0の時の直進
 void UpdateTargetAngle_Rightloop(float targetAngle[3]);
 void UpdateTargetAngle_Rightloop_short(float targetAngle[3]);
+void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]);      //着陸時にスロットルが0の時の右旋回
 void UpdateTargetAngle_Leftloop(float targetAngle[3]);
 void UpdateTargetAngle_Leftloop_short(float targetAngle[3]);
+void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]);     //着陸時にスロットルが0の時の左旋回
 void UpdateTargetAngle_Moebius(float targetAngle[3]);
 void UpdateTargetAngle_Glide(float targetAngle[3]);
 void UpdateTargetAngle_Takeoff(float targetAngle[3]);
@@ -278,7 +283,7 @@
         
     Init_PWM();
     Init_servo();
-    Init_sbus();    
+    Init_();    
     Init_sensors();
     //switch2.rise(ResetTrim);
     pc.attach(getSF_Serial, Serial::RxIrq);
@@ -359,8 +364,8 @@
     pc.printf("servo initialized\r\n");
 }
 
-//Sbus初期化
-void Init_sbus(){
+//初期化
+void Init_(){
     sbus.initialize();
     sbus.setLastfuncPoint(Update_PWM);
     sbus.startInterrupt();
@@ -803,8 +808,8 @@
                 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_R]);//L
+                //pc.printf("%d ,",pwm[AIL_R]);//R
+                //pc.printf("%d ,\r\n",pwm[AIL_R]);//L
                 //pc.printf("update_auto\r\n");
                 
                 break;
@@ -901,6 +906,8 @@
         static int bufcounter=0;
         
         SFbuf[bufcounter]=pc.getc();
+        
+        pc.printf("%s",SFbuf[bufcounter]);
         if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++;
             
         if(bufcounter==5 && SFbuf[4]=='F'){
@@ -1024,7 +1031,7 @@
 }
 //離陸-投下-着陸一連
 void Take_off_and_landing(float targetAngle[3]){
-    if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
+    /*if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
     
     switch(bombing_mode){
         case Takeoff:
@@ -1048,10 +1055,10 @@
                 bombing_mode = Approach;
             }
             break;
-        /*    
-        case Chicken:    
-            break;
-        */    
+            
+       // case Chicken:    
+         //   break;
+           
         case Transition:
             static int ApproachCount = 0;
             targetAngle[YAW]=180.0;
@@ -1061,14 +1068,16 @@
             if(ApproachCount>5) bombing_mode = Approach;
             break;
             
-        case Approach:
+            
+        case Approach*/
+        autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合
             UpdateTargetAngle_Approach(targetAngle);
-            break;
+        /*    break;
             
         default:
             bombing_mode = Takeoff;
             break;    
-    }
+    }*/
 }
 
 //離陸モード
@@ -1083,24 +1092,24 @@
 int Rotate(float targetAngle[3], float TargetYAW){
     float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);
 
-    if(diffYaw > LIMIT_STRAIGHT_YAW){
-        /*
-        if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle);
-        else UpdateTargetAngle_Rightloop(targetAngle);
-        */
+   /* if(diffYaw > LIMIT_STRAIGHT_YAW){
+        
+        //if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle);
+        //else UpdateTargetAngle_Rightloop(targetAngle);
+        
         UpdateTargetAngle_Rightloop_short(targetAngle);
         return 1;
     }else if(diffYaw < -LIMIT_STRAIGHT_YAW){
         UpdateTargetAngle_Leftloop_short(targetAngle);
-        /*
-        if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle);
-        else UpdateTargetAngle_Leftloop(targetAngle);
-        */
+        
+       // if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle);
+        //else UpdateTargetAngle_Leftloop(targetAngle);
+        
         return 1;
     }else{
         UpdateTargetAngle_GoStraight(targetAngle);
         return 0;
-    }
+    }*/
 }
 
 //チキラー投下
@@ -1128,18 +1137,36 @@
 
 //着陸モード(PCからの指令に従う)
 void UpdateTargetAngle_Approach(float targetAngle[3]){
-    pc.putc(g_buf[0]);
-    switch(g_buf[0]){
+    //pc.putc(g_buf[0]);]
+    static char rotatemode = 'G';
+    if(output_status == Manual) rotatemode = 'G';
+    
+    switch(g_landingcommand){
         case 'R':   //右旋回セヨ
+        if(zeroTHR==0){
+            UpdateTargetAngle_Rightloop_zero(targetAngle);
+            }
+            else{
             UpdateTargetAngle_Rightloop(targetAngle);
+            }
             break;
             
         case 'L':   //左旋回セヨ
+        if(zeroTHR==0){
+            UpdateTargetAngle_Leftloop_zero(targetAngle);
+            }
+            else{
             UpdateTargetAngle_Leftloop(targetAngle);
+            }
             break;
             
         case 'G':   //直進セヨ
+        if(zeroTHR==0){
+            UpdateTargetAngle_GoStraight_zero(targetAngle);
+            }
+            else{
             UpdateTargetAngle_GoStraight(targetAngle);
+            }
             break;
         
         case 'Y':   //指定ノヨー方向ニ移動セヨ
@@ -1158,6 +1185,7 @@
             targetAngle[ROLL] = 0.0;
             targetAngle[PITCH] = -3.0;
             autopwm[THR] = minpwm[THR];
+            zeroTHR=0;
             break;
     }
 }
@@ -1202,6 +1230,33 @@
     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
 }
 
+//直進(着陸時throttle0の時)
+void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]){
+
+    targetAngle[ROLL] = g_gostraightROLL;
+    targetAngle[PITCH] = g_gostraightPITCH;
+    autopwm[THR] = minpwm[THR];
+    
+    //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
+}
+
+//右旋回(着陸時スロットル0の時)
+void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]){ //右旋回
+
+    targetAngle[ROLL] = g_rightloopROLL;
+    targetAngle[PITCH] = g_rightloopPITCH ;
+    autopwm[RUD]=rightloopRUD;
+    autopwm[THR] = minpwm[THR];//SetTHRinRatio(g_loopTHR);
+    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]);
+}
+
+
 //右旋回
 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回
 
@@ -1249,6 +1304,23 @@
     //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
 }
 
+//左旋回(着陸時スロットル0のとき)
+void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]){
+    
+    targetAngle[ROLL] = g_leftloopROLL;
+    targetAngle[PITCH] = g_leftloopPITCH;
+    autopwm[RUD]=leftloopRUD;
+    autopwm[THR] = minpwm[THR];
+    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]);
+}
+
 void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){
     
     targetAngle[ROLL] = g_leftloopROLLshort;
@@ -1292,6 +1364,6 @@
     //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("\r\n");
     //pc.printf("%d",usensor.get_dist_cm()
 }
\ No newline at end of file