![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
MODSERIAL
Dependencies: HCSR04_2 MODSERIAL MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_22 by
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(){