crazyflie2 code reading subak.io

Size: px

Start display at page:

Download "crazyflie2 code reading subak.io"

주호 매
5 years ago
Views:

1 crazyflie2 code reading subak.io

2 Drone Drone? : D.Camp 5F C : ( ) 7 ~ 8

3 Drone Drone SW Drone SW

4 5 OpenSource Drone 5 Drone Source Code Review

5 OpenSource Drone 5 ArduPilot // diy drones, 3DR PX4,APM HobbyKing KK CC3D (openpilot) Naze32(SW: MultiWii, Baseflight and Cleanflight) FPV crazyflie

7 crazyflie crazyflie 2.0 MCU stm32f405, 4 dc, 4 led, mpu (G+A+M), usb. nrf51822(bluetooth4.0) FreeRTOS :, CRTP (Crazyflie Radio Trasport Protocol)

8 Drone Source Code Review :, -> <-.

9 Drone Source Code Review Boot (Drone ) Control (Loop) Comms ( )

10 Boot 10

11 Drone Source Code Review: Boot,, main() -> systemlaunch() ->!systemtask() systeminit() ->! configblockinit(); workerinit(); ledseqinit(); pminit(); comminit() ->! syslinktask usblinkinit(); radiolinkinit(); syslinkinit();! crtpinit(); commanderinit(); crtpregisterportcb(crtp_port_commander, commandercrtpcb); stabilizerinit(); ->!stabilizertask motorsinit(); imu6init(); sensfusion6init(); controllerinit(); workerloop(); // log, neopixel

12 Drone Source Code Review: FreeRTOS xtaskcreate() xqueuesend() xqueuerecieve() xsemaphoretake() xsemaphoregive() vtaskstartscheduler()

13 Drone Source Code Review: Boot 0 :. ( init/main.c ) 3

14 Drone Source Code Review: Boot 1 :? canstartmutex? canstartmutex = xsemaphorecreatemutex(); xsemaphoretake(canstartmutex, portmax_delay); xsemaphoregive(canstartmutex);

15 Drone Source Code Review: Boot 2 : roll, pitch, yaw, thrust? struct CommanderCrtpValues { float roll; float pitch; float yaw; uint16_t thrust; } attribute ((packed));

17 Drone Source Code Review: Boot 3 stabilizerinit() imu6init (Inertial Measurement Unit:, mpu9250)+ ( ms5611) sensfusion6init controllerinit!

18 Control 15

19 Control: stabilizertask. stabilizertask() crazyflie. : stabilizertask(). (...) 3

20 Control Commander: (Roll Pitch Yaw, Thrust). SensorFusion:. Stabilizer:.

21 Drone Source Code Review: Commander Roll, Pitch, Yaw thrust. commandergetrpy(&eulerrolldesired, &eulerpitchdesired, &euleryawdesired); commandergetthrust(&actuatorthrust);

22 Drone Source Code Review: SensorFusion ( ) + ( ) ( (Quaternion)) sensfusion6updateq(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,fusion_update_dt); (q0, q1, q2, q3) Quaternion (radian) sensfusion6geteulerrpy(&eulerrollactual, &eulerpitchactual, &euleryawactual);? x,y,z.

23 Drone Source Code Review: stabilizer // 250hz tick: RPY( ), RPY(Commander)-> RPY( RATE) controllercorrectattitudepid( eulerrollactual, eulerpitchactual, euleryawactual, eulerrolldesired, eulerpitchdesired, -euleryawdesired, &rollratedesired, &pitchratedesired, &yawratedesired ); // every tick: pid update controllercorrectratepid(gyro.x, -gyro.y, gyro.z, rollratedesired, pitchratedesired, yawratedesired); controllergetactuatoroutput(&actuatorroll, &actuatorpitch, &actuatoryaw); // update motor! distributepower(actuatorthrust, actuatorroll, actuatorpitch, -actuatoryaw);

24 PID: - - aka Proportional Integral Derivative controller?,,. ( ),,.

25 Crazyflie Stabilizer

26 Comms 10

27 Comms Commander UART (BLE) Link (Radio Link)

28 Drone Source Code Review: Commander commandercrtpcb(crtppacket* pk)! : commandercrtpcb?

29 Drone Source Code Review: UART(from BLE) crtprxtask(void *param) :!link->receivepacket(&p)?

30 Drone Source Code Review: Link (Receive) radiolinkreceivecrtppacket(crtppacket *p) : crtppacketdelivery?

31 Drone Source Code Review: Link radiolinksyslinkdispatch(syslinkpacket *slp) : radiolinksyslinkdispatch? static void syslinkrouteincommingpacket(syslinkpacket *slp) static void syslinktask(void *param)

32 Drone Source Code Review: Commander SmartPhone -> Drone NRF51822(BLE ) UART CRTPPacket STM32. syslinktask() -> syslinkrouteincommingpacket() -> radiolinksyslinkdispatch() -> radiolinkreceivecrtppacket(crtppacket *p) -> crtprxtask(void *param) -> commandercrtpcb(crtppacket* pk) commandergetrpy() Roll, Pitch, Yaw.!stabilizerTask()

33 CRTP 0xaa 0xaa 0x30 0x0e 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x3e 0xaa 0xaa: preamble 0x30: commander 0x0e: data size 0x00 0x00 0x00 0x00: roll 0x00 0x00 0x00 0x00: pitch 0x00 0x00 0x00 0x00: yaw 0x00 0x00 0x00 0x00: thrust 0x3e: checksum

34 Drone Source Code Review n Crazyflie Module :. 5. ls crazyflie-firmware/modules/src/ comm.c crtp.c info.c param.c sensfusion6.c commander.c crtpservice.c log.c pid.c stabilizer.c console.c expbrd.c mem.c pidctrl.c system.c controller.c exptest.c neopixelring.c platformservice.c worker.c

35 10

SW 2015. 02 5-1 89

SW 2015. 02 88 SW 2015. 02 5-1 89 SW 2015. 02 5-2 5-3 90 SW 2015. 02 5-4 91 SW 2015. 02 5-5 5-6 92 5-7 SW 2015. 02 93 SW 2015. 02 5-8 5-1 94 SW 2015. 02 5-9 95 SW 2015. 02 5-10 5-2 96 SW 2015. 02 5-11