![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
a
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_4 by
Diff: main.cpp
- Revision:
- 4:b66ab0bee119
- Parent:
- 3:954228fd64b3
diff -r 954228fd64b3 -r b66ab0bee119 main.cpp --- 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;//着陸時にスロットルが0かどうかの判断に使用 + 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]); //着陸時にスロットルが0の時の直進 void UpdateTargetAngle_Rightloop(float targetAngle[3]); void UpdateTargetAngle_Rightloop_short(float targetAngle[3]); +void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]); //着陸時にスロットルが0の時の右旋回 void UpdateTargetAngle_Leftloop(float targetAngle[3]); void UpdateTargetAngle_Leftloop_short(float targetAngle[3]); +void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]); //着陸時にスロットルが0の時の左旋回 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]); } +//直進(着陸時throttle0の時) +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]); +} + +//右旋回(着陸時スロットル0の時) +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]); } +//左旋回(着陸時スロットル0のとき) +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