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
Tartalom 1.
Bevezetés .............................................................................................................. 2
2.
Quadrocopterek jellemzői .................................................................................... 3 2.1.
Történelmi és általános áttekintés .............................................................................. 3
2.2.
Quadrocopterek működésének áttekintése................................................................ 4
2.3.
A kinematikai modell áttekintése ................................................................................ 5
A modell felépítése és hardverelemei .................................................................. 6
3. 3.1.
A megvalósított modell felépítése .............................................................................. 6
3.2.
A quadrokopter vezérlőjének hardveres felépítése .................................................... 8
3.2.1. A PmodGYRO bővítőkártya tulajdonságai és alkalmazása .................................. 10 3.2.2. A PmodACL bővítőkártya tulajdonságai és alkalmazása ..................................... 11 3.2.3. A PmodWi-Fi bővítőkártya tulajdonságai és alkalmazása ................................... 12 3.2.4. A PmodOC1 modul tulajdonságai és alkalmazása ............................................... 13 3.2.5. A PmodGPS kártya tulajdonságai és alkalmazása ............................................... 14
4.
A quadrocopter szoftveres szerkezete ................................................................ 15 4.1.
Áttekintés .................................................................................................................. 15
4.2.
A folyamatvezérlő és segéd program részek működése ........................................... 17
4.2.1. Inicializáló függvény............................................................................................. 18 4.2.2. Biztonság és hibakezelés ..................................................................................... 18 4.2.3. Belső állapotok kezelése...................................................................................... 18 4.2.4. Szögfüggvények számításáért felelős folyamat ................................................... 18 4.3.
Quadrocopter API ...................................................................................................... 20
4.4.
Wi-Fi kommunikáció működése és konfigurálása ..................................................... 24
4.4.1. Áttekintés ............................................................................................................ 24 4.4.2. Csatlakozás a hálózathoz ..................................................................................... 24 4.4.3. A kommunikáció működése ................................................................................ 26 4.5.
A VHDL hardverek és a belső buszrendszerhez kapcsolata ....................................... 28
4.5.1. A WiFi SPI hardverillesztő .................................................................................... 29 4.5.2. A quadrocon_0 hardverillesztő ........................................................................... 30 4.6.
A PWM jelgenerátor működése ................................................................................ 31
4.7.
A szabályozó működési elve ...................................................................................... 34
4.8.
A vezérlés és szabályozás szabálybázisa .................................................................... 35
4.9.
A szabályozó beállítása .............................................................................................. 36
4.10.
A szabályozó és vezérlő VHDL megvalósítása........................................................ 38
4.11.
Szenzorfúzió........................................................................................................... 40
4.12.
Az aluláteresztő szűrő működése .......................................................................... 42
4.12.1. A szűrő megvalósítása VHDL-ben ...................................................................... 44 4.13.
Az integrátor működése ........................................................................................ 47
4.13.1. Az integrátor megvalósítása VHDL-ben ............................................................. 48
5.
Az FPV (First Person View) működése ............................................................. 50
6.
Összegzés ........................................................................................................... 51
6.
Summary ............................................................................................................ 52
7.
Irodalomjegyzék ................................................................................................. 53
1
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
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]
2. ábra de Bothezat helikopter
1. ábra Breguet-Richet Gyroplane
3
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 (F1,F2,F3,F4) (3. ábra) megegyezik, akkor az, függőleges irányban (ZB) emelkedik illetve a felhajtóerő (FB) é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 (ZB) tengelyű forgását okozza (3. ábra Ψ, 4. ábra d). Oldalirányú mozgása a quadcopternek megfelelő
úgy
történik,
motorok
hogy
a
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 XB és YB (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
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 Ixx, Iyy és Izz a rendszer forgató tehetetlenségi nyomatékát jelenti. A ΊΦ, Ίθ és ΊΨ a rendszer x,y és z tengelyen ébredő forgatónyomatéka. 5
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 2200. (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 25 000 1/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 motorokhoz tartozó propeller befogó a tengelyre
5. ábra felszerelt BLDC motor és 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
í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 19.83 C
Motor BLDC Max. teljesítmény Kv érték üzemi terhelés árama üzemi hatásfok propellerek Üzemei teljesítmény maximális húzóerő ESC max. áram
360 W 2200 14.99 A 81.9% 10x4.5 136.1 W ~1300 g 30 A
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
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
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
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
9. ábra a Nexys 3 board és ESC-k tápellátása 3.2.1. 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
3.2.2. 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 szoftveresen
értéket kell
ad
vissza
kiintegrálni
a
és 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
3.2.3. 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 valamilyen
PC-vel.
beillesztett
kártya
megkeresi
az
használjuk A
rendszerbe
automatikusan
általunk
beállított
hálózatot és felcsatlakozik rá. Miután ez megtörtént a hálózaton megjelenő csomagokat
12. ábra PmodWIF modul
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
3.2.4. 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
13
4
P
15. ábra ESC illesztő áramkör
4
K
0
1
Res2
R
D
N
G
PIN4
3
2
1
3
P D
N 3
K
0
1 1
2
4 5
3
K
0
PIN0
P
5
G
1
Res2
R
G
PIN3
3
2
1
2
P D
N
G
D
2
R
N
PIN2
3
2
1
1 D
N
Res2
1
K
0
1
Res2
R
G
PIN1
3
2
1
P
P5 a modulhoz.
3.2.5. 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
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
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
4.2. A
A folyamatvezérlő és segéd program részek működése 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
4.2.1. 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. 4.2.2. 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ő. 4.2.3. 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.
4.2.4. 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
[10]. Ezekből Ax, Ay, Az 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
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
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
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 0 – 255-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
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 0-1024-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 0-1024-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 0-1024-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 0-1024-ig terjedhetnek.
23
4.4.
Wi-Fi kommunikáció működése és konfigurálása
4.4.1. Á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. 4.4.2. 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
ssid=quadrocopter
set
hostednetwork
mode=allow
key=asdfghjkl
netsh wlan start hostednetwork timeout 5 arp -s 192.168.1.5 00-1e-c0-09-cd-19 192.168.1.1 pause
19. ábra konfigurációs állomány kódja
24
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
„192.168.1.5” IP címre, méghozzá a
„192.168.1.1” 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
"quadrocopter"
#define MY_DEFAULT_NETWORK_TYPE
WF_INFRASTRUCTURE
#define MY_DEFAULT_SCAN_TYPE
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
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: #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"
21. ábra A nyers hálózati kulcs formátuma 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. 4.4.3. 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).
26
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 0x6 0x6 4 4 0x6 0x6 4 4 0x6 0x6 4 4 0x6 0x6 4 4 0x6 0x6 4 4 0x6 0x6 4 4 0x6 0x6 4 4
Utasítá s Byte1 0-255 0x10 (dec) 0-255 0x11 (dec) 1;0;-1 0x16 (dec)
Byte 2
Byte 3
Byte 4
0x00 0x00 0x00 Byte1: sebesség 0x00 0x00 0x00 Byte1: sebesség
0x19
intA
0x00 0x00 0x00 balra, áll, jobbra előjeles eltolás AintA intB intB B
0x1A
intC
intC
0xA0
0x00 0-255 (dec)
0x00 0x00 0x00
0xA1
intD
intD
előjeles eltolás C-D
0x00 0x00 0x00 Byte1: sebesség
quadro->PC gyroXszög gyroYszög gyroZszög
0x6 4 0x6 4 0x6 4
0x6 4 0x6 4 0x6 4
0x20
int1
int2
int3
int4
0x21
int1
int2
int3
int4
intx: szögelfordulás inty: szögelfordulás
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
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.
4.5.
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
4.5.1. 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 slv_reg0 slv_reg1 slv_reg2 slv_reg2 funkció slv_reg0 funkció
Funkciója SPI Tx adat/állapot SPI Rx adat IO kezelés
bit 0 Interrupt bit (0-7) Adat Tx
bit 1 Ssout bit 8 Start Tx
Hozzáférés Írás Olvasás Írás/Olvasás
bit 2 Reset bit 9 busy
bit 3 bit 4 WP (flash) Hibernate
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
4.5.2. 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_reg0 slv_reg1 slv_reg2 slv_reg3 slv_reg4 slv_reg5 slv_reg6 slv_reg7 slv_reg8 slv_reg9 slv_reg10 slv_reg11 slv_reg12 slv_reg13 slv_reg14 slv_reg15 slv_reg16 slv_reg17 slv_reg18 slv_reg19 slv_reg20 slv_reg21 slv_reg22 slv_reg23 slv_reg24 slv_reg25 slv_reg26 slv_reg27 slv_reg28 slv_reg29 slv_reg30
control regiszter Motor1 offset Motor2 offset Motor3 offset Motor4 offset x tengely szögelfordulás y tengely szögelfordulás z tengely szögelfordulás x tengely szögsebesség y tengely szögsebesség z tengely szögsebesség x tengely szögelfordulás komp. y tengely szögelfordulás komp. z tengely szögelfordulás komp. x tengely szögfelülírás y tengely szögfelülírás z tengely szögfelülírás x tengely gyorsulás y tengely gyorsulás z tengely gyorsulás belső PID P tag értéke xy belső PID I tag értéke xy belső PID D tag értéke xy kimenő ESC1 jel értéke kimenő ESC1 jel értéke kimenő ESC1 jel értéke kimenő ESC1 jel értéke belső PID P tag értéke z kívánt x tengelyű dőlés kívánt y tengelyű dőlés gázadás mértéke
Írás Írás Írás Írás Írás Olvasás Olvasás Olvasás Olvasás Olvasás Olvasás Írás Írás Írás Írás Írás Írás Olvasás Olvasás Olvasás Írás Írás Írás Olvasás Olvasás Olvasás Olvasás Írás Írás Írás Írás
slv_reg31(0) = 1 x tengelyű gyorsulásmérő szűrt y tengelyű gyorsulásmérő szűrt z tengelyű gyorsulásmérő szűrt külső PID P tag értéke xy külső PID I tag értéke xy külső PID D tag értéke xy külső PID P tag értéke z külső PID I tag értéke z külső PID D tag értéke z x tengelyű szögsebesség szűrt y tengelyű szögsebesség szűrt z tengelyű szögsebesség szűrt
Olvasás Olvasás Olvasás Írás Írás Írás Írás Írás Írás Olvasás Olvasás Olvasás
4. táblázat A quadrocon_0 periféria regiszterkiosztása
Amiről érdemes még itt szót ejteni az a control regiszter kiosztása, amely az 5. táblázatban látható. 30
slv_reg0 bit 0 funkció motor engedélyező
bit 1 Reset
bit 2 Start
bit 3 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.
4.6.
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 01024-ig fogadja el a bemenő vezérlőjeleket. Az időzítő számlálója a 100MHz-es órajel alapján 2 000 000-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 100000+(É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
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 3.2.4-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
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
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
-
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.
4.8.
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
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.
4.9.
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
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. 1-1.5 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
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
4.1 táblázat a szabályozó és vezérlő optimalizált hardverigénye az FPGA-ban # Adders/Subtractors : 20 32-bit adder :6 32-bit subtractor : 12 33-bit subtractor :2 # Latches : 729 1-bit latch : 729 # Comparators : 13 32-bit comparator greater :9 33-bit comparator greater :4 # Multiplexers : 1518 1-bit 2-to-1 multiplexer : 1486 10-bit 2-to-1 multiplexer : 10 32-bit 10-to-1 multiplexer :1 32-bit 11-to-1 multiplexer :1 32-bit 2-to-1 multiplexer : 17 32-bit 4-to-1 multiplexer :3 # FSMs :2 Slice Logic Utilization: Number of Slice Registers: 705 out of 18224 3% Number of Slice LUTs: 1112 out of 9112 12% Number used as Logic: 1112 out of 9112 12% Slice Logic Distribution: Number of LUT Flip Flop pairs used: 1473 Number with an unused Flip Flop: 768 out of 1473 52% Number with an unused LUT: 361 out of 1473 24% Number of fully used LUT-FF pairs: 344 out of 1473 23% 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
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
29. ábra A complementary filter és a szenzorjelek összehasonlítása
41
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 b0, b1, b2, bN 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
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) 6. táblázat: A FIR filterhardverigénye
Slice Logic Utilization: Number of Slice Registers: 1523 out of 18224 8% Number of Slice LUTs: 777 out of 9112 8% Number used as Logic: 777 out of 9112 8% Slice Logic Distribution: Number of LUT Flip Flop pairs used: 1843 Number with an unused Flip Flop: 320 out of 1843 17% Number with an unused LUT: 1066 out of 1843 57% Number of fully used LUT-FF pairs: 457 out of 1843 24% 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 : 10 16-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
43
Innen látható, hogy a LUT szeletek nagy százalékát a 10 db. összeadó és kivonó áramkör használja
fel.
becslések
szerint
megtervezett
véges
Előzetes egy
jól
állapotú
automatával kb. 4 % LUT mennyiség nyerhető illetve 2 db.
DEP48A1s
elemmel
kevesebbet használna a szűrő.
4.12.1. A szűrő megvalósítása VHDL-ben
4.11.1 ábra A FIR szűrő működésének folyamatábrája
A 4.11.1 á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
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
33. ábra A gyorsulásmérő szűrőjének impulzusválasza
34. ábra A gyroscope szűrőjének impulzusválasza
46
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. 1.5 - 2 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
Slice Logic Utilization: Number of Slice Registers: 305 out of 18224 1% Number of Slice LUTs: 502 out of 9112 5% Number used as Logic: 502 out of 9112 5% Slice Logic Distribution: Number of LUT Flip Flop pairs used: 594 Number with an unused Flip Flop: 289 out of 594 48% Number with an unused LUT: 92 out of 594 15% Number of fully used LUT-FF pairs: 213 out of 594 35% 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 : 288
7. táblázat Az integrátor hardverigénye az FPGA-ban 4.13.1. 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
36. ábra Az integrátor működésének folyamatábrája
49
5. Az FPV (First Person View) működése A fedélzeti kamerát, amely az élőképet streameli, egy raspberry pi, egy raspberry pi kamera és egy TP-LINK nano segítségével oldottuk meg (34. ábra). A választás azért esett az említett eszközökre, mert egyrészről ezek álltak rendelkezésre, másrészről kis fogyasztás és súly jellemzi őket. A Raspberry pi mint SBC (Single board Computer) linux alapú operációs rendszert futtat és található benne számunkra fontos hardverként egy H.264-es kódoló. A HD felbontású kamerakép tömörítését ez végzi el, így nem terheli a processzort. A linuxon Gstreamert és Ffmpeg-et használtunk. A kamerakép adatátvitele, külön a quadrocoptertől független WiFi hálózaton történik a TP-LINK nano-n keresztül.
34. ábra raspberry pi és a rasperry pi kamera
50
6. Összegzés A diplomamunka során sikerült összeépíteni és megvalósítani egy quadrocopter szabályozását és irányítását WiFi kapcsolaton keresztül, FPGA-s feldolgozással. A PID szabályozás paramétereinek optimális beállítása még nem sikerült, jelenleg a vezetéséhez minimális modellező tapasztalat szükséges. Maga a megoldás úttörőnek számít, mert még nem sok eredmény született ezen a szűk területen és a piacon se lehet fellelni hasonló vezérlőket. A kész programstruktúrából könnyen készíthető ASIC sorozatgyártás céljából. Jelenleg folyik még rádiófrekvenciás 802.15.4-es hálózatok RSSI értékének mérésére alkalmas hardveres illesztő fejlesztése, amely a diplomamunkában kezdeti állapota miatt nem lett kifejtve. Lehetséges jövőbeli célkitűzések közé tartozik egy komolyabb, kalman filterező algoritmus használata, amely a saját fizikai modelljét szimulálva pontosítaná a szabályozást. Az FPGA nagy számítási kapacitását kihasználva kamera, sztereokamera kép vagy optical flow sensor képfeldolgozásából pozíció tartásának és mérésének lehetőségét is meglehetne valósítani. Amennyiben ez sikerül, akkor a szabályozási láncot át lehetne alakítani fuzzy típusú rendszerre, mert sokkal hatékonyabb működne, mint a mostani. A quadrocopter teljesen nyílt forráskodú fejlesztésnek készült és tervezett annak publikálása is. Cél lenne egy közösség kialakítása, amely tovább tudná saját ötleteivel fejleszteni a vezérlőt. Ennek kezdeményezése már facebookon megtörtént és a [22] link alatt elérhető. Szeretnék köszönetet nyilvánítani Dr. Vásárhelyi Józsefnek valamint a Bay Zoltán Alkalmazott Kutatási Közhasznú Nonprofit Kft. Logisztikai és Gyártástechnikai Intézetének Mechatronikai Osztályának a szakmai és anyagi támogatásért, Tompa Tamás és Bartók Roland Villamos MSc-s hallgatóknak készségükért és segítségükért. A projekt a TÁMOP- 4.2.2/B-10/1-2010-0008 forrásait használta.
51
6. Summary In this thesis we created and realized the control and stabilization of a quadrocopter on an FPGA-based platform that can be controlled via a WiFi network. The PID control parameters are not fully optimized yet; some modelling skills are required to control the copter. The technical solution is pioneering, as there are not so many results that can be found in this area and there are no similar controllers on the market. From the completed programming structure, an ASIC can easily be generated for mass production. A hardware development is running now to sniff 802.15.4 radiofrequency networks for measuring RSSI values. In this thesis it is not explained, because it is in a too early stage of development. Regarding possible future goals, a kalman filter could be implemented that can model the quadrocopter’s own physical model to improve its stabilization. Using the high calculation capacity of the FPGA a camera, stereocamera picture or optical flow sensor positioning could be realized. If these are successfully implemented, then the regulation could be converted into a fuzzy-based system to improve the regulation quality. The quadrocopter is an entirely open-source development and we are planning to publish the full code. Our aim is to create a community that can improve the development with their own ideas and works. This community has been initiated on facebook and it can be found under the link [22]. I would like to say thank you to Dr. Vásárhelyi József and the Bay Zoltán Nonprofit Ltd. Logistic and Manufacturing Technology Institute, Department of Mechatronics for the technical and financial support, and also to Tompa Tamás and Bartók Roland, candidates of Electrical Engineering MSc, for their help. The project was founded by the TÁMOP4.2.2/B-10/1-2010-0008.
52
7. Irodalomjegyzék [1] - http://en.wikipedia.org/wiki/Quadcopter [2] - http://oru.diva-portal.org/smash/get/diva2:567121/FULLTEXT01.pdf [3] - http://www.isys.uni-stuttgart.de/forschung/mechatronik/robo/quadkopter/index.en.html [4] - http://www.ecalc.ch/xcoptercalc.php?ecalc&lang=en [5] - http://www.digilentinc.com/ [6] - http://www.digilentinc.com/Data/Products/PMOD-OC1/pmod-oc1-rm.pdf [7] - http://en.wikipedia.org/wiki/GPS_signals [8]
-
http://www.xilinx.com/support/documentation/sw_manuals/xilinx14_1/ds865_microblaze_mc s.pdf [9]
-
http://www.xilinx.com/support/documentation/sw_manuals/xilinx14_1/mb_ref_guide.pdf [10] - http://husstechlabs.com/projects/atb1/using-the-accelerometer/ [11] - http://www.microchip.com/wwwproducts/Devices.aspx?product=MRF24WB0MA [12]
-
http://www.microchip.com/stellent/idcplg?IdcService=SS_GET_PAGE&nodeId=2680&dDoc Name=en537041 [13] - http://technet.microsoft.com/en-us/library/cc755301(v=ws.10).aspx [14] - http://www.wireshark.org/tools/wpa-psk.html [15] [16]
-
http://robots.dacloughb.com/project-2/esc-calibration-programming/ -
http://www.hobbielektronika.hu/cikkek/will-
i_v20_robotika_es_avr_kezdoknek.html?pg=7 [17]
-
http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction§ion=ControlPID [18] - http://www.pieter-jan.com/node/11 [19] - http://en.wikipedia.org/wiki/Finite_impulse_response [20] - http://t-filter.appspot.com/fir/index.html [21] - http://en.wikipedia.org/wiki/Trapezoidal_rule [22] - https://www.facebook.com/DigilentFpgaQuadrokopter [23] - http://andrew.gibiansky.com/blog/physics/quadcopter-dynamics/
53
