[제어] 라즈베리파이 zero에서 드론 제작 (수정중)

[한창호]

참고매뉴얼: 만들면서 배우는 아두이노 드론 (gameplusedu.com)

1. 라즈베리파이 zero를 이용한 드론 띄우기

1) 라즈비안 OS 설치

- 최신 raspbian 다운로드  (현재: 2020-02-13, Kernel 4.19)

  2020-02-13-raspbian-buster-full.zip (https://www.raspberrypi.org/downloads/raspbian/)

- Etcher (mSD 메모리 쓰기 프로그램)을 이용해서 micro SD에 최신 raspbian을 설치한다.

  balenaEtcher-Portable-1.5.45 (https://etcher.io)

2) PC 무선 Wifi로 모바일 핫스팟 설정

시작메뉴->설정  에서 모바일 핫스팟 클릭

편집을 먼저하고 다른 디바이스와 인터넷 연결 공유를 켠다.

네트워크 연결을 보면 새로운 [로컬 영역 연결* 10] 이 생긴것을 볼 수 있다.

- 라즈베리파이가 연결되면 다음과 같이 뜬다.

* 라즈베리파이를 핫스팟 또는 무선 공유기 연결

- 다음 두 파일을 PC 윈도우 상에서 micro SD ,boot 폴더에 복사한다. 

- 라즈베리파이에 SD 카드를 꽂고 usb 전원을 인가하여 연결을 확인하면 된다.

* ssh 파일은 우클릭에서 새로만들기->텍스트 문서 로 생성한 후에 확장자를 제거한다.

* wpa_supplicant.conf 는 다음을 복사해서 붙여넣기 한다. (메모장 사용가능)

country=GB ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev update_config=1 network={         ssid="HotSpot"         psk="12341234"         key_mgmt=WPA-PSK }	ssid는  공유기 ssid 네트워크 이름 psk는  암호 priority는 숫자가 높은 것이 우선순위가 높다
network={         ssid="iptime2"         psk="mypasswd"         key_mgmt=WPA-PSK         pairwise=CCMP         group=CCMP         priority=1 }	WIFI를 하나 더 추가하면 우선순위 (priority 값) 에 따라 선택하여 연결한다. 파란색은 생략 가능하다.

- 메모장으로 확인하면 아래 쪽에 Unix(LF)로 되어 있어야 정상적으로 동작한다.

* 무선 공유기에 연결된 경우

- PC에서 네트워크는 공유기의 WIFI로 접속한다. (무선 wifie 접속)

- PC에서 공유기 웹사이트(192.168.0.1)를 통해 라즈베리파이 접속 IP를 확인한다.

3) PC에서 putty로 연결하기

- 접속->SSH->키교환-> 그룹14, 그룹1을 위로 올린다:  Putty 접속 에러[expected key exchange group packed from server] 발생시 putty설정을 암호 알고리즘 선택 정책의 순서를 다음과 같이 순서를 변경한다. 

- 창(window)->변환->UTF-8 설정 (한글설정)

- Putty로 라즈베리파이 IP로 연결한다.  (접속 IP: 192.168.0.7,    아이디: pi,  패스워드: raspberry)

- 라즈베리파이 무선 Wifi 암호코드 암호화 하기 (외부노출 방지)

login as: pi
password: raspberry
$ sudo passwd root    <- 루트 암호를 설정한다. (암호는 raspberry로 동일하게)
$ su                           <- 루트로 로그인

$ wpa_passphrase iptime mypasswd   <-  암호화된 패스워드 생성
   network={
        ssid="iptime2"
        #psk="mypasswd"
        psk=7b2453f1c01d208a5fa5e26d102b5c35a01b74301a081034b597ec4bc80a8650
  }
$ sudo nano /etc/wpa_supplicant/wpa_supplicant.conf   <- 파일 내의 psk 암호 변경
$ reboot

2. 개발환경 구축

1) VSCode 설치

 - 다운로드:  Documentation for Visual Studio Code

F1 키를 널러 Remote-SSH: Connect to Host

주소에 pi@192.168.137.119라고 치고 암호를 치고 들어가면 된다.  다만, 버전 문제로 다음과 같은 오류가 발생할 수도 있다.

-> 현재 파이제로 에서는 Remote-SSH로는 연결이 불가능하다. 따라서 Samba로 연결 후 접근해 보자.

2) Samba로 연결하기

- 파이제로에서 삼바 설치하기

$ sudo apt-get -y install samba          # 삼바 설치
$ sudo adduser pi                               # 공유계정            pi  (이미 존재 하므로 생략 가능하다)
$ sudo smbpasswd -a pi                     # 공유계정 암호    raspberry
$ sudo nano /etc/samba/smb.conf              # 설정파일

# 맨 아래 추가한다.
[share]                              # [ ] 대괄호는 섹션을 정의, 윈도우에서 접근할 때 폴더 이름
comment = samba server      # 간단한 공유 폴더 설명
path = /home/pi/Desktop      # 공유 디렉토리 경로
browsable = yes
create mask = 0770
directory mask = 0771
writable = yes
printable = no                      # 다른 사용자들도 이용 여부 설정
valid user = pi                      # 공유 디렉토리를 이용할 수 있는 사용자를 설정
guesk ok = no

$ service smbd restart
$ service smbd status

- 윈도우 PC에서 네트워크에서 우클릭하여 네트워크 드라이브 연결을 한다.

  \\192.168.137.119\share        (ip주소\세션 이름)

- 이제 VSCode로 접근이 가능하다.

3) VNC Viewer로 연결하기

- 라즈베리파이에서 VNC 설치 및 설정

$ sudo df -h                                              (용량확인)
$ sudo apt-get update
$ sudo apt-get upgrade
$ sudo apt-get autoremove
$ sudo apt-get install realvnc-vnc-server realvnc-vnc-viewer    (VNC 설치)
$ sudo apt-get install fonts-nanum                 (한글 폰트)
$ sudo apt-get install fcitx fcitx-hangul            (한글 입력기)
$ sudo im-config -n fcitx                              (한글 입력기 지정)
$ sudo raspi-config                                     (환경설정)

    5. Interfacing Options -> VNC  ->  YES
    5. Interfacing Options -> SSH  ->  YES
    7. Advanced Options ->  Resolution  ->  DMT Mode 9 (800x600) 해상도 

- PC에서 VNC Viewer로 접속한다.  (다운로드: https://www.realvnc.com/en/connect/download/viewer/)

   (id: pi,  pw: raspberry)

위 화면에서 초기설정은 Cancel 한다.

터미널에서 Ctrl + Space 를 누르면 한글/영어 입력 모드 전환

3. 드론 프로그램 작성

1) drone 소스 및 라이브러리 (wiringPi) 설치

- 소스파일 (https://goo.gl/kdSe6P)  - 소스 폴더: droneRPI

- 깃허브:  (https://github.com/WiringPi/WiringPi)

[RB071] AIR COPTER 라즈베리 파이 드론 키트.zip

4.78MB

WiringPi-master.zip

281.12KB

단계	설명
wiringPi 설치	$ wget https://t1.daumcdn.net/cfile/cafe/99B1E5425EAD32AE1A $ wget https://github.com/WiringPi/WiringPi/archive/refs/heads/master.zip $ mv 99B1E5425EAD32AE1A aircopter.zip $ sudo apt-get install zip unzip $ unzip aircopter.zip $ unzip master.zip $ cd WiringPi-master $ ./build $ gpio -v $ gpio readall $ cd ..
편집	$ nano dronRPi.cpp       또는 VSCode
컴파일	$ g++ _01_drone.cpp _02_gyro.cpp _03_balancing.cpp _04_remote.cpp _05_motor.cpp _06_print.cpp pca9685.cpp -o drone_rpi -lwiringPi
실행	$ ./drone_rpi

2) MPU6050 연결을 위한 I2C 시리얼통신 설정

라즈베리파이에서  /dev/i2c-1 을 open 하기위해 다음과 같이 설치한다.

단계	첫 번째 시도	두 번째 시도
설정방법 2가지	$ sudo raspi-config    Enable the I2C $ reboot	$ nano /boot/config.txt    dtparam=i2c_arm=on          (#을 제거한다.) $ nano /boot/cmdline.txt    bcm2708.vc_i2c_override=1   (맨 뒤에 추가한다.) $ nano /etc/modules    i2c-bcm2708                      (추가한다.)    i2c-dev $ sudo apt-get install i2c-tools libi2c-dev  (설치)
테스트	$ i2cdetect -y 1	0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f 00:          -- -- -- -- -- -- -- -- -- -- -- -- -- 10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 40: 40 -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- -- 70: -- -- -- -- -- -- -- 77

3) 블루투스 (HM-10)을 ttyS0에 연결하기

라즈베리 파이 zero에서는 PL011 UART는 BT 모듈에 연결되고, 미니 UART는 리눅스 콘솔 출력에 사용된다. 그외의 모델에서는 PL011을 리눅스 콘솔 출력에 사용한다.

/dev/ttyAMA0 (PL011 UART)이 BT 모듈에 할당된 경우, enable_uart의 기본값은 0
/dev/ttyS0 (미니 UART)가 BT 모듈에 할당된 경우, enable_uart의 기본값은 1

Unable to open serial device: No such file or directory 에러는 _04_remote.cpp에서 블루투스를 ttyS0을 연결해 사용하고자하는데서 에러가 발생한 경우이다.

$ echo "enable_uart=1" >> /boot/config.txt           (ttyS0를 BT모듈로 사용)
$ systemctl stop serial-getty@ttyS0.service
$ systemctl disable serial-getty@ttyS0.service
$ nano /boot/cmdline.txt    
    #Remove console=serial0,115200                         (ttyS0은 콘솔 사용 금지)

$ ls -l /dev | grep serial                (확인)

>> serial0 -> ttyS0

>> serial1 -> ttyAMA0

4) 드론 컨트롤러로 블루투스 (HM-10) 연결하여 테스트하기

- 스마트폰에서 드론 제어앱을 이용하여 BT로 라즈베리파이를 연결하여 에 데이터를 송수신 하는 프로그램이다.

- 또한 엔터키를 치면 HM-10을 위한 AT 명령을 입력할 수 있다.

// test_hm10.cpp
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <wiringPi.h>
#include <wiringSerial.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>

int kbhit()
{
    struct termios oldt, newt;                  /* 터미널에 대한 구조체 */
    int ch, oldf;
    tcgetattr(0, &oldt);                        /* 현재 터미널에 설정된 정보를 $
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);           /* 정규 모드 입력과 에코를 해제$
    tcsetattr(0, TCSANOW, &newt);               /* 새로 값으로 터미널을 설정한>$
    oldf = fcntl(0, F_GETFL, 0);
    fcntl(0, F_SETFL, oldf | O_NONBLOCK);       /* 입력을 논블로킹 모드로 설정>$
    ch = getchar();
    tcsetattr(0, TCSANOW, &oldt);               /* 기존의 값으로 터미널의 속성>$
    fcntl(0, F_SETFL, oldf);
    if(ch != EOF) {
        ungetc(ch, stdin);                      /* 앞에서 읽은 위치로 이전으로 $
        return 1;
    }
    return 0;
}

int main()
{
  int cnt_msg;
  int serial_port;
  char cmd[100];

  wiringPiSetup();

  if((serial_port = serialOpen("/dev/ttyS0", 115200)) < 0) {
    fprintf(stderr, "Unable to open serial device: %s\n", strerror(errno));
    return 1;
  }

  while(1) {
    // Bluetooth Serial 테스트
    if(serialDataAvail(serial_port)) {
      while(serialDataAvail(serial_port)) {
          char dat = serialGetchar(serial_port);
          printf("%c",dat);  //fflush(stdout);
          /*if(dat == '$') cnt_msg=0; else cnt_msg++;
          if(cnt_msg==4) printf("Type=%3d | ", dat);
          else if(cnt_msg==5) printf("R=%3d | ", dat);
          else if(cnt_msg==6) printf("P=%3d | ", dat);
          else if(cnt_msg==7) printf("Y=%3d | ", dat);
          else if(cnt_msg==8) printf("T=%3d", dat);*/
      }
      printf("\n");
    }

    // AT command mode for HM-10 (Enter Key 입력시)
    if(kbhit()) {
      getchar(); // remove buffer
      printf(">> AT, AT+NAME?, AT+ADDR?, AT+PASS?, AT+TYPE?, AT+BAUD? \nEnter A$
      scanf("%s",&cmd);
      int len = strlen(cmd);
      int i = 0;
      while(i<len) serialPutchar(serial_port,cmd[i++]);
      serialPutchar(serial_port,'\r');
      serialPutchar(serial_port,'\n');
      getchar();
    }
  }
  return 0;
}

* 블루투스 4.0 BLE (HM-10) 설정 하기

$ nano test_hm10.cpp                                         # 위 소스코드로 작성한다.
$ g++ test_hm10.cpp -o test_hm10 -lwiringPi         # 컴파일 한다.
$ ./test_hm10                                                    # 실행한다.

- 위 프로그램 test_hm10을 실행시키, 엔터키를 치면 HM-10을 설정할 수 있는 AT Command 입력 대기상태 (>>)가 된다.

>> AT
OK
>> AT+NAME?      (BLE 이름)
OK+NAME:AIR8523
>> AT+PASS?
OK+Get: 000000    (비밀번호)
>> AT+BAUD?
OK+Get: 4       (0: 9600, 1: 19200,  2: 38400,  3: 57600,  4: 115200,  5: 4800, 6: 2400, 7: 1200, 8: 230400)
>> AT+TYPE?
OK+Get: 0       (0: 모듈 bond 모드: Not need PIN Code)
>> AT+MODE?
OK+Get: 2       (0: Trasmission Mode,  1: PIO collection Mode + Mode 0,  2: Remote Control Mode + Mode 0)
>> AT+ROLE?
OK+Get: 0       (0: Peripheral (=Slave),  1: Central (=Master) )
>> AT+ADTY?
OK+Get: 0       (0: Advertising ScanResponse, Connectable 불루투스 페어링 모드)

5) 앱으로 연결 하기

- 다두이노 V1.0 버전 - 연결이 잘 안될 수 있다.

- 스마트폰에서 PlayStore에서 다두이노 드론 V2.0을 다운받아 설치한다.

- 아래쪽 가운데 Scan 버튼을 누른 후 connect 버튼을 눌러 연결한 후 동작시켜 보자.

6) 모터 테스트

- PCA9685 Datasheet https://www-users.cs.york.ac.uk/~pcc/Circuits/dome/datasheet/PCA9685_2.pdf

- PCA9685는 16개의 PWM 신호를 생성하여 16개의 모터를 제어할 수 있다.  (16-Channel PWM Servo Driver)

- PCA9685는 I2C 통신(1:N 통신 가능) 방식으로 SCL가 SDA를 연결한다.

- PWM 생성 방법은 LED_ON과 LED_OFF를 사용하여 PWM의 duty cycle을 조절한다.

- 하나의 LED 에는 ON_L, ON_H, OFF_L, OFF_H 로 4 byte로 구성된다. 총 16개의 LED 가 있다. (LED0~LED15)

- 예를 들어 LED_ON = 409, LED_OFF = 1228 이라면 PWM의 duty cycle은 (1228-409/4096) x 100%= 20%  이다.

- PCA9685 모드1과 모드2의 Control Registers

- LED ON/OFF,  PRE_SCALE 제어 레지스터

- 라즈베리Pi 터미널 창에서 PCA9685와 MPU6050 장치를 테스트해보자.

명령어	결과
$ i2cdetect -l	i2c 시리얼포트 검색 i2c-1 i2c        bcm2835 I2C adapter       I2C adapter
$ i2cdetect -y 1	i2c 1번 포트에 연결된 PCA9685 장치 검색      0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f 00:          -- -- -- -- -- -- -- -- -- -- -- -- -- 10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 40: 40 -- -- -- -- -- -- -- -- -- -- -- -- -- -- --       (48번은 PCA9685 장치) 50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- --       (68번은 MPU6050 장치) 70: 70 -- -- -- -- -- -- 77
$ i2cdump -y 1 0x40 (i2c 1번 포트에서 0x40(PCA9685)장치의 레지스터 읽기	No size specified (using byte-data access)      0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f    0123456789abcdef 00: 11 04 e2 e4 e8 e0 00 00 00 10 00 00 00 10 00 00    ??????...?...?.. 10: 00 10 00 00 00 10 00 00 00 10 00 00 00 10 00 00    .?...?...?...?.. 20: 00 10 00 00 00 10 00 00 00 10 00 00 00 10 00 00    .?...?...?...?.. 30: 00 10 00 00 00 10 00 00 00 10 00 00 00 10 00 00    .?...?...?...?.. 40: 00 10 00 00 00 10 XX XX XX XX XX XX XX XX XX XX    .?...?XXXXXXXXXX 50: XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX    XXXXXXXXXXXXXXXX 60: XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX    XXXXXXXXXXXXXXXX 70: XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX    XXXXXXXXXXXXXXXX 80: XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX    XXXXXXXXXXXXXXXX 90: XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX    XXXXXXXXXXXXXXXX a0: XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX    XXXXXXXXXXXXXXXX b0: XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX    XXXXXXXXXXXXXXXX c0: XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX    XXXXXXXXXXXXXXXX d0: XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX    XXXXXXXXXXXXXXXX e0: XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX    XXXXXXXXXXXXXXXX f0: XX XX XX XX XX XX XX XX XX XX 00 00 00 00 1e 00    XXXXXXXXXX....?.
$ i2cget -y 1 0x40 0x00	(i2c 1번 포트에서 0x40(PCA9685) 장치의 0x00 레지스터 값 읽기) 0x11
$ i2cget -y 1 0x68 0x75	(i2c 1번 포트에서 0x68(MPU6050) 장치의 0x75 레지스터 값 읽기) 0x68                                          (MPU6050 디바이스 ID 값)

- 다음 구동 소스를 통해 4개의 모터를 정/역회전, 속도를 변경시켜보자.

- 모터 구동을 하기 위해 배터리 전원을 연결한다. 스위치는 왼쪽으로 밀어야 배터리 전원이 ON 된다.

test_motor.cpp

#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

// PCA9685 register
#define MODE1             0x00
#define PRE_SCALE      0xFE
#define LED0_ON_L       0x06     (LED0 에는 ON_L, ON_H, OFF_L, OFF_H 4 byte가 있다.  LED0~ LED15)
#define LED0_OFF_L      0x08
#define ALLCALL            0x01     (전체 호출, i2c 조소1에 대한 pca9685 응답)
#define SLEEP               0x10     (Oscillator turn off)
#define AI                      0x20     (Auto Increment Register)
#define RESTART          0x80     (Restart signal)

int i2c_addr = 0x40;       (PCA9685 디바이스 주소)
int i2c_port = 0;
int pin[4]= { 0, 2, 1, 3 };  // motor id

int main(void)
{
  // setup i2c 통신
  wiringPiSetup();
  if((i2c_port = wiringPiI2CSetup(i2c_addr)) < 0) {
    fprintf(stderr, "Unable to open pca9685 device: %s\n", strerror(errno));
    return 1;
  }

  // setup PCA9685
  int freq = 1000;
  int prescale = (int)(25000000.0f / ( 4096 * freq));
  wiringPiI2CWriteReg8(i2c_port, MODE1, ALLCALL | AI | SLEEP);
  wiringPiI2CWriteReg8(i2c_port, PRE_SCALE, prescale);  // 프리스케일 설정시 SLEEP으로 잠시 멈춘다.
  wiringPiI2CWriteReg8(i2c_port, MODE1, ALLCALL | AI | RESTART);

  // run motor
  for(int i=0;i<4;i++) {   // PWM은 LED0~LED15로 16개의 모터를 제어하며, 4byte로 duty cycle을 구성한다.
    wiringPiI2CWriteReg16(i2c_port, LED0_OFF_L+pin[i]*4, (4096/10) & 0x1FFF);
    delay(1000);
  }

  // stop motor
  for(int i=0;i<4;i++)
    wiringPiI2CWriteReg16(i2c_port, LED0_OFF_L+pin[i]*4, 0);
}

$ g++ test_motor.cpp -o test_motor -lwiringPi

$ ./test_motor

7) MPU6050 테스트

- 참고: [MPU6050] 2. 각도값 계산 및 진동 제거하기 (tistory.com)

- 배선연결은 SCL과 SDA를 라즈베리파이의 i2c통신인 SCL/SDA에 각각 연결한다. PCA9685와 같이 연결한다.

- 자이로 설정:  Full Scale Range는 2000도/Sec로 설정하면 된다. (GYRO_CONFIG)

- LPF를 설정하여 노이즈를 줄일 수 있다.  (DLPF_CFG)

- MPU6050 자이로/가속도 센서로부터 데이터를 얻어보자. (다음 소스는 C 코드이다.)

// test_mpu6050.cpp
#include <stdio.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

#define MPU6050_ADDR    0x68
#define PWR_MGMT_1      0x6B
#define MPU6050_ACCEL_XOUT_H    0x3B
#define MPU6050_GYRO_XOUT_H     0x43
#define PWR_MGMT_1      0x6B
#define PWR_MGMT_2      0x6C
#define DLPF_CFG        0x1A
#define GYRO_CONFIG     0x1B
#define ACCEL_CONFIG    0x1C

#define ACCEL_SCALE_MODIFIER_2G         16384.0
#define ACCEL_SCALE_MODIFIER_4G         8192.0
#define ACCEL_SCALE_MODIFIER_8G         4096.0
#define ACCEL_SCALE_MODIFIER_16G        2048.0

#define GYRO_SCALE_MODIFIER_250DEG      131.0
#define GYRO_SCALE_MODIFIER_500DEG      65.5
#define GYRO_SCALE_MODIFIER_1000DEG     32.8
#define GYRO_SCALE_MODIFIER_2000DEG     16.4

#define ACCEL_RANGE_2G  0x00
#define ACCEL_RANGE_4G  0x08
#define ACCEL_RANGE_8G  0x10
#define ACCEL_RANGE_16G 0x18

#define GYRO_RANGE_250DEG       0x00
#define GYRO_RANGE_500DEG       0x08
#define GYRO_RANGE_1000DEG      0x10
#define GYRO_RANGE_2000DEG      0x18

#define RAD_TO_DEG              57.295779513082

 int main()
 {
   short gX,gY,gZ;
   int port;

   if (port = open("/dev/i2c-1", O_RDWR)) < 0) {
     printf("Unable to open i2c-1 device: %s\n", strerror(errno));
     exit(-1);
   }
   printf("Connected to i2c-1\n");

   wiringPiSetup();

   ioctl(i2c_port, I2C_SLAVE, MPU6050_ADDR);
   wiringPiI2CWriteReg8(i2c_port, PWR_MGMT_1, 0x80);   // PWR_MGMT_1 -- DEVICE_RESET 1
   delay(5);

   wiringPiI2CWriteReg8(port, DLPF_CFG, 0x03);         // DLPF_CFG -- 0x01: 2ms, 0x03: Acc 4.9ms, Gyro 4.8ms
   wiringPiI2CWriteReg8(port, GYRO_CONFIG, GYRO_RANGE_2000DEG);  // 초당 2000 deg를 분석한다.
   wiringPiI2CWriteReg8(port, ACCEL_CONFIG, ACCEL_RANGE_8G);
   wiringPiI2CWriteReg8(port, PWR_MGMT_1, 0x03);   // PWR_MGMT_1 -- DEVICE RESET 0; SLEEP 0; CHECK, 시작
   printf("MPU6050 is initialized.\n");

   while(1) {
     gX = (wiringPiI2CReadReg8(port, 0x43) & 0xFF) <<8  | (wiringPiI2CReadReg8(port, 0x44) & 0xFF);
     gY = (wiringPiI2CReadReg8(port, 0x45) & 0xFF) <<8  | (wiringPiI2CReadReg8(port, 0x46) & 0xFF);
     gZ = (wiringPiI2CReadReg8(port, 0x47) & 0xFF) <<8  | (wiringPiI2CReadReg8(port, 0x48) & 0xFF);

    aY = ( wiringPiI2CReadReg8(port, 0x3B) << 8  | wiringPiI2CReadReg8(i2c_port, 0x3C) ) >> 2;
    aX = ( wiringPiI2CReadReg8(port, 0x3D) << 8  | wiringPiI2CReadReg8(i2c_port, 0x3E) ) >> 2;
    aZ = ( wiringPiI2CReadReg8(port, 0x3F) << 8  | wiringPiI2CReadReg8(i2c_port, 0x40) ) >> 2;

     printf("aX=%6d | aY=%6d | aZ=%6d \t gX=%6d | gY=%6d | gZ=%6d\n", aX, aY, aZ, gX,gY,gZ);
   }
 }

$ g++ test_mpu6050.cpp -o test_mpu6050 -lwiringPi

$ ./test_mpu_6050

- MPU6050 센서를 움직였을 때의 결과값이다. 차후 0으로 보정해야 한다.

수평일 때	x축으로 움직였을 때	z축으로 움직였을 때
gX=    35 | gY=   -57 | gZ=   -62 gX=    24 | gY=   -55 | gZ=  -103 gX=    30 | gY=   -56 | gZ=   -50 gX=    44 | gY=   -44 | gZ=   -88 gX=    18 | gY=   -63 | gZ=   -71 gX=    40 | gY=   -68 | gZ=   -88 gX=    39 | gY=   -50 | gZ=   -74	gX= -2774 | gY=   -41 | gZ=   244 gX= -2765 | gY=   -38 | gZ=   290 gX= -2765 | gY=   -68 | gZ=   287 gX= -2774 | gY=   -31 | gZ=   319 gX= -2778 | gY=    -4 | gZ=   316 gX= -2790 | gY=    -7 | gZ=   333 gX= -2806 | gY=   -21 | gZ=   341	gX=    78 | gY=   -75 | gZ=  -819 gX=    75 | gY=   -46 | gZ=  -927 gX=   244 | gY=   393 | gZ= -1216 gX=   -70 | gY=  -109 | gZ= -1628 gX=   169 | gY=  -200 | gZ= -1960 gX=    36 | gY=  -103 | gZ= -2384 gX=   137 | gY=   -75 | gZ= -2594

- 각도 구하기

가속도와 자이로 센서로부터 얻은 데이터로 각도를 구해보자. 상보필터를 통해 자이로센서의 누적오차를 보정할 수 있다.

- 자이로 값으로 각도를 계산하는 코드이다. 상보필터를 사용해서 보정한다.

double xx = ((double)acc_raw[X] * (double)acc_raw[X]);
double yy = ((double)acc_raw[Y] * (double)acc_raw[Y]);
double zz = ((double)acc_raw[Z] * (double)acc_raw[Z]);

// accelation degree
acc_deg[PITCH]= -atan2(acc_raw[X],sqrt(yy+zz)) * RAD_TO_DEG;
acc_deg[ROLL] = -atan2(acc_raw[Y],sqrt(xx+zz)) * RAD_TO_DEG;
acc_deg[YAW]  = 0;

// gyro rate
gyro_rate[PITCH]= (double)gyro_raw[X] / GYRO_SCALE_MODIFIER_500DEG;
gyro_rate[ROLL] = (double)gyro_raw[Y] / GYRO_SCALE_MODIFIER_500DEG;
gyro_rate[YAW]  = (double)gyro_raw[Z] / GYRO_SCALE_MODIFIER_500DEG;

dt.t_now = micros();

dt.t_period = (dt.t_now - dt.t_prev) / 1000000.0;
dt.t_prev = dt.t_now;

// Calc angle using complimentary filter (상보필터)
comp_angle[PITCH] = 0.93*(comp_angle[PITCH]+ gyro_rate[PITCH] * dt.t_period) $
comp_angle[ROLL]  = 0.93*(comp_angle[ROLL] + gyro_rate[ROLL]  * dt.t_period) $
comp_angle[YAW]  += gyro_rate[YAW] * dt.t_period;

8) PID 제어

- 드론이 제대로 이륙을 못할 경우 PID 값을 수정해보자.

- PID 기초: https://m.blog.naver.com/lagrange0115/220616818649

결과 창을 살펴보자

- ACC(PRY): 각속도 (Acceleration)가 오차가 발생하는 것을 보여준다. 안정적일 필요가 있다.

- GyroRate: 자이로 값으로 0에 가만히 있어야 한다.

- PID1: 각속도에 대한 PID 제어 값이 적용된 term 값들이다.

- PID2: 자이로에 대한 PID 제어 값이 적용된 term 값들이다.

- Motor(ABCD): 모터에 전달되는 최종 값이다.

- th: throttle 값으로 컨트롤 앱에서 속도를 올리면 모터가 빠르게 동작한다.

- dt: 시간 간격이다.  미분 시간

9) 최종 테스트

- 보드 스위치를 왼쪽을 밀어서 배터리 전원 (ON) 을 사용하며, USB 전원을 제거한다.

- putty 또는 VNC Viewer를 통해 라즈베리파이에 연결하고, 터미널창에서  ./drone_rpi 를 실행한다.

- 드론 컨트롤러 앱을 이용해서 블루투스를 연결하고, 드론을 동작 시킨다.

* 배터리 문제

  - 한 동안 드론이 바닥에 붙어 뜨질 않아서 소스코드 문제인줄 알았는데 결국 배터리 문제로 판정 났다. 충전시 전압이 4.3v 는 되야 드론이 뜬다.

비교	불량 배터리	정상 배터리
모양	빵빵하게 부풀어 올러 곧 터질 것 같다.	납작하다.
전압	완충시 4.0v 이하  방전시 3.7v 이하	완충시 4.3v,  방전시 3.7v

 - 불량/정상 배터리 차이 영상이다.

* 원격 파일 복사

PC에서 파일을 가져오려면 다음과 같이 cmd 창에서 scp 명령어를 이용해 가져올 수 있다.

>> scp pi@192.168.0.3:/home/pi/Desktop/droneRPi/droneRPi.cpp .

scp [IP주소]:폴더/파일 [저장폴더]

* 수정중인 파일

- 하나의 파일로 만들어 보았다. 현재 테스트 중

droneRPi_230831.cpp

16.55KB