통신 알고리즘 - Multiwii 드론 통신 분석

https://github.com/Rupakpoddar/Drone/tree/main

GitHub - Rupakpoddar/Drone: Transmitter and flight controller code for Arduino and HC12 Transceiver module based drone.

 

이분이 만든 드론의 통신 부분을 분석한다. 

 

[송신기]

날라오는 데이터가 "throttle,yaw,pitch,roll,aux1,aux2\n" 라는 것 빼고는 그닥 참고 할 것은 없다.

 

/*
 * Written by Rupak Poddar
 * https://youtu.be/T0L7FtrbVBs
 * https://github.com/Rupakpoddar/Drone
 */

#include <Adafruit_SSD1306.h>
#include <NeoSWSerial.h>
#include "bitmap.h"
#define LEDBUILTIN 13
#define THROTTLE A0
#define YAW A1
#define PITCH A2
#define ROLL A3
#define AUX1 5
#define AUX2 6
#define PADDING 10

NeoSWSerial HC12(3, 2);
Adafruit_SSD1306 display(128, 64);

unsigned long lastReceiveTime = 0;
int meanVal[4];

int throttle = 512;
int yaw = 512;
int pitch = 512;
int roll = 512;
int aux1 = 1;
int aux2 = -1;

void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setTextWrap(true);
  display.drawBitmap(0, 0, bootlogo, 128, 64, WHITE);
  display.display();
  delay(1000);

  pinMode(THROTTLE, INPUT);
  pinMode(YAW, INPUT);
  pinMode(PITCH, INPUT);
  pinMode(ROLL, INPUT);
  pinMode(AUX1, INPUT_PULLUP);
  //pinMode(AUX2, INPUT_PULLUP);
  Serial.begin(9600);
  HC12.begin(9600);
  delay(100);

  // Calibrate Joysticks
  for(int i = 0; i < 4; i++){
    int sum = 0;
    for(int j = 0; j < 5; j++){
      if(i == 0){
        sum += analogRead(THROTTLE);
      }
      else if(i == 1){
        sum += analogRead(YAW);
      }
      else if(i == 2){
        sum += analogRead(PITCH);
      }
      else{
        sum += analogRead(ROLL);
      }
    }
    meanVal[i] = sum/5;
    if( i == 1 || i == 3){
      meanVal[i] = 1023 - meanVal[i]; 
    }
  }
}

void loop() {
  // Analog Read
  throttle = analogRead(THROTTLE);
  if(throttle < (meanVal[0] - PADDING)){
    throttle = map(throttle, 0, meanVal[0], 0, 512);
  }
  else if(throttle > (meanVal[0] + PADDING)){
    throttle = map(throttle, meanVal[0], 1023, 512, 1023);
  }
  else{
    throttle = 512;
  }
  
  yaw = 1023 - analogRead(YAW);
  if(yaw < (meanVal[1] - PADDING)){
    yaw = map(yaw, 0, meanVal[1], 0, 512);
  }
  else if(yaw > (meanVal[1] + PADDING)){
    yaw = map(yaw, meanVal[1], 1023, 512, 1023);
  }
  else{
    yaw = 512;
  }
  
  pitch = analogRead(PITCH);
  if(pitch < (meanVal[2] - PADDING)){
    pitch = map(pitch, 0, meanVal[2], 0, 512);
  }
  else if(pitch > (meanVal[2] + PADDING)){
    pitch = map(pitch, meanVal[2], 1023, 512, 1023);
  }
  else{
    pitch = 512;
  }
  
  roll = 1023 - analogRead(ROLL);
  if(roll < (meanVal[3] - PADDING)){
    roll = map(roll, 0, meanVal[3], 0, 512);
  }
  else if(roll > (meanVal[3] + PADDING)){
    roll = map(roll, meanVal[3], 1023, 512, 1023);
  }
  else{
    roll = 512;
  }
  
  aux1 = digitalRead(AUX1);

  // Transmit
  HC12.print(throttle);
  HC12.print(",");
  HC12.print(yaw);
  HC12.print(",");
  HC12.print(pitch);
  HC12.print(",");
  HC12.print(roll);
  HC12.print(",");
  HC12.print(aux1);
  HC12.print(",");
  HC12.print(aux2);
  HC12.println("");

  // Display
  display.clearDisplay();
  display.fillRect(0, map(throttle, 0, 1023, display.height(), 0), display.width()/12, display.height(), SSD1306_INVERSE);
  display.fillRect(2*display.width()/12, map(yaw, 0, 1023, display.height(), 0), display.width()/12, display.height(), SSD1306_INVERSE);
  display.fillRect(4*display.width()/12, map(pitch, 0, 1023, display.height(), 0), display.width()/12, display.height(), SSD1306_INVERSE);
  display.fillRect(6*display.width()/12, map(roll, 0, 1023, display.height(), 0), display.width()/12, display.height(), SSD1306_INVERSE);
  display.fillRect(8*display.width()/12, map(aux1, 0, 1, display.height(), 0), display.width()/12, display.height(), SSD1306_INVERSE);
  display.drawRect(10*display.width()/12, 0, display.width()/12, display.height(), SSD1306_INVERSE);
  display.display();
 
  delay(50);  // Important delay

  if(HC12.available() > 0){
    String State = HC12.readStringUntil('\n');
    Serial.println(State);
    lastReceiveTime = millis();
    digitalWrite(LEDBUILTIN, LOW);
  }
  unsigned long currentTime = millis();
  if ( currentTime - lastReceiveTime > 1000 ) {
    digitalWrite(LEDBUILTIN, HIGH);
    Serial.println("Connection Lost");
    delay(100);
  }
}

 

[수신기]

1.코드

MultiWii의 HC12RX.cpp 파일에서 데이터 수신 함수 HC12_Read_RC 를 정의 해놓았다.

/*
MIT License

Copyright (c) 2021 Rupak Poddar

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <NeoSWSerial.h>
#include "Arduino.h"
#include "config.h"
#include "def.h"
#include "types.h"
#include "MultiWii.h"
#include "HC12RX.h"

#if defined(HC12RX)

#define PADDING 10
#define THROTTLE_INC 11

int16_t HC12_rcData[RC_CHANS];

NeoSWSerial HC12(7, 8);

unsigned long lastReceiveTime = 0, lastRunTime = 0;
const char delimiter = ',';
byte boundLow;
byte boundHigh;
String input;

int throttle = 0;
int yaw = 512;
int pitch = 512;
int roll = 512;
int aux1 = 1;
int aux2 = 0;

void HC12_Init() {
  HC12.begin(9600);
  pinMode(LED_BUILTIN, OUTPUT);
}

void HC12_Read_RC() {	
  unsigned long currentTime = millis();
  if((currentTime - lastRunTime) >= 100){
    int numDelimiter = 0;
    if(HC12.available() > 0){
      input = HC12.readStringUntil('\n');
      for(int i = 0; i < input.length(); i++){
        if(input[i] == delimiter){
          numDelimiter++;
        }
      }
      if(numDelimiter == 5){
        //Serial.println(input); // <---
        lastReceiveTime = millis();
      
        boundLow = input.indexOf(delimiter);
        int throttle_det = input.substring(0, boundLow).toInt();
        if(throttle_det >= 600){
          if(throttle <= (1023 - THROTTLE_INC)){
            throttle += THROTTLE_INC;
          }
        }

        if(throttle_det <= 424){
          if(throttle >= THROTTLE_INC){
            throttle -= THROTTLE_INC;
          }
        }
        
        boundHigh = input.indexOf(delimiter, boundLow+1);
        yaw = input.substring(boundLow+1, boundHigh).toInt();
      
        boundLow = input.indexOf(delimiter, boundHigh+1);
        pitch = input.substring(boundHigh+1, boundLow).toInt();

        boundHigh = input.indexOf(delimiter, boundLow+1);
        roll = input.substring(boundLow+1, boundHigh).toInt();

        boundLow = input.indexOf(delimiter, boundHigh+1);
        aux1 = input.substring(boundHigh+1, boundLow).toInt();
        /*
        if(aux1 == 0){
          // Do nothing
        }
        else{
          // Do nothing
        }
        */
        aux2 = input.substring(boundLow+1).toInt();

        if((throttle_det < PADDING) && (yaw < PADDING) && (roll > (1023 - PADDING)) && (pitch < PADDING)){
          throttle = 0;
        }
      }   
    }
    currentTime = millis();
    if (currentTime - lastReceiveTime > 1000) {
      digitalWrite(LED_BUILTIN, HIGH);
      if(throttle >= (THROTTLE_INC/5)){
        throttle -= (THROTTLE_INC/5);
      }
      yaw = 512;
      pitch = 512;
      roll = 512;
      aux1 = 1;
      aux2 = 0;
      //Serial.println("Connection Lost"); // <---
    }
    else{
      digitalWrite(LED_BUILTIN, LOW);
      HC12.println("OK");
    }
    lastRunTime = millis();
    HC12_rcData[THROTTLE] = map(throttle, 0, 1023, 1000, 2000);
    HC12_rcData[YAW] = map(yaw, 0, 1023, 1000, 2000);
    HC12_rcData[PITCH] = map(pitch, 0, 1023, 1000, 2000);
    HC12_rcData[ROLL] = map(roll, 0, 1023, 1000, 2000);
    HC12_rcData[AUX1] = map(aux1, 0, 1, 2000, 1000);
    HC12_rcData[AUX2] = 1000;
  }
}
#endif

 

 

2.구조 분석

우선 시점에 관한 주요한 변수 3개가 있다.

*지역변수 CurrentTime : 현재시점

*전역변수 lastRunTime :  HC12_Read_RC  함수가 마지막으로 사용을 마친 시점

*전역변수 lastReceiveTime : 데이터를 마지막으로 수신받은 시점

 

알고리즘 순서는 아래와 같다.

if((currentTime - lastRunTime) >= 100){
	if(HC12.available() > 0){
    	// 1. 데이터를 수신한다.
        // 2. 구분자 개수를 센다
        // 3. lastReceiveTime을 갱신한다.
        // 4. 구분자 개수가 5개인 경우 데이터를 가공한다.    
    }
    CurrentTime=millis(); //5.CurrentTime 을 갱신한다.
    if (currentTime - lastReceiveTime > 1000) { 
    	//6. 4번의 데이터 가공에서 1초 이상 걸린 경우 통신 신호 소실로 간주한다.
	
    }
	else{
    	//6.1초보다는 덜 걸린 경우 통신 상태 양호로 간주
    }
    lastRunTime = millis(); //7. 가공한 데이터를 배열에 정리한다.
    

}

 

데이터 수신 함수는 1번 사용될 때 마다

1)데이터 수신 > 2)데이터 유효성 검사(구분자 검사) > 3)데이터 가공 > 4) 통신상태 체크 >  5)가공 데이터 정리  를 거친다.

 

드론에서는 HC12 모듈 2개로 송신과 수신을 나누어두었다. 

양방향이 아니라. 단방향이라고 봐야한다. 

드론의 자이로 센서 값을 조종기에 송신해주지는 않는다.