Design and Implementation of a Drone Quadcopter Using Low Cost Microcontrollers

Equa ion Chap e 1 Sec ion 1 Mas e ’s Thesis MsC Deg ee in Indus ial Enginee ing Design and Implemen a ion o a D one Quadcop e Using Low Cos Mic ocon olle s Au ho : Pablo López Flo es Tu o : Ramón González Ca ajal Dep . Elec onic Enginee ing Highe Technical School o Enginee ing Uni e si y o Se ille Se illa, 2020 2 P oyec o Fin de Más e Más e Uni e si a io en Ingenie ía Indus ial Design and Implemen a ion o a D one Quadcop e Using Low Cos Mic ocon olle s Au o : Pablo López Flo es Tu o : Ramón González Ca ajal Ca ed á ico Dp o. de Ingenie ía Elec ónica Escuela Técnica Supe io de Ingenie ía Uni e sidad de Se illa Se illa, 2020 4 Mas e ’s Thesis: Design and Implemen a ion o a D one Quadcop e Using Low Cos Mic ocon olle s Au ho : Pablo López Flo es Tu o : Ramón González Ca ajal The ibunal appoin ed o judge he wo k indica ed abo e, which is composed o he ollowing p e esso s: P esiden : Vocals: Sec e a y: Ag ee o g an him he quali ica ion o : Se illa, 2020 The sec e a y o he ibunal 6 To my amily and iends 8 Acknowledgemen s I would like o exp ess my g a i ude o all he people who has suppo ed me du ing he de elopmen o his p ojec . Fi s o my amily and iends, who has been always by my side and encou aged me o keep wo king, e en hough he appea ance o di icul ies. Wi hou hem, his would no ha e been possible. Thanks o Ramón o his suppo as well, always welcome o my doub s, ha has made his p ojec keep going un il he end. A special men ion and acknowledgemen o he GIE’s Ha dwa e Team (G upo de Ingenie ía Elec ónica) o he Uni e si y o Se ille o p in ing he PCB’s, especially o Da id Pablos Má quez. And o all he o he s, ha wi hou he need, ha e also helped in any way. As a inal p ojec ha his is, I do no wan o end my jou ney in he Enginee ing School o Se ille wi hou showing my since e app ecia ion o all o my colleges. Also o he p o esso s and o he wo ke s, which make possible o many people o become no only an enginee , bu also a bigge pe son. To all o hem, hank you. 16 2 COMPONENT SELECTION n his sec ion, we a e going o show he chosen d one’s componen s. As common c i e ia, we chose he bes sui able pa s o build a ace-size d one quadcop e , keeping he p ice as low as possible. The emo e con olle componen s a e de ailed in he sec ion 3.1. Be o e going in o he de ails o he ac ual de ices chosen, we need o unde s and which elemen s do we need o build a d one. We can di ide he comple e sys em in ee g oups: he d one’s pla o m, he ligh con olle and he emo e con olle . 1) The d one’s pla o m consis s o a ame, whe e ou mo o s a e a ached. In o de o powe hem up, we need a ba e y, which supplies a powe dis ibu ion boa d, whe e all he mo o s a e connec ed. We also need o ha e an elec onic speed con olle (ESC) o each mo o . No e ha he necessi y o he ESCs comes om he ype o mo o s ha we a e using, like mos o he d ones do, b ushless [11]. The easons why we decided o choose his ype o mo o s a e: - Li e ime and main enance, because hey ha e no b ushes. - Be e speed and o que due o he absence o b ushes. B ush ic ion inc eases wi h speed. - E iciency. B ushless mo o s do no ha e con ac esis ance losses and he hea . - Wide speed ange han b ushed mo o s. - Be e hea dissipa ion due o he cons uc ion compa ed o b ushed mo o s. 2) The ligh con olle is in cha ge o se ing he igh powe o each mo o . The p inciple o wo k is simple. On he one hand, an Ine ial Measu emen Uni is used o es ima e he o ien a ion o he d one. On he o he han, he desi ed o ien a ion is ecei ed ia adio. The du y o he ligh con olle is o calcula e he co ec powe o each mo o , so ha he ligh is s able and he ac ual o ien a ion mee s he desi ed one. 3) The emo e con olle ac s as he in e ace be ween he d one and he use , who can send he desi ed o ien a ion o e ime. To do ha , we need a adio communica ion sys em. Each needed elemen s a e now desc ibed. - Fligh Con olle The chosen de ice o de elop he ligh con olle has been he mic ocon olle ESP8266, moun ed in he ESP- 12E de elopmen boa d, p oduced by Ai-Thinke . This is a low cos mic ocon olle , whose mos popula capabili y is Wi-Fi communica ions. Al hough we a e no using his ea u e, gi en hose lis ed in [1], his “highly in eg a ed and du abili y MCU” (Mic ocon olle Uni ) was conside ed sui able o he in ended wo k. As desc ibed in [2] “The ESP8266 is a low-cos Wi-Fi mic ochip, wi h a ull TCP/IP s ack and mic ocon olle capabili y, p oduced by Esp essi Sys ems in Shanghai” In he nex image, we can see he pin ou . I Figu e 2.1. ESP8266 Pinou And he key ea u es in ( om [1]): ESP-12E Mic ocon olle ESP8266 ESP8266 P ocesso L106 32-bi RISC @80MHz ESP8266 Memo y - 32 KiB ins uc ion RAM - 32 KiB ins uc ion cache RAM - 80 KiB use -da a RAM - 16 KiB ETS sys em-da a RAM ESP8266 Fea u es - 16 GPIO - IEEE 802.11 b/g/n Wi-Fi - SPI - I²C (so wa e implemen a ion)[6] - I²S in e aces wi h DMA (sha ing pins wi h GPIO) - UART on dedica ed pins, plus a ansmi -only UART can be enabled on GPIO2 18 - 10-bi ADC (successi e app oxima ion ADC) Numbe o GPIO a ailable on ESP-12E 10 Ex e nal QSPI Flash Memo y 4 MiB TTL- o-USB adap e CH340G Powe Supply 4B2X (Inpu : 5V / Ou pu : 3.3V) Weigh 5.8 g Uni P ice 2.63 € The p og amming so wa e used o his p ojec has been he open sou ce A duino SDK (So wa e De elopmen Ki ). - F ame Spidex 220 FPV D one By Qua e nium Wheelbase 220mm Weigh 95g Uni P ice 3.24€ - Mo o s Tu nigy D2206-2300KV 31g B ushless Mo o CCW Tu nigy D2206-2300KV 31g B ushless Mo o CW RPM/V 2300k Vol age 2~4s LiPoly (7.4~14.8 ) Ou pu <310W Max Cu en 23A Sugges ed p op 5045~6045 Th us 880g (5045 p op/4s) E iciency <3.3 g/W ESC 30A Sha M5 CCW Mo o Moun Holes M3 x 16/19mm Dimensions 28 x 19.5mm Weigh 31g Uni P ice (x4 used) 7.52 € - ESCs (Elec onic Speed Con olle s) Tu nigy Mul is a BLheli_32 ARM 21A 2g Race Spec ESC 2~4S (OPTO) Cons an Cu en 21A Cells 2~4S Inpu Vol age 8.4 ~ 16.8 BEC None (op o only) MCU A m Co ex-M0 Timing Au o F equency 48MHz P og ammable Yes PCB Size 25 x 12 x 5mm Weigh 2g Uni P ice (x4 used) 6.26€ 20 - Radio Modules Eby e E34-2G4D20D F equency 2.4 – 2.518 GHz Powe 10 – 20 dBm Recei ing sensi i i y -102 dBm Ai da a a e Sel -adap ed baud a e Baud a e 1200 – 115200 Manu ac u e Tes ed Dis ance 2 km (In open and clea ai , wi h maximum powe , 5dBi an enna gain, heigh o 2m.) In e ace UART Weigh 6.3 g Uni P ice (x3 used) 4.31 € No e ha he hi d adio module was used as a comminicaion sni e . - An enna Eby e TX2400-JK-11 F equency 2.4GHz In e ace SMA-J Resis ance 50Ω Gain 2.5dB Ma e ial Rubbe Leng h 90mm Weigh 8g Uni P ice (x3 used) 4.46 $ - IMU “MPU-9250 is a mul i-chip module (MCM) consis ing o wo dies in eg a ed in o a single QFN package. One die houses he 3-Axis gy oscope and he 3-Axis accele ome e . The o he die houses he AK8963 3-Axis magne ome e om Asahi Kasei Mic ode ices Co po a ion. Hence, he MPU-9250 is a 9-axis Mo ionT acking de ice ha combines a 3-axis gy oscope, 3-axis accele ome e , 3-axis magne ome e and a Digi al Mo ion P ocesso ™ (DMP) all in a small 3x3x1mm package a ailable as a pin-compa ible upg ade om he MPU- 6515”. Sou ce: [4]. MPU-9250 Gene al P ope ies In e ace I2C Consump ion 3.5mA ope a ing cu en when all 9 mo ion sensing axes and he DMP a e enabled Vdd 2.4 – 3.6V Fu he ea u es - Use -p og ammable digi al il e s - 512 by e FIFO bu e Accele ome e P og ammable ange ±2g, ±4g, ±8g, ±16g ADC esolu ion 16-bi No mal ope a ing cu en 450 μA Gy oscope P og ammable ange ±250, ±500, ±1,000, ±2,000°/sec ADC esolu ion 16-bi Ope a ing cu en 3.2 mA Sleep mode cu en 8 μA Hall Senso ADC Resolu ion 14-bi (0.6 μT/LSB) 22 Full scale measu emen ange ±4800 μT Weigh 2.5 g Uni P ice 3.60 € - Ba e y Tu nigy 1000mAh 3S Lipo 20C Capaci y 1000mAh Con igu a ion 3S1P / 11.1 / 3CELL Discha ge 20C Cons an / 30C Bu s Weigh 87g Dimensions 77 x 33 x 20 mm Plug JST-XH Uni P ice 6.32 € - Dis ibu ion boa d: HobbyKing Mini Powe Dis ibu ion Boa d Amps 50 A Size 36 x 36 x 1.5mm Pads 2 inpu h ough holes, 8pos i e and 8negi i e h ough hole ou pu s Weigh 4g Uni P ice 1.09€ - P opelle s Dalp ops Bull Nose 4045 P opelle s CW/CCW Se (2 pai s) Size 4045 (4.0” x 4.5”) Hub 5mm Ma e ial Glass ein o ced polyca bona e Weigh 3.1g 2 Pai s P ice 0.49 € Dalp op Q4045 Bull Nose 4 Blade P opelle s CW/CCW Se (2 pai s) Size 4045 (4.0" x 4.5") Colo Blue Hub 5mm Ma e ial High quali y lexible composi e Blades 4 2 Pai s P ice 1.34 € - P opelle Nu s Aluminum Low P o ile Nyloc Nu M5 Sil e CW and CCW 24 Hub 5mm Uni P ice (x4 used) 0.275 € - To al amoun Adding up all he he p ices, he o al is €100.42 3 HARDWARE DEVELOPMENT o con inue, we a e going o de ail he design o he wo pieces o ha dwa e needed o he p ojec . On he one hand, a ci cui o he emo e con olle was needed, so we could send commands o he ha dwa e on- boa d. Bo h. The wo PCB (P in ed Ci cui Boa ds) was de eloped using Eagle 7.7.0. 3.1. Remo e Con olle Ha dwa e Design The Remo e Con olle (RC) ha dwa e has been based on he MS430 mic ocon olle om Texas Ins umen s. In pa icula , we ha e used he MSP430G2553 (PDIP o ma ). As desc ibed in Componen Selec ion, his mic ocon olle belongs o he MSP430G2x53 se ies, which a e: “ul a-low-powe mixed signal mic ocon olle s wi h buil -in 16- bi ime s, up o 24 I/O capaci i e- ouch enabled pins, a e sa ile analog compa a o , and buil -in communica ion capabili y using he uni e sal se ial communica ion in e ace. In addi ion he MSP430G2x53 amily membe s ha e a 10-bi analog- o-digi al (A/D) con e e ”. [5]. O he common cha ac e is ics a e: 16MHz MCU, 16KB lash and 512B SRAM. Con inuing now wi h he desc ip ion on he PCB componen s, o p o ide powe o he ci cui , we ha e a 1.5V ba e ies holde , wi h ou o hem (6V). The ol age is educed o 3.3V using he ol age egula o LM1117. To gene a e he use inpu s, we ha e a couple o wo-axis gimbals. Each o hem consis s o wo po en iome e s, plus a push bu on. O he wo addi ional bu ons ha e been added. No e ha only one o he bu ons is used. Those po en iome e s a e ead using he inpu s o he in e nal 10-bi con e e . P io o he mic ocon olle ’s inpu , we ha e an adap a ion ci cui based on a low pass RC il e and a ol age ollowe ci cui , using he Op- Amp LM6144. The bu ons, connec ed o a pull-up esis o om one side and o g ound in he o he , a e di ec ly ead using he GPIOs. T 32 4 CONTROL DESIGN AND MATLAB SIMULATIONS nlike a plane, he ligh s abili y o a d one quadcope depends o g ea ex en on he con ol inpu s. On he one hand, hanks o he ae odynamic o ces in he adi ional planes, hose can ly s able wi hou he need o any con ol inpu , as long as he ai speed is enough. Tha is no he case o he quadcop e s, which only depends on he o ces o each mo o in o de o keep in he ai . Many ac o s can a ec o he s abili y o he ligh , such as he changing wind speed and di ec ion, displacemen o he cen e o g a i y o he d one, o small disc epancies in he speed/ us a io o each mo o and p opelle . Fo his eason, i would be almos impossible o con ol i only wi h he inpu s on he adio ansmi e . The need o an in e nal ligh con olle ha akes ca e o all o hose sou ce e o s is unques ionable. In he ollowing image, we can see he h ee angles ha we need o deal wi h: Pi ch, Roll and Yaw. The objec i e o ou con olle is o main ain he desi ed a i ude o he d one, by keeping each o hem unde he desi e alues. Figu e 4-1. In blue: angles o con ol. In g een: di ec ion o o a ion o each mo o . To do ha , we need o calcula e he co ec mo o ’s inpu s based on he angle eadings. As al eady said, we a e using an IMU (Ine ial Measu emen Uni ), which p o ides us wi h accele a ions and angula eloci ies. Tha in o ma ion is p ocessed o es ima e he ins an aneous alue o each angles and he a e o change. The a i ude es ima ion has been implemen ed using an exis ing open sou ce C++ lib a y, RTIMULib (2015). Once we know how he d one is mo ing, we need o ac on he mo o s o co ec he de ia ions in he ajec o y. Looking a he p e ious image, we see he posi i e di ec ion o he angles. The e o e, we can de ine he needed ac ions o he angles o change: U - To ge a posi i e pi ch, he powe o he mo o s 1 and 2 need o be highe han 3 and 4. - To ge a posi i e oll, he powe o he mo o s 2 and 3 need o be highe han 1 and 4. - To ge a posi i e yaw, he powe o he mo o s 2 and 4 need o be highe han 1 and 3. I we wan he al i ude o emain he same when u ning he d one in one o he h ee axes, we need ha he o e all powe emains unchanged. Fo his eason he co ec pi ch, oll and yaw mo emen s a e achie ed by inc easing he powe in wo o he mo o s and educing he same amoun in he o he wo, as pe he poin ed abo e. As is logical, o inc ease he al i ude wi hou any change in angle, he powe needs o be inc eased he same amoun o all o hem. Rega ding he yaw mo emen , we make use o an angula momen um, p oduced by he di e ence o speed o each pai o mo o s (and p opelle s). No e ha wo o he mo o s o a es clockwise (2 and 4) and wo o a es coun e -clockwise (1 and 3). Thank o his, he o que o ces coun e ac each o he ; ideally, i o a ing all a he same speed, he quadcop e will no yaw. Fu he mo e, he posi ion o each o hem is key. Only wi h hose de ined in he p e ious image, we can dissocia e he yaw mo emen om he o he wo: i inc easing he speed o he mo o s 1 and 3 and dec easing 2 and 4 (o ice e sa), he d one will no pi ch o oll. 4.1 Con ol Design – PID The chosen con ol a chi ec u e has been an independen PID (P opo ional In eg al De i a i e) con olle o each angle. Hence, h ee PID con olle s ha e been implemen ed. As said in [6], “a p opo ional–in eg al–de i a i e con olle (PID con olle o h ee- e m con olle ) is a con ol loop mechanism employing eedback ha is widely used in indus ial con ol sys ems and a a ie y o o he applica ions equi ing con inuously modula ed con ol. A PID con olle con inuously calcula es an e o alue e( ) as he di e ence be ween a desi ed se poin (SP) and a measu ed p ocess a iable (PV) and applies a co ec ion based on p opo ional, in eg al, and de i a i e e ms (deno ed P, I, and D espec i ely), hence he name”. In ou speci ic case, he se poin s a e he h ee angles (Pi ch, Roll, Yaw), and he p ocess a iables a e he measu emen s o hose angles, collec ed by he IMU. Al hough he nex image co esponds o he Ma lab simula ions, explained in he nex sec ion, we can use i isualize he con olle a chi ec u e. No e how he independen PIDs a e combined. 34 Figu e 4-2. PID Con olle Model – Combina ion o he h ee con olle s. On he le , we see he calcula ion o he e o s. Fo simula ion pu poses, a andom sou ce was added o he Ma lab model o simula e noise in he senso s. In he cen e, we see he PID modules. No e ha he e a e wo inpu s: he e o and an enable signal o he “I” componen . On he igh , we see he me ge o he PID ou pu s. Following he logic o he p e ious sec ion, - The M1 powe would be: +main powe +pi ch - oll -yaw co ec ions. - The M2 powe would be: +main powe +pi ch + oll +yaw co ec ion. - The M3 powe would be: +main powe -pi ch + oll -yaw co ec ions. - The M4 powe would be: +main powe -pi ch - oll +yaw co ec ions. Fo pi ch and oll, he PID inpu s a e he angles (in deg ees), whe eas he yaw mo emen is con olled based on he angula eloci y (in deg ees pe second). - PID Con olle disc e isa ion. The exp ession o he ou pu o a con inuous PID con olle 𝑢(𝑡) is, om [7]: 𝑢(𝑡)=𝐾𝑝(𝑒(𝑡)+1 𝑇𝑖∫ 𝑒(𝜏)𝑑𝜏+𝑇𝑑𝑑𝑒(𝑡) 𝑑𝑡 𝑡 0) (4–1) Whe e 𝐾𝑝 is he p opo ional gain, 𝑒(𝑡) is he e o , 𝑇𝑖 is he in eg al ime cons an and 𝑇𝑑 is he de i a i e ime cons an . As we a e dealing wi h a eal sys em, we need o disc e ise he con olle , by app oxima ing he e o cu e. The bes way o do ha is using he apezoidal me hod. Figu e 4-3. Disc e isa ion o he e o cu e. The app oxima ion o he in eg al is: ∫𝑒(𝜏)𝑑𝜏 𝑡 0=∑(𝑒(𝑖−1+𝑒(𝑖)−𝑒(𝑖−1) 2)𝑇)= 𝑘 𝑖=1 ∑𝑇𝑒𝑖+𝑒𝑖−1 2 𝑘 𝑖=1 (4–2) So ha , 𝑢𝑘−𝑢𝑘−1 = 𝑞0𝑒𝑘+𝑞1𝑒𝑘−1+𝑞2𝑒𝑘−2 (4–3) Whe e, 𝑞0=𝐾𝑝(1+ 𝑇 2𝑇𝑖+𝑇𝑑 𝑇) 𝑞1=𝐾𝑝(𝑇 2𝑇𝑖−1+2𝑇𝑑 𝑇) 𝑞1= 𝐾𝑝(𝑇𝑑 𝑇) (4–4) 𝑇 is he sample ime o he sys em. Gi en he p ocessing ime es ic ions o ou con olle , he sample ime used is 15ms (66.67 Hz). Now, he objec i e is o calcula e he bes sui able PID pa ame e s, so ha he con olle in e ac s p ope ly wi h he sys em, i.e. s abilising i . In o de o calcula e he con olle ’s pa ame e s, i s , a Ma lab Simulink model o he d one has been cons uc ed o ge ini ial alues, based on he heo e ical physical beha iou . A se ield es PID uning was la e pe o med o adjus hem u he , based on he ac ual pe o mance 36 4.2 Con ol Design – Ex a ea u es Apa om he PID implemen a ion, we also needed o add an ex a ea u e o he basic con olle . We ha e o keep in mind how he in eg al componen o he con olle wo ks. Gi en an e o o e ime, he in eg al pa a ises by adding a co ec ion o he cu en con ol ou pu s. This is good o e ase any small e o s in he desi ed angles when he d one is lying, as he o he componen s (P o D) would no be e y eac i e o hem. Howe e , his could cause p oblems when he e o o e ime does no dec ease. As said be o e, a co ec ion is added when an e o o e ime exis s. The e o e, i ha e o keeps o a long pe iod, i would esul in a la ge in eg al co ec ion. This is he case when he d one is on he g ound. In his si ua ion, he angle alues will keep cons an , no necessa ily equal o he e e ence, so he e o o e ime will be e y big. I we y o ly he d one wi h such a la ge ini ial co ec ion, he quadcop e would des abilize as soon as he ligh s a s. To sol e ha we ha e added an ex a ea u e, ha allows us o disable he in eg al pa un il he e o s o e ime can be educed, i.e. un il he d one is in he ai . In addi ion, when simula ing his ea u e in Ma lab, we ha e seen ha by doing ha , he d one esponse imp o es, as we obse e less o e shoo when delaying he ime when he in eg al pa enables. In he nex image we can see a couple o simula ions, whe e we compa e he pi ch esponse when he in eg al pa is delayed (in blue) and no (in ed). Figu e 4-4. Compa ison o he pi ch esponse when he in eg al pa is delayed (in blue) and no (in ed). 4.3 Con ol Signals As said in Componen Selec ion, he chosen mo o s has been he Tu nigy D2206-2300KV 31g B ushless Mo o s, plus Tu nigy Mul is a BLheli_32 ARM ESC’s. Thanks o he ESC’s i mwa e, we do no need o wo y abou he ac ual ee-phase powe inpu signals o he mo o s, bu send he desi ed speed o he ESC’s. This is done using a PWM signal o 150 Hz. The minimum se poin ( eloci y = ze o) is in e p e ed when he du y cycle is 15% and maximum when 30%. 4.4 Ma lab Simulink Model As said abo e, a Ma lab Simulink model ha e been designed o ge ini ial pa ame e s o he PID con olle s. In his sec ion, we a e desc ibing in de ail how he model has been done. The model consis s o h ee main pa s, as seen in he nex igu e: Figu e 4-5. Gene al o e iew o he Ma lab Simulink Model 1) The Physical Model, a he igh . This module simula es he physics o he d one. Gi en ce ain o ces applied on each mo o o e ime, he posi ion and o ien a ion in ela ion o he Wo ld is calcula ed. To implemen ha we ha e used he Simscape Mul ibody solids, join s and o ces. A he bo om, we can see he join be ween he d one model and he g ound. 2) Measu emen s block, a he op. In his module, we collec he mo emen measu emen s, which a e he inpu s o he PID module. 3) PID Con olle Model, a he le . Based on he measu emen s, his block implemen he PID calcula ions. 38 4.4.1 Physical Model Inpu s: Fo ces om he mo o s and g ound. Ou pu : Posi ion and speed o he d one. Figu e 4-6. Simulink – Physical Model The physical model has been c ea ed using he Simscape Mul ibody oolbox. In ela ion o a Re e ence F ame, we ha e added all he pa s ha make up he d one: The Base, Mo o s, Ha dwa e and Ba e y Holde . The sou ce o each CAD model is h ps://g abcad.com/. Then, de ining he weigh o densi y in o ma ion o each solid, he oolbox is able o calcula e he cen e o g a i y and he dynamics o he d one, based on he applied inpu o ces. Figu e 4-7. Simulink – Physical Model. Mo o s 40 Figu e 4-8. Simulink – Physical Model. Ha dwa e Figu e 4-9. Simulink – Physical Model. Ba e y Holde The isual esul o all o hose componen s can be seen in he nex igu e. Figu e 4-10. Simulink – Visual ou pu o he physical model 4.4.2 Measu emen s Block Inpu : 6-deg ee o eedom a ibu e om he physical model. Ou pu : Pi ch, Roll, Yaw and Ve ical Posi ion. This block is based on [8]. 48 5.1.1 Pe iphe als Con igu a ion The i s ac ion is o con igu e he pe iphe als, hough. We need o selec he igh con igu a ion pa ame e s, in acco dance o he way we will use hem la e . No e ha all he igu es ela ed o he pe iphe als can be ound in he de ice da ashee ([9]). - Clock Con igu a ion. Gi en ha he mic ocon olle ’s ask does no equi e much speed ( he unc ional pe iod is 5ms), we can use a ela i ely slow one, like 8MHz, which allows us o sa e some ba e y. As we can see in he nex image, we use he RSEL and DCO bi s o selec he desi ed equency. Figu e 5-2. MSP430 Clock F equency con igu a ion pa ame e s The implemen ed code can be ound a he end o his sub-sec ion. - UART Con igu a ion. To con igu e he UART, we i s selec he con ol bi s o he GPIO Po 1 as seen below. Figu e 5-3. MSP430 UART con igu a ion pa ame e s The chosen clock souce has been SMCLK (8MHz). Rega ding he communica ion speed, we a e using he maximun speed ha he adio module can handle (115200 bauds), so ha he ime needed o each se ial ansmission is he lowes possible. To achie e ha eloci y, we need o con igu e he Clock p escale egis e s (UCBRx). Gi en ha he suo ce clock is 8MHz: 𝑈𝐶𝐵𝑅𝑥=8000000 115200 =69 (0𝑥0045) The implemen ed code can be ound a he end o his sub-sec ion. - GPIO Con igu a ion. As desc ibed in he sec ion Remo e Con olle Ha dwa e Design, we a e using a numbe o GPIO o con ol he LED and he bu ons. We can see he mapping in he nex able. HW Po Bi Inpu /ou pu LED 2 3 Ou pu Bu on 1 2 4 Inpu Bu on 2 2 2 Inpu Le Gimbal Bu on 2 1 Inpu The implemen ed code can be ound a he end o his sub-sec ion. - ADC Con igu a ion To help unde s and he ADC con igu a ion, we can see an o e iew o i s egis e s in he ollowing image. 50 Figu e 5-4. O e iew o he MSP430 ADC pe iphe al The chosen ADC con igu a ion is desc ibed as ollows ( he pa ame e s no men ioned a e le as de aul ):  ADC10CTL0 (ADC10 Con ol Regis e 0) Con igu a ion: o Vol age Re e ence: VR+ = VCC and VR- = VSS (SREFx = 000) o Sample-and-hold ime: 16 × ADC10CLKs (ADC10SHTx = 10) o Enabled: Yes (ADC10ON = 1) o Mul iple Sample and con e sion: Au oma ically igge ed (MSC = 1) o In e up : Enabled (ADC10IE = 1)  ADC10CTL1 (ADC10 Con ol Regis e 1) Con igu a ion: o Inpu channel selec : highes channel o a sequence o con e sions = A7 (INCHx = 0111) o Con e sion sequence mode selec : Sequence-o -channels (CONSEQx = 01)  ADC10AE0 (Analog (Inpu ) Enable Con ol Regis e 0) Con igu a ion: o Analog enable channels: all excep A2 (ADC10AE0x = 0xFB)  ADC10DTC1 (Da a T ans e Con ol Regis e 1) Con igu a ion: o Numbe o ans e s in each block: 8 (ADC10DTC1 = 8) In he nex able, we can see he analogic inpu s mapping. Measu emen Analogic Po Powe (Le Gimbal – Ve ical Mo emen ) A0 Pi ch (Righ Gimbal – Ve ical Mo emen ) A3 Roll (Righ Gimbal – Ho izon al Mo emen ) A6 Yaw (Le Gimbal – Ho izon al Mo emen ) A7 - Time Con igu a ion In case o he ime pe iphe als, he o e iew would be he ollowing: Figu e 5-5. O e iew o he MSP430 Time pe iphe al As we can see in he i mwa e low diag am om he s a o his sec ion, we need o wai 5ms o do each ac ion. To do ha we use he Time A, con igu ed o execu e i s in e up ion ou ine wi h a pe iod o 5ms. To achie e i , we ha e con igu ed he pe iphe al as ollows ( he pa ame e s no men ioned a e le as de aul ):  TACTL (Time _A Con ol Regis e ) Con igu a ion: o Clock Sou ce: SMCLK, 8MHz (TASSELx = 10) o Mode Con ol: Up mode: he ime coun s up o TACCR0. (MCx = 01)  TACCR0 (Time _A Cap u e/Compa e Regis e 0) Con igu a ion: o Time _A cap u e/compa e egis e : TACCR0 = 40000 52 TACCR0 =8000000 𝐻𝑧 1/0.005𝑠=40000  TACCTL (Cap u e/Compa e Con ol Regis e ) Con igu a ion o Cap u e/compa e in e up enable: Yes (CCIE = 1). When he coun is equal o TACCR0, he in e up is igge ed, as MCx = 01. MCx = 01 5.1.2 Low Powe Mode The MSP430 amily is well known as i s low consump ion, hanks o a a ie y o Low Powe Modes. The ypical consump ion is shown in he nex igu e. /*** Clock Con igu a ion. 8MHz ***/ WDTCTL = WDTPW + WDTHOLD; /* S op WDT */ i (CALBC1_8MHZ==0xFF) /* I calib a ion cons an e ased */ { while(1); /* do no load, ap CPU!! */ } DCOCTL = 0; /* Selec lowes DCOx and MODx se ings */ BCSCTL1 = CALBC1_8MHZ; /* Se DCO */ DCOCTL = CALDCO_8MHZ; /*** UART Con igu a ion ***/ P1SEL = BIT1 + BIT2 ; /* P1.1 = RXD, P1.2=TXD */ P1SEL2 = BIT1 + BIT2 ; /* P1.1 = RXD, P1.2=TXD */ UCA0CTL1 |= UCSSEL_2; /* SMCLK */ #i de CPU8_UART115200 UCA0BR0 = 0x45; /* 8MHz / 115200 = 69 (0x0045) */ UCA0BR1 = 0x00; #else UCA0BR0 = 0x41; /* 8MHz / 9600 = 833 (0x0341) */ UCA0BR1 = 0x03; #endi UCA0MCTL = UCBRS0; /* Modula ion UCBRSx = 1 */ UCA0CTL1 &= ~UCSWRST; /* Ini ialize USCI s a e machine */ IE2 |= UCA0RXIE; /* Enable USCI_A0 RX in e up */ /*** GPIO Con igu a ion ***/ P2DIR |= BIT3; /* Ou pu pin o LED */ P2DIR &= ~BIT4; /*Inpu Mode - B1*/ P2DIR &= ~BIT2; /*Inpu Mode - B2*/ P2DIR &= ~BIT1; /*Inpu Mode - Bu on Gimbal Le */ /*** ADC Con igu a ion ***/ ADC10CTL0 = ADC10SHT_2 + ADC10ON + ADC10IE + MSC; /* * ADC10SHT_2 : S/H ime. 16 × ADC10CLKs * ADC10ON : ADC10 On/Enable * ADC10IE : ADC10 In e up Enable * MSC : Mul iple SampleCon e sion * SREF_6 : VR+ = VeREF+ and VR- = VREF-/ VeREF-. De ices wi h VeREF+/- pins only. */ #i de EXTERN_REFERENCE_VREF_GROUND ADC10CTL0 |= SREF_7; #endi ADC10CTL1 = CONSEQ_1 + INCH_7; /* Con e sion code singed o ma , inpu A1 */ ADC10AE0 = 0xFB; /* Analog inpu 's enable */ ADC10DTC0 = 0xE1; ADC10DTC1 = 8; /* Numbe o co e sions on each DTC block (Da a T ans e Con olle )*/ /*** Time A0 Con igu a ion ***/ TA0CTL = TASSEL_2; /* SMCLK */ TA0CCR0 = 40000; /* 5ms */ TA0CCTL0 = CCIE; /* CCR0 in e up enabled */ Figu e 5-6. MSP430 Low Powe Modes The chosen Low Powe Mode has been LPM1, which allows us o sa e he mos ene gy while keeping he SMCLK is ac i e, so ha he ime ’s in e up can be igge ed o wake he mic ocon olle up. No e ha SMCLK has been he chosen sou ce clock o he ‘Time A’, as desc ibed be o e. 5.1.3 GPIO Read/w i e and ADC Measu emen s In o de o ead he s a us o he bu ons and u n he LED on/o , we access o he co esponden po . Fo example: !(P2IN & BIT4) o ead he s a us o he BIT4 o Po 2 (ou bu on 1). P2OUT |= BIT1 o se an ‘1’ in he BIT1 o he Po 2 (ou LED). Rega ding he bu ons eadings, we ha e implemen ed an an i-bouncing 10ms il e , so we ge id o e oneous alues when he bu ons a e being p essed o un-p essed. To do ha , we use coun e s o wai ce ain ime a e he ul ilmen o he condi ions. The code implemen a ion o he Bu on 1 would be: (no e ha o he o he wo bu ons he code is equi alen ) 54 Rega ding he ADC measu emen s, as pe he pe iphe al cons uc ion, each con e sion sequence s a s om he mos signi ican channel (INCH_x), and ends on A0. I we wan ed o con e A0, A3, A6 and A6 in he same sequence, all he channels will need o be con e ed ine i ably. A he beginning o each con e sion sequence, we need o indica e whe e he memo y space is (o 8 unsigned in ege s). In ou case i will be: ADC10SA = ADCMeasu eme s5;. And hen igge he sequence by ADC10CTL0 |= ENC + ADC10SC;. As shown in he p e ious low diag am, a con e sion is done each 5ms, and he messages a e sen each 30ms. So once he 30ms elapse, we make he a e age o he las 6 measu emen s. Addi ionally we need o igh shi he measu emen s by 2, so he 10-bi da a i s he 8-bi based UAR ansmissions: 5.1.4 Message Cons uc ion Be o e going in o de ail abou he ac ual adio messages, we a e going o desc ibe he use ’s pe spec i e ope a ion o he emo e con olle . As we see in he ollowing image, and in acco dance wi h ha desc ibed in he ha dwa e de elopmen sec ion, he emo e con olle consis o wo po en iome e s and h ee bu ons. In he nex image we can see he Use In e ace diag am o he Remo e Con olle . /* GPIO S a us Reading/W i e De ini ions */ #de ine B1_PRESSED !(P2IN & BIT4) (...) /*** Read Bu ons ***/ /* B1 */ i (B1_PRESSED) bu onCoun e B1++; else { bu onP essedFo Su e = 0; bu onCoun e B1 = 0; i ( ime A e P essingB1 > 0) ime A e P essingB1--; } i (bu onCoun e B1 >= MS_10) { bu onP essedFo Su e = 1; ime A e P essingB1 = TIMER_AFTER_PRESSING_START_TIME; } /* Fil e he 5ms-based ADC measu emen s. 30ms window */ i (++ <= 6) { o (i=0;i<8;i++) ADCMeasu eme s30[i] += ADCMeasu eme s5[i]; } i ( >= 6) /* When 30ms elapsed, make he a e age o he p e ious eadings */ { =0; /* Also igh shi he esul , so he egis e is con e ed 10bi -> 8bi */ /* No e ha he UART communica ion is 8bi -based */ o (i=0;i<8;i++) ADCMeasu eme s30[i] = (ADCMeasu eme s30[i] / 6 ) >> 2; (...) } Figu e 5-7. Use In e ace diag am o he Remo e Con olle The ope a ion o each o hem is: - Le Gimbal: o Ve ical mo emen : Inc eases/Dec eases he main powe (inc easing upwa ds). The middle posi ion co esponds o a powe o 0% a he beginning. This alue is changed when he joys ick is mo ed up/down. o Ho izon al mo emen : Yaw (inc easing o he le ). The middle posi ion co esponds o a se poin o 0 deg ees pe second. o GBL (Bu on o he Gimbal a Le ): Cu o he main powe o ze o. - Righ Gimbal: o Ve ical mo emen : Pi ch (inc easing upwa ds). The middle posi ion co esponds o a se poin o 0 deg ees by de aul (can be changed wi h B1). o Ho izon al mo emen : Roll (inc easing o he igh ). The middle posi ion co esponds o a se poin o 0 deg ees by de aul (can be changed wi h B1). - Sepa a e bu ons: o B1 (Bu on 1). Is used as a im. When i is p essed and eleased, he pi ch and oll alues a e ozen un il he igh gimbal e u ns o he middle posi ion. These pi ch and oll angles ( im alues) will be he de aul se poin s (ins ead o ze o). Fu he mo emen s will be added o hem. I is u he de ailed in he sec ion Running S a e, as he ac ual implemen a ion o his unc ionali y has been done in he d one’s i mwa e. o B2 (Bu on 2). I is used o slow down he a e o change o he main powe . This is also de ailed in he sec ion Running S a e, as he ac ual implemen a ion o his unc ionali y has been done in he d one’s i mwa e. To es ablish a communica ion be ween he RC Con olle wi h he d one, we a e using a couple o adio modules (E34-2G4D20D). Fi s , we send a message o he ansmi e one o e UART. The message is hen sen h ough he ai , and ecei ed by he o he in he d one. Apa om he message ha we wan o ansmi , he adio modules cons uc a mo e complica ed one, con aining he add esses, and o he ields ha a e no unde he con ol o us. Wha we know so a abou he ac ual communica ion is [10]: “The module has da a enc yp ion and comp ession capabili ies. The da a ansmi ed by he module in he ai is andom, and he da a in e cep ion loses i s meaning h ough s ic enc yp ion and dec yp ion algo i hms. The 56 da a comp ession unc ion has he p obabili y o educing he ansmission ime, educing he p obabili y o in e e ence, imp o ing eliabili y and ansmission e iciency.” Ou messages s uc u e is as ollows: Figu e 5-8. Message S uc u e I consis s o 7 by es, whe e he i s wo a e he ‘Heade ’ and es he ‘Da a’. The ‘Heade ’ is used o synch onise he ecep ion o he message. The i s ou by es o he ‘Da a’ ep esen s he inpu s on he po en iome e s o RC con olle om 0 o 255. 0 is sen when he po en iome e s a e in he le o down posi ion, whe eas 255 is sen when in he igh o up posi ions. The las by e shows he s a us o each bu on. In his case, only he leas h ee signi ican bi s a e used: 0b00000[B2][B1][BGLe ]. Whe e [B2] is he s a us o he Bu on 2, [B1] is he s a us o he Bu on 1 and [BGLe ] is he s a us o he bu on o he le gimbal. ‘1’ ep esen s p essed and ‘0’ no p essed. No e ha none CRC (Cyclic Redundancy Check) has been implemen ed, conside ed being unnecessa y, as he adio modules implemen eliabili y unc ions al eady. In addi ion, gi en ha we ecei e a new message each 30ms, we can a o d any punc ual e o . The code implemen a ion is shown. No e ha we need o copy each by e in UCA0TXBUF. /* Message Heade */ send('S'); send('T'); /* Send he message con en */ send(ADCMeasu eme s30[7]); /* A0 (P1.0) - Powe */ send(ADCMeasu eme s30[4]); /* A3 (P1.3) - Pi ch */ send(ADCMeasu eme s30[1]); /* A6 (P1.6) - Roll */ send(ADCMeasu eme s30[0]); /* A7 (P1.7) - Yaw */ bu onsS a us = (bu onB2P essedFo Su e<<2 | bu onP essedFo Su e<<1 | bu onBGLe P essedFo Su e); send(bu onsS a us); oid send(cha by e) { while (!(IFG2&UCA0TXIFG)); /* USCI_A0 TX bu e eady? */ UCA0TXBUF = by e; } 5.2 D one Con olle Fi mwa e In his sec ion, we a e going o de ail he d one’s i mwa e design, he p og am ha will be unning in he on- boa d ESP8266. To compile and load he code in o he mic ocon olle we ha e used A duino IDE 1.8.13. The main s uc u e o he d one’s i mwa e is shown in he image below. We can see he possible di e en s a us whe e he done can be, he main asks ha a e execu ed on each s a e, and he common asks, ha p o ides in o ma ion and ime managing o he i s ones. Figu e 5-9. D one’s Fi mwa e S uc u e – Main S a e Machine When he de ice is boo ed up, we execu e some s a -up ac ions o ini ialise he sys em, which leads o he E o s a e i unsuccess ul, o going o wa d o he nex s a us. A e being in he Console and Ready s a us, he d one is able o ly when in he Running s a e. Once he ligh inishes, he s a us changes o Finished. In wha ollows, we desc ibe each s a us and common asks sepa a ely. 5.2.1 S a -up S a e Func ionali y implemen ed in he ollowing unc ion: - se up() The s a -up sequence is execu ed a e he de ice is powe ed on, and allows us o ini ialise he sys em. In he nex image, we can see he ac ions pe o med. 64 esponsible o managing he lag and he da a. Be o e in e p e ing he ecei ed da a, we ese he communica ions wa chdog: As we also discussed in 5.1.4, he bu ons s a us a e encoded in he las by e o he message. The e o e, o decode i , we jus need o ead each indi idual bi . Rega ding he BGL bu on bi , we conside ha i is ac ually ac i a ed when ecei ing BUTTON_GBLEFT_PRESSED_COUNT_UP consecu i e 1’s: A e ha , he powe se poin is calcula ed. As we a e using wo joys icks whose es ing posi ion is in he middle, he main powe is selec ed by he pilo inc emen ally. To s a he ligh he le joys ick needs o be mo ed upwa ds, so he main powe is inc eased. The mo e e ical mo emen , he mo e a e o change o i . Addi ionally by p essing he bu on 2 (B2), he maximum a e is educed om MAX_REF_POWER_DELTA_PER_SAMPLE o MAX_REF_POWER_DELTA_PER_SAMPLE_BUTTON_PRESSED. These pa ame e s a e de ined in D oneCon ol_De ines.h. All he needed calcula ions a e done in he unc ion calcula eSe Pn Powe (), esul ing in he main powe , s o ed in se Pn Powe ( ange: 0 - 1). To ge he angles se poin s om he ecei ed da a, we i s il e hem, so hey do no change oo quickly. We oid emo eCon olle Re e enceCalcula ion( oid) { /* Low pass il e auxilia y a iables */ s a ic uin 8_ commDa aPi chHis o y[COMM_DATA_FILTER_LENGTH]; s a ic uin 8_ commDa aRollHis o y[COMM_DATA_FILTER_LENGTH]; s a ic uin 16_ bu onGBLe P essedCoun ; /* I da a ecei ed */ i (RCda aRecei ed == ue) { RCda aRecei ed = alse; newRe e ence = ue; /*** Feed he communica ion wa chdogs by se in he coun o ze o ***/ wa chDogCommCoun STe m = 0; wa chDogCommCoun LTe m = 0; (...) } /*** Bu ons ***/ bu onB1P essed = ((commDa a[COMM_DATA_BUTTONS_ID] & COMM_DATA_B1_MASK) > 0) ? 1:0; bu onB2P essed = ((commDa a[COMM_DATA_BUTTONS_ID] & COMM_DATA_B2_MASK) > 0) ? 1:0; bu onGBLe P essed = ((commDa a[COMM_DATA_BUTTONS_ID] & COMM_DATA_GBLe _MASK) > 0) ? 1:0; i (bu onGBLe P essed == ue) { bu onGBLe P essedCoun ++; } else { bu onGBLe P essedCoun = 0; } i (bu onGBLe P essedCoun >= BUTTON_GBLEFT_PRESSED_COUNT_UP) { bu onGBLe KeepedP essed = ue; } else /*** Calcula e he Se Poin s - 1***/ /* Main Powe */ calcula eSe Pn Powe (); ha e implemen ed ha o make su e ha he PID inpu s a e smoo h enough. Ne e heless, i can be e en ually emo ed, so he d one handling is imp o ed. Then he se poin s a e scaled om he aw da a [0, 255] o [0, MAX_REF_PITCH_DEG], [0, MAX_REF_ROLL_DEG] and [0, MAX_REF_YAW_DDPS]. These pa ame e s a e also de ined in D oneCon ol_De ines.h. A e doing ha we ha e he se poin s s o ed in se Pn Pi ch, se Pn Roll and se Pn YawdDPS. The las hing o be done is o apply he im alues. As said in he B2 bu on de ini ion abo e, “when he B2 bu on is p essed and eleased, he ecei ed pi ch and oll alues a e ozen un il he igh gimbal e u ns o he middle posi ion. These pi ch and oll angles ( im alues) will be he de aul se poin s (ins ead o ze o). Fu he mo emen s will be added o hem”. No e ha his unc ionali y has been implemen ed in he d one i sel : when he joys ick is in he any posi ion, we will always ecei e he same co esponden da a, ega dless o he bu on p essing. When we de ec a alling edge in B2, he se poin s a e s o ed in se Pn Pi chToHold and se Pn RollToHold. These alues will be o ced o be he se poin s ega dless o he posi ion o he igh gimbal, un il i each he middle posi ion. Then o de ec he middle posi ion, we wai un il he pi ch and oll da a a e wi hin SET_PNT_PITCH_ZERO_LOW_LIMIT, SET_PNT_PITCH_ZERO_HIGH_LIMIT and SET_PNT_ROLL_ZERO_LOW_LIMIT, SET_PNT_ROLL_ZERO_HIGH_LIMIT. These pa ame e s a e also de ined in he D oneCon ol.ino ile. F om his ime, he alues ecei ed will be added o he im ones (se Pn Pi ch += imPi c; se Pn Roll += imRoll;). /*** Apply il e o he e e ence pa ame e s ***/ commDa aPi ch = 0; commDa aRoll = 0; o (uin 16_ j=1;j<COMM_DATA_FILTER_LENGTH;j++) { commDa aPi chHis o y[j-1] = commDa aPi chHis o y[j]; commDa aRollHis o y[j-1] = commDa aRollHis o y[j]; commDa aPi ch += commDa aPi chHis o y[j-1]; commDa aRoll += commDa aRollHis o y[j-1]; } commDa aPi ch += commDa a[COMM_DATA_PITCH_ID]; commDa aPi ch = commDa aPi ch/COMM_DATA_FILTER_LENGTH; commDa aRoll += commDa a[COMM_DATA_ROLL_ID]; commDa aRoll = commDa aRoll/COMM_DATA_FILTER_LENGTH; commDa aPi chHis o y[COMM_DATA_FILTER_LENGTH-1] = commDa a[COMM_DATA_PITCH_ID]; commDa aRollHis o y[COMM_DATA_FILTER_LENGTH-1] = commDa a[COMM_DATA_ROLL_ID]; /*** Calcula e he Se Poin s - 2***/ /* Pi ch */ se Pn Pi ch = MAX_REF_PITCH_DEG - (MAX_REF_PITCH_DEG / commDa a_1_Mean) * commDa aPi ch; /* Roll */ se Pn Roll = - MAX_REF_ROLL_DEG + (MAX_REF_ROLL_DEG / commDa a_2_Mean) * commDa aRoll; /* Yaw ( eloci y) */ se Pn YawdDPS = MAX_REF_YAW_DDPS - (MAX_REF_YAW_DDPS / commDa a_3_Mean) * commDa a[COMM_DATA_YAW_ID]; 66 2) emo eCon olle Re e enceCalcula ion() In he igh hand o he p e ious low diag am we can see he way we upda e he con ol ou pu s o he mo o s, in mo o sUpda e(). Once he sample ime elapses (sampleTimeElapsed is ac i a ed), we pe o m he ollowing ac ions. Fi s we execu e he unc ion lyingCondi ionMoni o (), whe e we judge i he ligh has s a ed, based on he change o he angles since he execu ion o he a ming p ocedu e and he ecei ed powe se poin . The ac ual condi ions o de e mine he lying condi ion a e: o 1- The las FLY_CONDITION_IMU_FILTER_LENGTH (3 by de aul ) IMU eadings o pi ch o oll angles has changed by FLY_CONDITION_ANGLE_CHANGE (5 deg ees by de aul ). o 2- The powe se poin is equal o g ea e han FLY_CONDITION_REF_WARNING_POWER (42% by de aul ). o 3- FLY_CONDITION_TIMER_MS (50 milliseconds by de aul ) ha e elapsed since he p e ious wo condi ions we e ul illed. A e he h ee condi ions a e ue, we enable he ex a ea u e explained in Con ol Design – Ex a ea u es, by ac i a ing he in eg al pa o he PID. F om his momen , he con en o he unc ion lyingCondi ionMoni o () hen is no longe execu ed. i (bu onB1P essed == 0 && bu onB1P essedP e == 1) { bu onB1FallingEdge = ue; } bu onB1P essedP e = bu onB1P essed; i ((se Pn Pi ch < SET_PNT_PITCH_ZERO_HIGH_LIMIT && se Pn Pi ch > SET_PNT_PITCH_ZERO_LOW_LIMIT) && (se Pn Roll < SET_PNT_ROLL_ZERO_HIGH_LIMIT && se Pn Roll > SET_PNT_ROLL_ZERO_LOW_LIMIT)) /* The igh joys ick is in he middle posi ion */ { wai Un ilSe Pn Recei edZe o = alse; } se Pn Pi ch += imPi ch; /* Apply im alues */ se Pn Roll += imRoll; i (bu onB1FallingEdge == ue) /* Upda e im alues i bu on eleased */ { imPi ch = se Pn Pi ch; imRoll = se Pn Roll; se Pn Pi chToHold = se Pn Pi ch; se Pn RollToHold = se Pn Roll; wai Un ilSe Pn Recei edZe o = ue; bu onB1FallingEdge = alse; } i (wai Un ilSe Pn Recei edZe o) /* While he igh joys ick has no eached he middle posi ion */ { se Pn Pi ch = se Pn Pi chToHold; /* O e w i e he p e iously calcula ed alues*/ se Pn Roll = se Pn RollToHold; } The e o s be ween he IMU da a and he ecei ed se poin s a e calcula ed. No e ha he uni s o he i s wo a e deg ees, whe eas he yaw e o is measu ed in deci-deg ees pe second. The calcula ion o he IMU alues is desc ibed in he sec ion Common Tasks. Now, he PID con olle s a e implemen ed using he e Calcula ion() unc ion, which implemen s he PID o mulas desc ibed in Con ol Design – PID. /* Flying condi ion */ i ( ime T igge ed== alse && (se Pn Powe >= FLY_CONDITION_REF_POWER && (pi chChangeSinceS a > FLY_CONDITION_ANGLE_CHANGE || ollChangeSinceS a > FLY_CONDITION_ANGLE_CHANGE))) { imeFo FlyAc i a ion = cu en Time + FLY_CONDITION_TIMER_MS*1000; ime T igge ed = ue; beep(SHORT_BEEP); } i ( ime T igge ed== ue && cu en Time >= imeFo FlyAc i a ion) { lying = ue; d oneFlyingTimes amp = mic os()/1000; pi chA S a FlyingTime = IMUPi ch; ollA S a FlyingTime = IMURoll; /* Enable he in eg al componen o he PID con olle */ Ac i eTiPi ch = TiPi ch; Ac i eTiRoll = TiRoll; Ac i eKpPi ch = KpPi ch; Ac i eKpRoll = KpRoll; beep(SHORT_BEEP); } /*** Angle e o s calcula ion ***/ e o Pi ch = se Pn Pi ch - IMUPi ch; e o Roll = se Pn Roll - IMURoll; e o Yaw = se Pn YawdDPS - IMUYawdDPS; loa e Calcula ion ( loa T, loa Kp, loa Ti, loa Td, loa uP e , loa e o , loa p e E o , loa p e P e E o ) { loa u=0, q0, q1, q2; i (T!=0) { q0 = Kp*( 1 + T/(2*Ti) + Td/T ); q1 = Kp*( T/(2*Ti) - 1 - 2*Td/T ); q2 = Kp * Td/T; u = uP e + q0*e o + q1*p e E o + q2*p e P e E o ; } e u n u; } oid mo o sUpda e( oid) { (...) uPi ch = e Calcula ion(sampleTime, Ac i eKpPi ch, Ac i eTiPi ch, TdPi ch, uPi ch, e o Pi ch, p e E o Pi ch, p e P e E o Pi ch); uRoll = e Calcula ion(sampleTime, Ac i eKpRoll, Ac i eTiRoll, TdRoll, uRoll, e o Roll, p e E o Roll, p e P e E o Roll); uYaw = e Calcula ion(sampleTime, KpYaw, TiYaw, TdYaw, uYaw, e o Yaw, p e e o Yaw, p e P e e o Yaw); (...) } 68 Based on he ou pu s o each PID we calcula e and apply he composed powe o each mo o , as desc ibed in Con ol Design – PID. Ha ing done ha , we sa e he cu en da a in he snapsho s uc u e, so he da a can be e ie ed when he ligh inishes. 5.2.5 Finished and E o S a e Func ionali y implemen ed in he ollowing unc ion: - sendSnapsho () As said be o e, when no da a is ecei ed o 5 seconds, he s a e is changed o Finished. In his si ua ion, he mo o s a e shu down and he snapsho s (s uc In o snapsho [SNAPSHOT_MAX_NUMBER]) can be sen o e adio. These ac ions a e he same as hose done in he E o s a e. Due o memo y es ic ions, he maximum numbe o snapsho s is: #de ine SNAPSHOT_MAX_NUMBER 1500 As he snapsho s a e eco ded each 15ms (samplePe iod), i means ha he da a is cap u ed o 22.5 seconds. We can decide i we keep he i s o he las 22.5 seconds o he ligh wi h he line: iSnapsho = 0; /* Uncommen o o e w i e */, om he unc ion mo o sUpda e(). In o de o ac i a e he da a sending, a single da a message needs o be ecei ed. Fo example “ST00000”. powe M1 = se Pn Powe + uPi ch - uRoll - uYaw; powe M2 = se Pn Powe + uPi ch + uRoll + uYaw; powe M3 = se Pn Powe - uPi ch + uRoll - uYaw; powe M4 = se Pn Powe - uPi ch - uRoll + uYaw; pwm_M1 = ( PWMMAXRANGE / (1000.0 / PWM_FREQ) ) * (1 + powe M1); pwm_M2 = ( PWMMAXRANGE / (1000.0 / PWM_FREQ) ) * (1 + powe M2); pwm_M3 = ( PWMMAXRANGE / (1000.0 / PWM_FREQ) ) * (1 + powe M3); pwm_M4 = ( PWMMAXRANGE / (1000.0 / PWM_FREQ) ) * (1 + powe M4); analogW i e(M1, (in )pwm_M1); analogW i e(M2, (in )pwm_M2); analogW i e(M3, (in )pwm_M3); analogW i e(M4, (in )pwm_M4); snapsho [iSnapsho ]. imes amp = cu en Time/1000; snapsho [iSnapsho ]. e e encePowe = se Pn Powe *100; snapsho [iSnapsho ].se Pn Pi ch = se Pn Pi ch; snapsho [iSnapsho ].se Pn Roll = se Pn Roll; snapsho [iSnapsho ].se Pn Yaw = se Pn YawdDPS; snapsho [iSnapsho ].uPi ch = uPi ch*10000; snapsho [iSnapsho ].uRoll = uRoll*10000; snapsho [iSnapsho ].uYaw = uYaw*10000; snapsho [iSnapsho ].anglePi ch = IMUPi ch*100; snapsho [iSnapsho ].angleRoll = IMURoll*100; snapsho [iSnapsho ].angleYaw = IMUYawdDPS*100; 5.2.6 Common Tasks In his sec ion, he unc ionali y o he common asks is de ailed. Those se e as a sou ce o in o ma ion and iming o he main ac ions desc ibed in he p e ious sec ions. - imeFlagsManage() ask The imeFlagsManage() ask se es iming se ices o he o he . In he nex image, we can see he ou pu s o i . #de ine SNAPSHOT_MAX_NUMBER 1500 ex e n s uc s uc In o snapsho [SNAPSHOT_MAX_NUMBER]; oid sendSnapsho ( oid) { /* Shu down mo o s */ analogW i e(M1, (in )( PWMMAXRANGE / (1000.0 / PWM_FREQ) )); analogW i e(M2, (in )( PWMMAXRANGE / (1000.0 / PWM_FREQ) )); analogW i e(M3, (in )( PWMMAXRANGE / (1000.0 / PWM_FREQ) )); analogW i e(M4, (in )( PWMMAXRANGE / (1000.0 / PWM_FREQ) )); i (RCda aRecei ed == ue) /* Wai un il ecei e some da a h ough he adio module o s a p in ing */ { RCda aRecei ed = alse; delay(100); Se ial.p in ("%------------------------------ n n"); Se ial.p in (" es Name = 'T'; n n"); Se ial.p in ("KpPi ch = "); Se ial.p in (KpPi ch,5); Se ial.p in ("; nTiPi ch = "); Se ial.p in (TiPi ch,5); (...) o (uin 16_ iSnapsho =0;iSnapsho <SNAPSHOT_MAX_NUMBER;iSnapsho ++) { yield(); /* o buil in ESP8266 wa chDog */ /* End he ansmission i ound he i s emp y da a */ i (snapsho [iSnapsho ]. imes amp == 0) { iSnapsho = SNAPSHOT_MAX_NUMBER; con inue; } Se ial.p in (snapsho [iSnapsho ]. imes amp); Se ial.p in (", "); Se ial.p in (snapsho [iSnapsho ]. e e encePowe ); Se ial.p in (", "); Se ial.p in (snapsho [iSnapsho ].se Pn Pi ch); Se ial.p in (", "); Se ial.p in (snapsho [iSnapsho ].se Pn Roll ); (...) } } } 70 Figu e 5-16. D one’s Fi mwa e S uc u e – imeFlagsManage() ask - commUpda e() ask. The commUpda e() ask se es as an in e ace be ween he adio module and he i mwa e. I is s uc u ed as a s a e machine wi h h ee possible s a us, s o ed in commS a e. The ini ial one is he s a us ‘S’, whe e we wai un il ecei ing he i s by e o he messages heade . Once ecei ed he s a us is upda ed o ‘T’, whe e we expec o ge he second one. I any hing else ecei ed, we e u n o ‘S’. Al e na i ely, i he ull heade is co ec ly ecei ed, he s a us is changed o ‘Da a’, whe e we ead he subsequen 5 by es and s o e hem in commDa a[ ].The lag RCda aRecei ed is hen se o ue, so he o he asks can know ha new da a is a ailable. In he nex image, we can see a low diag am o his ask. Figu e 5-17. D one’s Fi mwa e S uc u e – commUpda e() ask - upda eIMU() ask. The upda eIMU() ask makes use o he RTIMULib lib a y o es ima e he pi ch, oll and yaw angles om he h ee accele a ions and angula eloci ies, p o ided by he accele ome e and he gy oscope o he IMU (MPU- 9250), espec i ely. The unc ion is called each 5ms om imeFlagsManage(). Fi s , we execu e he imu->IMURead() RTIMULib unc ion un il we ge a new se o ine ial da a. Then we call usion.newIMUDa a() o es ima e he angles om he p e ious da a. Al hough he IMU p o ides il e ed measu emen s al eady, in he s a ic es s, we ound ha his il e ing is enough only when he mo o s a e powe ed-o . When inc easing he powe , we s a o ge some noise in he measu emen s. As desc ibed in he men ioned sec ion, we could educe i by using isola ion g ound planes, bu we s ill needed o apply u he il e ing o hem. In he nex image, we can see i s s uc u e. 72 Figu e 5-18. D one’s Fi mwa e S uc u e – upda eIMU() ask - wa chDogComm() ask The wa chDogComm() ask supe ises he s a us o he communica ions, so ha i we do no ecei e da a o a ce ain pe iod o ime, a couple o ac ions a e pe o med: o I no ecei ing da a o 0.6 seconds (sho e m communica ion loss), he mo o s a e powe ed o . o I no ecei ing da a o 5 seconds (long e m communica ion loss), he main s a us o he d one is changed o Finished. The nex image shows i s s uc u e. Figu e 5-19. D one’s Fi mwa e S uc u e – wa chDogComm() ask - upda eUIn e ace() ask The upda eUIn e ace() ask con ols he s a us LED and he buzze . Rega ding he s a us LED, i is oggle each 500ms. No e ha in he commUpda e() ask he LED is also oggled when a new se o da a is ecei ed, so we will only see he 500ms oggling when no ecei ing da a. The buzze is ac i a ed depending on he execu ion o he beep() unc ion. When his unc ion is execu ed om any o he ask, as new en y is added o BeepSchedule. As we see in he example o he igu e below, i consis s o pai s o an ac ion and he ime when o execu e i . The s uc u e o he ask is also shown in he same igu e. 80 Figu e 6-7. Incidence o he Mo o ’s powe on he IMU angle measu emen s. Powe wi es immed and shielded. Now, inc easing he maximum es ed powe , we can see ha he noise inc eases when he powe eaches 65%, as shown in he nex image: Figu e 6-8. Incidence o he Mo o ’s powe (up o 80%) on he IMU angle measu emen s. Powe wi es immed and shielded. In he nex es e idence, we see an imp o emen a e doing hese wo ac ions: - Add an ex a g ound plane unde he PCB, which co e s an a ea bigge han he PCB - Modi y he RTIMULib con igu a ion o ecalib a e he IMU, so ha he ange o he accele a ions we e changed om ±8g o ±16g and he angula eloci ies ange om 1000 o 2000 deg ees pe seconds. Tha way, he noise is di ided by wo. Figu e 6-9. Incidence o he Mo o ’s powe (up o 80%) on he IMU angle measu emen s. Powe wi es immed and shielded. RTIMULib econ igu ed. Ex a g ound plane. The nex images show he implemen ed shields and he ex a g ound plane. Figu e 6-10. Shielded mo o s and ex a g ound plane o EMI educ ion. Top View 82 Figu e 6-11. Shileded mo o s and ex a g ound plane o EMI educ ion. Bo om View 6.2 Fligh Tes ing Wi h he ligh es s, we could e i y ha he comple e sys em wo ks in a eal scena io, and adjus he con olle ’s PID pa ame e s ollowing an i e a i e p ocess. In he nex h ee images, we can see he da a o he i s 22.5 seconds o a ligh es . The blue plo is he main powe ecei ed om he emo e con olle . The b own dashed line is he se poin o each angle. The eal angles a e shown in yellow and he con ol ou pu s in pu ple. As obse ed, he angles keep s able a ound he se poin , which was always ze o in his case. No e ha he yaw g aph shows he ac ual angle, espec o he ini ial one, no he angula eloci y. Figu e 6-12. Cap u ed da a du ing a ligh es - Pi ch Figu e 6-13. Cap u ed da a du ing a ligh es - Roll 84 Figu e 6-14. Cap u ed da a du ing a ligh es - Yaw Compa ing he simula ed PID’s pa ame e s alues, wi h he inal ones a e he ligh es s: Ini ial alues om simula ion Final alues om he ligh es s Kp Ti Td Kp Ti Td Pi ch 0.01 0.1 0.1 0.0018 0.7 0.1 Roll 0.01 0.1 0.1 0.0018 0.35 0.1 Yaw - - - 0.005 0.2 0 We can see ha he p opo ional gain needed o be educed, and he in eg al pa inc emen ed (i.e. o be less ac i e, no e ha he in eg al pa is p esen in he denomina o o he PID exp essions (4-4) om Con ol Design – PID) The de i a i e alues we e main ained. To conclude his sec ion, pa o one o he success ully s abilised ligh es s is shown in he ou ames below. Figu e 6-15. Video ames du ing a ligh es 86 7 CONCLUSIONS AND FUTURE WORK s pe wha we ha e desc ip ed in all he p e ious sec ions, he main conclusion o his p ojec is ha i is possible o implemen a unc ional d one quadcop e using low cos mic ocon olle s, like he ESP8266 and MSP430. Al hough i was no in ended a he beginning, we can also conclude ha du ing a d one design, he awa eness o he exis ence o elec omagne ic noise sou ces is key. Collec ed in his memo y, his p ojec could se e as an example o a design and he implemen a ion o a eliable embedded sys em, endo sed by s a ic and ligh es s. In addi ion, he adi ional me hodology o con ol sys ems was applied; s a ing om simula ions and ollowing wi h eal wo ld i e a ions, we achie ed a con ollable dynamic sys em. As a u u e wo k, he handling o he d one could keep being enhanced o ge mo e esponsi e and smoo he mo emen s. To achie e ha , mo e ligh es s and ex a ea u es would need o be conside ed, like dynamic change o PID pa ame e s, o he s udy o o he con ol app oaches. The implemen a ion o a Kalman Fil e o he a i ude es ima ions could also be a u u e wo k, so ha we do no need o use RTIMULib. Also, GPS and a i icial ision o au opilo unc ionali ies could be added, gi ing he possibili y o eco e om a possible loss o communica ions, o pe o m o he high le el asks demanded by he ma ke wi hin he d one’s g owing indus y, like su eillance, oo age eco ding o indus ial plan s main enance ac ions. A LIST OF FIGURES Figu e 2.1. ESP8266 Pinou 17 Figu e 3-1. Remo e Con olle Ci cui Schema ic. 26 Figu e 3-2. Remo e Con olle Layou . Top iew in ed and bo om iew in blue. 27 Figu e 3-3. P in ed Ci cui . Top laye on he le . Bo om laye on he igh . 28 Figu e 3-4. Assembled Remo e Con olle . Top iew on he le . Bo om iew on he igh . 28 Figu e 3-5. On-boa d Ci cui Schema ic. 29 Figu e 3-6. On-boa d ha dwa e Layou . We can see he op iew in ed and he bo om one in blue. 30 Figu e 3-7. On-boa d Ci cui P in ed Boa d. Top laye on he le and bo om laye on he igh . 30 Figu e 3-8. On-boa d Ci cui Assembly. 31 Figu e 3-9. D one Assembly. 31 Figu e 4-1. In blue: angles o con ol. In g een: di ec ion o o a ion o each mo o . 32 Figu e 4-2. PID Con olle Model – Combina ion o he h ee con olle s. 34 Figu e 4-3. Disc e isa ion o he e o cu e. 35 Figu e 4-4. Compa ison o he pi ch esponse when he in eg al pa is delayed (in blue) and no (in ed). 36 Figu e 4-5. Gene al o e iew o he Ma lab Simulink Model 37 Figu e 4-6. Simulink – Physical Model 38 Figu e 4-7. Simulink – Physical Model. Mo o s 39 Figu e 4-8. Simulink – Physical Model. Ha dwa e 40 Figu e 4-9. Simulink – Physical Model. Ba e y Holde 41 Figu e 4-10. Simulink – Visual ou pu o he physical model 41 Figu e 4-11. Simulink – Measu emen s Block 42 Figu e 4-12. Simulink – PID Con olle Model 43 Figu e 4-13. Simulink – PID Implemen a ion 43 Figu e 4-14. Compa ison o he pi ch angle esponse when using di e en PID pa ame e s 44 Figu e 4-15. Pi ch angle esponse when delaying he in eg al componen o he PID (in blue) 45 Figu e 4-16. Pi ch angle esponse when u he delaying he in eg al componen o he PID (in blue) 45 Figu e 4-17. Pi ch angle esponse when delaying oo much he in eg al componen o he PID (in blue) 46 Figu e 5-1. Remo e Con olle Fi mwa e Logic 47 Figu e 5-2. MSP430 Clock F equency con igu a ion pa ame e s 48 Figu e 5-3. MSP430 UART con igu a ion pa ame e s 49 Figu e 5-4. O e iew o he MSP430 ADC pe iphe al 50 Figu e 5-5. O e iew o he MSP430 Time pe iphe al 51 Figu e 5-6. MSP430 Low Powe Modes 53 Figu e 5-7. Use In e ace diag am o he Remo e Con olle 55 88 Figu e 5-8. Message S uc u e 56 Figu e 5-9. D one’s Fi mwa e S uc u e – Main S a e Machine 57 Figu e 5-10. D one’s Fi mwa e S uc u e – S a -up S a e 58 Figu e 5-11. RTIMULib-A duino Copy igh No ice and Pe mission No ice 59 Figu e 5-12. D one’s Fi mwa e S uc u e – Console S a e 60 Figu e 5-13. D one’s Fi mwa e S uc u e – Ready S a e 62 Figu e 5-14. Middle and A ming posi ions in he Remo e Con olle 62 Figu e 5-15. D one’s Fi mwa e S uc u e – Running S a e 63 Figu e 5-16. D one’s Fi mwa e S uc u e – imeFlagsManage() ask 70 Figu e 5-17. D one’s Fi mwa e S uc u e – commUpda e() ask 71 Figu e 5-18. D one’s Fi mwa e S uc u e – upda eIMU() ask 72 Figu e 5-19. D one’s Fi mwa e S uc u e – wa chDogComm() ask 73 Figu e 5-20. D one’s Fi mwa e S uc u e – upda eUIn e ace() ask 74 Figu e 6-1. commUpda e_ iming es esul s – Logic Analyse ou pu . 75 Figu e 6-2. mo o sUpda e_and_upda eIMU_ iming es esul s – Logic Analyse ou pu . 76 Figu e 6-3. Incidence o he Mo o ’s powe on he angle measu emen s, be o e doing any ac ion o educe i . 78 Figu e 6-4. Incidence o he Mo o ’s powe on he IMU angle measu emen s when he IMU is sepa a ed 15cm. 78 Figu e 6-5. D one con igu a ion du ing he i s bench es s. No e ha he mo o ’s powe cables a e long. 79 Figu e 6-6. Incidence o he Mo o ’s powe on he IMU angle measu emen s. Powe wi es immed. 79 Figu e 6-7. Incidence o he Mo o ’s powe on he IMU angle measu emen s. Powe wi es immed and shielded. 80 Figu e 6-8. Incidence o he Mo o ’s powe (up o 80%) on he IMU angle measu emen s. Powe wi es immed and shielded. 80 Figu e 6-9. Incidence o he Mo o ’s powe (up o 80%) on he IMU angle measu emen s. Powe wi es immed and shielded. RTIMULib econ igu ed. Ex a g ound plane. 81 Figu e 6-10. Shielded mo o s and ex a g ound plane o EMI educ ion. Top View 81 Figu e 6-11. Shileded mo o s and ex a g ound plane o EMI educ ion. Bo om View 82 Figu e 6-12. Cap u ed da a du ing a ligh es - Pi ch 83 Figu e 6-13. Cap u ed da a du ing a ligh es - Roll 83 Figu e 6-14. Cap u ed da a du ing a ligh es - Yaw 84 Figu e 6-15. Video ames du ing a ligh es 85 REFERENCES [1] h ps://www.esp essi .com/en/p oduc s/socs/esp8266, ESPRESSIF, ESP8266 O e iew [2] h ps://en.wikipedia.o g/wiki/ESP8266, Fea u es, SDKs, Ai-Thinke modules. [3] h p://www.eby e.com/en/p oduc - iew-news.aspx?id=153 [4] MPU-9250 P oduc Speci ica ion / PS-MPU-9250A-01, Re ision: 1.1 [5] MSP430G2x53, MSP430G2x13 Mixed Signal Mic ocon olle da ashee (Re . J) [6] h ps://en.wikipedia.o g/wiki/PID_con olle . PID con olle . [7] Con olle s Design. Con ol Enginee ing – Chap e 6. Teodo o Alamo Can a e o. [8] h ps://www.ma hwo ks.com/help/ae oblks/quadcop e -p ojec .h ml [9] MSP430x2xx Family Use s Guide (Re J). [10] h p://www.eby e.com/en/p oduc - iew-news.aspx?id=153, E34-2G4D20D Use Manual [11] h ps://osca liang.com/b ushed- s-b ushless-mo o /