航空研究会
栗原、松本Version暫定統合
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of
Autoflight2018_23
by 
航空研究会
Diff: main.cpp
- Revision:
- 10:652071c20bf6
- Parent:
- 9:f6367b7fd7be
- Child:
- 11:c0b9ad25d3db
--- a/main.cpp Sat Sep 15 06:14:03 2018 +0000
+++ b/main.cpp Sat Sep 15 15:34:20 2018 +0000
@@ -135,7 +135,7 @@
//InterruptIn switch2(PC_14);
MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
-HCSR04 usensor(PB_9,PB_8); //trig,echo 9,8
+//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);
@@ -167,9 +167,9 @@
float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;
unsigned int g_distance;
-Ticker USsensor;
+//Ticker USsensor;
static char g_buf[16];
-char g_landingcommand;
+char g_landingcommand='Z';
float g_SerialTargetYAW;
Timer t;
@@ -265,6 +265,8 @@
loop();
+
+
if(!CheckSW_Up(Ch7)){
led3=0;
}else{
@@ -298,7 +300,7 @@
Init_sensors();
//switch2.rise(ResetTrim);
pc.attach(getSF_Serial, Serial::RxIrq);
- USsensor.attach(&UpdateDist, 0.05);
+ //USsensor.attach(&UpdateDist, 0.05);
NVIC_SetPriority(USART1_IRQn,0);
NVIC_SetPriority(EXTI0_IRQn,1);
@@ -341,11 +343,16 @@
static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0};
SensingMPU();
+ NVIC_DisableIRQ(USART2_IRQn);
UpdateTargetAngle(targetAngle);
//Rotate(targetAngle, 30.0);
CalculateControlValue(targetAngle, controlValue);
+ NVIC_DisableIRQ(USART1_IRQn);
UpdateAutoPWM(controlValue);
- wait_ms(25);
+ NVIC_EnableIRQ(USART1_IRQn);
+ NVIC_EnableIRQ(USART2_IRQn);
+ wait_ms(20);
+ pc.printf("%c",g_landingcommand);
#if DEBUG_PRINT_INLOOP
DebugPrint();
#endif
@@ -796,6 +803,7 @@
static int16_t pwm[6];
static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]};
if(sbus.flg_ch_update == true){
+ NVIC_DisableIRQ(USART1_IRQn);
switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替
case Manual:
for(uint8_t i=0;i<6;i++){
@@ -814,8 +822,8 @@
pwm[RUD] = autopwm[RUD];
pwm[DROP] = autopwm[DROP];
pwm[AIL_L] = autopwm[AIL_L];
- pc.printf("%d ,",pwm[AIL_R]);//R
- pc.printf("%d ,\r\n",pwm[AIL_L]);//L
+ //pc.printf("%d ,",pwm[AIL_R]);//R
+ //pc.printf("%d ,\r\n",pwm[AIL_L]);//L
//pc.printf("update_auto\r\n");
break;
@@ -825,6 +833,7 @@
pwm[i] = sbus.manualpwm[i];
} //pc.printf("update_manual\r\n");
break;
+ NVIC_EnableIRQ(USART1_IRQn);
}
for(uint8_t i=0;i<6;i++){
if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
@@ -841,12 +850,14 @@
//pwmをサーボに出力。
void Output_PWM(int16_t pwm[5])
{
+ NVIC_DisableIRQ(USART1_IRQn);
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]);
+ NVIC_EnableIRQ(USART1_IRQn);
}
@@ -868,8 +879,8 @@
NVIC_DisableIRQ(USART1_IRQn);
NVIC_DisableIRQ(USART2_IRQn);
NVIC_DisableIRQ(TIM5_IRQn);
- NVIC_DisableIRQ(EXTI0_IRQn);
- NVIC_DisableIRQ(EXTI9_5_IRQn);
+ NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止
+ NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止
mpu6050.getRollPitchYaw_Skipper(rpy);
@@ -911,6 +922,8 @@
static char SFbuf[16];
static int bufcounter=0;
+ pc.printf("%d",bufcounter);
+
SFbuf[bufcounter]=pc.getc();
if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++;
@@ -919,9 +932,9 @@
if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
bufcounter = 0;
memset(SFbuf, 0, strlen(SFbuf));
- pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
+ //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
}
- else if(bufcounter>=5 && g_buf[4]!='F'){
+ else if(bufcounter>=5 ){
pc.printf("Communication Falsed.\r\n");
bufcounter = 0;
}
@@ -937,10 +950,10 @@
//超音波割り込み
-void UpdateDist(){
+/*void UpdateDist(){
g_distance = usensor.get_dist_cm();
usensor.start();
-}
+}*/
//8の字旋回
void UpdateTargetAngle_Moebius(float targetAngle[3]){
@@ -984,8 +997,7 @@
targetAngle[PITCH] = g_glideloopPITCH;
// autopwm[THR]=oldTHR;
- //シリアル通信受信の割り込みイベント登録
- pc.attach(ISR_Serial_Rx, Serial::RxIrq);
+
//時間計測開始設定
@@ -1003,11 +1015,14 @@
if(flg_tstart){
t_diff = t.read() - t_start;
//一定高度or15秒でled点灯
+ NVIC_DisableIRQ(EXTI9_5_IRQn);
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++;
+ NVIC_EnableIRQ(EXTI9_5_IRQn);
}else{
t_diff = 0;
groundcount = 0;
@@ -1017,7 +1032,9 @@
if(t_diff > 17){
autopwm[THR] = SetTHRinRatio(0.5);
}else{
+ NVIC_DisableIRQ(EXTI9_5_IRQn);
if(g_distance<150 && g_distance>0 ){
+ NVIC_EnableIRQ(EXTI9_5_IRQn);
THRcount++;
if(THRcount>5){
autopwm[THR] = SetTHRinRatio(0.6);
@@ -1031,6 +1048,7 @@
}
//離陸-投下-着陸一連
void Take_off_and_landing(float targetAngle[3]){
+ /*
if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
switch(bombing_mode){
@@ -1055,10 +1073,10 @@
bombing_mode = Approach;
}
break;
- /*
- case Chicken:
- break;
- */
+
+ //case Chicken:
+ //break;
+
case Transition:
static int ApproachCount = 0;
targetAngle[YAW]=180.0;
@@ -1069,13 +1087,17 @@
break;
case Approach:
+ */
+ autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合
UpdateTargetAngle_Approach(targetAngle);
+ /*
break;
default:
bombing_mode = Takeoff;
break;
}
+ */
}
//離陸モード
@@ -1131,60 +1153,83 @@
void ReturnChickenServo2(){
autopwm[DROP] = 1392;
pc.printf("second reverse\r\n");
-}
-
-//着陸モード（PCからの指令に従う）
-void UpdateTargetAngle_Approach(float targetAngle[3]){
- pc.putc(g_buf[0]);
- switch(g_buf[0]){
- case 'R': //右旋回セヨ
- UpdateTargetAngle_Rightloop(targetAngle);
- break;
-
- case 'L': //左旋回セヨ
- UpdateTargetAngle_Leftloop(targetAngle);
- break;
-
- case 'G': //直進セヨ
- UpdateTargetAngle_GoStraight(targetAngle);
- break;
+ }
+
+ //着陸モード（PCからの指令に従う）
+ void UpdateTargetAngle_Approach(float targetAngle[3]){
+
+ NVIC_DisableIRQ(USART2_IRQn);
+
+ switch(g_landingcommand){
+ case 'R': //右旋回セヨ
+ targetAngle[ROLL] = g_rightloopROLL;
+ targetAngle[PITCH] = g_rightloopPITCH ;
+ autopwm[RUD]=rightloopRUD; //RUD固定
+ 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;
+ autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
- case 'Y': //指定ノヨー方向ニ移動セヨ
- Rotate(targetAngle, g_SerialTargetYAW);
- break;
+ break;
+
+ case 'L': //左旋回セヨ
+ targetAngle[ROLL] = g_leftloopROLL;
+ targetAngle[PITCH] = g_leftloopPITCH;
+ autopwm[RUD]=leftloopRUD;
+ 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;
- case 'B': //ブザーヲ鳴ラセ
- //buzzer = 1;
- break;
+ break;
+
+ case 'G': //直進セヨ
+ targetAngle[ROLL] = g_gostraightROLL;
+ targetAngle[PITCH] = g_gostraightPITCH;
+
+
+ break;
+
+ case 'Y': //指定ノヨー方向ニ移動セヨ
+ Rotate(targetAngle, g_SerialTargetYAW);
+ break;
+
+ case 'B': //ブザーヲ鳴ラセ
+ //buzzer = 1;
+ break;
+
+ case 'D': //物資ヲ落トセ
+ Chicken_Drop();
+ break;
+
+ case 'C': //停止セヨ
+ targetAngle[ROLL] = 0.0;
+ targetAngle[PITCH] = -3.0;
+ autopwm[THR] = minpwm[THR];
+ break;
- case 'D': //物資ヲ落トセ
- Chicken_Drop();
- break;
-
- case 'C': //停止セヨ
- targetAngle[ROLL] = 0.0;
- targetAngle[PITCH] = -3.0;
- autopwm[THR] = minpwm[THR];
- break;
+ }
+ NVIC_EnableIRQ(USART2_IRQn);
+}
+
+void checkHeight(float targetAngle[3]){
+
+ static int targetHeight = 200;
+ NVIC_DisableIRQ(EXTI9_5_IRQn);
+ if(g_distance < targetHeight + ALLOWHEIGHT){
+ UpdateTargetAngle_NoseUP(targetAngle);
+ if(CheckSW_Up(Ch7)) led2 = 1;
+ }
+ else if(g_distance > targetHeight - ALLOWHEIGHT){
+ UpdateTargetAngle_NoseDOWN(targetAngle);
+ if(CheckSW_Up(Ch7)) led2 = 1;
+ }
+ else led2=0;
+ NVIC_EnableIRQ(EXTI9_5_IRQn);
}
-}
-
-void checkHeight(float targetAngle[3]){
- static int targetHeight = 200;
-
- if(g_distance < targetHeight + ALLOWHEIGHT){
- UpdateTargetAngle_NoseUP(targetAngle);
- if(CheckSW_Up(Ch7)) led2 = 1;
- }
- else if(g_distance > targetHeight - ALLOWHEIGHT){
- UpdateTargetAngle_NoseDOWN(targetAngle);
- if(CheckSW_Up(Ch7)) led2 = 1;
- }
- else led2=0;
-}
-
-void UpdateTargetAngle_NoseUP(float targetAngle[3]){
+ void UpdateTargetAngle_NoseUP(float targetAngle[3]){
//targetAngle[PITCH] += 2.0f;
//if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6);
@@ -1303,17 +1348,7 @@
//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(){
@@ -1329,10 +1364,10 @@
//pc.printf("\r\n");
//for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
//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\t",autopwm[AIL_L]); // 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("\r\n");
}
\ No newline at end of file