컵드론 펌웨어 분석

2015. 3. 28. 조한철(바람)

목차

펌웨어 History

SmartRover App

개발 환경

이클립스 IDE

아두이노 IDE

펌웨어 구조

폴더 구조

통신 구조

제어 구조

펌웨어 History

CupDrone 펌웨어

Multiwii 기반의 코드를 사용한 SkyRover Nano 프로젝트와 F/W 동일

센서 수량 및 일부 하드웨어 변경됨

Multiwii 2.3

Arduino AVR

SkyRover Nano

+

Flexbot 통신 I/F

Afroflight32

GCC STM32F103 부트로더 추가

+ +

FreeRTOS 적용

USB 통신

+

LCD Lib (u8glib)

+

CupDrone

SmartRover App

컵드론 조종을 위한 안드로이드 App으로 기존의 Flexbot오픈 소스를 수정함

Google play에서 “SmartRover”로 검색하여 설치

개발환경 – 이클립스 IDE

펌웨어 컴파일/편집을 위해 이클립스 IDE환경 사용

설치방법

Java SE (JDK) 설치 http://www.oracle.com/technetwork/java/javase/downloads/index.html

Eclipse IDE for C/C++ Developers 설치 https://eclipse.org/downloads/

GNU ARM Eclipse Plug-in 설치

Eclipse 실행 후 Help->Install New Software->Add 선택 후

내용 입력

Name : GNU ARM Eclipse Plug-ins

Loc : http://gnuarmeclipse.sourceforge.net/updates

개발환경 – 이클립스 IDE

GNU ARM Eclipse Plug-in 설치

GNU ARM C/C++ Cross Development Tools 선택 후 설치함

개발환경 – 이클립스 IDE

ARM GCC 설치

GNU Tools for ARM Embedded Processors

https://launchpad.net/gcc-arm-embedded 각 플랫폼에 맞는 버전

다운로드 후 설치

최종 설치 옵션에서 Add path to environment variable 선택

개발환경 – 이클립스 IDE

Cross Build Tools 설치

http://sourceforge.net/projects/gnuarmeclipse/files/Build%

20Tools/ Cross Build Tools.zip 다운로드

파이명 변경

파일 복사

C:\Program Files\GNU Tools ARM Embedded\4.9 2015q1\bin

에 3개 파일 복사

개발환경 – 이클립스 IDE

소스코드 다운로드 (github.com/oroca)

File->Import 선택

URI : https//github.com/oroca/SkyRover_Nano 입력

개발환경 – 이클립스 IDE

소스코드 다운로드 (github.com/oroca)

Clone URI 선택

Import existing projects 선택

개발환경 – 이클립스 IDE

소스코드 다운로드 (github.com/oroca)

SkyRover_Nano 프로젝트 선택

다운로드 Tool

STM32LD_GUI

STM32F 계열의 부트로더 프로토콜을 사용한 윈도우용 다운로드 프로그램

사용법 : http://cafe.naver.com/openrt/5943

소스코드 : https://github.com/chcbaram/stm32ld_gui

Visual C++ 6.0으로 작성됨

다운로드 Tool

STM32LD

컴맨드 방식의 다운로드 프로그램으로 윈도우/맥/리눅스에서 사용가능

소스코드 : https://github.com/chcbaram/stm32ld

윈도우에서 컴파일 (cygwin 및 gcc 설치 필요) gcc -o stm32ld main.c stm32ld.c main_OpenCM.c serial_win32.c -

DWIN32_BUILD

맥/리눅스에서 컴파일

Mac/Linux gcc -o stm32ld main.c stm32ld.c main_OpenCM.c serial_posix.c

사용방법

stm32ld 통신포트 통신속도 펌웨어파일 1

ex) stm32ld COM1 115200 main.bin 1

ex OpenCM 보드) stm32ld COM1 115200 main.bin 1 opencm

개발환경 – 아두이노 IDE

아두이노 개발환경에서 추가 기능 시험가능

멀티플랫폼 지원되는 1.6 버전에 SkyRover 보드 플랫폼 추가하는 형태

설치방법

아두이노IDE 1.6버전 다운로드

http://arduino.cc/en/Main/Software 에서 Zip파일로 다운로드 후 압축 해제

SkyRover 보드 추가

https://github.com/oroca/SkyRover_Ardu 에서 Download ZIP

으로 파일 다운로드

Arduino-1.6.1/hardware/oroca 폴더에 압축 해제

개발환경 – 아두이노 IDE

도구->보드->SkyRover 선택

개발환경 – 아두이노 IDE

파일->예제->SkyRover->예제 선택

펌웨어 구조

메모리 구조

펌웨어

부트로더

0x08000000

0x08003000

설정 저장

12KB

114KB

2KB

0x0801F800

0x08020000 • 부트로더는 전원 On/Reset시 처음 실행됨

• STM32F103은 EEPROM이 없기때문에 마지막 2KB영역을저장용 메모리로 사용

• 펌웨어 다운로드시 설정값은 유지됨

펌웨어 구조

폴더 구조

lib 폴더

RTOS/주변장치/USB/LCD 라이브러리

펌웨어 구조

폴더 구조

obj 폴더

컴파일시 발생회는 obj파일과 최종 bin 파일

펌웨어 구조

폴더 구조

src 폴더

소스코드 파일

Thread 폴더

FreeRTOS의 Thread 함수 모음

펌웨어 구조

주요 파일

Make 유틸리티를 사용하여 컴파일 및 링크 할 수 있는 정보 파일

스타트업 코드 링커 스크립트 파일

펌웨어 구조

주요 파일

main.c

main함수로 각종 센서 초기화/모터 설정등 수행

mw.c

멀티콥터 제어기능 수행하며, 센서/모터제어/통신 수행

sensors.c

장착된 센서를 자동으로 찾기 및 센서 데이터 읽기 수행

imu.c

가속도/자이로/지자계로 부터 Roll/Pith/Yaw등의 방향 계산

mixer.c

제어량값을 각 모터에 배분하는 역할

serial.c

멀티위 시리얼 인터페이스인 MSP 통신 기능 수행

펌웨어 구조

주요 파일

drv_***.c

하드웨어 드라이버 파일로 해당 하드웨어 제어를 담당

drv_timer.c

CPU의 PWM 타이머 및 포트 정의

drv_pwm.c

모터 출력 PWM 구성 정의

config.c

내부 FLASH에 저장되는 설정데이터의 초기값 정의

board.h

기체종류/사용센서/LED등 보드 관련된 설정값 정의

펌웨어 구조

함수 호출 구조

thread 생성

main()

thread_main()

thread_mw()

thread_menu()

thread_lcd()

- 멀티위 제어 기능 수행

- USB를 통한 메뉴 기능 수행

- I2C방식의 LCD 출력 기능 수행

펌웨어 구조

함수 호출 구조

thread_mw

hexairbotFrameComplete()

computeRC()

• 블루투스로 조종 데이터가 수신되었는지 검사

• 수신된 조종값(롤,피치,요,스로틀,AUX1,AUX2,AUX3,AUX4)을 rcData[] 배열에 저장 50Hz

computeIMU() • 가속도/자이로/지자계 센서로 부터 자세값 계산

annexCode() • 수신된 조종값 rcData[]의 범위는 1000~2000이며(중립은 1500), 제어범위 값 rcCommand[]에 0~500 값으로 변환

pid_controller()

mixTable()

writeServos()

writeMotors()

• rcCommnad[]값을 기준으로 PID제어에 따른 제어량값 계산

• 기체 종류에 따라 PID제어를 통해 나온 제어량은 모터 PWM으로 변환

• PWM값에 따른 모터 구동

280Hz

펌웨어 구조

통신 구조

App에서 MSP 프로토콜로 Roll/Pitch/Yaw/Throttle 데이터 전송

MSP 패킷

annexCode()

serialCom()

evaluateCommand()

serialRead()

명령어 수신시 실행

펌웨어 구조

MSP 패킷

멀티위에서 사용되는 시리얼 프로토콜

http://www.multiwii.com/wiki/index.php?title=Multiwii_Seri

al_Protocol

패킷 구성

App -> CupDrone

CupDrone -> App

펌웨어 구조

MSP 패킷

데이터 처리

펌웨어 구조

통신 명령

CupDrone제어를 위한 주요 명령(serial.c에 정의)

MSP_SET_RAW_RC_TINY 명령으로 기체 움직임 조종

#if defined(SKYROVER) #define MSP_SET_RAW_RC_TINY 150 #define MSP_ARM 151 #define MSP_DISARM 152 #define MSP_TRIM_UP 153 #define MSP_TRIM_DOWN 154 #define MSP_TRIM_LEFT 155 #define MSP_TRIM_RIGHT 156 #endif

Roll/Pitch/Yaw/Throttle/Aux 정보 기체 활성화 기체 비활성화

펌웨어 구조

MSP_SET_RAW_RC_TINY

evaluateCommand() { switch( cmdMSP ) { case MSP_SET_RAW_RC_TINY: for(i = 0;i < 4;i++) { serialRcValue[i] = 1000 + read8() * 4; } auxChannels = read8(); …

Index

0 0~250 – Roll 값

1 0~250 – Pitch 값

2 0~250 – Yaw 값

3 0~250 – Throttle 값

4 0~255 – Aux 값

7:6 bit - Aux1

5:4 bit - Aux2

3:2 bit - Aux3

1:0 bit - Aux4

Aux1

- 0 : Headfree Mode Off

- 2 : Headfree Mode On

Aux2

- 0 : 고도홀드 Off

- 2 : 고도홀드 On

펌웨어 구조

주요 설정

board.h

기체 종류 설정(HEX/QUAD)

사용센서 설정

//#define SKYROVER_HEX #define SKYROVER_QUAD

#define GYRO #define ACC #define MAG #define BARO #define ACC_AS_MAG #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO)

펌웨어 구조

주요 설정

config.c -> resetConf()

설정값 초기화시 데이터

기체 종류 설정(HEX/QUAD)

기능 활성화

#if defined(SKYROVER_HEX) mcfg.mixerConfiguration = MULTITYPE_HEX6; // 헥사콥터 #elif defined(SKYROVER_QUAD) mcfg.mixerConfiguration = MULTITYPE_QUADX; // 쿼드콥터 #else mcfg.mixerConfiguration = MULTITYPE_HEX6; // 헥사콥터 #endif

featureSet(FEATURE_SERIALRX); // 시리얼포트로 HexAirBot 인터페이스 수신을 위해 featureSet(FEATURE_MOTOR_STOP); // DC Brushed 모터 사용시 모터 정지시 PWM값을 0으로 하기 위해 featureSet(FEATURE_VBAT); // 배터리 전압체크 기능 활성화

펌웨어 구조

주요 설정

config.c -> resetConf()

시리얼통신 타입 및 모터 출력 설정

Yaw축 방향 설정

mcfg.serialrx_type = SERIALRX_HEXAIRBOT; // HexAirBot mcfg.minthrottle = 1150; mcfg.maxthrottle = 1850; mcfg.motor_pwm_rate = 1000; // Hz DC 브러시모터는 500 초과값을 설정

cfg.yaw_direction = 1;

제어 구조

QuadX 모터 방향

멀티콥터 움직임

Roll/Pitch/Yaw/상하 움직임의 합의 방향으로 이동

<Roll> <Pitch> <Yaw> <상/하>

+ + +

제어 구조

모터 제어량

Roll값이 기울어 졌을때 수평을 유지하기 위해 해당 모터가 빠르게 회전 필요

<Roll>

모터속도 느리게 모터속도 빠르게 FRONT_L 모터 제어량 = +1 x Roll기울기값

REAR_L 모터 제어량 = +1 x Roll기울기값

FRONT_R 모터 제어량 = -1 x Roll기울기값

REAR_R 모터 제어량 = -1 x Roll기울기값

제어 구조

모터 제어량

Roll/Pitch/Yaw/상하에 대한 모터 제어 방향의 합이 최종 모터 제어값

mixer.c 에 모터 제어방향 데이터 정의

FRONT_L 모터 제어량 = (+1xRoll 기울기값) + (-1xPitch 기울기값) + (-1xYaw 기울기값) REAR_L 모터 제어량 = (+1xRoll 기울기값) + (+1xPitch 기울기값) + (+1xYaw 기울기값) FRONT_R 모터 제어량 = (-1xRoll 기울기값) + (-1xPitch 기울기값) + (+1xYaw 기울기값) REAR_R 모터 제어량 = (-1xRoll 기울기값) + (+1xPitch 기울기값) + (-1xYaw 기울기값)

static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L };

Throttle Pitch

Yaw

Roll

제어 구조

제어 구조

THROTTLE

ROLL

PITCH

YAW

ROLL 측정값

PITCH 측정값

YAW 측정값

ROLL 오차값

PITCH 오차값

YAW 오차값

PID 제어기

PID 제어기

PID 제어기

ROLL 제어량

PITCH 제어량

YAW 제어량

Mixer

PWM1

PWM2

PWM3

PWM4

목표치 입력 제어기 출력

computeRC( ) annexCode( ) computeIMU( )

pidMultiWii( ) mixTable( ) writeMotors( )

제어구조

제어코드 예제

roll_error_angle = Sky.cmd_get_roll() - Sky.imu_get_angle_roll(); pitch_error_angle = Sky.cmd_get_pitch() - Sky.imu_get_angle_pitch(); yaw_error_angle = Sky.imu_get_gyro_yaw(); MotorPwm_RearR = Sky.cmd_get_throttle() + (-1*roll_error_angle) + ( 1*pitch_error_angle) + (-1*yaw_error_angle); MotorPwm_FrontR = Sky.cmd_get_throttle() + (-1*roll_error_angle) + (-1*pitch_error_angle) + ( 1*yaw_error_angle); MotorPwm_RearL = Sky.cmd_get_throttle() + ( 1*roll_error_angle) + ( 1*pitch_error_angle) + ( 1*yaw_error_angle); MotorPwm_FrontL = Sky.cmd_get_throttle() + ( 1*roll_error_angle) + (-1*pitch_error_angle) + (-1*yaw_error_angle); Sky.motor_set_speed_FRONT_L( MotorPwm_FrontL ); Sky.motor_set_speed_FRONT_R( MotorPwm_FrontR ); Sky.motor_set_speed_REAR_L ( MotorPwm_RearL ); Sky.motor_set_speed_REAR_R ( MotorPwm_RearR );

PID 제어기

감사합니다.