Quadrocopter megvalósítása FPGA alapú vezérléssel

Átírás

1 MISKOLCI EGYETEM GÉPÉSZMÉRNÖKI ÉS INFORMATIKAI KAR Villamosmérnök mester szak Folyamatirányítás és ipari kommunikáció szakirány Automatizálási és Infokommunikációs Intézeti Tanszék Quadrocopter megvalósítása FPGA alapú vezérléssel Diplomamunka Név: Borbély András Neptun kód: QWGZ0K Tervezésvezető: Dr. Vásárhelyi József. 2014, Miskolc

2 Tartalom Bevezetés... 2 Quadrocopterek jellemzői Történelmi és általános áttekintés Quadrocopterek működésének áttekintése A kinematikai modell áttekintése A modell felépítése és hardverelemei A megvalósított modell felépítése A quadrokopter vezérlőjének hardveres felépítése A PmodGYRO bővítőkártya tulajdonságai és alkalmazása A PmodACL bővítőkártya tulajdonságai és alkalmazása A PmodWi-Fi bővítőkártya tulajdonságai és alkalmazása A PmodOC1 modul tulajdonságai és alkalmazása A PmodGPS kártya tulajdonságai és alkalmazása A quadrocopter szoftveres szerkezete Áttekintés A folyamatvezérlő és segéd program részek működése Inicializáló függvény Biztonság és hibakezelés Belső állapotok kezelése Szögfüggvények számításáért felelős folyamat Quadrocopter API Wi-Fi kommunikáció működése és konfigurálása Áttekintés Csatlakozás a hálózathoz A kommunikáció működése A VHDL hardverek és a belső buszrendszerhez kapcsolata A WiFi SPI hardverillesztő A quadrocon_0 hardverillesztő A PWM jelgenerátor működése... 31

3 4.7. A szabályozó működési elve A vezérlés és szabályozás szabálybázisa A szabályozó beállítása A szabályozó és vezérlő VHDL megvalósítása Szenzorfúzió Az aluláteresztő szűrő működése A szűrő megvalósítása VHDL-ben Az integrátor működése Az integrátor megvalósítása VHDL-ben Az FPV (First Person View) működése Összegzés Summary Irodalomjegyzék

4 1. Bevezetés A diplomamunka célja, hogy a közismert multirotoros drónok irányítástechnikai (vezérlési és szabályozási) feladatait FPGA-s (FPGA - programozható logikai kapumátrix) rendszer a chippen megoldással valósítsuk meg. A megépített modell szemlélteti az így elért eredményeket. A drónok civil jellegű fejlesztése és alkalmazásai az utóbbi években egyre nagyobb igényt mutat különböző alkalmazási területeken. Ide sorolhatóak a mezőgazdasági, telemetriai, katasztrófavédelmi, objektumvédelmi, őrzési és újabban csomagszállítás területei. A technológiai fejlődés lehetővé tette, hogy az ilyen jellegű rendszereket manapság relatíve olcsón és elég jó megbízhatósági fokkal lehessen megvalósítani otthoni keretek között. A modellezők számára könnyen hozzáférhető eszközökkel ez nem nehéz feladat, sőt sok olyan modell kapható a piacon, ami a dobozból kivéve használható és a nyílt forráskódjuk segítségével egyedi célokra fejleszthető. Jogos a kérdés, hogy mi újdonságot hordozhat ez a megoldás? A válasz az elektronikában és szoftveres megoldásban keresendő. A jelenleg kapható eszközök mikroprocesszoros vagy mikrokontrolleres feldolgozásra épülnek, ami olcsón, jó hatásfokkal működik egy korlátozott számítási kapacitás mellett. A diplomamunkában fejlesztett drón ettől egy eltérő elektronikai megoldásra épül, ún. FPGA-ra, amely lehetőségek sokaságát hordozza magában az egyedi felépítése és működési elve miatt. Így az előbb említett egyszerű megoldásokat többszörösen is képes túlszárnyalni rugalmasságával és elérhető számítási teljesítményével, kibővítve az adott drón alkalmazási lehetőségeit. A tárgyalt rendszer egy quadrokopter vezérlését és szabályozását látja el egy nexys 3-as digilentes fejlesztő kártyával és hozzá tartozó moduljaival. A fejlesztésnél fontos szempont volt, hogy a lehető legtöbb feladatot ellátó funkció hardveresen valósuljon meg, de szükség volt egy szoftprocesszor elhelyezésére is az FPGA-ban bizonyos célfeladatok egyszerűbb ellátására. Végkifejletben a drón Wi-Fi hálózaton keresztül irányítható egy laptophoz csatlakoztatott joystick segítségével és egy egyszerű FPV-s (First Person View) megoldással látható annak fedélzeti képe is. 2

5 2. Quadrocopterek jellemzői 2.1. Történelmi és általános áttekintés A quadrocopter vagy más elnevezésben quadcopter egy 4 propelleres elrendezésű forgószárnyas repülőgép kategóriájába tartozó légi jármű, ami elsősorban UAV-k (Unmanned Aerial Device) vagy drónok közkedvelt technikai megoldásai közé tartozik. Egyszerű elve és elrendezése kissé bonyolult szabályozástechnikai problémákat vetnek fel, ezért sajnos a rendszer irányítása megfelelő korrigálás nélkül, egy gyakorlott pilóta számára is túl nagy koncentrációt kíván. Érdemes megjegyezni, hogy a legelső ismert működő modellt Louis Breguet építette meg 1907-ben (1. ábra), amely szintén az első olyan forgószárnyas repülőgép közé tartozik, amely képes volt önállóan a talajról függőleges irányú mozgással felszállni, 1 perc repülési idővel pár lábnak megfelelő magassággal. A következő jelentős előre lépésnek tekinthető volt 1922-ben épült George de Bothezat szerkezete, amit az első sikeres helikopternek könyveltek el a történelemben (2. ábra). A leghosszabb repülési idő 2,5 perc volt, 9 méteres magasságban és természetesen csak szélcsendes időben volt használható. Ez a probléma a mai korszerű quadrokoptereket is érzékenyen érinti, viszont már elég jól képesek a szabályozás segítségével korrigálni a széllökések okozta elmozdulásokat. Emberi szállításra még nem alkalmazzák az előbb említett problémák miatt, de jelenleg folynak kísérletek ilyen téren is. [1] 1. ábra Breguet-Richet Gyroplane 2. ábra de Bothezat helikopter 3

6 2.2. Quadrocopterek működésének áttekintése A 4 fix állásszögű propeller felhajtóerejének növelését és csökkentését a motorok fordulatszámának változtatásával lehet elérni. Ha a rendszer jól ki van egyensúlyozva és a felhajtóerő mind a 4 motoron (F 1,F 2,F 3,F 4 ) (3. ábra) megegyezik, akkor az, függőleges irányban (Z B ) emelkedik illetve a felhajtóerő (F B ) és a súlyerő függvényében süllyed vagy lebeg. A propellerek elrendezésénél látható, hogy 1 pár óramutató járásával megegyező 1 pár pedig annak ellenkező irányában forog, ez azért szükséges, mert az ébredő forgatónyomatékok így egyenlíthetőek ki illetve változtathatóak meg, ami a helikopter függőleges (Z B ) tengelyű forgását okozza (3. ábra Ψ, 4. ábra d). Oldalirányú mozgása a quadcopternek úgy történik, hogy a megfelelő motorok fordulatszámának növelésével és az átellenben lévő motorok fordulatszámának csökkentésével beállítható egy bizonyos szögbe (forgó tengelyek ϑ és Φ és 3. ábra). Ennek az lesz a következménye, hogy a felhajtóerő egy része a dőlés miatt egy oldalirányú erő komponenst fog eredményezni ezzel gyorsítva az X B és Y B (3. ábra) irányú 3. ábra quadrocopter egyszerű fizikai modellje elmozdulásokat (4. ábra b, c). Ennek a felépítésnek a nagy előnye, hogy nem igényel a mozgások megvalósításához bonyolult mechanikát (pl. imbolygó tárcsát) viszont a szabályozás bonyolultabb. [2] [3] 4. ábra quadrocopter mozgási elvei (a. emelkedés, b.mozgás oldalra, c. mozgás hátra, d tengely körüli forgás) 4

7 2.3. A kinematikai modell áttekintése Mivel a diplomamunka témája elsősorban a quadrotor gyakorlati megközelítésével foglalkozik, ezért csak minimálisan tárgyalja az ezzel kapcsolatos matematikai és fizikai leírásokat. Szabályozástechnikai szempontból két összefüggést érdemes itt megemlíteni, a rendszerben ébredő forgatónyomatékkal (2.1 képlet) és a szöggyorsulással (2.2 képlet) kapcsolatos tenzoros leírásokat. 2.1 képlet A rendszerben ébredő forgatónyomatékok A multirotoron ébredő forgatónyomatékok (Ί B ) forrása a forgó propellerek és a levegő newtoni hatás-ellenhatásából származó erő és nyomaték. Az x tengelyű csavaró nyomaték az egymással átellenben lévő ω 1 és ω 3 fordulatszámú propellerek különbségéből értelmezhető, ahonnan a k a fordulatszámnak megfelelő erőhatás arányossági tényező és az L a két rotor távolsága a középponttól. Ugyanez vonatkozik az y tengelyre is csak az ω 2 és ω 4 fordulatszám különbsége fogja forgatni a rendszert. Mivel az egymás melletti propellerek forgásiránya ellentétes, ezért a z tengelyen ébredő forgását az ellentétes forgásirányú propellerek forgatónyomaték különbsége fogja adni. Innen a b a forgatónyomaték arányossági tényezője a fordulatszámok függvényében. A 2.2 képleten látható a rendszer 2.2 képlet a szöggyorsulás matematikai leírása a rendszerre szöggyorsulásának leírása tenzoros formában. Itt az I xx, I yy és I zz a rendszer forgató tehetetlenségi nyomatékát jelenti. A ΊΦ, Ίθ és ΊΨ a rendszer x,y és z tengelyen ébredő forgatónyomatéka. 5

8 3. A modell felépítése és hardverelemei 3.1. A megvalósított modell felépítése A quadcopter fizikai összeállítását más meglévő modellek alapján illetve az xcoptercalc online webes alkalmazás segítségével választottuk ki [4]. Fontos szempont volt, hogy megfelelő minőségű alkatrészekhez tudjunk jutni a költségek minimalizálása mellett és legyen cserealkatrész is a kísérletekhez. A helikopter váza erre a célra készült alumínium és üvegszálas kitként kapható (x600 quadrocopter frame kit), melyet csak össze kellett szerelni, ennek a súlya 400g körüli volt, befoglaló négyzete ~600mm. Az összeállítás tömege rendkívül fontos a várható repülési idő, illetve a tolóerők meghatározásánál. A motorok és hozzátartozó ESC-k (Electronic Speed Control) kiválasztásánál jó ár-érték arányban sikerült találni alkatrészt, viszont nem a legoptimálisabb paramétereket biztosítja. A motorok úgynevezett kefenélküli egyenáramú DC motorok (BLDC), amiket előszeretettel használnak a modellezők hasonló célokra jó fordulatszám szabályozhatóságuk és nagy teljesítményük miatt. Lényegében ezeknek a kialakítása külső mágneses forgórészből és tekercselt állórészből áll, 3 fázisnak megfelelő bekötéssel. A hozzá kapható ESC-k, mint fordulatszám szabályozók induktív visszacsatolás alapján jól be tudják szabályozni terhelés alatt a kívánt fordulatszám értéket. A választott BLDC-k egyenként teljesítményben 360 W-t képesek leadni a KV értékük pedig (KV adott feszültség fordulatszám erősítése, ami esetünkben a 11.1V-os LiPo akkumulátor mellet kb. akár /min maximális fordulatszámot jelenthet). A hozzátartozó ESC maximálisan 30 A áramot képes folyamatosan leadni, ami bőven kielégíti a motor teljesítmény igényét. Az 5. ábrán látható a vázra felszerelt motor és rajta propeller. A 5. ábra felszerelt BLDC motor és motorokhoz tartozó propeller befogó a tengelyre propeller egy körmös megfogóval csatlakozik. Sajnos ezeknek van egy nagy hátránya, méghozzá az, hogy a propellerek egyenetlenségéből fakadó radiális ütések amplitúdóját felerősítik és nagyon nagy mechanikai rezgést visznek be a rendszerbe. A jó propeller adapterek nem a tengelyre, hanem a külső forgórészre csatlakoznak csavarokkal és a kialakításuknál fogva a propeller közrefogó része közelebb van a csapágyhoz 6

9 így, rezgéstanilag sokkal stabilabb lesz a quadrocopter. A motorok nagy energiaigényét egy 6000 mah 3 cellás LiPo akkumulátor elégíti ki, ami maximálisan 30C-nek megfelelő áramot tud leadni üzemszerűen (6A*30C = 180 A). A feszültsége cellánként 3.7V így a kapcsain mérhető feszültség 11.1V. Mérete és tömege jól illeszkedik az összeválogatott quadrotorhoz. Ezeket az akkumulátorokat kellő óvatossággal kell kezelni, mert könnyen fel tudnak robbanni rövidzár vagy erős ütés hatására, illetve nagy töltőáram miatt. A propellerek kiválasztásánál érdemes megjegyezni, hogy nagyobb méret jobb hatásfokot eredményez, esetünkben műanyag kompozitot használtunk olcsósága és ütésállósága miatt. Amennyibe szükséges, jobb repülési tulajdonságok érhetőek el drágább karbonszálas rotorral. Az összeszerelt quadrokopter közelítőleges paraméterei a következő táblázatban vannak összefoglalva (1. táblázat): Akkumulátor LiPo kapacitás terhelhetőség töltőáram tömeg max. számolt terhelés 6 Ah 30 C 10 C 480 g C Quadrokopter befoglaló méret (prop. Nélkül) összeszerelt tömeg vihető súly becsült repülési idő 600 mm 1348 g ~1400 g 17 perc Motor BLDC Max. teljesítmény 360 W Kv érték 2200 üzemi terhelés árama A üzemi hatásfok 81.9% propellerek 10x4.5 Üzemei teljesítmény W maximális húzóerő ~1300 g ESC max. áram 30 A Az xcoptercalc által generált motor karakterisztika a 6. ábrán hozzávetőlegesen mutatja, hogy növekvő terhelés hatására hogyan változnak az optimális teljesítmény, fordulatszám és hőmérséklet viszonyok, amiből látható, hogy ez az összeállítás viszonylag jónak és működőképesnek bizonyul. 7

10 6. ábra Az összeállított quadrokopter motor karakterisztikája 3.2. A quadrokopter vezérlőjének hardveres felépítése A vezérlő lelkét egy Digilent gyártmányú Nexys 3-as fejlesztő panel képezi (7. ábra), amin egy xilinx spartan 6 szériás FPGA kapott helyet [5]. A board rendelkezik még projekt szempontjából: 8 db. kétállású kapcsolóval, 5 db. nyomógombbal, 4 db. pmod csatlakozóval, amihez a digilent szabványos bővítő kártyái csatlakoztathatóak, egy 4 dekádos 7 szegmenses kijelzővel és 8db. LED-el. Az FPGA-ban található logikai elemek száma sajnos kevés egy olyan távlati cél megvalósításához, amiben a VHDCI csatlakozón keresztül sztereo kép jeleit lehessen feldolgozni. Később kiderült, hogy maga a board sem alkalmas a digilentes sztereokamera modul fogadásához, mert nem 7. ábra Nexys3 fejlesztő board lettek impedancia illesztve a NYÁK-on található csatlakozó vezetősávjai. A panelhez a pmod csatlakozókon keresztül kommunikációhoz PmodWi-Fi, szögelfordulás és szögsebesség méréséhez Pmodgyro, gyorsulás értékek méréséhez PmodACL, az ESC-k feszültségszintjének szintillesztéséhez PmodOC1 bővítő modul csatlakozik illetve még egy PmodGPS modul (8. ábra). A panel 8

11 tápellátását a 11.1 V-os akkumulátorról egy 7805-ös feszültségstabilizátor segítségével oldottuk meg, melynek hőelvezetését az alumínium merevítés szolgáltatja (9. ábra). A szenzorok által meghatározott irányok úgynevezett x elrendezésben vannak felhelyezve, tehát ezek nem esnek egybe a motorkarok által meghatározott tengelyekkel, egyébként ennek 45 fokkal elforgatott változatát + -nak szokás nevezni. Ez nem jelent különösebb gondot a szabályozásra való tekintettel. 8. ábra quadrokopter vezérlő és bővítő moduljai 9

12 9. ábra a Nexys 3 board és ESC-k tápellátása A PmodGYRO bővítőkártya tulajdonságai és alkalmazása A PmodGYRO bővítőkártya (10. ábra) egy L3G4200D gyroscope IC-t tartalmaz, amely 3 tengelyű szögsebesség értékek visszaadására képes 16 bites felbontással, maximálisan ± 2000 DPS (Degree Per Second) tartománnyal. Ez a tartomány csökkenthető 500 és 250 DPSre, de tapasztalatok alapján nem ajánlatos a jelenlegi alkalmazásnál. A mintavételezési frekvencia maximálisan 800 Hz, és ezt a beállítást használjuk az FPGA-s jelfeldolgozásnál a pontosabb eredmény érdekében. A szögsebesség értékek szöggé alakítását az FPGA végzi el VHDLben írt integrátorral. A vibrációkra szerencsére nem annyira érzékeny, mint a gyorsulásmérő, de itt is 10. ábra PmodGYRO modul erős szűrés javasolt a szabályozás stabilitásának megőrzésének érdekében a szögsebességek visszacsatolásánál. A TDK-ban FIR alul áteresztő szűrővel sikerült ezt a problémát orvosolni. A gyroscope IC rendelkezik még egy kellemetlen tulajdonsággal, úgynevezett offset hibával (konstans eltolásként fogható fel az eredeti értéktől), amit szoftveres úton könnyen ki lehet kompenzálni. 10

13 A PmodACL bővítőkártya tulajdonságai és alkalmazása Ez a bővítőkártya egy ADXL345-s 3 tengelyű gyorsulásmérőt tartalmaz 10 bites felbontással és választható ± 2,4,8,16 g mérési tartománnyal (11. ábra). Esetünkben a ±2 g-s felbontás a legjobb opció, mivel az állandó gravitációs gyorsulást használjuk abszolút viszonyítási szögként a quadrokopterben. Ez azért szükséges, mert a gyroscope IC szögsebesség értéket ad vissza és szoftveresen kell kiintegrálni a jelet szögelfordulás meghatározásához, aminek kellemetlen mellékterméke az ún. drift. Ez azt jelenti, hogy a valódi értéktől a sokadik integrálás után valamilyen mértékben el fog 11. ábra PmodACL modul térni a számolt szög és ez a hiba általában idő függvényében egyre csak növekszik. A szokásos technikai megoldás a jelenlegi problémára gyroszenzor, magnetométer és gyorsulásmérő fuzionálása. A 3 szenzor jeleit valamilyen algoritmussal, pl. kalman, madgwick vagy complementary filterrel feldolgozzuk, amivel így egy viszonylag pontos értéket tudunk meghatározni a rendszer jelenlegi helyzetéről. A quadrotocopterbe csak a complementary filter került kipróbálásra egyszerű elve és kis erőforrás igénye miatt, de valójában szükségtelen volt, mert egy ennél sokkal triviálisabb módszerrel sikerült megfelelő eredményeket elérni. Fontos megjegyezni, hogy magnetométer hiányában a Z tengely körüli elfordulás nincs fuzionálva a drónban, deviszont tervezett, hogy a GPS modul irány adatait felhasználjuk erre a célra. A gyorsulásmérő nagyon érzékeny a rendszer rezgéseire, így szükséges volt alul áteresztő FIR szűrő alkalmazására a zavarok kiszűréséhez, illetve másik kellemetlen tulajdonsága még az, hogy számunkra lassan reagál a gyorsulásviszonyok megváltozására, ami végül is indokot ad a gyroscope IC párhuzamos alkalmazására. Mondhatni, hogy a két szenzor egymás hibáit javítja ki, mivel az utóbbi nagyon gyorsan képes mérni a szögváltozásokat. 11

14 A PmodWi-Fi bővítőkártya tulajdonságai és alkalmazása A kártyán található egy Microchipes MRF24B0MA-s Wi-Fi modul, amely képes működni a 2.4 GHz-es b,g és n-s szabványú AES vagy WEP kódolással ellátott Wi-Fi hálózatokon elméleti 2 Mb-s átviteli sebességgel, de ez gyakorlatban 1Mb. (12. ábra) maximális sávszélességet jelent. A bővítőkártyát a quadrocopter kommunikációjához használjuk valamilyen PC-vel. A rendszerbe beillesztett kártya automatikusan megkeresi az általunk beállított hálózatot és felcsatlakozik rá. Miután ez megtörtént a hálózaton megjelenő 12. ábra PmodWIF modul csomagokat továbbítja a quadrocopternek feldolgozásra. Ennél a kártyánál külön kihívást jelentett az FPGA-hoz való illesztése, mivel az őt kezelő összes réteg stack-je szoftveresen volt megírva a microchipes mikrokontrollerekre, ami erős indok volt szoftprocesszor alkalmazására a rendszerben. Az elég gyéren dokumentált stack implementálása rengeteg időt elvett a fejlesztésből és csak a kommunikációhoz legszükségesebb függvények kerültek át az IP stackből. A tapasztalatokra támaszkodva ezt a modult még véletlenül sem használnám későbbi Wi-Fi kommunikáció megvalósítására, egyrészről a problémás stack okán, másrészről a megbízhatatlansága miatt, ami tovább szoftveres foltozást igényelt a programozásnál és természetesen ár-érték arányban nagyon rossz választás volt. A manapság kapható Wi-Fi modulok majdhogynem mindent lekezelnek belülről, még a TCP/IP stacket is és nincsenek kompatibilitási problémák, hogy csak az adott gyártó eszközeit támogatják, mint ennél. A hatótávolsággal nem voltak különösebb gondok az eddigi tesztek alapján egy 30m-es látótávolságon belül megfelelően működik egy laptopon található integrált Wi-Fi-ről létrehozott hálózaton belül. [11] 12

15 R 1 2seR 1 0 K P 1 1NIP P 2 2NIP G N D G N D G N D G N D R 2 2seR 1 0 K P 5 0NIP G N D R 3 2seR 1 0 K P 3 3NIP R 4 2seR 1 0 K P 4 4NIP A PmodOC1 modul tulajdonságai és alkalmazása Ez a kártya (13. ábra) tulajdonképpen 4 db. nyitott kollektoros kapcsolású MMBT3904 típusú BJT tranzisztort tartalmaz, 4 db. bázis oldali 470 Ohmos ellenállással és 4 db. védődiódával. Terhelhetőségük 100 ma, 40V maximális feszültségbírással [6]. Kapcsolási rajza a 14. ábrán látható. 13. ábra PmodOC1 modul 14. ábra PmodOC1 kapcsolási rajz [6] A PmodOC1-et a Nexys3-as kártya 3.3 V-s és az ESC.-k 5V-s feszültség szintjének illesztésére lett használva, amit ki kellett bővíteni a 15. ábrán található 10kΩ-os felhúzó ellenállásokat tartalmazó kapcsolással, innen a P1, P2, P3 és P4 csatlakozik az ESC.-khez a P5 a modulhoz. 15. ábra ESC illesztő áramkör 13

16 A PmodGPS kártya tulajdonságai és alkalmazása A PmodGPS kártya (16. ábra) tartalmaz egy GPS MT3329 modult, amely GPS pozíció meghatározására alkalmas külső pontosítás nélkül 2.5 m-es pontossággal, ez tovább javítható DGPS rendszer kibővítéssel. Alkalmazási szempontból nem bonyolult a kezelése, standard UART kommunikációt használ és minden beállítás nélkül automatikusan visszaadja a GPS pozíciót, a föld mágneses mezejének orientációját és jelenlegi időt. A chip 3 fajta módban tud feléledni, egy ún. hideg, meleg és forró indítással. Az indítások idejében van különbség, mivel a GPS rendszer sajátossága miatt bizonyos információk idő függvényében változnak és ezeknek a szórása elég lassú, ezért a pontos pozíció meghatározása hideg indításban 2 perc is lehet. A modul alján található egy gombelem foglalat, ami szükséges a meleg és forró indításokhoz, ilyenkor a GPS modul nem felejti el a tárolt GPS pozíció meghatározásához szükséges adatok, ezért gyorsan akár pár másodperc alatt is fel tud éledni és pontos pozíciót adni. 16. ábra Pmod GPS modul 14

17 4. A quadrocopter szoftveres szerkezete 4.1. Áttekintés Az FPGA-ban egy szoftprocesszoros hibrid rendszer került integrálásra a feladat során. A szoftprocesszor nem más, mint egy logikai elemekből kialakított processzor, ami önállóan képes futni a chipen belül. Erre azért volt szükség, mert bizonyos funkciók megvalósítása sokkal könnyebb és erőforrás takarékosabb, mint ugyanazt VHDL kóddal kialakítani és logikai cellákba erőszakolni. A Xilinx Platform Studioban egy 32 bites FPU (lebegőpontos egység) nélküli Microblaze nevezetű szoftprocesszoros rendszert építettem fel AXI 4 interface busszal. A perifériák amiket VHDL kódban írtunk, ezen a 32 bites AXI 4 buszrendszeren keresztül kommunikálnak a processzormaggal. A RISC architektúrájú vezérlőn a kódfejlesztés C-ben vagy C++-ban történik, a Xilinx Software Development Kiten keresztül. Ennél a rendszernél nem használtunk objektumorientált programozást. Az C-ben futó programrészek elsősorban a Wi-Fi modul menedzselését, hardver inicializálását és felügyeletét, szögfüggvények számolását és debugger funkciókat látnak el. A VHDL-ben készült hardverelemek lényegében a quadrokopter periféria elemeinek illesztését és kommunikációját végzik a legalsó szinten, majd a kinyert szenzoradatokat szűrik, kondicionálják a szabályozás számára és a processzorból érkező vezérlő jelek alapján a quadrokopter szabályozását ellátják. A Nexys 3 az FPGA felkonfigurálásához szükséges bitstream fájlt BPI flashben tárolja, amit Xilinx Impacton sikerült beállítani, így minden indításnál a megfelelő jumpereléssel a quadrocopter szoftvere töltődik be. A Nexys 3 board szoftveres támogatásával a rajta található perifériák illesztése nem okozott különösebb gondot, illetve a PmodACL és PmodGYRO illesztőkártyához sikerült kész VHDL kódot találni a Digilent oldalán, így némi átalakítással az SPI kommunikáció kialakítása könnyen megtörtént. A rendszer vázlatának blokksémája a 17. ábrán látható. Narancssárgával jelölve a C nyelven írt részeket, zölddel a VHDL-ben készült perifériákat, kékkel a hardveres elemeket. [8] [9] 15

18 17. ábra quadrocopter szoftveres felépítése A PC-n futó Java alapú alkalmazás a joystick jeleit és a quadrocopter beállításához szükséges paramétereket UDP kereteken keresztül küldi a Wi-Fi hálózatra, illetve fogadja és megjeleníti a tőle érkező csomagokat. Az alkalmazást Bartók Roland Villamos MSc-s hallgató készítette NetBeans fejlesztőkörnyezetben erre a célra és némi átalakítással alkalmassá lett téve a vezérléshez. 16

19 4.2. A folyamatvezérlő és segéd program részek működése A helikopter szoftverszerkezetének legtetején a folyamatvezérlő áll, ami a szoftprocesszoron a működéshez szükséges folyamatokat egy előre kitapasztalt és beállított lefutási időn belül meghívogatja végtelen ciklikussággal, ez alól kivétel, a minden bekapcsolás után egyszer lefutó inicializáló függvény. A fő folyamatok a következőek: Wi-Fi menedzsment, üzeneteinek fogadása és feldolgozása (Wi-Fipoll(); függvény) UDP csomagküldés a hálózatra (UDPpacketsend();függvény) Biztonság és hibakezelés (safety(); függvény) A belső állapot kezelése (ioandstate(); függvény) Debug funkciókért felelős folyamat (debug(); függvény) Szögfüggvények számítása (angle_cor(); függvény) A Wi-Fi modul gyors és megfelelő működéséhez szükséges bugmentesítő függvény (deallocate_routine(); függvény) A folyamatvezérlő állapotábrája a következő (17. ábra): 17. ábra A folyamatvezérlő lehetséges állapotai 17

20 Inicializáló függvény A start állapotban az init_startup(); függvény hívódik meg, amely a quadrocopter szoftver és hardverelemeit inicializálja és konfigurálja. Itt történik meg a Wi-Fi modul beállítása és hálózathoz való csatlakozásának kérése amennyiben nem debug módban indult el a rendszer. Az ESC-k indításához szükséges vezérlőjeleket ekkor küldi ki a program, miután a PWM jelgenerátort start módba rakta. A gyroscope IC offset hibáját is itt kompenzálja ki oly módon, hogy egy kb. 10 mp.-s időtartományon belül mintavételezi, összegzi, majd átlagot vonva belőle beállítja az eltolási értékeket a perifériákban, továbbá a tengelyek integrátorának értékeit nullába állítja. A szabályozó paramétereinek beállítása is itt megy végbe Biztonság és hibakezelés A kapcsolat megszakadásánál, ami a beérkező folyamatos szórású vezérlőcsomagok kimaradását jelentik kb. 0,5 mp.-en belül, a hibakezelő a quadrocoptert azonnal leállítja. Ugyanez történik, ha a rendszerben nem várt állapot következik be, pl. a gyro és gyorsulásmérő értékek egymástól nagy eltérést mutatnak vagy véletlen átfordult a modell, ami nem megengedettnek minősül és ez tűnt a legbiztonságosabbnak a tesztelések során. A hibaállapotot kiválthatja a felhasználó által megnyomott vezérlőgomb a joystickon. A szoftprocesszortól függetlenül működik a belső szabályozás ezért a microblaze véletlen befagyása is kiválthatja ugyanezt az állapotot, ami eddig nem fordult még elő Belső állapotok kezelése A Nexys3 boardon található nyomógombok lehetőséget adnak a quadrocopter bizonyos viselkedéseinek beállítására programozás nélkül. Debugger módban van opció a motorok felpörgetésére egy fix biztonságos fordulatszámmal a rezgések melletti szenzorértékek vizsgálatára. Az UART-on a rendszer kiadott vezérlőadatai, szenzor értékei vehetőek le. A nyomógombok megnyomásával újra lehet kalibrálni a gyroscope IC-t és az integrátor értékeket le lehet nullázni, illetve újracsatlakozást lehet kérni a Wi-Fi hálózatra Szögfüggvények számításáért felelős folyamat A folyamatrész a gyorsulásmérő adatait hivatott szögértéké alakítani illetve opcionálisan a gyroscope IC integrátor értékeit felülírni túl nagy viszonylagos eltérések esetén. Ennek a feladatnak a C nyelvű megvalósítására helymegtakarítás céljából volt szükség, mert a szögfüggvények és vektorértékek számítása nem éppen a legjobban megoldható VHDL-ben. A gyorsulásmérő adatok visszaszámolását a 18. ábra szemlélteti 18

21 [10]. Ezekből A x, A y, A z a gyorsulásmérő tengelyeinek értékei a Φ,ρ,θ pedig a számolt dőlés szöge. A dőlés szögének meghatározásához az állandó gravitációs gyorsulás 18. ábra gyorsulásértékek és szögértékek viszonya vektorát használjuk fel, ami a helikopter gyorsulásmentes mozgásánál jól levehető. Ezt természetesen a függvény vizsgálja és csak bizonyos hibahatáron belül ad vissza szögértéket. 19

22 4.3. Quadrocopter API A VHDL kóddal létrehozott hardverek és a szoftprocesszorra írt kód illesztésének megkönnyítéséhez készült egy API jellegű függvénygyűjtemény, amivel paraméterezni és monitorozni lehet a szabályozót és vezérlőt. Ez a gyűjtemény tulajdonképpen kondicionálja és címzi a megfelelő helyre az adott értéket. Az API a következő függvényeket tartalmazza: ConRegWrite() A control regiszter írása. Ezzel lehet élesíteni a motorokat, engedélyezni a hardverek működését, resetelni és az integrátor értékek írását engedélyeztetni. 32 bites int értéket fogad. ConRegmotor1() Az 1-es jelű motor direkt vezérlőparaméterét lehet vele megadni. 32 bites előjel nélküli int értéket fogad. ConRegmotor2() A 2-es jelű motor direkt vezérlőparaméterét lehet vele megadni. 32 bites előjel nélküli int értéket fogad. ConRegmotor3() A 3-es jelű motor direkt vezérlőparaméterét lehet vele megadni. 32 bites előjel nélküli int értéket fogad. ConRegmotor4() A 4-es jelű motor direkt vezérlőparaméterét lehet vele megadni. 32 bites előjel nélküli int értéket fogad. gyro_integrator_write() A gyroscope IC integrátorának értékeit lehet felülírni a függvénnyel. 3 db. int típusú bemeneti paramétert vár, sorrendben: x integrátor értéke, y integrátor értéke, z integrátor értéke. Ezek a szögelfordulás nyers értékeit reprezentálják. P_xy_axis_omega() A szögsebességhez tartozó PID kör x és y tengelyének arányos erősítési tényezőjét írja be. Előjel nélküli int értéket vár. I_xy_axis_omega() - A szögsebességhez tartozó PID kör x és y tengelyének integráló tag erősítési tényezőjét írja be. Előjel nélküli int értéket vár. D_xy_axis_omega() - A szögsebességhez tartozó PID kör x és y tengelyének differenciáló tag erősítési tényezőjét írja be. Előjel nélküli int értéket vár. P_z_axis_omega() - A szögsebességhez tartozó PID kör z tengelyének arányos erősítési tényezőjét írja be. Előjel nélküli int értéket vár. I_z_axis_omega() - A szögsebességhez tartozó PID kör z tengelyének integráló tag erősítési tényezőjét írja be. Előjel nélküli int értéket vár. 20

23 PID_mode() A PID szabályozás módját lehet vele kiválasztani. Előjel nélküli int értéket vár. A lehetséges módok a következőek: - mode 0 PID szögre szabályozással - mode 1 PID szögsebességre szabályozással - mode: 2 PID kaszkád szög és szögsebesség szabályozással integrator_saturation() A szabályozáshoz tartozó integrátor értékek maximális telítését lehet vele megadni. Előjel nélküli int értéket vár. P_xy_axis_degree() - A szögelforduláshoz tartozó PID kör x és y tengelyének arányos erősítési tényezőjét írja be. Előjel nélküli int értéket vár. I_xy_axis_degree degree() - A szögelforduláshoz tartozó PID kör x és y tengelyének integráló tagjának erősítési tényezőjét írja be. Előjel nélküli int értéket vár. D_xy_axis_degree degree() - A szögelforduláshoz tartozó PID kör x és y tengelyének differenciáló erősítési tényezőjét írja be. Előjel nélküli int értéket vár. P_z_axis_degree degree() - A szögelforduláshoz tartozó PID kör z tengelyének arányos erősítési tényezőjét írja be. Előjel nélküli int értéket vár. I_z_axis_degree degree() - A szögelforduláshoz tartozó PID kör z tengelyének integráló tagjának erősítési tényezőjét írja be. Előjel nélküli int értéket vár. angle_x() Az x tengely vezérlőjelét írja be, ez lehet a szabályozási módnak megfelelően szög vagy szögsebesség érték. Int értéket vár. angle_y() Az y tengely vezérlőjelét írja be, ez lehet a szabályozási módnak megfelelően szög vagy szögsebesség érték. Int értéket vár. angle_z() A z tengely vezérlőjelét írja be, ez lehet a szabályozási módnak megfelelően szög vagy szögsebesség érték. Int értéket vár. accelero_value_x() A gyorsulásmérő x tengelyén mérhető nyers értéket adja vissza integer típusban. accelero_value_y() A gyorsulásmérő y tengelyén mérhető nyers értéket adja vissza integer típusban. accelero_value_z() A gyorsulásmérő z tengelyén mérhető nyers értéket adja vissza integer típusban. 21

24 LED_control() A Nexys3-as kártyán a nyomógombok felett található LED-eket működteti. Az int érték alsó 8 bitjéhez van hozzárendelve a 8db. LED. Értelemszerűen a megfelelő helyiértékű bit állapota határozza meg annak állapotát. 1 - bekapcsolva 0 kikapcsolva seg7_control() A Nexys3-as kártyán található 7 szegmenses kijelzőre írja ki a beadott int típusú szám hexadecimális értékét. throttle() A 4 motor vezérlőjelének az értékét egyszerre állítja, tulajdonképpen a gázadás mértékét jelenteni. Pozitív int értéket vár ig. degree_x() Az integrátor által meghatározott x tengelyű szögelfordulás értékeket adja vissza int típusban. degree_y() Az integrátor által meghatározott y tengelyű szögelfordulás értékeket adja vissza int típusban. degree_z() Az integrátor által meghatározott z tengelyű szögelfordulás értékeket adja vissza int típusban. omega_x_f() Az aluláteresztő szűrővel kondicionált x tengelyű szögsebesség értékét adja vissza int típusban. omega_y_f() Az aluláteresztő szűrővel kondicionált y tengelyű szögsebesség értékét adja vissza int típusban. omega_z_f() Az aluláteresztő szűrővel kondicionált z tengelyű szögsebesség értékét adja vissza int típusban. omega_x() Az x tengelyű szűretlen szögsebesség értéket adja vissza int típusban. omega_y() Az y tengelyű szűretlen szögsebesség értéket adja vissza int típusban. omega_z() A z tengelyű szűretlen szögsebesség értéket adja vissza int típusban. omega_x_comp() Az x tengelyű szögsebesség kompenzáló értékét írja be. Int típusú értéket vár. omega_y_comp() Az y tengelyű szögsebesség kompenzáló értékét írja be. Int típusú értéket vár. omega_z_comp() A z tengelyű szögsebesség kompenzáló értékét írja be. Int típusú értéket vár. buttoncheck() A Nexys3-as kártyán található nyomógombok állapotát adja vissza. A beadott int érték alsó 4 bitjének megfelelően az adott gomb állapotával tér vissza. (igaz vagy hamis) 22

25 switchcheck() - A Nexys3-as kártyán található kapcsolók állapotát adja vissza. A beadott int érték alsó 8 bitjének megfelelően az adott gomb állapotával tér vissza. (igaz vagy hamis). get_motor1_value() Az 1-es jelű motor aktuális vezérlő értékével tér vissza int típussal. A felvehető érték ig terjedhet. get_motor2_value() Az 2-es jelű motor aktuális vezérlő értékével tér vissza int típussal. A felvehető értékek ig terjedhetnek. get_motor3_value() Az 3-es jelű motor aktuális vezérlő értékével tér vissza int típussal. A felvehető értékek ig terjedhetnek. get_motor4_value() Az 4-es jelű motor aktuális vezérlő értékével tér vissza int típussal. A felvehető értékek ig terjedhetnek. 23

26 4.4. Wi-Fi kommunikáció működése és konfigurálása Áttekintés Az MRF24WB0MA modult a Microchip forgalmazza a saját mikrokontrollereinek Wi-Fi kommunikációjának biztosításához. A chip önmagában a legalsó MAC réteget tudja csak lekezelni minden mást szoftveres úton kellett megvalósítani. Ennek kiindulási alapjául szolgált a Microchip TCP/IP stack szoftvergyűjteménye, amely tartalmazza az említett modulnak a driverét is a microchipes platformokra.[12]. Jogos lehet a felvetés, hogy miért alkalmaztuk ezt a problémás modult, erre a legegyszerűbb válasz az, hogy ez volt elérhető. Az elég rosszul és néhol egyáltalán nem dokumentált szoftvergyűjtemény megértése és átalakítása sok időt vett igénybe, mivel maga a stack rengeteg olyan funkciót is tartalmaz, ami a feladat szempontjából felesleges. Így csak a legszükségesebb átalakított részek kerültek át a quadrocopter Wi-Fi kezelőjébe, némi megkötéssel Csatlakozás a hálózathoz A quadrocopter irányítása egy laptopról létrehozott előre konfigurált infrastructure vagy ad-hoc típusú Wi-Fi hálózatról lehetséges. Windows operációs rendszer alatt ez parancssor alól gyorsan megoldható, amire készült egy.bat kiterjesztésű parancsfájl, amely a következő egyszerű utasításokat tartalmazza (19. ábra): netsh wlan stop hostednetwork timeout 1 netsh wlan set hostednetwork mode=allow ssid=quadrocopter key=asdfghjkl netsh wlan start hostednetwork timeout 5 arp -s e-c0-09-cd pause 19. ábra konfigurációs állomány kódja 24

27 Az első parancs a meglévő hosztolt Wi-Fi hálózatot leállítja és utána a timeout utasítással vár 1 mp.-et arra, hogy ez a művelet befejeződjön. Ezután felkonfigurálja azt quadrocopter névvel és asdfghjkl WPA2 AES kulccsal [13]. A következő lépésben újra elindítja a Wi-Fi networkot és 5 mp. várakozás után az operációsrendszer ARP listájára regisztrálja a quadrocopter Wi-Fi moduljának MAC címét a IP címre, méghozzá a IP címet kapott eszköz hálózatára, ami esetünkben a laptop Wi-Fi modulja. A művelet megkezdése előtt a DHCP támogatást ki kell kapcsolni az adott hálózaton és statikus IP címet kell hozzárendelni az eszközökhöz. A helikopteren található modul csatlakozási konfigurálása a WF_connect.c fájlban lehetséges a következő definíciók által: #define MY_DEFAULT_SSID_NAME #define MY_DEFAULT_NETWORK_TYPE #define MY_DEFAULT_SCAN_TYPE "quadrocopter" WF_INFRASTRUCTURE WF_ACTIVE_SCAN #define MY_DEFAULT_CHANNEL_LIST {} #define MY_DEFAULT_LIST_RETRY_COUNT (5) #define MY_DEFAULT_EVENT_NOTIFICATION_LIST (WF_NOTIFY_CONNECTION_ATTEMPT_SUCCESSFUL WF_NOTIFY_CONNECTION_ATTEMPT_FAILED WF_NOTIFY_CONNECTION_TEMPORARILY_LOST WF_NOTIFY_CONNECTION_PERMANENTLY_LOST WF_NOTIFY_CONNECTION_REESTABLISHED) #define MY_DEFAULT_SECURITY WF_SECURITY_WPA2_WITH_KEY 20. ábra Az MRF24Wb0MA Wi-Fi modul csatlakozási konfigurációs paraméterei 25

28 Az SSID_NAME a csatlakozni kívánt hálózat nevét jelenti, a NETWORK_TYPE a hálózat típusát, itt Windows 8 alatt WF_INFRASTRUCTURE Windows 7 alatt pedig WF_ADHOC beállítással tud csatlakozni az operációsrendszerrel létrehozott AP-ra. A CHANNEL_LIST -el azokat a csatornákat lehet beállítani alapértelmezettként, amin keresi a Wi-Fi networkot, ha nem kap értéket, akkor mindegyiket végignézi. A RETRY_COUNT a hálózat csatlakozás próbálkozási számait jelenti. A NOTIFICATION_LIST -el megadhatjuk a hálózati esemény visszajelző menedzsment jelzéseket. A MY_DEFAULT_SECURITY a hálózat biztonsági módját jelenti. A hálózati kulcs megadása a következő formátumban történik: A 21. ábra tartalmazza a nyers hálózati kulcs formátumát, amelynek pontosan így kell kinézni a sortördelésekkel együtt. Ez nem más, mint a asdfghjkl WPA2 AES passphrase és a "quadrocopter" Wifi hálózat SSID nevéből kapott hexadecimális kulcs. Ez egy időigényes kriptográfiai művelet végeredménye, és az első kapcso7lódás esetén ki kell számolnia a modulnak, illetve bármilyen hasonló eszköznek. Ez az idő akár 30 mp. is lehet, de az ilyen megadási módnál ez 1-2 mp.-re lecsökken. A nyerskulcsot a [14]-es számú link alatt generáltam. #define MY_DEFAULT_PSK "\ \x1c\x1b\x55\x19\x44\xaa\x7f\x2e\ \x00\x0b\x0d\xe9\xb7\x46\x23\x47\ \x80\x37\xab\x6f\x11\xf1\xbe\x5b\ \x4b\x47\xdc\xce\x28\x14\x4e\xae" A kommunikáció működése Miután a modul felcsatlakozott a hálózatra, csak kizárólag a saját MAC címére küldött csomagokat fogadja. A megfelelő keretfej megléte kell a feldolgozáshoz, máskülönben eldobja. DHCP és ARP támogatás kimaradt a megbízhatóbb és gyorsabb működés miatt, ezért szükséges az előbb tárgyalt parancsfájlban felkonfigurálni az ARP listát. A Java programból érkező csomagok standard UDP-t használnak. A következő táblázat tartalmazza a kommunikációs protokoll parancsait (2. táblázat). 21. ábra A nyers hálózati kulcs formátuma 26

29 2. táblázat Az UDP kommunikáció protokoll táblázata PC->quadro X dőlés Y dőlés Z tengely forgás OffsetAB OffsetCD Vészleáll Gázadás Keretfej Utasítá s Byte1 Byte 2 Byte 3 Byte 4 0x6 0x x10 (dec) 0x00 0x00 0x00 Byte1: sebesség 0x6 0x x11 (dec) 0x00 0x00 0x00 Byte1: sebesség 0x6 0x6 1;0; x16 (dec) 0x00 0x00 0x00 balra, áll, jobbra 0x6 0x6 előjeles eltolás A x19 inta inta intb intb B 0x6 0x x1A intc intc intd intd előjeles eltolás C-D 0x6 0x xA0 0x00 0x00 0x00 0x00 0x6 0x xA1 (dec) 0x00 0x00 0x00 Byte1: sebesség quadro->pc gyroxszög gyroyszög gyrozszög 0x6 4 0x6 4 0x6 4 0x6 4 0x20 int1 int2 int3 int4 intx: szögelfordulás 0x6 inty: 4 0x21 int1 int2 int3 int4 szögelfordulás 0x6 4 0x22 int1 int2 int3 int4 intz: szögelfordulás Az X dőlés és Y dőlés a tengelyirányú dőlések nagyságát adja meg egy 30 fokos maximális kitérés tartománya mellett, a középérték, vagyis 0 fok 128 decimális értéknek felel meg. A Z tengely körüli forgás fix sebességre van beállítva, amely 3 állású lehet. Az offset AB és CD a motorok trim értékét adják meg, ezeket a súly és felhajtóerők eloszlásának függvényében kell beállítani, ami elméletben annyit jelent, hogyha megfelelő a trimmerelés, akkor gázadáskor ideális esetben merőlegesen repülne a quadrocopter felfelé. A gázadás értékével az összes motor fordulatszámát lehet egyszerre növelni. A PC oldal felé a quadrocopter szögelfordulásainak adatai a gyroxszög, gyroyszög, gyrozszög formátum szerint kerülnek ki a hálózatra broadcast címzéssel. A vezérlőadatok ciklikus küldése a PC-n futó Java alkalmazásban állítható, jellemzően 0,1 másodperces frissítési időkkel használva. A fejlesztés során néha tapasztalható volt, hogy a quadcopter Wi-Fi modulja befagy illetve, nagy késéssel juttatja el az adatokat, ezért a folyamatvezérlőbe be lett építve egy ciklikus függvény, amely időnként üríti a WiFi modul belső csomagok tárolására szolgáló cache-ét. Itt még szeretném 27

30 megjegyezni, hogy nem megszakításos alapon, hanem pollingolással kezeljük az MRF24WB0MA-t, ami talán az egyik oka lehet az előbb említett problémának A VHDL hardverek és a belső buszrendszer kapcsolata A szoftprocesszor a VHDL-ben írt hardverekkel a 32 bites AXI 4 interface buszon kommunikál. Az egyes perifériák illesztését a Xilinx Platform Studion belül generáltam a create or import peripherial menüponttal, ahol az AXI 4 Lite egyszerű vezérlőregiszteres mintát használtam. Itt mindegyik perifériához egy 32 bit nagyságú, maximum 32 db. írható vagy olvasható vezérlőregisztert lehet rendelni. A projektben a digilentes platform support mellett, ami automatikusan felillesztette a Nexys3 hardveres illesztőit, még 2 db. custom periféria kapott helyet. Ebből az egyik a Wi-Fi modul SPI a másik pedig a quadrocopter többi perifériáját összefogó illesztő. (22. ábra). 22. ábra Az AXI busz perifériái A busz master a Microblaze_0 szoftprocesszor. A sárgával jelölt perifériák a Nexys 3 board support package hardverelemei. Innen a DIP_Switches_8bit a kapcsolók, a LEDs_8bits a kapcsolók felett található LED-ek, a Digilent_SevSEg_Disp a 7 szegmenses kijelző, az RS232_UART az USB-s UART, és a Push_Buttons_4Bits a nyomógombok periféria illesztői. Értelemszerűen a zölddel jelölt a quadrocon_0 a quadrocopter, a wifispi_0 pedig a Wi-Fi SPI kommunikációt megvalósító hardver illesztője. 28

31 A WiFi SPI hardverillesztő A Wi-Fi modul SPI kommunikációjához 3 vezérlőregisztert használunk, amelyek a következő táblázatban vannak listázva (3. táblázat): Wifispi_0 Regiszter neve Funkciója Hozzáférés slv_reg0 SPI Tx adat/állapot Írás slv_reg1 SPI Rx adat Olvasás slv_reg2 IO kezelés Írás/Olvasás slv_reg2 bit 0 bit 1 bit 2 bit 3 bit 4 funkció Interrupt Ssout Reset WP (flash) Hibernate slv_reg0 bit (0-7) bit 8 bit 9 funkció Adat Tx Start Tx busy 3. táblázat A Wifispi_0 periféria regiszterkiosztása Az SPI a kiküldendő bájtot az slv_reg0, 0-7-ig tartó bitjeiről olvassa be akkor, ha a Start Tx (slv_reg0(8)) bitje 1-es állapotba kerül. A slv_reg0 9. bitje jelzi a hardver foglaltságát, amennyiben üzenetet küld vagy fogad, akkor ez a bit 1-es állapotot vesz fel. A beérkezett adat a slv_reg1-ből olvasható ki. Az slv_reg2 a Wi-Fi modul hardveres lábait vezérli, ami egyaránt ki és bemenet. A táblázatból az Interrupt a megszakítás, az SSout a slave eszköz kiválasztó, a Reset a resetelő, a WP a flash írás és olvasás, a Hibernate pedig a hibernáló lábat mappeli. A VHDL modul kifejezetten abból a célból készült, hogy a megfelelő sebességű, protokoll szerint elvárt kommunikációt biztosítson a szoftprocesszornak. 29

32 A quadrocon_0 hardverillesztő A quadrocopter magját képező hardvert 32 db. szintén 32 bit szélességű regiszterrel generáltuk, ami idővel már nem volt elég a fejlesztés során kibővített részeknek, így manuálisan 64 regiszternek megfelelő tartományt kellett kialakítani a buszvezérlő VHDL kódjának átírásával. A kibővített részt egy kisebb trükkel lehet elérni, méghozzá a slv_reg31 0. bitjének írásával lehet választani, hogy a két 32 bites mappelés közül melyiket szeretnénk használni. A kiosztást a 4. táblázat tartalmazza: slv_reg31(0) = 0 slv_reg31(0) = 1 slv_reg0 control regiszter Írás x tengelyű gyorsulásmérő szűrt Olvasás slv_reg1 Motor1 offset Írás y tengelyű gyorsulásmérő szűrt Olvasás slv_reg2 Motor2 offset Írás z tengelyű gyorsulásmérő szűrt Olvasás slv_reg3 Motor3 offset Írás külső PID P tag értéke xy Írás slv_reg4 Motor4 offset Írás külső PID I tag értéke xy Írás slv_reg5 x tengely szögelfordulás Olvasás külső PID D tag értéke xy Írás slv_reg6 y tengely szögelfordulás Olvasás külső PID P tag értéke z Írás slv_reg7 z tengely szögelfordulás Olvasás külső PID I tag értéke z Írás slv_reg8 x tengely szögsebesség Olvasás külső PID D tag értéke z Írás slv_reg9 y tengely szögsebesség Olvasás x tengelyű szögsebesség szűrt Olvasás slv_reg10 z tengely szögsebesség Olvasás y tengelyű szögsebesség szűrt Olvasás slv_reg11 x tengely szögelfordulás komp. Írás z tengelyű szögsebesség szűrt Olvasás slv_reg12 y tengely szögelfordulás komp. Írás slv_reg13 z tengely szögelfordulás komp. Írás slv_reg14 x tengely szögfelülírás Írás 4. táblázat A quadrocon_0 periféria slv_reg15 y tengely szögfelülírás Írás regiszterkiosztása slv_reg16 z tengely szögfelülírás Írás slv_reg17 x tengely gyorsulás Olvasás slv_reg18 y tengely gyorsulás Olvasás slv_reg19 z tengely gyorsulás Olvasás slv_reg20 belső PID P tag értéke xy Írás slv_reg21 belső PID I tag értéke xy Írás slv_reg22 belső PID D tag értéke xy Írás slv_reg23 kimenő ESC1 jel értéke Olvasás slv_reg24 kimenő ESC1 jel értéke Olvasás slv_reg25 kimenő ESC1 jel értéke Olvasás slv_reg26 kimenő ESC1 jel értéke Olvasás slv_reg27 belső PID P tag értéke z Írás slv_reg28 kívánt x tengelyű dőlés Írás slv_reg29 kívánt y tengelyű dőlés Írás slv_reg30 gázadás mértéke Írás Amiről érdemes még itt szót ejteni az a control regiszter kiosztása, amely az 5. táblázatban látható. 30

33 slv_reg0 bit 0 bit 1 bit 2 bit 3 funkció motor engedélyező Reset Start integrátor felülír 5. táblázat A control regiszter kiosztása A bit 0-vál lehet engedélyezni a motorokat vezérlő ESC-k számára a vezérlőjel kiadását, ami elsősorban biztonsági okból került bele. A Reset az összes quadrocon_0-ban található hardver részt alapállapotba illetve a Start bit indító állapotba rakja. A bit 3 engedélyezi a szögelfordulást számoló integrátor jelenlegi értékének felülírását. Ezt a funkciót a gyorsulásmérő abszolút szögétől való nagy eltéréskor használja ki az algoritmus A PWM jelgenerátor működése Az ESC-k vezérléséhez hasonló jelet kell kiadni, mint a szervó motoroknál használatos PWM jel, tehát ezeknek az irányítása is egy egyszerű négyszögjel alapú impulzussorozattal történik. Ezt a 23. ábra szemlélteti [16]. 23. ábra Az ESC-t vezérlő PWM jel [15] A jel periódus ideje 20ms. és az alapértéknek számító kitöltési ideje 1 ms., a maximális érték pedig 2ms.-nál értelmezendő, így a középérték 1.5 ms.-ot jelent. Az ESC jellemzően 128 különböző értéknek megfelelő kvantálással dolgozik. A VHDL-ben írt PWM generátor ig fogadja el a bemenő vezérlőjeleket. Az időzítő számlálója a 100MHz-es órajel alapján ig számol, nullázza magát és elindítja a 4 ESC kitöltését generáló 4 db. számlálót, amely a (Érték*100) képlet alapján váltja át állapotát és vár a következő ciklusig. A jelgenerálás csak akkor működik, ha a control regiszter Start bitje 1-es és motor engedélyező bit is 1-es állapotú. Az FPGA lábain mért jelek a 24. ábrán láthatóak 0 értékű vezérlésnél. 31

34 24. ábra Az FPGA által generált PWM jelek 0 vezérlő értéknél Ahogy az oszcilloszkóp (24. ábra) kijelzőjén is látható, a jelek negáltan jelennek meg és azért, mert a es fejezetben tárgyalt szintillesztő kapcsolás fordítani fogja őket, tehát az ESC-hez kikerülő PWM a számára elfogadott kondícióban lesz. A méréséket egy Tektronix MDO4104-es típusú oszcilloszkópon végeztük el, amely mutatja a bal alsó sarokban a jelek kitöltésének idejét. A lábakat logikai analizátor bemeneteire kötöttük. További mérések középértékben (512) a 25. ábrán és változó értékekben a 26. ábrán láthatóak. 32

35 25. ábra Az FPGA által generált PWM jelek (512) vezérlő értéknél 26. ábra Az FPGA által generált PWM jelek 1 csatorna (1024), 2 csatorna (0) 3 csatorna (256) 4 csatorna (512) 33

36 4.7. A szabályozó működési elve A quadrocopterben található szabályozás, a joystickon kapott alapjel és a szenzorok által meghatározott pozíció értékek felhasználásával beállítja annak kívánt helyzetét. A felhasználó a joystick botját mozgatva tudja x és y irányú tengelyek szerint a helikopter dőlését megváltoztatni és ennek megfelelően oldalirányban mozgatni. Az egyik görgő segítségével lehet a motorok felhajtóerejét növelni vagy csökkenteni, illetve egy másik görgővel a Z tengelyű elfordulást lehet beállítani. A szabályozás, ami esetünkben egy kéthurkos kaszkád PID, ezeknél a műveleteknél kizárólag csak az alapjel által meghatározott szögértékek tartását végzi el. A működési blokkvázlata a 27. ábrán látható. 27. ábra A szabályozókör elvi működése A belső kör a szögsebesség visszacsatolásából a beállás gyorsaságának jellegét a külső kör a szögérték visszacsatolásából az irányításra való viselkedését vagy mondhatni minőségét határozza meg. A kettős PID használata sokkal pontosabb és gyorsabb reagálást eredményez, az egyszerű PID-el ellentétben, viszont a beállítása bonyolultabb, illetve több hardverelemet kell kialakítani az FPGA-ban. A PID valójában 3 irányítástechnikai tag összegeként fogható fel, itt az arányos P tag egy szorzó tag, az I egy integráló jellegű a D pedig differenciáló viselkedést mutat. Az arányos tag csak a hibajel értékét szorozza fel, az integráló tag a pontosabb beállást segíti és lomhábbá teszi a rendszert, a D tag egyfajta gyorsító tagként érzékelhető [17]. A VHDL hardverünk, amely ezeket a funkciókat valósítja meg az egyes tagok szorzó értékére 16 bites integer számot vár. Az I érték telítésére egy fix limit van meghatározva illetve minden resetnél nullázódik. Az x tengelyű és y tengelyű kitérések szabályozó köre függetlenül, de azonos értékek mellett dolgoznak, azért mert a fizikai rendszer szimmetrikus. A jelenlegi fejlesztési állapotban a PID szabályozási módjait és paramétereit futás időben lehet állítani a PC-n keresztül. A szabályozási módok következőek: 34

37 - 1x PID szögérték hibajelként - 1x PID szögsebesség hibajelként - kaszkád PID szög illetve szögsebesség visszacsatolással A szögsebességre való visszacsatolási szabályozási mód olyan viselkedési ad a quadrokopternek, ami hasonló a modellezők által közkedvelt 450-es típusú lapátszög állítható helikopterek irányításához, viszont egy nehezen kontrollálható vezetést tesz lehetővé, de ezzel akár 3D -s ügyességi mutatványokra is alkalmassá teszik az eszközt. Elsősorban ebben a módban lehet beállítani a belső kör stabilitását ellenőrzött körülmények mellett. A szögértékre való szabályozás a külső körnek felel meg itt gyakorlatilag egy 1 körös PID szögre szabályozás valósul meg. A kaszkád PID a két szabályozási stratégiát kombinálva egy nem lineáris jelnek megfelelő szabályozási karakterisztikát ad ki, amely nagymértékben növeli a kör stabilitását és gyorsaságát. A tapasztalatok azt mutatják, hogy az előbb tárgyalt algoritmus nagyon érzékeny a mechanikai rezgések által generált zajra, ami képes a repülés minőségét nagymértékben rontani, ezért szükség volt FIR aluláteresztő szűrők tervezésére a helikopterben A vezérlés és szabályozás szabálybázisa A vezérlőjel hatására a szabályozás végeredményeként kialakuló beavatkozó jel, ami a motorok fordulatszámát adja meg, a következő szabálybázis alapján értelmezhető (4.1 képlet): ( é ő) ( é ő) ( é ő) ( ) ( á á ) ( é ő) ( é ő) ( é ő) ( ) ( á á ) ( é ő) ( é ő) ( é ő) ( ) ( á á ) ( é ő) ( é ő) ( é ő) ( ) ( á á ) 4.1 képlet A quadrocopter motorjainak szabálybázisa 35

38 28. ábra A motorok forgásának és tengelyeinek viszonya A 28. ábra alapján és az 1. képletből látható, hogy dőlés esetén az egyik motor fordulatszámát növeljük és a másik oldalit pedig ugyanannyival csökkentjük azért, hogy a felhajtóerő állandó maradjon. A Z tengelyű elfordulás beállítása is hasonlóan történik, csak ebben az esetben egyszerre csökkentjük a szögelfordulásnak megfelelően a két azonos forgásirányú propeller fordulatszámát és a másik kettőét pedig ugyanannyival növeljük, így a felhajtóerő és a dőlés szöge nem fog megváltozni. Értelemszerűen a motor trimmerelés offset értékei és a gázadás felhajtóereje is hozzáadódik az adott motor vezérlőjeléhez A szabályozó beállítása Az optimális szabályozási beállítása eltérő modellfelépítéseknél eltérő paraméterekkel lehetséges a különböző felhajtóerő, súlyponteltolódás és rendszer tehetetlenség miatt. Első lépésben célszerű a PID paraméterek 1-be állításával a szabályozás letiltása és a súlypont a középponttól való eltolódásának függvényében a felhajtóerőkkel kiegyensúlyozni a modellt. Ez lényegében úgy történik, hogy a légpárnahatás figyelembevételével megnézzük a szabályozatlan modell természetes dőléséből adódó elmozdulását, ami egy bizonyos jól 36

39 meghatározható irányt fog jelenteni. (Ezt óvatosan kell végezni, mert felborulhat a rendszer.) Ebből adódóan amerre a modell el szeretne mozdulni az azon az oldalon lévő motorokra egy bizonyos offset vezérlési értéket (eltolási) adunk, melynek hatására megnövekedik a felhajtóerejük, így a súlyponteltolódásból származó forgatónyomaték több ilyen próbálkozás után ki fog egyenlítődni. Tehát a motor offsetek állítása addig célszerű, még a multirotor a gázadáskor közel merőlegesen akar felrepülni és nem próbál meg valamelyik irányban eldőlni. Még egyszer kihangsúlyoznám, hogy a légpárnahatás sokat hamisíthat, ezért célszerű ilyen célból tesztpadot csinálni, amely a talajszint felett kb méter magasságban tartja a quadrocoptert. Amennyiben a forgatónyomatékok beállítása megtörtént, akkor következő lépésben, tesztpadon vagy kikötözéssel a kaszkád belső körét célszerű paraméterezni. Az arányos tag értékének növelésével közel az oszcilláció határáig érdemes játszani vele és valamennyire ettől a határértéktől visszább venni egy meghatározott értékkel. Fontos megjegyezni, hogy az ESC-k fordulatszámának szabályozása nem a legjobb, ezért feltöltött akkumulátor állapotra érdemes beállítani, ellenkező esetben a merülés határánál lévő akkumulátor paraméterei oszcillációt és stabilitás vesztést okozhat. Az I tagra kis értékeket érdemes adni, ha nagyon zavaros a szenzorjel, azzal van mód csillapítani azoknak ráhatását a rendszerre, illetve szeles időben stabilabbá teszi a széllökésekre való reagálást- A túlzott értékadás jellemzően lassabb amplitúdójú oszcillációt eredményez, mint a P tag esetében. A D tag szintén a széllökésekre való gyors reagálást teszi lehetővé, a nagy értékadás a rendszer stabilitását tudja veszélyeztetni a túlzott oszcillációval. Ha belső kör beállítása megtörtént, akkor a külső kör paramétereit kell behangolni hasonló logikával, mint az elsőnél. 37

40 4.10. A szabályozó és vezérlő VHDL megvalósítása A vezérlő és szabályozó részt szétbontva 2 db. VHDL fájl tartalmazza a ruleandcontol.vhd és PID.vhd. A két részt jobb volt szétszedni és blackboxként kezelni programozástechnikai szempontból. Az első a szabálybázist tartalmazza a második pedig a kaszkád PID elemeit. A két hardverelem erőforrás szempontjából rengeteget használ, ezért az implementáláskor erősen optimalizálni kellett a kódot, mert a teljes rendszer (beleértve a szoftprocesszort is) fordításánál a mappelő nem tudta bepakolni az időzítések tartása mellet annak ellenére, hogy a synthesis becslése szerint az FPGA-nak 80%-át használja csak fel LUT szeletek szempontjából. Ez a 4.1-es ábrán látható. 4.1 ábra a synthesis tool becslése az FPGA telítettségre Sajnos a teljes rendszer optimalizálása még nem történt meg, ezért még egy nyitott problémának marad meg az, hogy az összes funkciót egyszerre lehessen használni az FPGAban. A megkötések közé tartozik az, hogy egyenlőre a szabályozó csak 1 fajta szabályozástechnikai módot használhasson vagy az egyik aluláteresztő szűrőt ki kell venni a fordításból. A szabályozóban és vezérlőben a felhasznált logikai elemek számát úgy sikerült megtakarítani, hogy a végrehajtandó műveleteket a lehető legkevesebb aritmetikai áramkör berakásával meglehessen oldani. Ezen célból determinisztikus véges állapotú automatákat kellett definiálni és az egyes részműveletekhez egy egységes aritmetikai funkciót kellett kialakítani. Az automata állapotai a részeredmények kapuzását hajtják végre és minden egyes váltásnál egy aritmetikai műveletsort fog kiszámolni az ezt megvalósító process, majd ezeknek az eredményeit a megfelelő helyre kapuzza. Végeredményben az állapotok számával arányos órajel idővel szerint számolja ki a telje műveletet. 38

41 4.1 táblázat a szabályozó és vezérlő optimalizált hardverigénye az FPGA-ban # Adders/Subtractors : bit adder : 6 32-bit subtractor : bit subtractor : 2 # Latches : bit latch : 729 # Comparators : bit comparator greater : 9 33-bit comparator greater : 4 # Multiplexers : bit 2-to-1 multiplexer : bit 2-to-1 multiplexer : bit 10-to-1 multiplexer : 1 32-bit 11-to-1 multiplexer : 1 32-bit 2-to-1 multiplexer : bit 4-to-1 multiplexer : 3 # FSMs : 2 Slice Logic Utilization: Number of Slice Registers: 705 out of % Number of Slice LUTs: 1112 out of % Number used as Logic: 1112 out of % Slice Logic Distribution: Number of LUT Flip Flop pairs used: 1473 Number with an unused Flip Flop: 768 out of % Number with an unused LUT: 361 out of % Number of fully used LUT-FF pairs: 344 out of % Number of unique control sets: 25 Specific Feature Utilization: Number of BUFG/BUFGCTRL/BUFHCEs: 8 out of 16 50% Number of DSP48A1s: 19 out of 32 59% A 4.1 táblázat tartalmazza az optimalizálással elért hardverhasználatot az FPGA-ban. Ebből látható, hogy még így is szinte ez a legerőforrásigényesebb része az egész rendszernek. A kaszkád PID szabályozó automatája 23 állapotú, tehát 23 órajel alatt végzi el az össze műveletet, a vezérlő rész pedig 11 állapotú, tehát az előzőeknek megfelelően 11 órajel alatt végez. 39

42 4.11. Szenzorfúzió Korábban is említettük, hogy két szenzort használunk a szögértékek meghatározására, amely gyorsulásmérő és gyroscope IC-re támaszkodik. A gyorsulásmérő esetünkben az abszolút szögértékek visszaadását valamilyen pontatlansággal és lomhasággal a gyroscope pedig az integrálással számolt szögeket adja vissza relatíve pontosan és elég gyorsan, viszont idővel elcsúsznak az értékei. A szenzorfúzió azért szükséges, mert így a két érzékelő képes kijavítani egymás hibáit és sokkal megbízhatóbb lesz a rendszer. Erre több módszer is ismeretes köztük a Kalman, Madgwick és Complementary filterezés. A quadrocopterben Complementary filtert használtunk, egyszerűsége és kielégítő pontossága miatt. Az előbbi két algoritmus sokkal bonyolultabban működik és ennek ellenére nem ad számunkra pontosabb eredményeket. A használt filter lényegében a két szenzor szögértékeit súlyozott átlag formájában veszi figyelembe a következő példa alapján: A mi esetünkben a gyro súlyozás 0.9 és a gyorsulásmérőé 0.1. A filter hatékonyságáról nem tudtunk méréseket végezni, ezért egy internetről talált példával szemléltetjük a minőségi jellegét [18] (29. ábra). A quadrocopter képes egy másik megoldással is jól repülni. A fúzió előtti ötlet volt, hogy kizárólag a gyroscope értékeire támaszkodva integrálással meghatározott szögeket vesszük figyelembe és időnként a szoftver megnézi, hogy mennyi az abszolút eltérése a gyorsulásmérőből számoltaktól. Ha ez az eltérés egy előre beállított értéknél nagyobb volt, akkor az felülírta az aktuális gyorsulásmérő szögeivel az integrátor értékeit. 40

43 29. ábra A complementary filter és a szenzorjelek összehasonlítása 41

44 4.12. Az aluláteresztő szűrő működése A gyroscope szögének és a gyorsulásmérő értékeinek zajmentesítésére FIR típusú aluláteresztő szűrő került alkalmazásra az FPGA-ban. Ennek működési elvét a 30. ábra szemlélteti. 30. ábra FIR filter működési elve Lényegében a szűrő működési elve az, hogy a régebbi mérési adatokból egy konvolúciós függvény alapján alakítja ki a szűrőkarakterisztikát. Ennek a függvénynek az értékeit a b 0, b 1, b 2, b N szorzótagok tartalmazzák, amelyek 1-nél kisebb float típusú értékek. Az ábrán a Z -1 a Z transzformált előző időegységét jelenti. A kimenő értéket a vett mérési adatok és a hozzájuk tartozó súlyozás szorzatának összegeként számolja [19]. A mi rendszerünkhöz a szűrőkarakterisztikát nagyon alacsony frekvencián kellett beállítani. A tervezéshez egy magyar fejlesztésű online, webről elérhető alkalmazást használtunk a Tfilter designert [20]. Fontos szempont volt az, hogy a szűrő elemeinek számát minimalizáljuk a megfelelő levágási frekvencia, ripple és átmeneti tartomány beállításával, így kellő helyet takarítsunk meg az FPGA-ban. Ugyanezen célból a szűrő súlyfüggvényének számai integer értékekké vannak alakítva úgy, hogy 2 hatványának megfelelő számmal fel vannak szorozva. Így fixpontos műveletekkel lehet rajtuk dolgozni. A végeredmény ugyanezen felszorzott értékkel van elosztva, tehát kb. ugyanazt az eredményt kapjuk, mint a lebegőpontos aritmetikánál némi torzítással. Ez azért szükséges, hogy tovább helyet tudjunk megtakarítani az FPGA-ban. A kialakított szűrőkarakterisztikákat a 31., 32.,33. és 34. ábrán lehet megtekinteni. Az 42

45 impulzusválasz diagramokon jól látható, hogy a szűrű szorzóértékei szimmetrikusak, ezért ez további lehetőséget ad arra, hogy a VHDL hardver elemeinek szükségletét minimalizáljuk. A jelenlegi megoldás hardverigényeit a 6. táblázat foglalja össze: (synthesis jelentés) Slice Logic Utilization: Number of Slice Registers: 1523 out of % Number of Slice LUTs: 777 out of % Number used as Logic: 777 out of % Slice Logic Distribution: Number of LUT Flip Flop pairs used: 1843 Number with an unused Flip Flop: 320 out of % Number with an unused LUT: 1066 out of % Number of fully used LUT-FF pairs: 457 out of % Number of unique control sets: 37 Specific Feature Utilization: Number of BUFG/BUFGCTRL/BUFHCEs: 1 out of 16 6% Number of DSP48A1s: 3 out of 32 9% 64x14-bit single-port distributed Read Only RAM : 1 # MACs : 3 17x14-to-32-bit MAC : 3 # Adders/Subtractors : bit adder : 3 2-bit adder : 1 23-bit adder : 3 24-bit subtractor : 3 # Counters : 3 20-bit up counter : 1 6-bit up counter : 1 9-bit up counter : 1 # Registers : 1680 Flip-Flops : 1680 # Comparators : 1 9-bit comparator greater : 1 # Multiplexers : 3 17-bit 31-to-1 multiplexer : 3 6. táblázat: A FIR filterhardverigénye Innen látható, hogy a LUT szeletek nagy százalékát a 10 db. összeadó és kivonó áramkör használja fel. Előzetes becslések szerint egy jól megtervezett véges állapotú automatával kb. 4 % LUT mennyiség nyerhető illetve 2 db. DEP48A1s elemmel kevesebbet használna a szűrő. 43

46 A szűrő megvalósítása VHDL-ben ábra A FIR szűrő működésének folyamatábrája A ábrán látható a kód működésének blokkvázlata. A hardver működésének frekvenciáját egy előre beállított számláló határozza meg, ami esetünkben 100 Hz-nek felel meg. Az 50 Hz-es maximális mintavételezési frekvenciára felkonfigurált gyorsulásmérő adatok feldolgozását ezen a működési frekvencián kielégíti a tervezett szűrő. Ami azt illeti a teljes konvolúció műveletet az előbb említett frekvenciával hajtja végre. A reset jel hatására minden értéket nulláz. Az adatfrissítés állapotában az indexszámnak megfelelő helyre beírja az aktuális gyorsulásértékeket és növeli az értékét 1-el. Ha elérte az indexszám a maximális értéket akkor azt lenullázza és kezdi a beírást előröl. Amint a friss értékek bekerültek az block ramba, akkor elkezdi az összes FIR szorzóérték részeredményének kiszámítását. 1 órajelre csak 1 műveletet hajt végre mindhárom tengelyen. Ezekkel még a számláló lejárta előtt végez. Amint a számláló lejárt, akkor az aktuális szűrt értékek kikerülnek egy meghatározott regiszterbe. 44

47 31. ábra A gyorsulásmérő szűrő karakterisztikája függőleges tengely erősítés [db] vízszintes tengely frekvencia [Hz] 31. ábra A gyorsulásmérő szűrő karakterisztikája függőleges tengely erősítés [db] vízszintes tengely frekvencia [Hz] 32. ábra A gyroscope szögsebességének szűrő karakterisztikája függőleges tengely erősítés [db] vízszintes tengely frekvencia [Hz] 45

48 33. ábra A gyorsulásmérő szűrőjének impulzusválasza 34. ábra A gyroscope szűrőjének impulzusválasza 46

49 4.13. Az integrátor működése A szögsebesség értékeket numerikus integrálással alakítjuk szögelfordulássá. A szenzor a maximális mintavételezési frekvencián működik. A szögértéknél nem volt szükséges szűrőt használni, mert az integrátornak elég nagy a csillapítása, tehát a szűretlen szögsebességek a bemenetei. Az integrálást első megoldásban euler, majd trapéz formulával közelítettük. Az utóbbi sokkal pontosabbnak bizonyult, mert mint ahogy a 35. ábrán is látszik, a két numerikus módszert összehasonlítva ugyanazt a területet a trapézformula sokkal pontosabban reprodukálja. A tapasztalatok szerint tizedfok felbontással sem látszik a szögérték ugrálása és az elcsúszások is kb perc után is 1-2 fok körüli, az ismétlési pontossága is kielégítő. 35. ábra euler (baloldali) és trapézformula (jobboldali) numerikus integrálási módszerének szemléltetése [21] Az integrátor hardver igénye a 7. táblázatban megtalálható. Sajnos a megoldás még egyáltalán nincs optimalizálva így nagyon sok erőforrást igényel az összeadó, kivonó és szorzó áramkörök tekintetében. 47

50 Slice Logic Utilization: Number of Slice Registers: 305 out of % Number of Slice LUTs: 502 out of % Number used as Logic: 502 out of % Slice Logic Distribution: Number of LUT Flip Flop pairs used: 594 Number with an unused Flip Flop: 289 out of % Number with an unused LUT: 92 out of % Number of fully used LUT-FF pairs: 213 out of % Number of unique control sets: 4 Specific Feature Utilization: Number of BUFG/BUFGCTRL/BUFHCEs: 1 out of 16 6% Macro Statistics # Adders/Subtractors : 9 31-bit adder : 3 32-bit adder : 3 32-bit subtractor : 3 # Counters : 1 17-bit up counter : 1 # Accumulators : 3 32-bit up loadable accumulator : 3 # Registers : 288 Flip-Flops : táblázat Az integrátor hardverigénye az FPGA-ban Az integrátor megvalósítása VHDL-ben A hardver működésének folyamatábráját a 36. ábra mutatja. Az integrátor 20 MHz-es órajellel dolgozik jelenleg, ami indokolatlannak mondható, de ettől függetlenül jól számol. A későbbiekben tervezett ennek a kódrészletnek az optimalizálása is, mert a 3 tengely integrálási értékeit nem időkritikus 1 órajel alatt kiszámolni. Ez a hardver végzi el a szögértékek offset kompenzálását is, amelynek az értékeit a szoftprocesszortól kapja meg. 48

51 36. ábra Az integrátor működésének folyamatábrája 49