みなさん、初めまして大学生のMiyamotoです。私はArduPlane2.72のコードを編集してGPSを使用しない自立型室内飛行機を作っています。八の字飛行を行うプログラムを作ったのですが実際に飛ばしたところ、ずっと左旋回をしたまま円周運動をしていてただのCIRCLEモードと同じような飛行をしていました。

プログラムは以下の様になっています。どなたか宜しければ間違っているところや改善点の方を教えてください。目標をとしては八の字飛行をずっと繰り返したいです。機体は自作のバルサ材とフィルムを用いた室内用のラダー機です。

プログラムの流れとしては

static uint8_t eight_counterとしてカウンタ変数を定義。

フライトモード切替時にeight_counter　= 0として変数に0を代入、next_WP = current_locとする。

機体の運動プログラムはeight_counterが0ならばロール角を+(LIM_ROLL_CD/2),1ならば-(LIM_ROLL_CD/2)維持したまま切替地点に戻るように飛行を行い、切替地点の2m近くに来たらeight_counterの値を0か1にする流れです。下が実際のコードです。フライトモード名はEIGHT_FLIGHT

<Arduplane.pde>

static uint8_t eight_counter;

static uint32_t eight_timer_zero;   
static uint32_t eight_timer_one;

..

static void update_current_flight_mode(void)
{ ....

case EIGHT_FLIGHT:{
         switch(eight_counter){
            case 0:  
            nav_roll_cd  = g.roll_limit_cd / 2;
            break;
            case 1:
            nav_roll_cd  = -(g.roll_limit_cd /2);
            break;
         }
            calc_nav_pitch();
            calc_throttle();
        }
            break;

...

static void update_navigation()
{ ..
    switch(control_mode) {

....

 case EIGHT_FLIGHT:{
      if(eight_counter == 0 && (millis() - eight_timer_zero >5000) && (wp_distance < 2)){
      eight_counter = 1;
      eight_timer_one = millis();
          }
      else if(eight_counter == 1 && (millis()-eight_timer_one >5000) && (wp_distance < 2)){
       eight_counter = 0;

　　　eight_timer_zero = millis();
     }
    }
    break;

....

<system.pde>

..

static void set_mode(enum FlightMode mode)
{ ...

case EIGHT_FLIGHT:
       eight_counter = 0;

　　　eight_timer_zero = millis();
        next_WP = current_loc;
        break;

.....

}