航空研究会
/
Autoflight2018_39
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of
Autoflight2018_38
by 
航空研究会
Diff: main.cpp
- Revision:
- 32:9d0646372abe
- Parent:
- 31:dba3216c2755
- Child:
- 33:bcc14ca08e84
--- a/main.cpp Sat Sep 22 09:46:42 2018 +0000
+++ b/main.cpp Sun Sep 23 03:20:58 2018 +0000
@@ -151,7 +151,7 @@
OperationMode operation_mode = StartUp;
BombingMode bombing_mode = Takeoff;
-static int16_t autopwm[8] = {1500,1500,1180,1500,1454,1500};
+static int16_t autopwm[8] = {1500,1500,1176,1500,1454,1500};
//1号機
static int16_t trimpwm[6] = {1580,1600,1176,1404,1440,1448};
@@ -355,7 +355,7 @@
led4 = !led4;
}
- pc.attach(getSF_Serial, Serial::RxIrq);
+ //pc.attach(getSF_Serial, Serial::RxIrq);
NVIC_SetPriority(USART2_IRQn,4);
FirstROLL = nowAngle[ROLL];
@@ -562,6 +562,7 @@
if(count_op > changeModeCount){
operation_mode = BombwithPC;
pc.printf("Goto Bombing mode\r\n");
+ pc.attach(getSF_Serial, Serial::RxIrq);
count_op = 0;
}
}else count_op = 0;
@@ -574,12 +575,12 @@
if(count_op > changeModeCount){
operation_mode = RightLoop;
pc.printf("Goto RightLoop mode\r\n");
+ pc.attach(NULL, Serial::RxIrq);
count_op = 0;
}
}else count_op = 0;
Take_off_and_landing(targetAngle);
break;
-
default:
operation_mode = StartUp;
break;
@@ -930,6 +931,7 @@
//udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
//各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
+/*
void Update_PWM()
{
NVIC_DisableIRQ(USART1_IRQn);
@@ -977,6 +979,88 @@
sbus.flg_ch_update = false;
Output_PWM(pwm);
}
+*/
+
+/*---SBUS割り込み処理---*/
+
+//udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
+//各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
+void Update_PWM()
+{
+ NVIC_DisableIRQ(USART1_IRQn);
+ static int16_t pwm[6];
+ static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]};
+ static int16_t FailsafeCounter=0;
+
+ if(sbus.flg_ch_update == true){
+ switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替
+ case Manual:
+ for(uint8_t i=0;i<6;i++){
+ pwm[i] = sbus.manualpwm[i];
+ }
+ oldTHR = sbus.manualpwm[THR];
+ //pc.printf("update_manual\r\n");
+ NVIC_EnableIRQ(USART1_IRQn);
+ break;
+
+ case Auto:
+ pwm[AIL_R] = autopwm[AIL_R]; //sbus.manualpwm[AIL];
+ pwm[ELE] = autopwm[ELE];
+ pwm[THR] = autopwm[THR];
+ pwm[RUD] = autopwm[RUD];
+ pwm[DROP] = autopwm[DROP];
+ pwm[AIL_L] = autopwm[AIL_L];
+
+ NVIC_EnableIRQ(USART1_IRQn);
+ break;
+
+ default:
+ for(uint8_t i=0;i<6;i++){
+ pwm[i] = sbus.manualpwm[i];
+ } //pc.printf("update_manual\r\n");
+ NVIC_EnableIRQ(USART1_IRQn);
+ break;
+ }
+
+ for(uint8_t i=0;i<6;i++){
+ if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
+ temppwm[i]=pwm[i];
+ }
+
+ }
+ //else(sbus.flg_ch_update == false) pc.printf("0\r\n");
+ /* if(sbus.failsafe_status==SBUS_SIGNAL_OK){
+ pc.printf("OK\r\n");
+ }
+ */
+
+ if(sbus.failsafe_status==SBUS_SIGNAL_LOST||sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++;
+ else FailsafeCounter=0;
+
+ if(FailsafeCounter>10){
+ for(uint8_t i=0;i<6;i++) pwm[i] = trimpwm[i];
+
+ while(sbus.failsafe_status!=SBUS_SIGNAL_OK){
+ servo1.pulsewidth_us(pwm[0]);
+ servo2.pulsewidth_us(pwm[1]);
+ servo3.pulsewidth_us(pwm[2]);
+ servo4.pulsewidth_us(pwm[3]);
+ servo5.pulsewidth_us(pwm[4]);
+ servo6.pulsewidth_us(pwm[5]);
+ led1 = !led1;
+ led2 = !led2;
+ led3 = !led3;
+ led4 = !led4;
+ wait_ms(35);
+ FailsafeCounter=0;
+ pc.printf("%d\r\n",sbus.failsafe_status);
+ //if(sbus.failsafe_status!=SBUS_SIGNAL_FAILSAFE)break;
+ }
+ }
+
+ sbus.flg_ch_update = false;
+ Output_PWM(pwm);
+}
//pwmをサーボに出力。
@@ -1215,7 +1299,7 @@
//離陸-投下-着陸一連
void Take_off_and_landing(float targetAngle[3]){
- /*
+
if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
switch(bombing_mode){
@@ -1232,18 +1316,22 @@
UpdateTargetAngle_Takeoff(targetAngle);
+ NVIC_DisableIRQ(EXTI9_5_IRQn);
if(g_distance>150) TakeoffCount++;
else TakeoffCount = 0;
+ NVIC_EnableIRQ(EXTI9_5_IRQn);
if(TakeoffCount>5){
autopwm[THR] = 1180+320*2*0.5;
+ autopwm[ELE] = 1200;
pc.printf("Now go to Approach mode!!");
bombing_mode = Approach;
}
+
break;
//case Chicken:
//break;
-
+ /*
case Transition:
static int ApproachCount = 0;
targetAngle[YAW]=180.0;
@@ -1252,26 +1340,48 @@
if(Judge==0) ApproachCount++;
if(ApproachCount>5) bombing_mode = Approach;
break;
-
+ */
case Approach:
- */
+
autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合
UpdateTargetAngle_Approach(targetAngle);
- /*
+
break;
default:
bombing_mode = Takeoff;
break;
+
}
- */
+
}
//離陸モード
void UpdateTargetAngle_Takeoff(float targetAngle[3]){
//pc.printf("%d \r\n",g_distance);
+ static int tELE_start = 0;
+ static bool flg_ELEup = false;
+ int t_def = 0;
+ if(!flg_ELEup && CheckSW_Up(Ch7)){
+ tELE_start = t.read_ms();
+ flg_ELEup = true;
+ pc.printf("timer start\r\n");
+ }else if(!CheckSW_Up(Ch7)){
+ tELE_start = 0;
+ flg_ELEup = false;
+ }
+ if(flg_ELEup){
+ t_def = t.read_ms() - tELE_start;
+
+ //1.5秒経過すればELE上げ舵へ
+ if(t_def>500) targetAngle[PITCH]=-25.0;
+ else{
+ t_def = 0;
+ targetAngle[PITCH]=g_gostraightPITCH;
+ }
+ }
targetAngle[ROLL] = g_gostraightROLL;
- targetAngle[PITCH] = g_loopTHR;
+ //targetAngle[PITCH] = g_loopTHR;
autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合
}
@@ -1604,10 +1714,10 @@
//for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ 小数点以下2ケタ
//pc.printf("%d\t",autopwm[AIL_L]); // pc.printf("%d\t",autopwm[RUD]);
//pc.printf("%d",g_distance);
- NVIC_DisableIRQ(EXTI9_5_IRQn);
- pc.printf("g_distance = %d",g_distance);
- NVIC_EnableIRQ(EXTI9_5_IRQn);
+ //NVIC_DisableIRQ(EXTI9_5_IRQn);
+ //pc.printf("g_distance = %d",g_distance);
+ //NVIC_EnableIRQ(EXTI9_5_IRQn);
//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");
}
\ No newline at end of file