컵드론펌웨어분석 2015. 3. 28. 조한철 ( 바람 )
목차 펌웨어 History SmartRover App 개발환경 이클립스 IDE 아두이노 IDE 펌웨어구조 폴더구조 통신구조 제어구조
펌웨어 History CupDrone 펌웨어 Multiwii 기반의코드를사용한 SkyRover Nano 프로젝트와 F/W 동일 센서수량및일부하드웨어변경됨 Arduino AVR Multiwii 2.3 Flexbot 통신 I/F FreeRTOS 적용 LCD Lib (u8glib) Afroflight32 + + + + + SkyRover Nano GCC STM32F103 부트로더추가 USB 통신 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-> 예제선택
펌웨어구조 메모리구조 2KB 설정저장 0x08020000 0x0801F800 부트로더는전원 On/Reset 시처음실행됨 STM32F103 은 EEPROM 이없기때문에마지막 2KB 영역을저장용메모리로사용 펌웨어다운로드시설정값은유지됨 114KB 펌웨어 12KB 부트로더 0x08003000 0x08000000
펌웨어구조 폴더구조 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_mw() - 멀티위제어기능수행 thread_main() thread_menu() - USB 를통한메뉴기능수행 thread_lcd() - I2C 방식의 LCD 출력기능수행
펌웨어구조 함수호출구조 thread_mw hexairbotframecomplete() 블루투스로조종데이터가수신되었는지검사 50Hz computerc() 수신된조종값 ( 롤, 피치, 요, 스로틀,AUX1,AUX2,AUX3,AUX4) 을 rcdata[] 배열에저장 computeimu() 가속도 / 자이로 / 지자계센서로부터자세값계산 annexcode() 수신된조종값 rcdata[] 의범위는 1000~2000 이며 ( 중립은 1500), 제어범위값 rccommand[] 에 0~500 값으로변환 280Hz pid_controller() rccommnad[] 값을기준으로 PID 제어에따른제어량값계산 mixtable() writeservos() writemotors() 기체종류에따라 PID 제어를통해나온제어량은모터 PWM 으로변환 PWM 값에따른모터구동
펌웨어구조 통신구조 App 에서 MSP 프로토콜로 Roll/Pitch/Yaw/Throttle 데이터전송 annexcode() serialcom() MSP 패킷 serialread() 명령어수신시실행 evaluatecommand()
펌웨어구조 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() 시리얼통신타입및모터출력설정 mcfg.serialrx_type = SERIALRX_HEXAIRBOT; // HexAirBot mcfg.minthrottle = 1150; mcfg.maxthrottle = 1850; mcfg.motor_pwm_rate = 1000; // Hz DC 브러시모터는 500 초과값을설정 Yaw 축방향설정 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/ 상하에대한모터제어방향의합이최종모터제어값 FRONT_L 모터제어량 = (+1xRoll 기울기값 ) + (-1xPitch 기울기값 ) + (-1xYaw 기울기값 ) REAR_L 모터제어량 = (+1xRoll 기울기값 ) + (+1xPitch 기울기값 ) + (+1xYaw 기울기값 ) FRONT_R 모터제어량 = (-1xRoll 기울기값 ) + (-1xPitch 기울기값 ) + (+1xYaw 기울기값 ) REAR_R 모터제어량 = (-1xRoll 기울기값 ) + (+1xPitch 기울기값 ) + (-1xYaw 기울기값 ) mixer.c 에모터제어방향데이터정의 Throttle 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 }; Yaw Pitch Roll
ROLL 측정값 PITCH 측정값 YAW 측정값 제어구조 제어구조 computerc( ) annexcode( ) computeimu( ) pidmultiwii( ) mixtable( ) writemotors( ) THROTTLE PWM1 ROLL ROLL 오차값 PID 제어기 ROLL 제어량 PWM2 PITCH PITCH 오차값 PID 제어기 PITCH 제어량 Mixer PWM3 YAW YAW 오차값 PID 제어기 YAW 제어량 PWM4 목표치입력제어기출력
제어구조 제어코드예제 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(); PID 제어기 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 );
감사합니다.