航空研究会
a
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of
Autoflight2018_4
by 
航空研究会
Revision 4:b66ab0bee119, committed 2018-09-13
- Comitter:
- taknokolat
- Date:
- Thu Sep 13 06:16:12 2018 +0000
- Parent:
- 3:954228fd64b3
- Commit message:
- a
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Sep 12 05:13:58 2018 +0000
+++ b/main.cpp Thu Sep 13 06:16:12 2018 +0000
@@ -110,7 +110,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確認用
+//RawSerial pc2(PB_6,PB_7, 115200); //確認用
SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
DigitalOut led1(PA_0); //黄色のコネクタ
@@ -141,6 +141,8 @@
int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180};
int16_t oldTHR = 1000;
+int zeroTHR=1;//着陸時にスロットルが０かどうかの判断に使用
+
static float nowAngle[3] = {0,0,0};
@@ -166,7 +168,7 @@
void Init_PWM();
void Init_servo(); //サーボ初期化
-void Init_sbus(); //SBUS初期化
+void Init_(); //初期化
void Init_sensors();
void DisplayClock(); //クロック状態確認
@@ -186,7 +188,7 @@
int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min);
inline int16_t SetTHRinRatio(float ratio);
-//sbus割り込み
+//割り込み
void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力
void Output_PWM(int16_t pwm[6]); //pwmをサーボへ出力
@@ -213,10 +215,13 @@
//自動操縦
void UpdateTargetAngle_GoStraight(float targetAngle[3]);
+void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]); //着陸時にスロットルが０の時の直進
void UpdateTargetAngle_Rightloop(float targetAngle[3]);
void UpdateTargetAngle_Rightloop_short(float targetAngle[3]);
+void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]); //着陸時にスロットルが０の時の右旋回
void UpdateTargetAngle_Leftloop(float targetAngle[3]);
void UpdateTargetAngle_Leftloop_short(float targetAngle[3]);
+void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]); //着陸時にスロットルが０の時の左旋回
void UpdateTargetAngle_Moebius(float targetAngle[3]);
void UpdateTargetAngle_Glide(float targetAngle[3]);
void UpdateTargetAngle_Takeoff(float targetAngle[3]);
@@ -278,7 +283,7 @@
Init_PWM();
Init_servo();
- Init_sbus();
+ Init_();
Init_sensors();
//switch2.rise(ResetTrim);
pc.attach(getSF_Serial, Serial::RxIrq);
@@ -359,8 +364,8 @@
pc.printf("servo initialized\r\n");
}
-//Sbus初期化
-void Init_sbus(){
+//初期化
+void Init_(){
sbus.initialize();
sbus.setLastfuncPoint(Update_PWM);
sbus.startInterrupt();
@@ -803,8 +808,8 @@
pwm[DROP] = autopwm[DROP];
pwm[AIL_L] = autopwm[AIL_L];
//pc.printf("%d ,",pwm[ELE]);//ELE
- pc.printf("%d ,",pwm[AIL_R]);//R
- pc.printf("%d ,\r\n",pwm[AIL_R]);//L
+ //pc.printf("%d ,",pwm[AIL_R]);//R
+ //pc.printf("%d ,\r\n",pwm[AIL_R]);//L
//pc.printf("update_auto\r\n");
break;
@@ -901,6 +906,8 @@
static int bufcounter=0;
SFbuf[bufcounter]=pc.getc();
+
+ pc.printf("%s",SFbuf[bufcounter]);
if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++;
if(bufcounter==5 && SFbuf[4]=='F'){
@@ -1024,7 +1031,7 @@
}
//離陸-投下-着陸一連
void Take_off_and_landing(float targetAngle[3]){
- if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
+ /*if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
switch(bombing_mode){
case Takeoff:
@@ -1048,10 +1055,10 @@
bombing_mode = Approach;
}
break;
- /*
- case Chicken:
- break;
- */
+
+ // case Chicken:
+ // break;
+
case Transition:
static int ApproachCount = 0;
targetAngle[YAW]=180.0;
@@ -1061,14 +1068,16 @@
if(ApproachCount>5) bombing_mode = Approach;
break;
- case Approach:
+
+ case Approach*/
+ autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合
UpdateTargetAngle_Approach(targetAngle);
- break;
+ /* break;
default:
bombing_mode = Takeoff;
break;
- }
+ }*/
}
//離陸モード
@@ -1083,24 +1092,24 @@
int Rotate(float targetAngle[3], float TargetYAW){
float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);
- if(diffYaw > LIMIT_STRAIGHT_YAW){
- /*
- if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle);
- else UpdateTargetAngle_Rightloop(targetAngle);
- */
+ /* if(diffYaw > LIMIT_STRAIGHT_YAW){
+
+ //if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle);
+ //else UpdateTargetAngle_Rightloop(targetAngle);
+
UpdateTargetAngle_Rightloop_short(targetAngle);
return 1;
}else if(diffYaw < -LIMIT_STRAIGHT_YAW){
UpdateTargetAngle_Leftloop_short(targetAngle);
- /*
- if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle);
- else UpdateTargetAngle_Leftloop(targetAngle);
- */
+
+ // if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle);
+ //else UpdateTargetAngle_Leftloop(targetAngle);
+
return 1;
}else{
UpdateTargetAngle_GoStraight(targetAngle);
return 0;
- }
+ }*/
}
//チキラー投下
@@ -1128,18 +1137,36 @@
//着陸モード（PCからの指令に従う）
void UpdateTargetAngle_Approach(float targetAngle[3]){
- pc.putc(g_buf[0]);
- switch(g_buf[0]){
+ //pc.putc(g_buf[0]);]
+ static char rotatemode = 'G';
+ if(output_status == Manual) rotatemode = 'G';
+
+ switch(g_landingcommand){
case 'R': //右旋回セヨ
+ if(zeroTHR==0){
+ UpdateTargetAngle_Rightloop_zero(targetAngle);
+ }
+ else{
UpdateTargetAngle_Rightloop(targetAngle);
+ }
break;
case 'L': //左旋回セヨ
+ if(zeroTHR==0){
+ UpdateTargetAngle_Leftloop_zero(targetAngle);
+ }
+ else{
UpdateTargetAngle_Leftloop(targetAngle);
+ }
break;
case 'G': //直進セヨ
+ if(zeroTHR==0){
+ UpdateTargetAngle_GoStraight_zero(targetAngle);
+ }
+ else{
UpdateTargetAngle_GoStraight(targetAngle);
+ }
break;
case 'Y': //指定ノヨー方向ニ移動セヨ
@@ -1158,6 +1185,7 @@
targetAngle[ROLL] = 0.0;
targetAngle[PITCH] = -3.0;
autopwm[THR] = minpwm[THR];
+ zeroTHR=0;
break;
}
}
@@ -1202,6 +1230,33 @@
//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
}
+//直進(着陸時throttle０の時）
+void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]){
+
+ targetAngle[ROLL] = g_gostraightROLL;
+ targetAngle[PITCH] = g_gostraightPITCH;
+ autopwm[THR] = minpwm[THR];
+
+ //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
+}
+
+//右旋回（着陸時スロットル０の時）
+void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]){ //右旋回
+
+ targetAngle[ROLL] = g_rightloopROLL;
+ targetAngle[PITCH] = g_rightloopPITCH ;
+ autopwm[RUD]=rightloopRUD;
+ autopwm[THR] = minpwm[THR];//SetTHRinRatio(g_loopTHR);
+ 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;
+
+ //checkHeight(targetAngle);
+ //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
+}
+
+
//右旋回
void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回
@@ -1249,6 +1304,23 @@
//printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
}
+//左旋回（着陸時スロットル０のとき）
+void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]){
+
+ targetAngle[ROLL] = g_leftloopROLL;
+ targetAngle[PITCH] = g_leftloopPITCH;
+ autopwm[RUD]=leftloopRUD;
+ autopwm[THR] = minpwm[THR];
+ 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;
+ //autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_Ratio_leftloop;
+ //checkHeight(targetAngle);
+
+ //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
+}
+
void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){
targetAngle[ROLL] = g_leftloopROLLshort;
@@ -1292,6 +1364,6 @@
//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");
//pc.printf("%d",usensor.get_dist_cm()
}
\ No newline at end of file