Diff: main.cpp

Revision:

1:f383708a5a52

Parent:

0:17f575135219

Child:

2:23daa5fa28b4

--- a/main.cpp	Fri Sep 07 04:11:48 2018 +0000
+++ b/main.cpp	Fri Sep 07 09:47:37 2018 +0000
@@ -26,6 +26,12 @@
 #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 GAIN_CONTROLVALUE_TO_PWM 3.0
 
@@ -93,6 +99,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確認用
 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
 
 DigitalOut led1(PA_0);  //黄色のコネクタ
@@ -104,6 +111,7 @@
 MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
 HCSR04 usensor(PB_9,PB_8); //trig,echo  9,8 
 
+PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL);
 PID pid_ELE(g_kpELE,g_kiELE,g_kdELE);
 PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD);
 
@@ -174,6 +182,7 @@
 void SendArray();
 void getSF_Serial();
 float ConvertByteintoFloat(char high, char low);
+void ISR_Serial_Rx();
 
 //SD設定
 int GetParameter(FILE *fp, const char *paramName,char parameter[]);
@@ -705,7 +714,9 @@
     dt = (float)((t_now - t_last)/1000000.0f) ;
     t_last = t_now;
 
-    controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);    
+
+    //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);
 }
 
@@ -714,12 +725,12 @@
     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[RUD] = trimpwm[RUD] + reverce[RUD] * addpwm[ROLL];
+    //autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH];
+    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[RUD] = ThresholdMaxMin(autopwm[RUD], maxpwm[RUD], minpwm[RUD]);
+    autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]);
 }
 
 inline float CalcRatio(float value, float trim, float limit){
@@ -756,9 +767,10 @@
         switch(output_status){    //マニュアルモード,自動モード,自動着陸もモードを切替
             case Manual: 
                 for(uint8_t i=0;i<6;i++){
-                    pwm[i] = sbus.manualpwm[i];
-                    pc.printf("%d ,",pwm[i]);
+                    pwm[i] = sbus.manualpwm[i];  
                 }
+                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;
@@ -930,49 +942,57 @@
     static int t_start = 0;
     static bool flg_tstart = false;
     int t_diff = 0;
+    static int groundcount = 0;
     
     targetAngle[ROLL] = g_glideloopROLL;
     targetAngle[PITCH] = g_glideloopPITCH;
     
+    autopwm[THR]=oldTHR;
+    //シリアル通信受信の割り込みイベント登録
+    pc.attach(ISR_Serial_Rx, Serial::RxIrq);
+    
+/*    
     //時間計測開始設定
     if(!flg_tstart && CheckSW_Up(Ch7)){
         t_start = t.read();
         flg_tstart = true;
         pc.printf("timer start\r\n");
-    }else if(flg_tstart && !CheckSW_Up(Ch7)){
+    }else if(!CheckSW_Up(Ch7)){
         t_start = 0;
         flg_tstart = false;
     }
 
+
     //フラグが偽であれば計測は行わない    
     if(flg_tstart){
         t_diff = t.read() - t_start;
         //一定高度or15秒でled点灯
-        if((g_distance<200 && g_distance>0) || t_diff > 15){
+        if((groundcount>5 && g_distance>0) || t_diff > 15){
             led2 = 1;
             //pc.printf("Call [Stop!] calling!\r\n");
         }
+        if(g_distance<180 && g_distance > 0) groundcount++;
     }else{
         t_diff = 0;
+        groundcount = 0;
         led2 = 0;
     }
     
-    if(t_diff > 18){
+    if(t_diff > 17){
         autopwm[THR] = SetTHRinRatio(0.5);
     }else{
         if(g_distance<150 && g_distance>0 ){
             THRcount++;
             if(THRcount>5){
-                autopwm[THR] = SetTHRinRatio(0.5);
+                autopwm[THR] = SetTHRinRatio(0.6);
                 //pc.printf("throttle ON\r\n");
             }
         }else{
             autopwm[THR] = 1180;
             THRcount = 0;
         }
-    }
+    }*/
 }
-
 //離陸-投下-着陸一連
 void Take_off_and_landing(float targetAngle[3]){
     if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
@@ -1192,6 +1212,17 @@
     
     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
 }
+void ISR_Serial_Rx()
+{
+    // シリアルの受信処理
+    char data = pc.getc();
+    
+    if(data=='C'){
+    autopwm[THR]=minpwm[2];
+    wait(60.0);
+    }
+    
+}
 
 //デバッグ用
 void DebugPrint(){