航空研究会
jevois完成版 sizeofに変更
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of
Autoflight2018_54
by 
航空研究会
Diff: main.cpp
- Revision:
- 28:aa44903a01e1
- Parent:
- 27:61876b34ded4
- Child:
- 29:887a38540508
--- a/main.cpp Fri Sep 21 13:11:19 2018 +0000
+++ b/main.cpp Fri Sep 21 20:03:32 2018 +0000
@@ -376,21 +376,18 @@
void loop(){
static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0};
SensingMPU();
- NVIC_DisableIRQ(USART2_IRQn);
UpdateTargetAngle(targetAngle);
CalculateControlValue(targetAngle, controlValue);
- NVIC_DisableIRQ(USART1_IRQn);
UpdateAutoPWM(controlValue);
- NVIC_EnableIRQ(USART1_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
+
- NVIC_SetPriority(TIM5_IRQn,4);
- NVIC_SetPriority(USART2_IRQn,2);
+ //NVIC_SetPriority(TIM5_IRQn,4);
+ //NVIC_SetPriority(USART2_IRQn,2);
wait_ms(23);
- NVIC_SetPriority(TIM5_IRQn,2);
- NVIC_SetPriority(USART2_IRQn,4);
+ //NVIC_SetPriority(TIM5_IRQn,2);
+ //NVIC_SetPriority(USART2_IRQn,4);
// pc.printf("6\r\n");
@@ -399,7 +396,7 @@
//NVIC_EnableIRQ(USART2_IRQn);
#if DEBUG_PRINT_INLOOP
//Sbusprintf();
- DebugPrint();
+ //DebugPrint();
#endif
}
@@ -419,7 +416,7 @@
servo4.pulsewidth_us(trimpwm[RUD]);
servo5.period_ms(14);
- servo5.pulsewidth_us(1392);
+ servo5.pulsewidth_us(trimpwm[DROP]);
servo6.period_ms(14);
servo6.pulsewidth_us(trimpwm[AIL_L]);
@@ -459,6 +456,8 @@
}
void UpdateTargetAngle(float targetAngle[3]){
+
+
static int16_t count_op = 0;
#if DEBUG_SEMIAUTO
switch(operation_mode){
@@ -595,6 +594,7 @@
led1 = 0;
}
+
}
int GetParameter(FILE *fp, const char *paramName,char parameter[]){
@@ -869,6 +869,7 @@
}
void CalculateControlValue(float targetAngle[3], float controlValue[3]){
+
static int t_last;
int t_now;
float dt;
@@ -881,9 +882,11 @@
//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);
+
}
-void UpdateAutoPWM(float controlValue[3]){
+void UpdateAutoPWM(float controlValue[3]){
+ NVIC_DisableIRQ(USART1_IRQn);
int16_t addpwm[2]; //-500~500
addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ：機首下げ正 レバー:機首上げ正
addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ：右回転正（8月13日時点；左回転が正！） レバー:右回転正
@@ -895,6 +898,7 @@
autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]);
+ NVIC_EnableIRQ(USART1_IRQn);
}
inline float CalcRatio(float value, float trim, float limit){
@@ -902,13 +906,13 @@
}
bool CheckSW_Up(Channel ch){
- NVIC_DisableIRQ(USART1_IRQn);
+
if(SWITCH_CHECK < sbus.manualpwm[ch]){
return true;
}else{
return false;
}
- NVIC_EnableIRQ(USART1_IRQn);
+
}
int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min){
@@ -928,11 +932,12 @@
//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]};
if(sbus.flg_ch_update == true){
- NVIC_DisableIRQ(USART1_IRQn);
+
switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替
case Manual:
for(uint8_t i=0;i<6;i++){
@@ -942,6 +947,7 @@
pc.printf("%d ,\r\n",pwm[5]);//L*/
oldTHR = sbus.manualpwm[THR];
//pc.printf("update_manual\r\n");
+ NVIC_EnableIRQ(USART1_IRQn);
break;
case Auto:
@@ -954,16 +960,18 @@
//pc.printf("%d ,",pwm[AIL_R]);//R
//pc.printf("%d ,\r\n",pwm[AIL_L]);//L
//pc.printf("update_auto\r\n");
-
+ 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;
- NVIC_EnableIRQ(USART1_IRQn);
+
}
+
for(uint8_t i=0;i<6;i++){
if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
temppwm[i]=pwm[i];
@@ -1048,7 +1056,7 @@
void getSF_Serial(){
//NVIC_DisableIRQ(USART1_IRQn);
- NVIC_DisableIRQ(EXTI0_IRQn);
+ //NVIC_DisableIRQ(EXTI0_IRQn);
//NVIC_DisableIRQ(TIM5_IRQn);
@@ -1056,10 +1064,15 @@
static int bufcounter=0;
+
+
if(pc.readable()) { // 受信確認
SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
- if(SFbuf[0]!='S') return;
+ if(SFbuf[0]!='S'){
+ //pc.printf("x");
+ return;
+ }
}
@@ -1074,18 +1087,20 @@
if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
bufcounter = 0;
memset(SFbuf, 0, strlen(SFbuf));
+ NVIC_ClearPendingIRQ(USART2_IRQn);
//pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
}
- else if(bufcounter>=5 ){
+ else if(bufcounter>=5){
//pc.printf("Communication Falsed.\r\n");
memset(SFbuf, 0, strlen(SFbuf));
bufcounter = 0;
+ NVIC_ClearPendingIRQ(USART2_IRQn);
}
//NVIC_EnableIRQ(TIM5_IRQn);
- NVIC_EnableIRQ(EXTI0_IRQn);
+ //NVIC_EnableIRQ(EXTI0_IRQn);
//NVIC_EnableIRQ(USART1_IRQn);
}
@@ -1171,7 +1186,9 @@
//pc.printf("Call [Stop!] calling!\r\n");
}
- if(g_distance<180 && g_distance > 0) groundcount++;
+ if(g_distance<180 && g_distance > 0) {
+ groundcount++;
+ }
NVIC_EnableIRQ(EXTI9_5_IRQn);
}else{
t_diff = 0;
@@ -1306,7 +1323,7 @@
}
//着陸モード（PCからの指令に従う）
- void UpdateTargetAngle_Approach(float targetAngle[3]){
+void UpdateTargetAngle_Approach(float targetAngle[3]){
static bool zeroTHR=true;//着陸時のスロットル動作確認用
@@ -1320,8 +1337,7 @@
}
- NVIC_DisableIRQ(USART2_IRQn);
-
+ NVIC_DisableIRQ(USART2_IRQn);
switch(g_landingcommand){
case 'R': //右旋回セヨ
if(zeroTHR==false){
@@ -1331,29 +1347,31 @@
targetAngle[ROLL] = g_rightloopROLL;
targetAngle[PITCH] = g_rightloopPITCH ;
autopwm[RUD]=g_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;
+ 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;
}
- 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;
}
- }
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'L': //左旋回セヨ
if(zeroTHR==false){
UpdateTargetAngle_Leftloop_zero(targetAngle);
- }
+ }
else{
- targetAngle[ROLL] = g_leftloopROLL;
- targetAngle[PITCH] = g_leftloopPITCH;
- autopwm[RUD]=g_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;
- }
- break;
+ targetAngle[ROLL] = g_leftloopROLL;
+ targetAngle[PITCH] = g_leftloopPITCH;
+ autopwm[RUD]=g_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;
+ }
+ NVIC_EnableIRQ(USART2_IRQn);
+ break;
case 'G': //直進セヨ
if(zeroTHR==false){
@@ -1363,19 +1381,22 @@
targetAngle[ROLL] = g_gostraightROLL;
targetAngle[PITCH] = g_gostraightPITCH;
}
-
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'Y': //指定ノヨー方向ニ移動セヨ
Rotate(targetAngle, g_SerialTargetYAW);
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'B': //ブザーヲ鳴ラセ
//buzzer = 1;
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'D': //物資ヲ落トセ
Chicken_Drop();
+ NVIC_EnableIRQ(USART2_IRQn);
break;
case 'C': //停止セヨ
@@ -1383,10 +1404,16 @@
targetAngle[PITCH] = -3.0;
autopwm[THR] = minpwm[THR];
zeroTHR=false;
+ NVIC_EnableIRQ(USART2_IRQn);
break;
+
+ default :
+ NVIC_EnableIRQ(USART2_IRQn);
+ break;
+
}
- NVIC_EnableIRQ(USART2_IRQn);
+
}
void checkHeight(float targetAngle[3]){
@@ -1449,13 +1476,13 @@
autopwm[RUD]=g_rightloopRUD; //RUD固定
autopwm[THR] = SetTHRinRatio(0.5); //手動スロットル記憶
-
+ /*
if (nowAngle[ROLL]>20.0||nowAngle[ROLL]<-20.0){
t2.start();
pc.printf("Timer start.");
}
if(0.0<t2.read()<5.0){
- pc.printf("tagetAngle is changed.");
+ //pc.printf("tagetAngle is changed.");
targetAngle[ROLL] = rightloopROLL2;
}
else {
@@ -1464,7 +1491,7 @@
pc.printf("Timer stopped.");
targetAngle[ROLL] = g_rightloopROLL;
}
-
+*/
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;
}
@@ -1573,12 +1600,12 @@
//for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
//for(uint8_t i=1; i<4; i++) pc.printf("%d ",sbus.manualpwm[i]);
//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<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",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