참고매뉴얼: 만들면서 배우는 아두이노 드론 (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)
| 단계 | 설명 |
| 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주소]:폴더/파일 [저장폴더]
* 수정중인 파일
- 하나의 파일로 만들어 보았다. 현재 테스트 중