DIY드론 프로젝트[6] - Main Test Code

이번 글이 좀 늦었네요.

원래 테스트는 이미 작성했는데, 헤더파일끼리 충돌이 나서 원인을 찾느라 늦었습니다. ㅎㅎ

 

이번에는 드론 본체를 테스트 할수있는 소스를 작성해 보겠습니다.

 

<Servo.h>랑 <SoftwareSerial.h> 이 두개의 해더파일이 같은 타이머1을 사용해서

충돌이 발생합니다.

그래서 ESC모터 제어는 따로 작성했습니다.

 

Main Test Code

#include <Wire.h>
#include <SPI.h>
#include <SoftwareSerial.h>

#include "RF24.h"
#include "I2Cdev.h"
#include "TinyGPS.h"
#include "MPU6050.h"
#include "MS5611.h"
#include "Kalman.h"

#define Button      2

#define Motor_1     4
#define Motor_2     5
#define Motor_3     6
#define Motor_4     7

#define Servo_1     8
#define Servo_2     9
#define Servo_3     10
#define Servo_4     11

#define Tx          12
#define Rx          13

#define CE          22
#define CSN         23

#define LED_1       24
#define LED_2       25
#define LED_3       26
#define LED_4       27
#define LED_5       28
#define LED_6       29
#define LED_7       30
#define LED_8       31
#define LED_9       32
#define LED_10      33
#define LED_11      34
#define LED_12      35
#define LED_13      36
#define LED_14      37
#define LED_15      38

#define Buzzer      39

#define Cell_1      A1
#define Cell_2      A2
#define Cell_3      A3
#define Cell_4      A4

#define HMC5883L_DEFAULT_ADDRESS    0x1E
#define HMC5883L_RA_DATAX_H         0x03
#define HMC5883L_RA_DATAZ_H         0x05
#define HMC5883L_RA_DATAY_H         0x07

#define MIN_MOTOR_SPEED             1000
#define MAX_MOTOR_SPEED             1700

#define SEA_PRESSURE  102040

static byte address[2][6] = { "00000", "11111" };
char send_data[32];
char receive_data[32];

float Battery[4];

int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;
int16_t magX, magY, magZ;

double accXangle, accYangle;
double gyroXangle, gyroYangle;
double gyroXrate, gyroYrate;
double compAngleX, compAngleY; 
double kalAngleX, kalAngleY;

double dt;
double alpha;
uint32_t timer;

// http://www.kma.go.kr/weather/observation/aws_table_popup.jsp
// 위치 검색후 sea_press에 입력
const float sea_press = 1015.4;
double altitude, pressure;
long temperature;

// 사용할 클래스 선언
RF24 nRF24(CE, CSN);
SoftwareSerial gps_Serial(Tx, Rx);
TinyGPS gps;
MPU6050 mpu6050 = MPU6050();
MS5611 ms5611 = MS5611();
Kalman kalmanX;
Kalman kalmanY;

void setup() {
    Init();
}

void loop() {
    // 하나씩 주석을 해제하면서 사용하세요.
    //Buzzer_test();
    //Button_test();
    //LED_test();
    //GPS_test();
    //Check_test();
    //nRF24_test();
    //GY_test();
}

/****************************************************************************/

void Init() {
    Serial.begin(9600);
    
    // 부저
    pinMode(Buzzer, OUTPUT);
    
    // 버튼
    pinMode(Button, INPUT_PULLUP);
    
    // LED
    pinMode(LED_1, OUTPUT);
    pinMode(LED_2, OUTPUT);
    pinMode(LED_3, OUTPUT);
    pinMode(LED_4, OUTPUT);
    pinMode(LED_5, OUTPUT);
    pinMode(LED_6, OUTPUT);
    pinMode(LED_7, OUTPUT);
    pinMode(LED_8, OUTPUT);
    pinMode(LED_9, OUTPUT);
    pinMode(LED_10, OUTPUT);
    pinMode(LED_11, OUTPUT);
    pinMode(LED_12, OUTPUT);
    pinMode(LED_13, OUTPUT);
    pinMode(LED_14, OUTPUT);
    pinMode(LED_15, OUTPUT);
    
    // GPS
    gps_Serial.begin(9600);
    
    // 전압 측정
    pinMode(Cell_1, INPUT);
    pinMode(Cell_2, INPUT);
    pinMode(Cell_3, INPUT);
    pinMode(Cell_4, INPUT);
    
    // nRF24L01
    // RF24_PA_MIN < RF24_PA_LOW < RF24_PA_HIGH < RF24_PA_MAX
    // RF24_PA_MAX로 해주려면 10uF를 연결해주는 것이 좋다.
    nRF24.begin();
    
    nRF24.setPALevel(RF24_PA_MIN);
    
    nRF24.openReadingPipe(1, address[1]);
    nRF24.openWritingPipe(address[0]);
    
    nRF24.startListening();


    // GY-86
    mpu6050.initialize();
    ms5611.reset();
    
    if(!mpu6050.testConnection()) {
        Serial.println("MPU6050 Error!");
        return 0;
    }

    if(!ms5611.begin(MS5611_ULTRA_HIGH_RES)) {
        Serial.println("MS5611 Error!");
        return 0;
    }
    
    mpu6050.setI2CMasterModeEnabled(false);
    mpu6050.setI2CBypassEnabled(true);
    
    Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS);
    Wire.write(0x02); 
    Wire.write(0x00);  // 연속 읽기 모드로 설정
    Wire.endTransmission();
    delay(5);
    
    Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS);
    Wire.write(0x00);
    Wire.write(B00011000);  // 75Hz
    Wire.endTransmission();
    delay(5);
    
    mpu6050.setI2CBypassEnabled(false);
    
    mpu6050.setSlaveAddress(0, HMC5883L_DEFAULT_ADDRESS | 0x80);
    mpu6050.setSlaveRegister(0, HMC5883L_RA_DATAX_H);
    mpu6050.setSlaveEnabled(0, true);
    mpu6050.setSlaveWordByteSwap(0, false);
    mpu6050.setSlaveWriteMode(0, false);
    mpu6050.setSlaveWordGroupOffset(0, false);
    mpu6050.setSlaveDataLength(0, 2);
    
    mpu6050.setSlaveAddress(1, HMC5883L_DEFAULT_ADDRESS | 0x80);
    mpu6050.setSlaveRegister(1, HMC5883L_RA_DATAY_H);
    mpu6050.setSlaveEnabled(1, true);
    mpu6050.setSlaveWordByteSwap(1, false);
    mpu6050.setSlaveWriteMode(1, false);
    mpu6050.setSlaveWordGroupOffset(1, false);
    mpu6050.setSlaveDataLength(1, 2);
    
    mpu6050.setSlaveAddress(2, HMC5883L_DEFAULT_ADDRESS | 0x80);
    mpu6050.setSlaveRegister(2, HMC5883L_RA_DATAZ_H);
    mpu6050.setSlaveEnabled(2, true);
    mpu6050.setSlaveWordByteSwap(2, false);
    mpu6050.setSlaveWriteMode(2, false);
    mpu6050.setSlaveWordGroupOffset(2, false);
    mpu6050.setSlaveDataLength(2, 2);
    
    mpu6050.setI2CMasterModeEnabled(true);
    
    delay(50);
}

/****************************************************************************/

void Button_test() {
    Serial.print(digitalRead(Button));
    delay(200);
}

/****************************************************************************/

void Buzzer_test() {
    tone(Buzzer, 2000, 200);
    delay(1000);
}

/****************************************************************************/

void LED_test() {
    digitalWrite(LED_1, HIGH);
    digitalWrite(LED_2, HIGH);
    digitalWrite(LED_3, HIGH);
    digitalWrite(LED_4, HIGH);
    digitalWrite(LED_5, HIGH);
    digitalWrite(LED_6, HIGH);
    digitalWrite(LED_7, HIGH);
    digitalWrite(LED_8, HIGH);
    digitalWrite(LED_9, HIGH);
    digitalWrite(LED_10, HIGH);
    digitalWrite(LED_11, HIGH);
    digitalWrite(LED_12, HIGH);
    digitalWrite(LED_13, HIGH);
    digitalWrite(LED_14, HIGH);
    digitalWrite(LED_15, HIGH);
    delay(500);
    digitalWrite(LED_1, LOW);
    digitalWrite(LED_2, LOW);
    digitalWrite(LED_3, LOW);
    digitalWrite(LED_4, LOW);
    digitalWrite(LED_5, LOW);
    digitalWrite(LED_6, LOW);
    digitalWrite(LED_7, LOW);
    digitalWrite(LED_8, LOW);
    digitalWrite(LED_9, LOW);
    digitalWrite(LED_10, LOW);
    digitalWrite(LED_11, LOW);
    digitalWrite(LED_12, LOW);
    digitalWrite(LED_13, LOW);
    digitalWrite(LED_14, LOW);
    digitalWrite(LED_15, LOW);
    delay(500);
}

/****************************************************************************/

void GPS_test() {
    while(gps_Serial.available()) {
        Serial.write(gps_Serial.read());
    }
}

/****************************************************************************/

void Check_test() {
    Battery[0] = (analogRead(Cell_1) * 25.0) / 1024.0;
    Battery[1] = (analogRead(Cell_2) * 25.0) / 1024.0 - Battery[0];
    Battery[2] = (analogRead(Cell_3) * 25.0) / 1024.0 - Battery[0] - Battery[1];
    Battery[3] = (analogRead(Cell_4) * 25.0) / 1024.0 - Battery[0] - Battery[1] - Battery[2];
    
    Serial.print(Battery[0]);
    Serial.print(" V\t");
    Serial.print(Battery[1]);
    Serial.print(" V\t");
    Serial.print(Battery[2]);
    Serial.print(" V\t");
    Serial.print(Battery[3]);
    Serial.println(" V");
    delay(200);
}

/****************************************************************************/

void nRF24_test() {
    if(nRF24.available()) {// 송수신기가 정상적이면
        // 데이터 받기
        nRF24.read(&receive_data, sizeof(receive_data));
        if(strlen(receive_data) > 0) { //문자를 받으면
            Serial.print("받음 : ");
            Serial.println(receive_data);
        }
    }

    if(!digitalRead(Button)) { // 버튼1을 누르면
        nRF24.stopListening(); // 수신모드를 정지
        
        strcpy(send_data, "Main");
        
        if(nRF24.write(send_data, sizeof(send_data))) { // 데이터 보내기
            Serial.print("보냄 : ");
            Serial.println(send_data);
        }
        else Serial.println("fail");
        
        nRF24.startListening(); // 다시 수신모드
    }
    
    delay(50);
}

/****************************************************************************/

void GY_test() {
    mpu6050.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ);
    
    magX = mpu6050.getExternalSensorWord(0);
    magY = mpu6050.getExternalSensorWord(2);
    magZ = mpu6050.getExternalSensorWord(4);
    
    temperature = ms5611.readTemperature(true);
    pressure = ms5611.readPressure(true);
    altitude = ms5611.getAltitude(pressure, SEA_PRESSURE);
    
    dt = (double)(micros() - timer) / 1000000;
    timer = micros();
    
    accXangle = (atan2(accY, accZ)) * RAD_TO_DEG;
    accYangle = (atan2(accX, accZ)) * RAD_TO_DEG;
    
    gyroXrate = (double)gyroX / 131.0;
    gyroYrate = -((double)gyroY / 131.0);
    gyroXangle += gyroXrate * dt;
    gyroYangle += gyroYrate * dt;

    alpha = 1.0 / (1.0 + dt);
    
    //compAngleX = (0.93 * (compAngleX + (gyroXrate * dt))) + (0.07 * accXangle);
    //compAngleY = (0.93 * (compAngleY + (gyroYrate * dt))) + (0.07 * accYangle);

    compAngleX = (alpha * (compAngleX + (gyroXrate * dt))) + ((1.0 - alpha) * accXangle);
    compAngleY = (alpha * (compAngleY + (gyroYrate * dt))) + ((1.0 - alpha) * accYangle);
    
    kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, dt);
    kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, dt);
    
    // 가공하지 않은 데이터
    /*
    Serial.print("Ax : ");Serial.print(ax);
    Serial.print("\tAy : ");Serial.print(ay);
    Serial.print("\tAz : ");Serial.println(az);
    
    Serial.print("\t|\tGx :\t");Serial.print(gx);
    Serial.print("\tGy :\t");Serial.print(gy);
    Serial.print("\tGz :\t");Serial.print(gz);
    
    Serial.print("\t|\tMx :\t");Serial.print(mx);
    Serial.print("\tMy :\t");Serial.print(my);
    Serial.print("\tMz :\t");Serial.print(mz);
    
    Serial.print("\t|\t온도 :\t");Serial.print(temperature);
    Serial.print("\t기압 :\t");Serial.print(pressure);
    Serial.print("\t고도 :\t");Serial.println(altitude);
    */

    // 가공된 데이터
    
    Serial.print("accX : ");
    Serial.print(kalAngleX);
    Serial.print("\taccY : ");
    Serial.println(kalAngleY);
    
    //Serial.println(alpha);
    delay(100);
}

/****************************************************************************/

 

 

1. GPS_test()

GPS테스트를 실행시키면

$GPRMC,--------------
$GPVTG,-------------
$GPGGA,-------------
$GPGSA,-------------
$GPGSV,-------------
$GPGSV,-------------
$GPGSV,-------------
$GPGLL,-------------

이러한 형식으로 나오는것을 볼수 있습니다.

요기서 3번째 줄인 "$GPGGA---"를 아래에 사이트에 입력해주면 현재 위치를 알수 있습니다.

 

http://www.gonmad.co.uk/nmea.php

NMEA GPS data to GoogleMaps display

"NMEA Data Capture" 칸에 "$GPGGA---"를 넣어주고 "Filter again"을 눌러줍니다.

그리고 나서 "Mapping"을 눌러주면 아래에 위도와 경도가 나오는데 이것을 "위도,경도" 형식으로 구글지도에 검색을하면 정확한 위치가 나오게 됩니다.

 

 

2. GY_test()

GY_test()를 실행시키면 

센서로부터 받아온 값을 가공하고 이를 다시 칼만 필터에 넣어주었습니다.

 

칼만 필터란?

https://ko.wikipedia.org/wiki/%EC%B9%BC%EB%A7%8C_%ED%95%84%ED%84%B0

칼만 필터 - 위키백과, 우리 모두의 백과사전

 

 

3. nRF24_test()

송수신 테스트는 컨트롤러와 본체 둘다 있어야 가능한 테스트입니다.

본체와 컨트롤러에 전원을 넣어주고

본체의 버튼, 컨트롤러의 1번 버튼을 눌러주면 시리얼 모니터를 통해 서로 전송이 되는것을 볼수 있습니다. 

 

혹시 본 소스에대해 이해가 되지 않는 분들은 댓글 달아주세요.

Main_test.zip

0.09MB