11. SBUS 비행제어

• 준비물

• 회로도
  

• 소스 코드 다운로드

  Raduino_mini_sbus_fc_lib_V2.ino

  
  

• 코드

#include "raduino_sbus.h"
 
#include "raduino_fc.h"
 
#include <Wire.h>                     // i2C 통신을 위한 라이브러리
 
SBUS sbus(Serial2);
 
FC Flight(Serial1);
 
 
 
boolean startFlag = false;
 
 
 
unsigned long currentMillis, previousMillis = 0;
 
 
 
int ledPin = PC13;
 
 
 
// We'll use timer 1
 
HardwareTimer timer(1);
 
 
 
void setup() {
 
 
 
    sbus.begin();
 
    sbus.SBUS_OUT= true; //enable SBUS_OUT
 
    
 
    // Pause the timer while we're configuring it
 
    timer.pause();
 
 
 
    // Set up period
 
    timer.setPeriod(10000); // in 10 microseconds
 
 
 
    // Set up an interrupt on channel 1
 
    timer.setChannel1Mode(TIMER_OUTPUT_COMPARE);
 
    timer.setCompare(TIMER_CH1, 1);  // Interrupt 1 count after each update
 
    timer.attachCompare1Interrupt(handler_10ms);
 
 
 
    // Refresh the timer's count, prescale, and overflow
 
    timer.refresh();
 
 
 
    // Start the timer counting
 
    timer.resume();
 
 
 
    Flight.begin();
 
    Serial.begin(115200);  
 
 
 
    pinMode(LED_BUILTIN, OUTPUT);
 
    digitalWrite(LED_BUILTIN, HIGH);
 
 
 
    delay(100);
 
 
 
    // roll positive right
 
    // pitch positive forward
 
    //Flight.Trim(20,10); //roll: 0.0 , pith: -5.0
 
    Serial.println("Triming...");
 
 
 
    delay(5000);
 
    
 
    // binding.....
 
    Flight.Bind();
 
    Serial.println("binding...");
 
 
 
//     delay(100);
 
 //   Flight.Cali();  
 
 //   Serial.println("Calibration...");
 
      
 
}
 
 
 
int ChannelValue[4];
 
 
 
void loop() 
 
{
 
  int ret = Flight.Event();
 
  if(ret==1)digitalWrite(LED_BUILTIN,LOW); 
 
  if(ret>0)Serial.println(Flight.retString);
 
  
 
 
 
  if ( startFlag == true ) {
 
    ChannelValue[3] += 80;
 
  }
 
 
 
  if( ChannelValue[0] < 30 && ChannelValue[1] < 30 && ChannelValue[2] > 1000 && ChannelValue[3] < 110 ){   // 시동 걸때
 
      currentMillis = millis();
 
      if (currentMillis - previousMillis >= 1500){
 
        previousMillis = currentMillis;
 
        startFlag = 1;
 
      }
 
  }
 
 
 
  if( ChannelValue[0] > 1000 && ChannelValue[1] < 30 && ChannelValue[2] < 30 && ChannelValue[3] < 110 ){   // 시동 끌때
 
      currentMillis = millis();
 
      if (currentMillis - previousMillis >= 1500){
 
        previousMillis = currentMillis;
 
        startFlag = 0;
 
      }
 
  }
 
 
 
  Serial.print(ChannelValue[0]);  //312 ~1690 우스틱 좌우 roll
 
  Serial.print("   |   ");
 
  Serial.print(ChannelValue[1]); //325 ~1673 좌스틱 좌우 pitch
 
  Serial.print("   |   ");
 
  Serial.print(ChannelValue[2]); //352 ~1698 좌스틱 상하 yaw
 
  Serial.print("   |   ");
 
  Serial.println(ChannelValue[3]);  // 330 ~1680 우스틱 상하 Throttle  
 
}
 
 
 
void handler_10ms(void) {
 
    sbus.process();
 
  
 
  int ch1  = sbus.getChannel(1); 
 
  int ch2 = sbus.getChannel(2);
 
  int ch3 = sbus.getChannel(3);
 
  int ch4  = sbus.getChannel(4);  
 
 
 
  ChannelValue[0]= map(ch1,300,1700,0,1023); //roll
 
  ChannelValue[1]= map(ch2,1700,300,0,1023); //pitch
 
  ChannelValue[2]= map(ch4,300,1700,0,1023); //yaw
 
  ChannelValue[3]= map(ch3,300,1700,0,1023); //throttle
 
 
 
   if ( startFlag == true ) Flight.Tx(ChannelValue[0],ChannelValue[1],ChannelValue[2],ChannelValue[3]);
 
}