[제어] 라즈베리파이 zero 드론(RB071) 제작

[한창호]

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

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

* 개발환경 설정

단계	설명
우분투 설치	* Raspberry Pi Imager 설치 프로그램 다운로드: https://www.raspberrypi.com/software/  - SD카드를 microSD Adapter에 꽂아서 PC에 연결하고, Imager를 실행한다. * Raspberry Pi Imager 설정하고 굽기    -----------------------------------------------------    * 디바이스 선택       -> Raspberry Pi Zero    * 운영체제 선택       -> Raspberry Pi OS (other)  -> Raspberry Pi OS Lite (32-bit) 선택    * 저장소 선택       -> USB Device 16.0GB (SD카드)    * 사용자 지정       -> 호스트 이름:  rpi-drone       -> 로컬화:  수도 서울, 시간대: Asia/Seoul, 키보드 kr       -> 사용자이름: user, 비밀번호: 1234        -> WiFi :  SSID:  HotSpot00,  비밀번호: 12341234       -> 원격접근: SSH 인증: 비밀번호 인증 사용 선택       -> 라즈베리 파이 커넥트: 비활성화    -----------------------------------------------------
모바일 핫스팟 설정	* 컴퓨터에 무선 LAN 연결하기 (예, ipTIME A3000mini) * 시작메뉴->설정  에서 모바일 핫스팟 클릭 -> 편집 ->  다른 디바이스와 인터넷 연결 공유를 켠다. * 라즈베리파이가 연결되면 접속한 IP 주소가 보인다.
또는 무선 공유기에 연결	- PC에서 네트워크는 공유기의 WIFI로 접속한다. (무선 wifie 접속) - PC에서 공유기 웹사이트(192.168.0.1)를 통해 라즈베리파이 접속 IP를 확인한다.
putty 접속	* 드론 전원 연결하기 * Putty로 라즈베리파이에 연결한다.  (접속 IP: 192.168.0.7,    아이디: user,  패스워드: 1234)   - 창(window)->변환->UTF-8 설정 (한글설정)

3. 드론 프로그램 작성

* 소스코드

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

4.78MB

WiringPi-master.zip

281.12KB

* drone 소스 및 라이브러리 (wiringPi) 설치, 그리고 구동하기

단계	설명
소스코드	- 소스파일 (https://goo.gl/kdSe6P)  - 소스 폴더: droneRPI - 깃허브:  (https://github.com/WiringPi/WiringPi)
I2C 모듈 활성화	라즈베리파이에서  /dev/i2c-1 을 open 하기위해 다음과 같이 설정한다. 방법1) $ sudo raspi-config 3 Interface Options -> I5 I2C -> Would you like the ARM I2C interface to be enabled? -> Yes 선택 -> Finish $ sudo apt-get install i2c-tools libi2c-dev -y 방법2) $ nano /boot/config.txt    dtparam=i2c_arm=on          (#을 제거한다.) $ nano /boot/cmdline.txt    bcm2708.vc_i2c_override=1   (맨 뒤에 추가한다.) $ nano /etc/modules    i2c-bcm2708                      (추가한다.)    i2c-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
HM10통신설정	라즈베리 파이 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을 연결해 사용하고자하는데서 에러가 발생한 경우이다. 방법1)  $ sudo raspi-config 3 Interface Options -> I6 Serial Port -> Would you like a login shell to be accessible over serial? -> No 선택 -> Would you like the serial port hardware to be enabled?  -> Yes 선택 -> Finish -> Reboot Yes 선택 방법2) $ 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
소스코드 설치 및 실행	다음 코드를 모두 복사해서 putty 창에서 마우스 우클릭으로 붙여넣기를 하여 한번에 실행한다. sudo apt update sudo apt upgrade sudo apt-get install zip unzip # download source wget https://t1.daumcdn.net/cfile/cafe/99B1E5425EAD32AE1A wget https://github.com/WiringPi/WiringPi/archive/refs/heads/master.zip # 코드 풀기 mv 99B1E5425EAD32AE1A aircopter.zip unzip aircopter.zip unzip master.zip # wire install cd WiringPi-master ./build gpio -v gpio readall cd $ cd droneRPI # 파일 리스트 $ ls _00_drone.h    _02_gyro.cpp       _04_remote.cpp  _06_print.cpp  pca9685.h _01_drone.cpp  _03_balancing.cpp  _05_motor.cpp   pca9685.cpp # 코드 편집 $ nano _01_drone.cpp # 컴파일 $ 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 # 드론 컨트롤러 앱을 이용해서 블루투스를 연결하고, 드론을 동작 시킨다. 부팅시 바로 실행하려면 $ echo 'droneRPI/done_rpi
부팅시 드론 실행	# 서비스파일 생성 $ sudo nano /etc/systemd/system/drone_rpi.service # 다음 코드를 모두 복사붙여넣기 한다. [Unit] Description=Drone RPI After=network.target [Service] Type=simple ExecStart=/home/user/droneRPI/drone_rpi Restart=always RestartSec=2 User=pi [Install] WantedBy=multi-user.target # 서비스 등록 $ sudo systemctl daemon-reload $ sudo systemctl enable drone_rpi.service # 재부팅후 동작 확인 $ sudo reboot $ systemctl status drone_rpi.service
WiFi 절전 모드	WiFi 절전 모드 끄기 # 확인 $ iwconfig wlan0 wlan0     IEEE 802.11  ESSID:"HotSpot00"           Mode:Managed  Frequency:2.462 GHz  Access Point: B0:38:6C:F8:0E:78           Bit Rate=65 Mb/s   Tx-Power=31 dBm           Retry short limit:7   RTS thr:off   Fragment thr:off           Power Management:on           Link Quality=70/70  Signal level=-34 dBm           Rx invalid nwid:0  Rx invalid crypt:0  Rx invalid frag:0           Tx excessive retries:3  Invalid misc:0   Missed beacon:0 # 절전모드 끄기 설정 $ sudo nano /etc/NetworkManager/conf.d/wifi-powersave.conf ------------ [connection] wifi.powersave = 2 ------------ # 재시작 (WiFi가 끊겼다가 다시 연결된다.) $ sudo systemctl restart NetworkManager # 확인 $ iwconfig wlan0 -> Power Management:on:off
SSH로 VSCode 연결 -> 실패	# VSCode에서 Remote - SSH Extension 설치 # Connect to Host... > + Add New SSH Host... 입력창에 ssh user@192.168.137.188 -A  다시 하단 >< 아이콘을 누르고 Connect To Host로 가면 다음과 같이 입력한 이름이 뜬다. 그리고 Linux를 선택하면 VSCode에서 연결을 시도한다. [UnsupportedArch]: Error: UnsupportedArch (The remote host's architecture is not supported) 위와 같은 에러가 발생하는 경우는 VSCode가 라즈베리파이 zero (OS 32bit)를 지원하지 않기 때문이다. 라즈베리파이 Zero W의 CPU가 ARMv6(ARM11 코어) 아키텍처를 사용한다. VSCode는 지원을 중단했다. 방법은 SSH Remote Extensions 버전을 0.107 이하 버전으로 설치해본다.
Samba로 VS Code 연결 -> 성공	# 삼바 설치 $ sudo apt update $ sudo apt install samba samba-common-bin -y # 공유할 폴더 설정 추가 $ sudo nano /etc/samba/smb.conf                 # 다음 코드를 맨 아래 복사해서 붙여넣기 한다. [PiShare] comment = Raspberry Pi Share path = /home/user browseable = yes writeable = yes guest ok = no create mask = 0777 directory mask = 0777 $ sudo smbpasswd -a user                            # 원하는 삼바 접속 비밀번호 설정 $ sudo systemctl restart smbd # 윈도우 파일 탐색기에서 네트워크 드라이버 연결하기 \\192.168.137.188\PiShare # VSCode에서 파일 열기

1) 드론 컨트롤러로 블루투스 (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 불루투스 페어링 모드)

* 앱으로 연결 하기

- 다두이노 V1.0 버전 (구 버전)

- PlayStore에서 다두이노 드론 V3.0을 다운받아 설치한다.

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

2) 모터 테스트

- 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

3) 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;

4) PID 제어

- 드론이 제대로 이륙을 못할 경우 다음과 같은 값들을 체크해보자.

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

구분	설명
체크사항	결과값을 살펴보자 * ACC(PRY): 각속도 (Acceleration)  - 드론을 제어할 때는 사용하지 않는다.  - 가만히 있을 때 오차가 발생하는 것이 보이면, 좀 더 안정적일 필요가 있다. * GyroRate: 자이로  - 가만히 있을 때는 0 값이 된다. * PID: 각속도에 대한 PID 제어 값이 적용된 term 값들이다.  - Pitch, Roll, Yaw는 가만히 있을 때는 0에 된다. * Motor(ABCD): 모터에 전달되는 최종 값이다.  - Throtle 값을 최대로 올리면 255까지 올라가는지 확인한다. 이때 모든 A,B,C,D 값이 올라가야 한다. * dt: 수행 시간 간격이다.  - 여기서는 7ms 속도가 난다.
실행결과	$ nano ~/droneRPI/_01_drone.cpp 위 소스코드에서 print() 관련 함수는 주석을 제거한다. 그리고 print("\n")을 붙여주면 다음 그림과 같이 출력된다.

* 배터리 문제

  - 한 동안 드론이 바닥에 붙어 뜨질 않아서 소스코드 문제인줄 알았는데 결국 배터리 문제로 판정 났다. 충전시 전압이 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주소]:폴더/파일 [저장폴더]

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

단계	설명
파일	$ cd droneRPI $ nano drone_all.cpp
소스코드	#include <sys/types.h>      // open #include <sys/stat.h>      // open #include <fcntl.h>      // open #include <stdio.h>      // stderr #include <errno.h>      // errno #include <string.h>      // strerror #include <stdlib.h>      // exit #include <sys/ioctl.h>      // ioctl #include <stdint.h>      // uint8_t #include <wiringPi.h> #include <wiringPiI2C.h> #include <wiringSerial.h> #define I2C_SLAVE      0x0703 typedef struct { int port; } i2c_t;//1 typedef struct { int i2c_addr, PWR_MGMT_1, GYRO_XOUT_H, i2c_port; } mpu6050_t;//1 typedef struct { int16_t x, y, z; } gyro_raw_t;//1 typedef struct { int16_t x, y, z; } gyro_offset_t;//2 typedef struct { int16_t x, y, z; } gyro_adj_t;//3 typedef struct { double roll, pitch, yaw; } gyro_rate_t;//4 typedef struct { unsigned long t_prev, t_now; double t_period; } dt_t;//5 typedef struct { double roll, pitch, yaw; } gyro_angle_t;//6 typedef struct { double roll, pitch, yaw; } target_angle_t;//7 typedef struct { double roll, pitch, yaw; } balancing_force_t;//7 typedef struct { double value; } throttle_t;//8 typedef struct { double a, b, c, d; } motor_speed_t;//8 typedef struct { int serial_port; } hm10_t;//9 typedef struct { int i2c_addr, i2c_port; } pca9685_t; typedef struct { int a, b, c, d; } motor_t;//10 #define MODE1      0x00 #define PRE_SCALE      0xFE #define LED0_ON_L      0x06 #define LED0_OFF_L      0x08 #define AI      0x20 #define SLEEP      0x10 #define RESTART      0x80 i2c_t i2c;//1 mpu6050_t mpu6050 = {   .i2c_addr = 0x68,   .PWR_MGMT_1 = 0x6b,   .GYRO_XOUT_H = 0x43, };//1 gyro_raw_t gyro_raw;//1 gyro_offset_t gyro_offset;//2 gyro_adj_t gyro_adj;//3 gyro_rate_t gyro_rate;//4 dt_t dt;//5 gyro_angle_t gyro_angle;//6 target_angle_t target_angle;//7 balancing_force_t balancing_force;//7 throttle_t throttle = { .value = 0, };//8 motor_speed_t motor_speed;//8 hm10_t hm10;//9 motor_t motor = { .a = 0, .b = 2, .c = 1, .d = 3, };//10 pca9685_t pca9685 = { .i2c_addr = 0x40, };//10 // -------------------------- //   MPU 6050 설정 // -------------------------- void init_mpu6050(i2c_t& i2c, mpu6050_t& mpu6050) {       i2c.port = open("/dev/i2c-1", O_RDWR);       if(i2c.port < 0) {             fprintf (stderr,                    "Unable to open i2c-1 device: %s\n",                    strerror (errno));             exit(-1);       }       mpu6050.i2c_port = i2c.port;       ioctl(mpu6050.i2c_port, I2C_SLAVE, mpu6050.i2c_addr);       wiringPiI2CWriteReg8(mpu6050.i2c_port, mpu6050.PWR_MGMT_1, 0);             }//1 void read(       mpu6050_t& mpu6050,        gyro_raw_t& gyro_raw) {                    const int I2C_PORT = mpu6050.i2c_port;       const int GYRO_XH = mpu6050.GYRO_XOUT_H;                    ioctl(I2C_PORT, I2C_SLAVE, mpu6050.i2c_addr);              gyro_raw.x=(wiringPiI2CReadReg8(I2C_PORT, GYRO_XH+0 )&0xFF)<<8;       gyro_raw.x|=wiringPiI2CReadReg8(I2C_PORT, GYRO_XH+1 )&0xFF;       gyro_raw.y=(wiringPiI2CReadReg8(I2C_PORT, GYRO_XH+2 )&0xFF)<<8;       gyro_raw.y|=wiringPiI2CReadReg8(I2C_PORT, GYRO_XH+3 )&0xFF;       gyro_raw.z=(wiringPiI2CReadReg8(I2C_PORT, GYRO_XH+4 )&0xFF)<<8;       gyro_raw.z|=wiringPiI2CReadReg8(I2C_PORT, GYRO_XH+5 )&0xFF;             }//1 #define NSAMPLES 1000//2 void get_offset(       mpu6050_t& mpu6050,        gyro_offset_t& gyro_offset) {                    gyro_raw_t gyro_raw;       int32_t sumGyX = 0, sumGyY = 0, sumGyZ = 0;              for(int i=0;i<NSAMPLES;i++) {             read(mpu6050, gyro_raw);                          sumGyX += gyro_raw.x;               sumGyY += gyro_raw.y;               sumGyZ += gyro_raw.z;                          delay(1);       }              gyro_offset.x = (double)sumGyX/NSAMPLES;         gyro_offset.y = (double)sumGyY/NSAMPLES;         gyro_offset.z = (double)sumGyZ/NSAMPLES; }//2 // -------------------------- //   pca9685 설정 (모터 ESC제어) // -------------------------- void setupAI(pca9685_t& pca9685) {              const int I2C_PORT = pca9685.i2c_port;       int mode1;       mode1 = wiringPiI2CReadReg8(I2C_PORT, MODE1)&0xFF;       wiringPiI2CWriteReg8(I2C_PORT, MODE1, mode1|AI); } void setFreq(pca9685_t& pca9685, int frequency) {              const int I2C_PORT = pca9685.i2c_port;       int mode1;       int prescale;       mode1 = wiringPiI2CReadReg8(I2C_PORT, MODE1)&0xFF;       wiringPiI2CWriteReg8(I2C_PORT, MODE1, mode1|SLEEP);       prescale = (int)(25000000.0f/(4096*frequency));       wiringPiI2CWriteReg8(I2C_PORT, PRE_SCALE, prescale);       mode1 = wiringPiI2CReadReg8(I2C_PORT, MODE1)&0xFF;       wiringPiI2CWriteReg8(I2C_PORT, MODE1, mode1&~SLEEP);       delay(1);       mode1 = wiringPiI2CReadReg8(I2C_PORT, MODE1)&0xFF;       wiringPiI2CWriteReg8(I2C_PORT, MODE1, mode1|RESTART); } void setDuty(pca9685_t& pca9685, const int pin, const int duty_cycle) {              const int I2C_PORT = pca9685.i2c_port;       const int chan = pin*4;       const int duty_off = duty_cycle&0x1FFF;              wiringPiI2CWriteReg16(I2C_PORT, LED0_OFF_L+chan, duty_off); } void init_pca9685(       i2c_t& i2c,        pca9685_t& pca9685) {                          pca9685.i2c_port = i2c.port;             ioctl(pca9685.i2c_port, I2C_SLAVE, pca9685.i2c_addr);       setupAI(pca9685);             setFreq(pca9685, 1000); }//10 void run_motor(       pca9685_t& pca9685,        motor_t& motor,        motor_speed_t& motor_speed) {              int duty_cycle_a = (int)(motor_speed.a*4095.0/250);       int duty_cycle_b = (int)(motor_speed.b*4095.0/250);       int duty_cycle_c = (int)(motor_speed.c*4095.0/250);       int duty_cycle_d = (int)(motor_speed.d*4095.0/250);                    ioctl(pca9685.i2c_port, I2C_SLAVE, pca9685.i2c_addr);                    setDuty(pca9685, motor.a, duty_cycle_a);       setDuty(pca9685, motor.b, duty_cycle_b);       setDuty(pca9685, motor.c, duty_cycle_c);       setDuty(pca9685, motor.d, duty_cycle_d); }//10       // -------------------------- //  자이로 센서값 얻기 // -------------------------- void calc_gyro(       gyro_angle_t& gyro_angle,       gyro_rate_t& gyro_rate,       gyro_adj_t& gyro_adj,        gyro_raw_t& gyro_raw,        gyro_offset_t& gyro_offset,       dt_t& dt) {              // adjust to zero       gyro_adj.x = gyro_raw.x - gyro_offset.x;       gyro_adj.y = gyro_raw.y - gyro_offset.y;       gyro_adj.z = gyro_raw.z - gyro_offset.z;             // calc gyro       gyro_rate.roll  = gyro_adj.y/131.0;       gyro_rate.pitch = gyro_adj.x/131.0;       gyro_rate.yaw   = gyro_adj.z/131.0;       // calc angle       gyro_angle.roll  += gyro_rate.roll  * dt.t_period;       gyro_angle.pitch += gyro_rate.pitch * dt.t_period;       gyro_angle.yaw   += gyro_rate.yaw   * dt.t_period;          extern throttle_t throttle;//9     if(throttle.value==0) {         gyro_angle.pitch = 0;         gyro_angle.roll = 0;         gyro_angle.yaw = 0;     }//9 }//3 void calc_dt(dt_t& dt)  {         dt.t_now = micros();   dt.t_period = (dt.t_now - dt.t_prev)/1000000.0;   dt.t_prev = dt.t_now; }//5  // -------------------------- //  PID 계산 // -------------------------- double Kp = 1.3; double Ki = 1.0; double Kd = 1.0; /*  conf.Kp[ROLL]  = 1.94;    conf.Ki[ROLL]  = 0.0037;    conf.Kd[ROLL]  = 0.36;   conf.Kp[PITCH] = 1.94;    conf.Ki[PITCH] = 0.0037;    conf.Kd[PITCH] = 0.36;   conf.Kp[YAW]   = 0.94;    conf.Ki[YAW]   = 0.0055;    conf.Kd[YAW]   = 0; */ // 5. 함수 정의 void calc_pid(     balancing_force_t& bf, target_angle_t& target_angle,      gyro_angle_t& gyro_angle, gyro_rate_t& gyro_rate, dt_t& dt) {        double angle_error_roll  = target_angle.roll  - gyro_angle.roll;     double angle_error_pitch = target_angle.pitch - gyro_angle.pitch;     double angle_error_yaw   = target_angle.yaw   - gyro_angle.yaw;     bf.roll   = Kp * angle_error_roll ;     bf.pitch  = Kp * angle_error_pitch;     bf.yaw    = Kp/2.0 * angle_error_yaw;     bf.roll  += Ki * (-gyro_rate.roll);     bf.pitch += Ki* (-gyro_rate.pitch);     bf.yaw   += Ki * (-gyro_rate.yaw);     static double res_force_pitch;     static double res_force_roll;     static double res_force_yaw;     extern throttle_t throttle;     res_force_pitch += Kd * angle_error_pitch * dt.t_period;     res_force_roll  += Kd * angle_error_roll * dt.t_period;     res_force_yaw   += 0 * angle_error_yaw * dt.t_period;          if(throttle.value==0) {         res_force_pitch = res_force_roll = res_force_yaw = 0;     }          bf.roll += res_force_roll;     bf.pitch += res_force_pitch;     bf.yaw += res_force_yaw; }//7 void calc_motor(motor_speed_t& motor_speed, throttle_t& throttle, balancing_force_t& bf) {                          motor_speed.a = throttle.value + bf.roll + bf.pitch + bf.yaw;       motor_speed.b = throttle.value - bf.roll + bf.pitch - bf.yaw;       motor_speed.c = throttle.value - bf.roll - bf.pitch + bf.yaw;       motor_speed.d = throttle.value + bf.roll - bf.pitch - bf.yaw;              if(motor_speed.a < 0)  motor_speed.a = 0;       if(motor_speed.a > 250)  motor_speed.a = 250;       if(motor_speed.b < 0)  motor_speed.b = 0;        if(motor_speed.b > 250)  motor_speed.b = 250;       if(motor_speed.c < 0)  motor_speed.c = 0;        if(motor_speed.c > 250)  motor_speed.c = 250;       if(motor_speed.d < 0)  motor_speed.d = 0;        if(motor_speed.d > 250)  motor_speed.d = 250;              if(throttle.value==0) {             motor_speed.a = motor_speed.b = motor_speed.c = motor_speed.d = 0;       }//9 }//8       // -------------------------- //   블루투스 통신 HM10 // -------------------------- void init_hm10(hm10_t& hm10)  {       hm10.serial_port = serialOpen ("/dev/ttyS0", 115200);       if (hm10.serial_port < 0) {             fprintf (stderr,                    "Unable to open serial device: %s\n",                    strerror (errno));             exit(-1);       } }//9 void read_serial_data(       hm10_t& hm10,        throttle_t& throttle,        target_angle_t& target_angle) {                    static int cntMsg;              if(serialDataAvail (hm10.serial_port)) {             while(serialDataAvail (hm10.serial_port)) {                   char msp_data = serialGetchar (hm10.serial_port);                    if(msp_data == '$') cntMsg = 0;                   else cntMsg++;                       if(cntMsg == 8) throttle.value = msp_data;                   else if(cntMsg == 7) target_angle.yaw = -(msp_data-125);//12                   else if(cntMsg == 6) target_angle.pitch = -(msp_data-125);//12                   else if(cntMsg == 5) target_angle.roll = (msp_data-125);//12                   #define ANGLE_MAX   30                   if(target_angle.roll < -ANGLE_MAX) target_angle.roll = -ANGLE_MAX;                   else if(target_angle.roll > ANGLE_MAX) target_angle.roll = ANGLE_MAX;                   if(target_angle.pitch < -ANGLE_MAX) target_angle.pitch = -ANGLE_MAX;                   else if(target_angle.pitch > ANGLE_MAX) target_angle.pitch = ANGLE_MAX;                                if(target_angle.yaw < -ANGLE_MAX) target_angle.yaw = -ANGLE_MAX;                   else if(target_angle.yaw > ANGLE_MAX) target_angle.yaw = ANGLE_MAX;             }       } }//9             int main() {              wiringPiSetup();//1                   init_mpu6050(i2c, mpu6050);//1             get_offset(mpu6050, gyro_offset);//2             dt.t_prev = micros();       init_hm10(hm10);//9       init_pca9685(i2c, pca9685);//10              while(true) {                          read(mpu6050, gyro_raw);//1                         read_serial_data(hm10, throttle, target_angle);//9                         calc_dt(dt);//5             calc_gyro(gyro_angle, gyro_rate, gyro_adj, gyro_raw, gyro_offset, dt);//3             calc_pid(balancing_force, target_angle, gyro_angle,gyro_rate,dt);//7             calc_motor(motor_speed, throttle, balancing_force);//8                         run_motor(pca9685, motor, motor_speed);//10             static int cnt_loop;//1             cnt_loop++;//1             if(cnt_loop%100 != 0) continue;//1             printf("#RAW(X,Y,Z): %6d,%6d,%6d\n", gyro_raw.x, gyro_raw.y, gyro_raw.z);             printf("#OFF(X,Y,Z): %6d,%6d,%6d\n", gyro_offset.x, gyro_offset.y, gyro_offset.z);             printf("#ADJ(X,Y,Z): %6d,%6d,%6d\n", gyro_adj.x, gyro_adj.y, gyro_adj.z);             printf("#RATE(P,R,Y): %6.1f,%6.1f,%6.1f\n", gyro_rate.pitch, gyro_rate.roll, gyro_rate.yaw);             printf("#DT:%6.6f\n", dt.t_period);             printf("#ANGLE(P,R,Y):%6.1f,%6.1f,%6.1f\n", gyro_angle.pitch, gyro_angle.roll, gyro_angle.yaw);             printf("#FORCE(P,R,Y):%6.1f,%6.1f,%6.1f\n", balancing_force.pitch, balancing_force.roll, balancing_force.yaw);             printf("#SPEED(A,B,C,D):%6.1f,%6.1f,%6.1f,%6.1f\n", motor_speed.a, motor_speed.b, motor_speed.c, motor_speed.d);             printf("\n");       } }
컴파일	$ g++ drone_all.cpp -o drone -lwiringPi
실행	$ ./drone

droneRPi_230831.cpp

16.55KB