crazyflie2 code reading subak.io 2015-06-04
Drone Drone? : D.Camp 5F C : 2015. 6. 4 ( ) 7 ~ 8
Drone Drone SW Drone SW
5 OpenSource Drone 5 Drone Source Code Review 35 10 5
OpenSource Drone 5 ArduPilot // diy drones, 3DR PX4,APM HobbyKing KK CC3D (openpilot) Naze32(SW: MultiWii, Baseflight and Cleanflight) FPV crazyflie
crazyflie https://wiki.bitcraze.io/projects:crazyflie2:index crazyflie 2.0 MCU stm32f405, 4 dc, 4 led, mpu9250 9 (G+A+M), usb. nrf51822(bluetooth4.0) FreeRTOS :, CRTP (Crazyflie Radio Trasport Protocol)
Drone Source Code Review 35... :, -> <-. https://github.com/bitcraze/crazyflie-firmware
Drone Source Code Review.... 3. Boot (Drone ) Control (Loop) Comms ( )
Boot 10
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
Drone Source Code Review: FreeRTOS xtaskcreate() xqueuesend() xqueuerecieve() xsemaphoretake() xsemaphoregive() vtaskstartscheduler()
Drone Source Code Review: Boot 0 https://github.com/bitcraze/crazyflie-firmware :. ( init/main.c ) 3
Drone Source Code Review: Boot 1 :? canstartmutex? canstartmutex = xsemaphorecreatemutex(); xsemaphoretake(canstartmutex, portmax_delay); xsemaphoregive(canstartmutex);
Drone Source Code Review: Boot 2 : roll, pitch, yaw, thrust? struct CommanderCrtpValues { float roll; float pitch; float yaw; uint16_t thrust; } attribute ((packed));
Drone Source Code Review: Boot 3 stabilizerinit() imu6init (Inertial Measurement Unit:, mpu9250)+ ( ms5611) sensfusion6init controllerinit!
Control 15
Control: stabilizertask. stabilizertask() crazyflie. : stabilizertask(). (...) 3
Control Commander: (Roll Pitch Yaw, Thrust). SensorFusion:. Stabilizer:.
Drone Source Code Review: Commander Roll, Pitch, Yaw thrust. commandergetrpy(&eulerrolldesired, &eulerpitchdesired, &euleryawdesired); commandergetthrust(&actuatorthrust);
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.
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);
PID: - - aka Proportional Integral Derivative controller?,,. ( ),,.
Crazyflie Stabilizer
Comms 10
Comms Commander UART (BLE) Link (Radio Link)
Drone Source Code Review: Commander commandercrtpcb(crtppacket* pk)! : commandercrtpcb?
Drone Source Code Review: UART(from BLE) crtprxtask(void *param) :!link->receivepacket(&p)?
Drone Source Code Review: Link (Receive) radiolinkreceivecrtppacket(crtppacket *p) : crtppacketdelivery?
Drone Source Code Review: Link radiolinksyslinkdispatch(syslinkpacket *slp) : radiolinksyslinkdispatch? static void syslinkrouteincommingpacket(syslinkpacket *slp) static void syslinktask(void *param)
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()
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
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
10