ArduinoにFutaba S.Busを実装してドローンを動かす(2)

前回は簡単にS.Busプロトコルを紹介しました．
今回は，Arduino用の回路とプログラムを紹介します．

まずは今回使用する回路を紹介します．
Arduino Microにトランジスタの反転回路を繋げました．
トランジスタ反転回路は，C1815などのNPNトランジスタをエミッタ接地のスイッチとして使っただけです．図は左から順にBECとなります．
この出力(赤ワイヤー)からS.Busを出力します．

自律制御を目指すためには，

Arduino自身で制御
外部 -> Arduinoで制御

のいずれかで制御する必要があります．
今回は，2.を採用します．そのために，USBを介して他の計算機と通信させます．ここのプロトコルはS.Busの通信間隔7msを考慮して決定する必要があります．
また，各チャンネルの制御量は1024を基準に上下限660となります．(※私が使用したプロポの場合)

これらを踏まえたプログラムは以下の通りです．
自由記載欄があるので，好きに書いてください．

#include < TimerOne.h >
volatile unsigned int SBUS_Channel[16]; //設定データ(11bit)
byte SBUS_Packet[25];                   //送信データ(25byte)
byte Channel_Num = 0; //送信Channel要素番号
byte Channel_Bit = 0; //送信ChannelBit
byte Packet_Num = 0;  //送信Channek要素番号
byte Packet_Bit = 0;  //送信PacketBit

byte SBUS_Failsafe_Active = 0;
byte SBUS_Lost_Frame = 0;
void sendSBUS(){
  for(Packet_Num = 1; Packet_Num < 24; Packet_Num++) SBUS_Packet[Packet_Num] = 0x00;  //送信パケット初期化
  Packet_Bit = 0;
  Packet_Num = 1;
  for(Channel_Num = 0; Channel_Num < 16; Channel_Num++){  //Channel走査
    for(Channel_Bit = 0; Channel_Bit < 11; Channel_Bit++){//11bit走査
      if(Packet_Bit > 7){
        Packet_Bit = 0;
        Packet_Num++;
      }
      SBUS_Packet[Packet_Num] |= (((SBUS_Channel[Channel_Num]>>Channel_Bit) & 0x01)<<Packet_Bit);//左シフトと挿入
      Packet_Bit++;
    }
  }
  if(SBUS_Lost_Frame != 0) SBUS_Packet[23] |= (1<<2);         //フレームロストの判断
  if(SBUS_Failsafe_Active != 0) SBUS_Packet[23] |= (1<<3);    //Failsafeの判断
  Serial1.write(SBUS_Packet, 25);                             //送信
}

void setup(){
  Serial.begin(9600);
  Serial1.begin(100000,SERIAL_8E2);//FUTABA S.Bus出力,8bit(CS:2)
  delay(100);
  SBUS_Packet[0] = 0x0F;  //Start Byte
  SBUS_Packet[24] = 0x00; //End Byte
  SBUS_Channel[0] = 1024;//A,右レバー：左右，中央値,上下限±660
  SBUS_Channel[1] = 1024;//E,左レバー：上下，中央値,上下限±660
  SBUS_Channel[2] = 1024;//T,右レバー：上下，中央値,上下限±660
  SBUS_Channel[3] = 1024;//R,左レバー：左右，中央値,上下限±660
  SBUS_Channel[4] = 1024;//X1
  SBUS_Channel[5] = 1024;//X2
  SBUS_Channel[6] = 1024;//0x400:Atti., 0x1FF:Manual,0x605:GPS Atti. : 500,1020,1550?
  SBUS_Channel[7] = 1024;
  SBUS_Channel[8] = 1024;
  SBUS_Channel[9] = 1024;
  SBUS_Channel[10] = 1024;
  SBUS_Channel[11] = 1024;
  SBUS_Channel[12] = 1024;
  SBUS_Channel[13] = 1024;
  SBUS_Channel[14] = 1024;
  SBUS_Channel[15] = 1024;
  SBUS_Channel[16] = 1024;
  SBUS_Channel[17] = 1024;
  SBUS_Failsafe_Active = 0;   //Failsafe作動条件,1:許可
  SBUS_Lost_Frame = 0;        //ロストフレーム条件,1:許可
  Timer1.initialize(7000);    //割り込み間隔(送信間隔HighSpeed:7000[ms],LowSpeed:14000[ms])
  Timer1.attachInterrupt(sendSBUS);//割り込み関数
}

void loop(){
  static String str;
  if(Serial.available()){
    str = Serial.readStringUntil('\n');//文字列読み込み(改行を条件として)
    if(str.equals("C-GPS")){//GPS,COMPASSキャリブレーション
      Serial.println("C-GPS");
      for(int j=0;j<10;j++){
          SBUS_Channel[6]=511;//Manual
          delay(100);
          SBUS_Channel[6]=1541;//GPS Atti.
          delay(100);
      }
      while(!Serial.available());//終了入力待
      while(Serial.available())Serial.read();
      SBUS_Channel[6] = 0x400;//Atti.
      
    }else if(str.equals("C-PROP")){//プロポ(送信機信号)キャリブレーション
      SBUS_Channel[0] = 1024;//A,右レバー：左右，中央値,上下限±660
      SBUS_Channel[1] = 1024;//E,左レバー：上下，中央値,上下限±660
      SBUS_Channel[2] = 1024;//T,右レバー：上下，中央値,上下限±660
      SBUS_Channel[3] = 1024;//R,左レバー：左右，中央値,上下限±660
      SBUS_Channel[4] = 1024;//X1
      SBUS_Channel[5] = 1024;//X2
      delay(500);
      for(int i=0;i<6;i++){
        for(int j=364;j<=1684;j+=10){
          SBUS_Channel[i] = j;
          delay(20);
        }
        delay(100);
      }
      for(int i=3;i>=0;i--){
        for(int j=1684;j>=364;j-=10){
          SBUS_Channel[i] = j;
          delay(20);
        }
        delay(100);
      }
      for(int i=0;i<6;i++){
        SBUS_Channel[i] = 1024;
      }
      delay(500);
    }else if(str.equals("M-Serial")){//シリアル制御


      //ここに受信信号処理とAETRへの対応づけ


    }else if(str.equals("M-Init")){//制御量初期化
      SBUS_Channel[0] = 1024;//A,右レバー：左右，中央値,上下限±660
      SBUS_Channel[1] = 1024;//E,左レバー：上下，中央値,上下限±660
      SBUS_Channel[2] = 1024;//T,右レバー：上下，中央値,上下限±660
      SBUS_Channel[3] = 1024;//R,左レバー：左右，中央値,上下限±660
      SBUS_Failsafe_Active = 0;
    }else {//それ以外の信号は危険信号
      SBUS_Channel[0] = 1024;//A,右レバー：左右，中央値,上下限±660
      SBUS_Channel[1] = 1024;//E,左レバー：上下，中央値,上下限±660
      SBUS_Channel[2] = 1024;//T,右レバー：上下，中央値,上下限±660
      SBUS_Channel[3] = 1024;//R,左レバー：左右，中央値,上下限±660
      SBUS_Failsafe_Active = 1;
    }
  }
}

2018/5/16　プログラムの表示を修正

HSW雑記

このブログを検索

ArduinoにFutaba S.Busを実装してドローンを動かす(2)

コメント

コメントを投稿