Robot Zburator de Tip Quad Copter
Robot zburător de tip quad-copter
Proiect de diplomă
Cuprins
Lista figurilor
Lista tabelelor
Lista acronimelor
Introducere
Capitolul 1. Aspecte teoretice
1.1 Principii de zbor
1.2 PID
Capitolul 2. Descriere Hardware
2.1. Componentele principale utilizate
2.2. Circuitul electronic de test
2.3. Circuitul electronic al quad-copter-ului
2.4. Circuitul electronic al telecomenzii
2.5. Lista de componente și schemele celor două circuite
Capitolul 3. Descriere Software
3.1. Programul Quad-copter-ului
3.2. Programul Telecomenzii
Capitolul 4. Implementare și testare
Rezultate și concluzii
Bibliografie
Anexa A. Codul sursă
Codul Quad-copter-ului
Codul Telecomenzii
Anexa B. Scheme suplimentare ale circuitelor utilizate
Anexa C. Layout-ul și pozele circuitului dronei
Anexa D. Schema, lista de componente și pozele pentru circuitul de test
Anexa E. Poze ale telecomenzii și ale quad-copter-ului
Anexa F. Diagrama Gantt
Lista figurilor
Figura 1.1. Sensul de rotație al elicelor unui quad-copter 17
Figura 1.2. Configurația în + și configurația în X pentru un quad-copter 18
Figura 1.3. Schema de funcționare a unui controller de tip PID 19
Figura 2.1. Componentele kit-ului quad-copter-ului 21
Figura 2.2. Schema circuitului dronei 25
Figura 2.3. Schema circuitului telecomenzii 26
Figura 3.1. Schema cadrului protocolului de comunicație 32
Figura B.1. Schema circuitului XBee Explorer Regulated 69
Figura B.2. Schema convertorului de nivele logice 69
Figura C.1. Layout-ul circuitului dronei: a) fața superioară b) fața inferioară 72
Figura C.2. Pozele circuitului dronei: a) văzut de sus b) văzut de jos 73
Figura D.1. Schema circuitului de test 73
Figura E.1. Poza telecomenzii 75
Figura E.2. Poza quad-copter-ului 75
Lista tabelelor
Tabelul 2.1. Lista de componente pentru circuitul dronei 26
Tabelul 2.2. Lista de componente pentru circuitul telecomenzii 27
Tabelul D.1. Lista de componente pentru circuitul de test 74
Tabelul F.1. Diagrama Gantt 77
Lista acronimelor
ADC = Analog-to-digital convertor (Convertor analogic-numeric)
BPS = Bits per second (Biți pe secundă)
DMP = Digital motion processor (Procesor digital de mișcare)
EAGLE = Easily Applicable Graphical Layout Editor (Editorul de layout grafic ușor de implementat)
EEPROM = Electrically Erasable Programmable Read-Only Memory
ETSI = European Telecommunications Standards Institute (Institutul European de Standardizare pentru Telecomunicații)
ESC = Electronic speed control (Control electronic al vitezei)
FPV = First-person view
I2C = Inter-Integrated Circuit
IMU = Inertial measurement unit (Unitate de măsurare inerțială)
LiPo = Lithium Polymer
PCB = Printed circuit board (Cablaj imprimat)
PID = Proportional-integral-derivative
PWM = Pulse-width modulation (Modulație în impuls)
RC = Remote control
RPM = Revolutions per minute (Rotații pe minut)
SMD = Surface-mount device
SPI = Serial Peripheral Interface
TWI = Two Wire Interface
UART = Universal asynchronous receiver/transmitter
USART = Universal asynchronous receiver/transmitter
Introducere
Prin prezenta lucrare îmi propun să expun modul în care am implementat proiectul de licență cu tema „Robot zburător de tip quad-copter”. Dată fiind dezvoltarea recentă a tehnologiei și creșterea interesului față de acest domeniu, am considerat oportună abordarea detaliată a proiectului în cauză. Deși aparatele de zbor utilizate de oameni pentru satisfacerea nevoilor lor nu reprezintă o noutate, dronele de mici dimensiuni au căpătat amploare în ultimii ani. Dezvoltarea tehnologiei în acest domeniu a condus la scăderea costurilor de producție, facilitând răspândirea lor comercială.
Aplicațiile cele mai cunoscute sunt reprezentate de RC, FPV și telemetrie, însă există exemple în care dronele sunt folosite pentru a cânta la instrumente muzicale și chiar pentru a livra comenzi în cadrul unui restaurant, iar o mare companie americană de comerț online [1] dezvoltă în prezent drone capabile să livreze pe distanțe scurte produse de mici dimensiuni. De asemenea, există aplicații în care dronele sunt utilizate pentru a filma, atât în domeniul vizibil, cât și în infrarosu, zone greu accesibile, precum clădiri înalte, piloni utilizați în telecomunicații, suprafețe extinse de teren arabil sau forestier etc., cu scopul de a detecta posibile defecte, de a realiza auditul energetic al clădirilor înalte, de a realiza topografia terenurilor, estimarea lucrărilor de construcții, estimarea volumelor de minerit, verificarea podurilor și structurilor înalte, determinarea cantității de biomasă în agricultură, determinarea zonei afectate de dăunători și cantitatea de insecticid/ierbicid necesară, depistarea persoanelor aflate în dificultate în incendii la etaje înalte sau în păduri/câmpuri, observarea biodiversității fără intervenție umană etc.
De aceea, mi-am propus să realizez un robot zburător de tip quad-copter care să fie comandat de la distanță prin intermediul interfeței radio și care să fie stabil în zbor. Am ales structura cu patru motoare (tema este cunoscută și sub numele de ”quad-copter”) deoarece această configurație oferă un plus de stabilitate față de alte structuri, cum ar fi tri-copter sau elicopter cu două elice, fiind în același timp foarte versatilă și având o autonomie sporită.
Acestea, spre deosebire de elicopterele de aceleași dimensiuni, nu necesită componente mecanice pentru a modifica unghiul elicelor în timpul rotației. De asemenea, utilizând patru elice, este permis ca fiecare elice să aibă dimensiuni mai mici față de elicea corespunzătoare unui elicopter, conducând la o energie cinetică mai mică a fiecăreia în timpul zborului. Prin structura curentă se micșorează pagubele în cazul în care o elice lovește un obiect și se crește versatilitatea aparatului de zbor.
Utilizând patru motoare se crește forța de propulsie și se eficientizează consumul de energie.
În ceea ce privește metodologia folosită, m-am documentat pe tema subiectului, apoi am căutat componentele potrivite și am verificat dacă sunt fezabile pentru proiectul de față. După achiziția materialelor necesare, am conectat blocurile principale la un circuit realizat la materia ”Proiect 2” în vederea testării.
Programarea s-a realizat ”cu pași mici”, testând separat funcționarea componentelor, apoi am integrat întregul proiect. Unele soluții s-au dovedit a fi nepotrivite pentru subiectul în cauză, de aceea a trebuit să încerc alte variante până am ajuns la rezultatul dorit. Procedeul este descris în paginile următoare.
Capitolul 1. Aspecte teoretice
1.1 Principii de zbor
Un elicopter obișnuit are un rotor mare pentru a asigura forța de propulsie și o coadă mică în comparație cu rotorul pentru a compensa cuplul aerodinamic generat de rotor. În lipsa acestei cozi elicopterul s-ar învârti aproape la fel de rapid ca și elicea.
Spre deosebire de un elicopter, un quad-copter are patru rotoare care dezvoltă împreună o forță de propulsie ascendentă, fiecare rotor susținând aproximativ un sfert din greutatea totală a dronei, de aceea se pot utiliza motoare ce dezvoltă o putere mai mică, rezultând și un preț de cost mai scăzut. Dinamica unui quad-copter este definită prin modificarea relativă a forței de propulsie a fiecărui rotor.
Rotoarele sunt dispuse în formă de pătrat, având același sens două câte două (figura 1.1) cele poziționate diametral opus. În cazul în care toate rotoarele se rotesc în același sens, drona ar avea o mișcare asemănătoare cu un elicopter obișnuit lipsit de coadă. Girația este indusă de forțe neechilibrate. Cuplul unui rotor va crea un efect care va fi anulat de efectul creat de cuplul celui de-al doilea rotor care se rotește în sens opus, astfel dacă toate cele patru rotoare produc forțe de propulsie egale, quad-copter-ul va avea o poziție fixă.
Figura 1.1. Sensul de rotație al elicelor unui quad-copter [2]
Un quad-copter are patru grade de libertate: girația, ruliul, tangajul și propulsia.
Pentru quad-copter-e există două configurații posibile în funcție de poziția motoarelor față de axa principală: configurația în X și configurația în +. Figura 1.2 prezintă cele două configurații.
Figura 1.2. Configurația în + și configurația în X pentru un quad-copter [3]
Girația (întoarcerea la stânga sau la dreapta) este controlată prin creșterea vitezei motoarelor care se rotesc într-un sens și scăderea vitezei motoarelor care se rotesc în sens opus. Daca turația motoarelor care se rotesc într-un sens crește cu același număr de rotații pe minut cu care scade turația motoarelor care se rotesc în sens opus, nu se produce o propulsie suplimentară, astfel quad-copter-ul se rotește la stânga sau la dreapta, fără să apară o creștere sau o scădere a altitudinii dronei. Fenomenul descris este valabil și pentru configurația în + și pentru configurația în X.
Pentru configurația în +, ruliul (înclinarea la stânga sau la dreapta) este controlat prin creșterea turației unui motor și scăderea turației motorului care se rotește în sens opus, ambele motoare fiind situate pe axa transversală.
Pentru configurația în X, ruliul este controlat prin creșterea turației motoarelor de pe o parte a dronei, față de axa longitudinală, și scăderea turației motoarelor aflate pe partea opusă față de axa longitudinală.
Tangajul (ridicarea sau coborârea „botului” dronei) este controlat într-un mod similar cu ruliul, însă pentru configurația în + se realizează pentru motoarele aflate pe axa longitudinală, iar pentru configurația în X se realizează pentru motoarele aflate pe o parte și pe cealaltă parte față de axa transversală.
În momentul în care quad-copter-ul se înclină în față, în spate sau într-o parte, vectorul forței se împarte într-o componentă orizontală și o componentă verticală, ceea ce conduce la deplasarea dronei în direcția în care se înclină, direcție dată de componenta orizontală. De asemenea, datorită divizării vectorului forței, componenta verticală va avea o valoare mai mică, fapt care conduce la scăderea altitudinii aparatului de zbor. Pentru a compensa acest efect este nevoie de creșterea turației tuturor motoarelor. [2]
1.2 PID
Un controller proporțional-integrativ-derivativ (controller PID) este un mecanism de control în buclă folosit la scară largă în sistemele de control din industrie. Un controller de tip PID calculează valoarea erorii ca diferență între variabila măsurată în timp real și o valoare aleasă ca referință.
Controller-ul de tip PID implică utilizarea a trei parametri independenți precum termenul proporțional, cel integrativ și cel derivativ, notați cu P, I și D. Termenul P depinde de valoarea curentă a abaterii față de punctul de referință, I depinde de suma abaterilor anterioare și termenul D reprezintă o predicție a erorilor viitoare estimate pe baza schimbărilor curente. Suma ponderată a acestor trei parametri este utilizată pentru a regla un proces, utilizând un sistem bazat pe circuit în buclă închisă.
Rezultatul generat de un controller poate fi descris de răspunsul pe care îl are acesta față de abatere, de gradul prin care controller-ul se depărtează de punctul de referință și de gradul de oscilații. Schema de funcționare pentru un astfel de controller este prezentată în figura 1.3.
Există situații în care este nevoie doar de unul sau doi dintre parametrii controller-ului pentru a îndeplini cerințele sistemului de control. Acest deziderat se obține egalând ceilalți parametri cu zero. Un astfel de controller poate fi numit PI, PD, P sau I în absența celorlalți termeni.
Figura 1.3. Schema de funcționare a unui controller de tip PID [4]
Formula matematică descriptivă a procesului de față este următoarea:
„u(t) = Kpe(t) + Ki + Kde(t)” [5]
Kp – câștigul proporțional
Ki – câștigul integrativ
Kd – câștigul derivativ
e – eroarea = valoarea de referință – valoarea curentă
t – timpul curent
τ – variabilă de integrare care ia valori de la timpul 0 la timpul curent
Ajustarea parametrilor pentru atere, de gradul prin care controller-ul se depărtează de punctul de referință și de gradul de oscilații. Schema de funcționare pentru un astfel de controller este prezentată în figura 1.3.
Există situații în care este nevoie doar de unul sau doi dintre parametrii controller-ului pentru a îndeplini cerințele sistemului de control. Acest deziderat se obține egalând ceilalți parametri cu zero. Un astfel de controller poate fi numit PI, PD, P sau I în absența celorlalți termeni.
Figura 1.3. Schema de funcționare a unui controller de tip PID [4]
Formula matematică descriptivă a procesului de față este următoarea:
„u(t) = Kpe(t) + Ki + Kde(t)” [5]
Kp – câștigul proporțional
Ki – câștigul integrativ
Kd – câștigul derivativ
e – eroarea = valoarea de referință – valoarea curentă
t – timpul curent
τ – variabilă de integrare care ia valori de la timpul 0 la timpul curent
Ajustarea parametrilor pentru a ajunge la soluția optimă este foarte importantă. În cazul în care termenul proporțional are o valoare prea mare, sistemul se abate de la valoarea de referință și intră intr-o stare oscilantă față de reperul setat, având o oscilație constantă, crescătoare sau descrescătoare. În cazul în care parametrul are o valoare prea mică, sistemul nu poate urmări schimbările apărute.
Termenul integrativ are o contribuție dependentă atât de amplitudinea abaterii, cât și de durata acesteia. Parametrul reprezintă suma abaterilor instantanee de-a lungul timpului și oferă un offset pe care ar fi trebuit să-l dețină sistemul în starea inițială. Scopul termenului integrativ este de a accelera dinamica procesului spre îndeplinirea stării de referință și elimină abaterea constantă pe care o are un controller pur proporțional.
Derivata abaterii față de valoarea de referință se calculează prin determinarea pantei abaterii de-a lungul timpului, înmulțită cu acel câștig derivativ. Cu ajutorul rezultatului acestui produs se realizează o predicție a comportamentului sistemului și în acest mod se reduc oscilațiile, conducând la stabilitatea sistemului.
Pentru analiza matematică a procesului de tip PID poate fi utilă transformata Laplace:
G(s) = Kp + + Kds = =Kd
Pornind de la formula matematică a procesului de tip PID scrisă în formă Laplace se poate obține funcția de transfer a sistemului cu buclă închisă:
H(s) =
unde:
= ω02
= 2ζω0
Astfel se obține: G(s)H(s) = , formulă care poate fi utilă pentru a elimina polii care conduc la instabilitatea sistemului.
Pentru determinarea câștigurilor necesare unui sistem stabil și eficient de tip PID se adoptă de cele mai multe ori ajustarea manuală a acestora până când se obține rezultatul scontat. Astfel, pentru început parametrii Ki și Kd au valoarea zero. Apoi se crește valoarea parametrului Kp până cand sistemul intră în oscilație, pe urmă i se atribuie acestui termen aproximativ jumătate din valoarea obținută anterior. În continuare se crește valoarea parametrului Ki până când se corectează offset-ul într-un timp acceptabil pentru proces. În orice caz, o valoare prea mare a termenului Ki va conduce la instabilitate. În cele din urmă se crește valoarea coeficientului Kd. De asemenea, o valoare prea mare a acestuia va determina un răspuns mult prea rapid și va determina intrarea sistemului în oscilație.[5]
Capitolul 2. Descriere Hardware
2.1. Componentele principale utilizate
Pentru a achiziționa componentele necesare, am efectuat în prealabil o documentare pe internet, selectând componente cu fiabilitate ridicată, cu bune performanțe pentru proiectul curent, care să permită chiar și adăugarea de funcțiuni noi pe viitor. Mi-am propus să realizez un proiect modular pentru a schimba cu ușurință componente mecanice și/sau electronice avariate și pentru a permite înlocuirea unor părți existente cu unele având performanțe superioare.
Componenta cea mai importantă a acestui proiect este reprezentată de unitatea de senzori de mișcare MPU-9150 care conține un accelerometru pe trei axe, un giroscop pe trei axe și un magnetometru pe trei axe. Acesta funcționează la o tensiune cuprinsă între 2,4V si 3,46V. Comunicarea cu microcontroller-ul se face prin interfața I2C la o rată de maxim 400KHz. Unitatea este prevăzută cu un număr de convertoare analogic-numerice care furnizează datele utilizate pentru prelucrarea în urma căreia se obțin unghiurile Euler necesare pentru stabilizare. Unitatea a fost cumparată de pe www.robofun.ro .
În ceea ce privește cadrul, am cumpărat de pe site-ul online www.sierra.ro un kit (figura 2.1), numit Quadrocopter DJI F450 Flame Wheel, care conține placa centrală superioară și placa centrală inferioară, ambele realizate din PCB, patru elice de 8 x 4,5 inch și patru elice de 10 x 3,8 inch, câte două pentru fiecare fel pentru sensul trigonometric de rotație si câte două pentru sensul antitrigonometric de rotație, patru motoare brushless DJI 2212, patru regulatoare electronice de 30A, patru brațe color, o bandă velcro, cabluri, cheie și șuruburi de prindere și fixare.
Plăcile centrală și superioară sunt realizate din PCB și au o rezistența mecanică bună, având totodată trasee imprimate cu ajutorul cărora se conectează cablurile pentru putere de la acumulator la regulatoarele electronice.
Figura 2.1. Componentele kit-ului quad-copter-ului [6]
Dimensiunile totale ale cadrului sunt de 450 x 450 mm, având o greutate totala de 282 grame. Ansamblul este conceput să aibă o masă totală cuprinsă între 800g si 1600g, în condițiile în care un singur motor, conectat la un acumulator de tip LiPo de 14,8V, este capabil să producă un curent de aer care să ridice o masă de maxim 920g. Fiecare motor produce 920 rpm/V, totalizând 13616 rpm atunci când acumulatorul este la tensiunea nominală de 14,8V și ESC-urile, care de fapt sunt niște controller-e pentru motoare, primesc comanda de putere maximă.
Acumulatorul utilizat este Turnigy nano-tech, de tip LiPo cu patru celule – 4S, având o capacitate de 4000mAh, debitând la borne o tensiune nominală de 14,8V. Greutatea totală a acumulatorului este de 427g. Am ales acest model deoarece îndeplinește cerințele necesare unei bune funcționări, iar capacitatea ridicată permite un zbor pentru un timp cuprins între 15 și 30 minute. Acumulatorul a fost achiziționat de pe magazinul online www.hobbyking.com , alături de un încărcător specializat, IMAX B6, controlat de microprocesor.
Kit-ul de bază și acumulatorul au o masă totală de 933 grame. În condițiile în care partea electronică are o masă totală mai mică de 100 grame, am considerat că drona are o masă potrivită pentru un timp de zbor cuprins între 15 și 30 minute.
În ceea ce privește comunicația dintre quad-copter și telecomandă, am utilizat două module Xbee Pro Series 1 achiziționate de la magazinul online www.comet.srl.ro . Acestea funcționează la o tensiune cuprinsă între 2,8 și 3,4 V, la o frecvență de 2,4GHz, respectând standardul 802.15.4, și au o putere de emisie maximă de 50mW ce permite comunicația la o distanță maximă de 1600 m. Însă, conform recomandărilor ETSI, pentru frecvențe aparținând benzii de 2,4 GHz nu se poate emite cu o putere mai mare de 10mW/MHz, de aceea modulele Xbee vor fi setate la o putere de emisie de 10mW, raza de acțiune scăzând considerabil, pentru a respecta normele în vigoare. Comunicația cu microcontroller-ul se realizează prin interfața serială de date UART cu o rată maximă de 250000 bps.
Am ales microcontroller-ul ATmega128L produs de către Atmel deoarece reprezintă o platformă performantă de calcul, îndeplinind cu succes necesitățile impuse de proiectul curent, și datorită faptului că funcționează la o tensiune de 3,3V. Elementele esențiale care motivează alegerea făcută sunt memoria Flash de 128Kbytes și memoria EEPROM de 4kbytes, două canale PWM de 8 bits și șase canale PWM cu rezoluție programabilă de pâna la 16 bits, opt canale ADC de 10 bits, interfața TWI, SPI și două interfețe USART. Microcontroller-ul necesită un oscilator cu cuarț extern care generează o frecvență de tact de 7,3728 MHz.
Componentele electronice funcționează la o tensiune de 3,3V furnizată de stabilizatorul de tensiune BA033T fabricat de ROHM care generează un curent maxim de 1,0 A.
2.2. Circuitul electronic de test
Circuitul se bazează pe schema din figura D.1., care respectă conectarea de bază după datasheet-urile componentelor. JF este conectorul la care se atașează cablurile provenite de la baterie. L și C formează un filtru trece jos cu rolul de a elimina perturbațiile introduse de cele patru motoare, rezultând o tensiune continuă. Dioda D1 are rol de protecție împotriva inversării polarității la bornele conectorului JF. Ulterior, semnalul filtrat ajunge la stabilizatorul U3 care, împreună cu cele două condensatoare, C12 si C13, de 100nF, conectate la masă, furnizează la borna VOUT o tensiune stabilizată de 5V.
U2 este un convertor de nivel serial de tip MAX232 care realizează trecerea de la semnale cu amplitudini ale căror valori sunt de ±10V la semnale având amplitudini ale căror valori sunt de 0 – +5V. Pentru a realiza această conversie, circuitului integrat îi sunt necesare condensatoarele C7, C8, C9 și C10. Conectorul DB9 se atașează la MAX232 pentru a se putea conecta la unul dintre porturile de tip COM ale calculatorului.
În ceea ce privește circuitul de comandă, am utilizat un microcontroller ATmega32 care funcționează la frecvența de 7,3728 MHz, frecvență asigurată de cristalul de cuarț X1 împreună cu condensatoarele C2 și C3.
Componentele R1 și C1 formează circuitul de „power-on reset” datorită timpului de încărcare al condensatorului,care la momentul aplicării tensiunii la borne forțează nivelul de tensiune aferent pinului RESET al microcontroller-ului să tindă la 0, ceea ce conduce la un nivel logic de 0, resetând microcontrller-ul. După un timp aproximativ egal cu 5R1C1 condensatorul este încărcat datorită rezistenței R1 și determină un nivel de tensiune aferent celei de la ieșirea sursei de alimentare. Acest circuit de reset asigură pornirea microcontroller-ului în condiții bune de funcționare, timpul de comutare dintre nivelele logice fiind mai mare decât timpul de stabilizare al sursei de alimentare. [7]
Condensatoarele C5 și C6 sunt lipite între pinii 10-11 și 30-31 și au rol de decuplare, prevenind apariția zgomotului de comutare pe liniile de alimentare. De asemenea, condensatorul C4 are rolul de a filtra semnalul de intrare pentru a minimiza riscul de resetare al microcontroller-ului.
Dioda LED se leagă la portul PD6 al circuitului de comandă prin intermediul rezistenței R2, cu rol de limitare a curentului prin diodă. De asemenea, butonul SW1 se leagă între portul PD5 și masă cu scopul de a se obține valoarea 0 logic în momentul apăsării.
Microcontroller-ul se conectează la convertorul de nivel (figura B.2.) sau la modulul de comunicație XBEE prin intermediul pinilor TxD și RxD, selecția realizându-se manual prin intermediul a doi jumper-i.
Modulul XBEE se conectează la adaptorul XBEE Explorer (figura B.1.) care are un stabilizator de 3.3V și un circuit care realizează conversia de nivel de la 5V la 3,3V și invers pentru logica de comandă. Astfel, pinii adaptorului care prezintă interes pentru proiectul de față sunt VCC, GND, DOUT și DIN. De asemenea, acesta furnizează tensiunea de 3,3V stabilizată necesară circuitului MPU-9150 și circuitului care convertește semnalele seriale dintre microcontroller-ul ATmega și MPU-9150.
În final trebuie să amintim și cei doisprezece pini lipiți în configurație 4×3 cu ajutorul cărora se conectează cele patru ESC-uri, 4 pini se conectează la GND, 4 la VCC si ultimii 4 se conectează la pinii OCR0, OCR1A, OCR1B și OCR2 ai microcontroller-ului.
Lista finală de componente pentru circuitul de test este prezentată în tabelul D.1.
2.3. Circuitul electronic al quad-copter-ului
Circuitul final al quad-copter-ului este foarte asemănător cu circuitul de test, având câteva schimbări. În primul rând circuitul funcționează la tensiunea nominală de 3,3V. Microcontroller-ul utilizat este un ATmega128L, variantă care funcționează și la tensiunea de 3,3V.
În al doilea rând am utilizat un convertor de nivele serial ADM3202 care se conectează la multiplexorul 74HC4053D. Acesta din urmă realizează selecția dintre semnalul provenit de la interfața UART și semnalul provenit de la programatorul ISP. Pentru microcontroller-ul ATmega128L programarea se realizează pe pinii PDI,PDO și nu pe pinii MISO,MOSI cum este cazul procesorului ATmega32. Schema circuitului dronei este prezentată în figura 2.2. și lista de componente este detaliată în tabelul 2.1.
De asemenea, tensiunea nominală fiind de 3.3V nu a mai fost nevoie de un convertor de nivele logice pentru comunicația dintre microcontroller și unitatea de senzori de mișcare MPU-9150 și nici de un adaptor pentru XBee.
2.4. Circuitul electronic al telecomenzii
Circuitul telecomenzii pornește de la schema de bază care respectă conectarea componentelor conform foilor de catalog, așa cum am descris anterior. Diferențele față de circuitul de test apar în ceea ce privește microcontroller-ul utilizat, ATmega16, și condensatoarele SMD care se conectează la cristalul de cuarț, având o capacitate a cărei valoare este de 33pF. De asemenea, se adaugă două joystick-uri, fiecare fiind format din două potențiometre, și trei potențiometre adiționale al căror rol va fi descris ulterior. Fiecare potențiometru se conectează la bornele VCC și GND și la pinii ADC1 – ADC7 ai microcontroller-ului.
Schema circuitului telecomenzii este prezentată în figura 2.3. și lista de componente aferentă este expusă în tabelul 2.2. De asemenea, poza telecomenzii poate fi vizualizată în figura E.1.
2.5. Lista de componente și schemele celor două circuite
a)
b)
Figura 2.2. Schema circuitului dronei
Tabelul 2.1. Lista de componente pentru circuitul dronei
Figura 2.3. Schema circuitului telecomenzii
Tabelul 2.2. Lista de componente pentru circuitul telecomenzii
Capitolul 3. Descriere Software
3.1. Programul Quad-copter-ului
Fișierul main.c
Este fișierul principal al programului și conține funcția main() și alte funcții descrise în continuare. Codul sursă este prezent în totalitate în Anexa A. Codul sursă.
Liniile 1-25
Se includ fișiere de tip header și se definesc variabilele globale necesare algoritmului de tip PID.
Liniile 27-32
Se definește variabila globală inter care este inițializată cu valoarea 1 în rutina de întrerupere scrisă în continuare, semnalând o întrerupere externă pe portul INT0. Întreruperea este generată de către unitatea de senzori de mișcare MPU9150 atunci când s-a terminat procesul de citire a datelor de la senzori, odată la 200 ms, conform inițializărilor descrise în paragrafele următoare.
Liniile 35-71
Această secțiune de cod cuprinde implementarea funcțiilor Compute_ROLL() și Compute_PITCH() care realizează corecția necesară stabilizării quad-copter-ului cu ajutorul unui algoritm de tip PID. Cele două funcții sunt similare și se bazează pe calcularea erorii dintre valoarea setată, adică valoarea unghiului la care trebuie să se afle drona, și valoarea curentă a acesteia, suma erorilor precedente care va determina partea integrativă a algoritmului și diferența dintre eroarea curentă și eroarea anterioară, realizând o predicție a comportamentului sistemului, reprezentând partea derivativă a algoritmului. Funcțiile sunt de tip void, iar în urma apelului lor variabilele globale ROLLO și PITCHO vor avea valori conform formulei PID-ului: kp * error + ki * errSum + kd * dErr, unde kp reprezintă câștigul proporțional, ki – câștigul integrativ și kd – câștigul derivativ, iar error – diferența dintre valoarea setată și valoarea curentă a unghiului la care se află drona, errSum – suma erorilor precedente și dErr – diferența dintre eroarea curentă și eroarea anterioară.
Liniile 73-114
Începutul funcției main, funcția principală a programului. Se definesc variabilele locale, se apelează funcția Init() care inițializează microcontroller-ul, funcția TWIM_Init(100000) care inițializează interfața TWI pentru o frecvență de 100kHz, apoi se apelează funcția mpu6050_init() care inițializează unitatea de senzori de mișcare. După funcțiile de inițializare se aplică o întârziere de 10us, respectiv 10ms pentru a se realiza aceste procese cu succes. În continuare se aplică instrucțiunea #asm("sei") prin care se permit întreruperile globale.
Liniile 116-193
Se intră în bucla infinită și se restart-ează Watchdog Timer-ul. Am utilizat acest timer pentru a restart-a microcontroller-ul în cazul în care se intră într-un proces infinit nedorit, fiind necesară apelarea funcției wdogtrig() la anumite momente de timp pentru a restart-a Watchdog Timer înainte să ajungă la valoarea prestabilită – funcționare normală. În continuare se verifică dacă există un caracter pe interfața serială, iar în caz afirmativ se verifică dacă acesta are valoarea NULL. Astfel se realizează sincronizarea cu telecomanda, implementând un protocol de comunicare format dintr-un cadru de 8 octeți, primul fiind folosit pentru sincronizare (caracterul NULL), următorii patru sunt folosiți pentru a comanda quad-copter-ul prin putere totală aplicată motoarelor, valoarea unghiului pentru tangaj, valoarea unghiului pentru ruliu și control girație, iar ultimii trei octeți sunt folosiți pentru a inițializa parametrii necesari algoritmului de tip PID. Prin interfața serială se transmit variabile de tip unsigned char, având valori cuprinse între 0 și 255, de aceea sunt necesare anumite scalări pentru a aduce valorile în gama necesară algoritmului, precum și anumite conversii de tip cast, cum este cea de la unsigned char la float.
Scalările realizate conduc la o gamă de -30…+30 grade pentru valoarea de referință aferentă unghiurilor pentru ruliu și tangaj, 0…1 pentru termenul proporțional și 0…100 pentru termenul derivativ.
În continuare se verifică dacă a avut loc o întrerupere generată de un eveniment extern, ceea ce semnifică disponibilitatea informațiilor utile pe interfața TWI, iar în caz afirmativ se citesc aceste informații. Acestea sunt prelucrate de un filtru complementar care combină datele provenite de la giroscop cu cele provenite de la accelerometru într-o proporție de 0.98 pentru datele de la giroscop și 0.02 pentru datele de la accelerometru. Astfel se realizează o filtrare de tip trece-jos, giroscopul oferind informații precise pe termen scurt, însa având o eroare de drift pe termen lung, iar accelerometrul oferind o precizie ridicată pe termen lung, însă având un zgomot mare pe termen scurt.
Se obțin unghiurile aferente ruliului și tangajului și se utilizează aceste valori pentru a apela funcțiile Compute_ROLL() și Compute_PITCH() care vor determina corecțiile necesare redresării dronei față de valoarea setată.
În final se transmite fiecărui motor nivelul de putere total, compus din nivelul general de putere primit de la telecomandă și corecțiile necesare redresării dronei.
Fișierul INIT.c
Acest fișier conține funcția Init() care grupează inițializările generate de către CodeVision AVR conform cerințelor utilizatorului.
Liniile 1-29
Se inițializează registrele DDRx și registrele PORTx, x având valorile 1,2,3 și 4. Majoritatea registrelor se inițializează cu valoarea implicită 0, cu câteva excepții: DDRB = 0x08, care semnifică setarea bitului 3 al portului B ca ieșire și DDRD = 0b11110000, care semnifică setarea biților 4,5,6 și 7 ai portului D tot ca ieșire.
Liniile 32-87
Se inițializează registrele aferente timer-elor și întreruperilor. Timer-ele sunt configurate pentru modul Fast PWM, obținând o frecvență pentru PWM de 50Hz. De asemenea, se inițializează registrele aferente întreruperii externe INT0 pentru modul Front Crescător (Rising Edge).
Liniile 89-99
Această secvență cuprinde inițializarea registrelor aferente comunicației UART pentru rata de 57600 biți pe secundă, mod asincron, activ pentru emisie și recepție, iar în ceea ce privește parametrii de comunicare 8 octeți informaționali, unul de stop, fără paritate.
În următoarele linii este notabilă inițializarea registrului WDTCR care semnifică frecvența de restart-are a microcontroller-ului utilizând Watchdog Timer utilizând un prescaler cu o valoare de 2048k față de frecvența de oscilație de 7,3728 MHz.
Fișierul UART.c
Acest fișier conține secvența de cod generată de către CodeVision AVR pentru comunicarea pe interfața UART, grupând parametri specifici, implementarea întreruperilor pentru emisie și recepție, funcțiile getchar() și putchar() cu ajutorul cărora se transmit și se recepționează caractere pe interfața UART.
Fișierul TWI.c
Acest fișier conține parametrii și funcțiile specifice comunicației pe interfața TWI, precum TWIM_Init(), TWIM_Start(), TWIM_Stop(), TWIM_Write, TWIM_ReadAck și TWIM_ReadNack().
Fișierul FUNCT.c
Acest fișier conține #define-urile din program cu caracter global, precum și funcțiile necesare accesării unității de senzori de mișcare MPU-9150 conform protocolului descris în foaia de catalog [9].
Fișierele REG.h și mpu6050registers.h
Aceste fișiere conțin toate registrele necesare accesării unității de senzori MPU-9150, cel de-al doilea fișier având numele mpu6050registers.h deoarece unitatea folosită conține unitatea de senzori MPU-6050 și un magnetometru pe trei axe.
3.2. Programul Telecomenzii
Fișierul main.c
Acest fișier reprezintă programul principal al telecomenzii și conține funcția main(), integrând mai multe fișiere.
Se includ fișierele de tip header, se intră în funcția main() , apoi se apelează funcția Init() prin care se inițializează microcontroller-ul și se intră în bucla infinită.
Fișierele Init.c și UART.c
Pentru explicații suplimentare, a se vedea capitolul 3.1. al prezentei lucrări, întrucât explicațiile sunt valabile și în acest caz.
Fișierul FUNCT.c
Fișierul de față conține funcția prin care se realizează conversia analog-digitală pentru un anumit canal al microcontroller-ului, selectat cu ajutorul parametrului transmis la apelul funcției, și întreruperea principală a programului care conține protocolul de comunicație. Acest protocol este format prin transmiterea caracterului Null, urmat de șapte octeți care se formează secvențial și conțin valorile obținute prin conversia analog-digitală pentru canalele 1…7 ale microcontroller-ului. Primul octet reprezintă valoarea puterii generale transmise dronei, cel de-al doilea – valoarea tangajului, cel de-al treilea – valoarea ruliului, cel de-al patrulea – valoarea girației, iar ultimii trei reprezintă valorile parametrilor PID. Octeții vor avea valori cuprinse între 7 și 247 deoarece aceasta este gama dinamică a potențiometrelor utilizate.
Cadrul care se transmite odată la 20ms și conține informațiile utile controlului dronei, formând protocolul de comunicație, are structura prezentată în figura 3.1.
Figura 3.1. Schema cadrului protocolului de comunicație
Capitolul 4. Implementare și testare
Modalitatea de distribuire a timpului necesar dezvoltării proiectului este prezentată într-o manieră grafică în Anexa F. Diagrama Gantt.
M-am decis asupra kit-ului care conține un cadru de 450 x 450mm deoarece are dimensiunile potrivite pentru a dezvolta și testa un quad-copter, un cadru mai mare sau mai mic putând fi implementat ulterior în proiect fără a crea dificultăți, în condițiile în care partea electronică este funcțională, aceasta putând fi montată cu usurință pe altă platformă.
A doua componentă asupra căreia mi-am îndreptat atenția a fost unitatea de senzori de mișcare MPU-9150, aleasă după o documentare atentă asupra opțiunilor prezente. Aceasta îndeplinește toate cerințele impuse de proiect, având performanțe foarte bune. De asemenea, un criteriu important în alegerea făcută este reprezentat de posibilitatea de a folosi DMP-ul cu care este dotat circuitul pentru a extrage în mod direct informațiile legate de poziția circuitului în spațiu, fără alte prelucrări suplimentare din partea microcontroller-ului. DMP-ul (Digital Motion Processor) reprezintă un procesor conținut de circuitul MPU-9150 care combină datele provenite de la accelerometru, giroscop și magnetometru cu scopul de a obține valorile unghiurilor față de poziția de referință: orizontală, verticală și nord. Acest procesor este realizat special pentru a calcula rapid valorile amintite anterior pentru a degreva microcontroller-ul unui sistem în care este integrat MPU-9150 de alte procese laborioase.
Având componenta principală a quad-copter-ului am putut trece la faza de dezvoltare a codului, utilizând circuitul realizat la materia Proiect 2, un breadboard și un convertor de nivele serial, cu scopul de a obține valoarea ruliului și tangajului.
Pentru început, am căutat în foile de catalog ale unității de senzori de mișcare principiul de funcționare și registrele potrivit configurației de care aveam nevoie. Apoi am căutat în help-ul programului pe care l-am folosit pentru compilarea codului, CodeVision AVR, informații legate de accesarea interfeței TWI.
După ce am implementat forma finală a funcțiilor de care aveam nevoie, am încercat să inițializez unitatea de senzori de mișcare, apoi să citesc două registre care conțineau informația legată de temperatură. Am reușit să citesc temperatura la nivelul senzorului și să o afișez în consolă prin intermediul portului serial. Ulterior am început să dezvolt codul necesar fuziunii datelor provenite de la accelerometru și giroscop pentru obținerea unghiurilor Euler dorite: ruliul și tangajul.
Am încercat să dezvolt codul folosind suita de programe Atmel Studio 6, fiind necesar mai înainte să realizez funcții de bază, precum citirea caracterelor de la tastatură, aprinderea unei diode LED sau afișarea caracterelor în consolă utilizând portul serial deoarece erau diferențe vizibile față de compilatorul CodeVision AVR. În final am ales compilatorul CodeVision AVR deoarece generarea automată a unor secvențe de cod a făcut programarea foarte facilă.
De asemenea, a trebuit să găsesc alte metode de a afișa caractere în consolă, față de cele oferite de către compilatorul CodeVision AVR, și pentru a realiza acest deziderat a trebuit să convertesc variabile de tip float la un string de caractere pentru a trimite informații pe portul serial.
După ce am vizualizat în consolă valorile corecte ale unghiurilor corespunzătoare poziției în spațiu a unității de senzori de mișcare, am decis să combin aceste rezultate cu mișcarea motoarelor. Pentru început, am conectat ESC-urile la circuitul de test și am încercat să configurez corect registrele astfel încât să funcționeze motoarele. Am observat că trei dintre motoare funcționează corespunzător pentru o frecvență a semnalului de control modulat tip PWM de 450 Hz, conform specificațiilor ESC-urilor, iar cel de-al patrulea motor funcționează corect dacă frecvența semnalului de tip PWM este de 50 Hz.
Programul pentru circuitul quad-copter-ului a depășit dimensiunea de 16kb aferentă memoriei Flash a microcontroller-ului ATmega16 și de aceea a trebuit să implementez un microcontroller ATmega32 care are o memorie de program de 32kb.
Având un program care controla motoarele și un alt program care prelucra datele pentru a obține unghiuri Euler, a mai rămas doar combinarea acestora pentru faza curentă. Astfel, „fuziunea” celor două coduri s-a realizat cu ușurință.
A mai fost nevoie să implementez câte un algoritm de tip PID pentru tangaj și pentru ruliu, setând pentru această fază parametrii P = 1, I = 0 și D = 0, care practic „șuntează” algoritmul PID, rămânând doar termenul proporțional cu scopul de a vedea cum reacționează sistemul, urmând să determin ulterior valorile corespunzătoare pentru aceștia.
Pentru telecomandă am ales să folosesc un modul XBee Pro, care funcționează la o frecvență de 2.4GHz, astfel evitând eventuale probleme legate de alocarea spectrului de frecvențe radio. De asemenea, modulul are o rază mare de acțiune, de 1500m, și realizează prelucrări interne care conduc la o corecție foarte bună a informațiilor recepționate.
Pentru a programa modulele XBee am folosit XBee Explorer USB care utilizează convertorul de nivele serial FT232. Astfel, am conectat pe rând câte un modul XBee Pro la calculator și am modificat o serie de parametrii: ID – identitatea rețelei, valoare care trebuie să fie identică pentru amândouă modulele, iar pentru fiecare modul în parte parametrii DH și DL trebuie să aibă valoarea parametrilor SH și SL de la celălalt modul.
În continuare am decis să realizez un circuit imprimat pentru quad-copter utilizând microcontroller-ul ATmega128L produs de către Atmel deoarece reprezintă o platformă performantă de calcul, îndeplinind cu succes necesitățile impuse de proiectul curent, și datorită faptului că funcționează la o tensiune de 3,3V. Elementele esențiale care motivează alegerea făcută sunt memoria Flash de 128Kbytes și memoria EEPROM de 4kbytes, doua canale PWM de 8 bits și șase canale PWM cu rezoluție programabilă de pâna la 16 bits, opt canale ADC de 10 bits, interfața TWI, SPI și două interfețe USART. Microcontroller-ul necesită un oscilator cu cuarț extern care generează o frecvență de tact de 7,3728 MHz.
De asemenea, în realizarea proiectului au mai fost necesare: un multiplexor triplu de câte două canale 74HC4053, un transceiver ADM3202, rezistoare, condensatoare, inductoare SMD și conectori necesari.Toate aceste componente funcționează la o tensiune de 3,3V furnizată de stabilizatorul de tensiune BA033T fabricat de ROHM care generează un curent maxim de 1,0 A.
Pentru realizarea schemelor si layout-ului circuitelor, inclusiv fișierelor Gerber necesare implementării PCB-ului într-o unitate de producție, am utilizat programul gratuit Eagle. Am realizat mai multe variante ale schemelor și ale layout-ului, îmbunătățind de fiecare dată design-ul. Pentru vizualizarea PCB-ului am accesat site-ul http://mayhewlabs.com/3dpcb unde am încărcat fișierele Gerber și am putut vizualiza PCB-ul așa cum ar fi trebuit acesta să fie în realitate.
Pentru a vizualiza figurile care conțin pozele și layout-ul circuitului dronei, a se verifica Anexa C. Layout-ul și pozele circuitului dronei.
În vederea efectuării testelor, am utilizat placa de test existentă, la care am adăugat condensatorul C4 și inductorul L1 care formează un filtru trece jos pentru a elimina eventuale componente nedorite introduse de motoare. Frecvența de tăiere a filtrului trece jos este de aproximativ 300Hz. De asemenea, a fost nevoie să adaug și adaptorul pentru modulul XBee Pro, Xbee Explorer Regulated, care are un stabilizator de 3,3V și un convertor de nivele logice prin care se poate realiza comunicarea dintre XBee (tensiune nominală de 3,3V) și microcontroller-ul ATmega32 (tensiune nominală de 5V). Unitatea de senzori de mișcare a fost lipită pe placă, însă a fost nevoie și de un convertor de nivele logice pentru comunicarea cu microcontroller-ul deoarece acest modul funcționează tot la o tensiune nominală de 3,3V.
În plus, am adăugat pinii prin care se conectează la circuitul de comandă ESC-urile și încă 6 pini (2×3) cu ajutorul cărora se realizează manual comutația între convertorul de nivele serial MAX232 și XBee Pro cu ajutorul a doi jumper-i.
După ce a fost realizat circuitul dronei, am încărcat codul existent și am verificat funcționarea acestuia.
Pasul următor a fost realizarea telecomenzii pornind de la schema circuitului de test dezvoltat la materia Proiect 2, adăugând două joystick-uri și trei potențiometre.
După finalizarea circuitului, a urmat partea de testare, verificând dacă între anumiți pini am tensiunea potrivită, apoi dacă se realizează cu succes comunicarea prin intermediul portului serial.
În continuare am realizat un protocol simplu de comunicare între cele două circuite, programul de test încărcat pe microcontroller-ul quad-copter-ului conducând la stingerea și aprinderea unei diode LED în cazul în care se transmitea un anumit caracter de la telecomandă. Obținând rezultatele scontate, am implementat un protocol mai complex de comunicare între circuite.
În această fază am avut toate elementele necesare testării propriu-zise a quad-copter-ului (figura E.2.). Pentru început am utilizat doar parametrii proporțional și derivativ pentru algoritmul de control și am eliminat din ecuație parametrul integrativ deoarece acesta conduce la un volum de calcul foarte mare pentru microcontroller și nu are un impact la fel de puternic asupra stabilității dronei precum au ceilalți doi parametri. Am realizat o scalare a parametrilor implicați în algoritmul de control automat la valoarea 1 pentru P și D și am realizat primul test pe teren. Ca măsură de siguranță am legat drona de o greutate de 5 kg prin intermediul unui fir. Rezultatele nu au fost cele așteptate deoarece drona nu s-a putut stabiliza în aer indiferent de valorile pentru P și D transmise de la telecomandă cu ajutorul potențiometrelor de reglaj.
Pentru a doua încercare am realizat o scalare la valoarea 1,3 pentru parametrul P și 100 pentru parametrul D și au început să se observe schimbări, deși stabilizarea prezenta un caracter foarte „agresiv”. Testul nu a fost realizat cu elicele montate pe quad-copter.
A treia încercare a fost realizată pentru parametrii P și D scalați la valorile 1,3 și 10 și deja s-a putut observa o îmbunătățire pentru testul fără elice și de aceea s-a putut trece la testarea pe teren. Nici de această dată însă nu s-a realizat o stabilizare acceptabilă pentru dronă astfel încât să zboare la punct fix.
Rezultate și concluzii
Probleme apărute și modalități de soluționare
O primă dificultate a fost legată de stabilitatea microcontroller-ului. Acest circuit avea un comportament nestandard, restartându-se fără să țină cont de un tipar, cel puțin aparent.
Pentru a rezolva dificultățile apărute, am încercat diverse metode de a accesa unitatea de senzori de mișcare prin intermediul interfeței TWI, utilizând librăria <twi.h> a compilatorului CodeVision AVR, crezând că aceasta este cauza restart-ării defectuoase a microcontroller-ului.
După ce am găsit o formă „acceptabilă” pentru funcțiile cu ajutorul cărora am accesat MPU-9150, în sensul în care microcontroller-ul părea să fie mai stabil, am început să dezvolt programul necesar combinării datelor provenite de la accelerometru și giroscop și obținerii ruliului și tangajului.
Sarcina s-a dovedit destul de dificilă întrucât aveam problemele menționate anterior și nu găsisem o formă suficient de bună a filtrului complementar. Am încercat să portez bucăți de cod realizate pentru compilatorul Arduino sau cele oferite de Microchip sau STM.
Începând să suspectez unitatea de senzori de mișcare pentru instabilitatea microcontroller-ului, am conectat-o pe aceasta la un circuit Arduino Uno și am compilat și programat microcontroller-ul de pe Arduino cu un cod furnizat de https://www.sparkfun.com/ care, în urma prelucrărilor, afișa în mod corect pe consolă cele trei unghiuri Euler pe care doream să le obțin utilizând platforma de la materia Proiect 2.
După o perioadă de încercări nereușite, am luat decizia de a încerca să conectez circuitul de test la placa Arduino Uno în mod direct, șuntând stabilizatorul de tensiune LM7805. Rezultatele au fost foarte sugestive, microcontroller-ul fiind foarte stabil, astfel am determinat cauza problemelor inițiale legate de restart-area microcontroller-ului: pentru alimentarea circuitului utilizam o sursă în comutație folosită pentru alimentarea modulelor cu LED-uri, având o tensiune nominală de 12V și o intensitate a curentului maximă de 5A, rezultând o putere totală maximă de 60W, sursă necorespunzătoare pentru testarea funcționării modulelor pentru proiectul curent.
O a doua problemă a fost dată de încercarea de a utiliza DMP-ul circuitului MPU-9150 pentru a obține valorile unghiurilor dorite, însă nu am putut folosi această funcție deoarece microcontroller-ul utilizat pentru testare (ATmega32) are o memorie de tip EEPROM cu o capacitate de stocare inferioară celei necesare utilizării DMP-ului: 1024 kb în loc de 2048 kb.
Într-un final am renunțat la abordarea proiectului folosind DMP-ul și am continuat varianta în care este implementată fuziunea datelor „raw” preluate de la unitatea de senzori de mișcare și prelucrarea acestora cu ajutorul unui filtru complementar.
O altă dificultate a fost legată de controlul celui de-al patrulea motor care nu funcționa, celelalte trei fiind acționate cu ușurință. Pentru a rezolva acest neajuns, am determinat pe cale experimentală că frecvența potrivită pentru cel de-al patrulea ESC este de 50Hz și astfel am remediat acest neajuns.
În continuare au apărut din nou anumite probleme legate de stabilitatea microcontroller-ului: după un anumit timp de funcționare, circuitul se reseta. Pentru a verifica în ce fel se comportă sistemul, am păstrat comunicarea prin intermediul portului serial. De aceea, am putut să vizualizez ce valori obțin prin prelucrarea informațiilor de la senzori în timp ce motoarele urmăreau într-un mod direct variațiile față de poziția de referință impusă de zero grade, atât pentru ruliu, cât și pentru tangaj. O soluție a fost să înlocuiesc secvența de cod care implementa un filtru Mahony [8] pentru fuziunea datelor provenite de la accelerometru și giroscop cu o secvență de cod aferentă unui filtru complementar. Filtrul Mahony presupune conversia datelor neprelucrate în cuaternioni, variabile bazate pe un sistem matematic cu patru dimensiuni, apoi conversia acestor elemente în unghiurile dorite, respectiv tangaj și ruliu. Astfel, complexitatea filtrului Mahony este foarte ridicată și poate crea probleme unor platforme de calcul în timp real cum este microcontroller-ul unei drone. Spre deosebire de această metodă, filtrul complementar este foarte simplu și necesită mai puține prelucrări față de varianta inițială, având o complexitate redusă.
În procesul de dezvoltare și testare a proiectului curent am realizat operațiuni care au condus la distrugerea a două circuite integrate MAX232, a unui modul XBee Pro montat pe telecomandă și a unui circuit de control (ESC) pentru motoare. Soluția a fost să înlocuiesc aceste componente, modulul de comunicație fiind schimbat cu un XBee care are o putere de emisie de maxim 1mW (față de 50mW cât are XBee Pro), iar ESC-ul defect fiind schimbat cu un model mai nou și mai performant.
De asemenea, în timpul procesului de testare s-a observat că un canal al unui joystick afecta valorile obținute de către microcontroller cu ajutorul ADC-ului pentru celelalte canale. În urma verificărilor am constatat că microcontroller-ul are un canal defect pentru ADC (canalul 0) și de aceea am schimbat canalul ADC-ului (cu canalul 7). Astfel, s-au eliminat problemele datorate „interferenței inter-joystick”.
Posibile implementări viitoare
O primă opțiune este adăugarea unui ecran LCD pe telecomandă pentru a putea vizualiza diverși parametri de zbor, precum ruliul, tangajul și tensiunea bateriei. Modulele XBee montate pe circuitul dronei și pe circuitul telecomenzii sunt bidirecționale și fac posibilă telemetria. De asemenea, circuitul dronei a fost proiectat să conțină și două rezistențe care formează un divizor de tensiune pentru a aduce tensiunea de la valoarea maximă de 16V la valoarea maximă de 3,3V cu scopul de a putea fi citită de ADC-ul microcontroller-ului ATmega128L.
Adăugarea unui modul GPS, precum și a unui barometru și a unui senzor de proximitate poate fi utilă atât pentru telemetrie, cât și pentru dezvoltarea unei drone autonome.
De asemenea, s-a luat în calcul și dezvoltarea unui sistem de orientare și stabilizare pentru camera video, de tip gimbal, pe două sau trei axe, utilizând o unitate de senzori de mișcare MPU-9150, cu scopul de a realiza filmări aeriene cu ajutorul unei camere GoPro Hero 3 existente.
Bibliografie
[1] http://www.bbc.com/news/technology-25180906
[2] Oscar. Build A Quadcopter From Scratch – Hardware Overview. http://blog.oscarliang.net/build-a-quadcopter-beginners-tutorial-1/, accesat la data de 17.06.2014.
[3] http://copter.ardupilot.com/wiki/px4fmu-only-wiring/, accesat la data de 17.06.2014.
[4] S.P. Bhattacharyya. Modern PID control. Texas A&M University. A Short Course at University of California, Berkeley. http://msc.berkeley.edu/PID/modernPID1-overview.pdf, accesat la data de 19.06.2014.
[5] http://en.wikipedia.org/wiki/PID_controller, accesat la data de 18.06.2014.
[6] http://www.sierra.ro/Quadrocopter-DJI-F450-KIT-Flame-Wheel-(cu-motor–ESC–elice)-p5793p.html, accesat la data de 17.06.2014.
[7] http://ham.elcom.pub.ro/proiect2/files/cpu.pdf, accesat la data de 17.06.2014.
[8] Kenneth J. Jensen. Generalized Nonlinear Complementary Attitude Filter. http://arxiv.org/pdf/1110.0274.pdf, accesat la data de 19.02.2014
[9] http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/IMU/PS-MPU-9150A.pdf, accesat la data de 23.02.2014.
[10] http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/, accesat la data de 07.03.2014.
[11] https://github.com/Pansenti/MPU9150Lib, accesat la data de 19.02.2014.
[12] Pieter Jan. Reading a IMU Without Kalman: The Complementary Filter. http://www.pieter-jan.com/node/11, accesat la data de 24.02.2014.
[13] http://physudo-e.blogspot.ro/2013/09/deutsche-version-using-atmega-as-i2c.html, accesat la data de 12.02.2014.
[14] http://playground.arduino.cc/Main/MPU-9150, accesat la data de 19.02.2014.
[15] http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Wireless/Zigbee/XBee-Regulated-v14.pdf, accesat la data de 22.06.2014.
[16] http://dlnmh9ip6v2uc.cloudfront.net/datasheets/BreakoutBoards/Logic_Level_Bidirectional.pdf, accesat la data de 22.06.2014.
Anexa A. Codul sursă
Codul Quad-copter-ului
Fișierul main.c
#include <mega32.h>
#include <io.h>
#include <stdio.h>
#include <math.h>
#include <delay.h>
#include <string.h>
#include <stdlib.h>
#include "INIT.c"
#include "UART.c"
#include "TWI.c"
#include "FUNCT.c"
#include "REG.h"#define dt 1/20
/*Variabile pentru algoritmul PID*/
float Input, ROLLO, ROLLSET;
float errSum, lastErr;
float kp, ki, kd;
float timechange = 1/20;
float Input2, PITCHO, PITCHSET;
float errSum2, lastErr2;
float kp2, ki2, kd2;
char inter = 0;
interrupt [EXT_INT0] void ext_int0_isr(void)
{
inter = 1;
}
volatile uint8_t buffer[14];
//////////Începutul secvenței de cod preluate de pe site-ul http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction///////////
void Compute_ROLL()
{
float dErr, error;
wdogtrig();
/*Compute all the working error variables*/
error = ROLLSET – Input;
errSum += error;
dErr = error – lastErr;
/*Compute PID Output*/
ROLLO = kp * error + ki * errSum + kd * dErr;
/*Remember some variables for next time*/
lastErr = error;
}
void Compute_PITCH()
{
float dErr, error;
wdogtrig();
/*Compute all the working error variables*/
error = PITCHSET – Input2;
errSum2 += error ;
dErr = error – lastErr2;
/*Compute PID Output*/
PITCHO = kp2 * error + ki2 * errSum2 + kd2 * dErr;
/*Remember some variables for next time*/
lastErr2 = error;
}[10]
//////////Sfârșitul secvenței de cod preluate de pe site-ul http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction///////////
void main(void)
{
long int time;
int ax, ay, az, gx, gy, gz;
float roll, pitch;
unsigned char a,b,c,d,P,I,D,temp;
int forceMagnitudeApprox, rollAcc, pitchAcc;
Init();
TWIM_Init(100000);
delay_us(10);
mpu6050_init();
delay_ms(50);
#asm("sei")
LED1 = 1;
while(1)
{
wdogtrig();
if(rx_counter) // dacă un caracter este prezent pe portul serial
{
temp = getchar();
if(temp == 0)
{
wdogtrig();
a = getchar();
b = getchar();
c = getchar();
d = getchar();
P = getchar();
I = getchar();
D = getchar();
PITCHSET = (float)b*0.25 – 31.75;
ROLLSET = (float)c*0.25 – 31.75;
kp = (float)((I-7)*1.3)/247;
kd = (float)(D-7)*100/247;
kp2 = kp;
kd2 = kd;
if(ROLLSET > 15.0)
LED1 = ~LED1;
if(inter == 1)
{
mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, (uint8_t *)buffer);
ax = (((int16_t)buffer[0]) << 8) | buffer[1];
ay = (((int16_t)buffer[2]) << 8) | buffer[3];
az = (((int16_t)buffer[4]) << 8) | buffer[5];
gx = (((int16_t)buffer[8]) << 8) | buffer[9];
gy = (((int16_t)buffer[10]) << 8) | buffer[11];
gz = (((int16_t)buffer[12]) << 8) | buffer[13];”[9]
„pitch += ((float)gx / MPU6050_GGAIN) *dt;
roll -= ((float)gy / MPU6050_GGAIN) *dt;
forceMagnitudeApprox = abs(ax + ay + az);
if(forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
{
pitchAcc = atan2((float)ay, (float)az)*180/M_PI;
pitch = pitch*0.98 + pitchAcc*0.02;
rollAcc = atan2((float)ax, (float)az)*180/M_PI;
roll = roll*0.98 + rollAcc*0.02;
} [12]
Input = roll;
Compute_ROLL();
Input2 = pitch;
Compute_PITCH();
inter = 0;
}
M1 = a – (unsigned char)(PITCHO/2) + (unsigned char)(ROLLO/2);
M2 = a + (unsigned char)(PITCHO/2) + (unsigned char)(ROLLO/2);
M3 = a – (unsigned char)(PITCHO/2) – (unsigned char)(ROLLO/2);
M4 = a + (unsigned char)(PITCHO/2) – (unsigned char)(ROLLO/2);
}
}
}
}
Fișierul INIT.c
void Init()
{
// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;
// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x08;
// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0b00000000;
// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD=0x00;
DDRD= 0b11110000; // D.6 is LED
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF
// OC0 output: Disconnected
TCCR0=0x6B;
TCNT0=0x00;
OCR0=0x09;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer1 Stopped
// Mode: Normal top=0xFFFF
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=(1<<COM1A1) | (0<<COM1A0) | (1<<COM1B1) | (0<<COM1B0) | (0<<WGM11) | (1<<WGM10);
TCCR1B=(0<<ICNC1) | (0<<ICES1) | (0<<WGM13) | (1<<WGM12) | (0<<CS12) | (1<<CS11) | (1<<CS10);
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x09;
OCR1AL=0x00;
OCR1BH=0x09;
OCR1BL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x6C;
TCNT2=0x00;
OCR2=0x09;
// External Interrupt(s) initialization
// INT0: On
// INT0 Mode: Rising Edge
// INT1: Off
// INT2: Off
GICR|=0x40;
MCUCR=0x03;
MCUCSR=0x00;
GIFR=0x40;
// Timer(s)/Counter(s) Interrupt(s) initialization
//TIMSK=0x10;
TIMSK=0x00;
// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud Rate: 57600
UCSRA=0x00;
UCSRB=0xD8;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x07;
// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;
// ADC initialization
// ADC disabled
ADCSRA=0x00;
// SPI initialization
// SPI disabled
SPCR=0x00;
// TWI initialization
// TWI disabled
TWCR=0x00;
// Watchdog Timer initialization
// Watchdog Timer Prescaler: OSC/2048k
WDTCR=0x0F;
}
Fișierul UART.c
#ifndef RXB8
#define RXB8 1
#endif
#ifndef TXB8
#define TXB8 0
#endif
#ifndef UPE
#define UPE 2
#endif
#ifndef DOR
#define DOR 3
#endif
#ifndef FE
#define FE 4
#endif
#ifndef UDRE
#define UDRE 5
#endif
#ifndef RXC
#define RXC 7
#endif
#define FRAMING_ERROR (1<<FE)
#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<DOR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)
// USART Receiver buffer
#define RX_BUFFER_SIZE 8
char rx_buffer[RX_BUFFER_SIZE];
#if RX_BUFFER_SIZE <= 256
unsigned char rx_wr_index,rx_rd_index,rx_counter;
#else
unsigned int rx_wr_index,rx_rd_index,rx_counter;
#endif
// This flag is set on USART Receiver buffer overflow
bit rx_buffer_overflow;
// USART Receiver interrupt service routine
interrupt [USART_RXC] void usart_rx_isr(void)
{
char status,data;
status=UCSRA;
data=UDR;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
{
rx_buffer[rx_wr_index++]=data;
#if RX_BUFFER_SIZE == 256
// special case for receiver buffer size=256
if (++rx_counter == 0) rx_buffer_overflow=1;
#else
if (rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0;
if (++rx_counter == RX_BUFFER_SIZE)
{
rx_counter=0;
rx_buffer_overflow=1;
}
#endif
}
}
#ifndef _DEBUG_TERMINAL_IO_
// Get a character from the USART Receiver buffer
#define _ALTERNATE_GETCHAR_
#pragma used+
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index++];
#if RX_BUFFER_SIZE != 256
if (rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#endif
#asm("cli")
–rx_counter;
#asm("sei")
return data;
}
#pragma used-
#endif
// USART Transmitter buffer
#define TX_BUFFER_SIZE 8
char tx_buffer[TX_BUFFER_SIZE];
#if TX_BUFFER_SIZE <= 256
unsigned char tx_wr_index,tx_rd_index,tx_counter;
#else
unsigned int tx_wr_index,tx_rd_index,tx_counter;
#endif
// USART Transmitter interrupt service routine
interrupt [USART_TXC] void usart_tx_isr(void)
{
if (tx_counter)
{
–tx_counter;
UDR=tx_buffer[tx_rd_index++];
#if TX_BUFFER_SIZE != 256
if (tx_rd_index == TX_BUFFER_SIZE) tx_rd_index=0;
#endif
}
}
#ifndef _DEBUG_TERMINAL_IO_
// Write a character to the USART Transmitter buffer
#define _ALTERNATE_PUTCHAR_
#pragma used+
void putchar(char c)
{
while (tx_counter == TX_BUFFER_SIZE);
#asm("cli")
if (tx_counter || ((UCSRA & DATA_REGISTER_EMPTY)==0))
{
tx_buffer[tx_wr_index++]=c;
#if TX_BUFFER_SIZE != 256
if (tx_wr_index == TX_BUFFER_SIZE) tx_wr_index=0;
#endif
++tx_counter;
}
else
UDR=c;
#asm("sei")
}
#pragma used-
#endif
Fișierul TWI.c [13]
#include <stdint.h>
#define F_CPU 7372800
#define TRUE 1
#define FALSE 0
/****************************************************************************
TWI State codes
****************************************************************************/
// General TWI Master staus codes
#define TWI_START 0x08 // START has been transmitted
#define TWI_REP_START 0x10 // Repeated START has been transmitted
#define TWI_ARB_LOST 0x38 // Arbitration lost
// TWI Master Transmitter staus codes
#define TWI_MTX_ADR_ACK 0x18 // SLA+W has been tramsmitted and ACK received
#define TWI_MTX_ADR_NACK 0x20 // SLA+W has been tramsmitted and NACK received
#define TWI_MTX_DATA_ACK 0x28 // Data byte has been tramsmitted and ACK received
#define TWI_MTX_DATA_NACK 0x30 // Data byte has been tramsmitted and NACK received
// TWI Master Receiver staus codes
#define TWI_MRX_ADR_ACK 0x40 // SLA+R has been tramsmitted and ACK received
#define TWI_MRX_ADR_NACK 0x48 // SLA+R has been tramsmitted and NACK received
#define TWI_MRX_DATA_ACK 0x50 // Data byte has been received and ACK tramsmitted
#define TWI_MRX_DATA_NACK 0x58 // Data byte has been received and NACK tramsmitted
// TWI Slave Transmitter staus codes
#define TWI_STX_ADR_ACK 0xA8 // Own SLA+R has been received; ACK has been returned
#define TWI_STX_ADR_ACK_M_ARB_LOST 0xB0 // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned
#define TWI_STX_DATA_ACK 0xB8 // Data byte in TWDR has been transmitted; ACK has been received
#define TWI_STX_DATA_NACK 0xC0 // Data byte in TWDR has been transmitted; NOT ACK has been received
#define TWI_STX_DATA_ACK_LAST_BYTE 0xC8 // Last data byte in TWDR has been transmitted (TWEA = “0”); ACK has been received
// TWI Slave Receiver staus codes
#define TWI_SRX_ADR_ACK 0x60 // Own SLA+W has been received ACK has been returned
#define TWI_SRX_ADR_ACK_M_ARB_LOST 0x68 // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned
#define TWI_SRX_GEN_ACK 0x70 // General call address has been received; ACK has been returned
#define TWI_SRX_GEN_ACK_M_ARB_LOST 0x78 // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned
#define TWI_SRX_ADR_DATA_ACK 0x80 // Previously addressed with own SLA+W; data has been received; ACK has been returned
#define TWI_SRX_ADR_DATA_NACK 0x88 // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned
#define TWI_SRX_GEN_DATA_ACK 0x90 // Previously addressed with general call; data has been received; ACK has been returned
#define TWI_SRX_GEN_DATA_NACK 0x98 // Previously addressed with general call; data has been received; NOT ACK has been returned
#define TWI_SRX_STOP_RESTART 0xA0 // A STOP condition or repeated START condition has been received while still addressed as Slave
// TWI Miscellaneous status codes
#define TWI_NO_STATE 0xF8 // No relevant state information available; TWINT = “0”
#define TWI_BUS_ERROR 0x00 // Bus error due to an illegal START or STOP condition
#define TWIM_READ 1
#define TWIM_WRITE 0
/*******************************************************
Public Function: TWIM_Init
Purpose: Initialise the TWI Master Interface
Input Parameter:
– uint16_t TWI_Bitrate (Hz)
Return Value: uint8_t
– FALSE: Bitrate too high
– TRUE: Bitrate OK
*******************************************************/
uint8_t TWIM_Init (uint32_t TWI_Bitrate)
{
/*
** Set TWI bitrate
** If bitrate is too high, then error return
*/
TWBR = ((F_CPU/TWI_Bitrate)-16)/2;
if (TWBR < 11) return FALSE;
return TRUE;
}
/*******************************************************
Public Function: TWIM_Start
Purpose: Start the TWI Master Interface
Input Parameter:
– uint8_t Device address
– uint8_t Type of required Operation:
TWIM_READ: Read data from the slave
TWIM_WRITE: Write data to the slave
Return Value: uint8_t
– TRUE: OK, TWI Master accessible
– FALSE: Error in starting TWI Master
*******************************************************/
uint8_t TWIM_Start (uint8_t Address, uint8_t TWIM_Type)//1 = read, 0 = write
{
uint8_t twst;
/*
** Send START condition
*/
TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
/*
** Wait until transmission completed
*/
while (!(TWCR & (1<<TWINT)));
/*
** Check value of TWI Status Register. Mask prescaler bits.
*/
twst = TWSR & 0xF8;
if ((twst != TWI_START) && (twst != TWI_REP_START)) return FALSE;
/*
** Send device address
*/
TWDR = (Address<<1) + TWIM_Type;
TWCR = (1<<TWINT)|(1<<TWEN);
/*
** Wait until transmission completed and ACK/NACK has been received
*/
while (!(TWCR & (1<<TWINT)));
/*
** Check value of TWI Status Register. Mask prescaler bits.
*/
twst = TWSR & 0xF8;
if ((twst != TWI_MTX_ADR_ACK) && (twst != TWI_MRX_ADR_ACK)) return FALSE;
return TRUE;
}
/*******************************************************
Public Function: TWIM_Stop
Purpose: Stop the TWI Master
Input Parameter: None
Return Value: None
*******************************************************/
void TWIM_Stop (void)
{
/*
** Send stop condition
*/
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);
/*
** Wait until stop condition is executed and bus released
*/
while (TWCR & (1<<TWINT));
}
/*******************************************************
Public Function: TWIM_Write
Purpose: Write a byte to the slave
Input Parameter:
– uint8_t Byte to be sent
Return Value: uint8_t
– TRUE: OK, Byte sent
– FALSE: Error in byte transmission
*******************************************************/
uint8_t TWIM_Write (uint8_t byte)
{
uint8_t twst;
/*
** Send data to the previously addressed device
*/
TWDR = byte;
TWCR = (1<<TWINT)|(1<<TWEN);
/*
** Wait until transmission completed
*/
while (!(TWCR & (1<<TWINT)));
/*
** Check value of TWI Status Register. Mask prescaler bits
*/
twst = TWSR & 0xF8;
if (twst != TWI_MTX_DATA_ACK) return 1;
return 0;
}
/*******************************************************
Public Function: TWIM_ReadAck
Purpose: Read a byte from the slave and request next byte
Input Parameter: None
Return Value: uint8_t
– uint8_t Read byte
*******************************************************/
uint8_t TWIM_ReadAck (void)
{
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWEA);
while (!(TWCR & (1<<TWINT)));
return TWDR;
}
/*******************************************************
Public Function: TWIM_ReadAck
Purpose: Read the last byte from the slave
Input Parameter: None
Return Value: uint8_t
– uint8_t Read byte
*******************************************************/
uint8_t TWIM_ReadNack (void)
{
TWCR = (1<<TWINT)|(1<<TWEN);
while(!(TWCR & (1<<TWINT)));
return TWDR;
}
Fișierul FUNCT.c
#include "mpu6050registers.h"
#include "REG.h"
#include <stdint.h>
#include <delay.h>
#include <io.h>
#include <stdlib.h>
#include <string.h>
#include <math.h> //include libm
#define M1 OCR0
#define M3 OCR1A
#define M4 OCR1B
#define M2 OCR2
#define LED1 PORTD.6 // PORTx is used for output
#define wdogtrig() #asm("wdr") // call often if Watchdog timer enabled
//////////Începutul secvenței de cod preluate de pe site-ul https://github.com/Pansenti/MPU9150Lib//////////
//definitions
#define MPU6050_ADDR 0x68 //device address – 0x68 pin low (GND), 0x69 pin high (VCC)
//definitions for raw data
//gyro and acc scale
#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
#define MPU6050_GYRO_LSB_250 131.0
#define MPU6050_GYRO_LSB_500 65.5
#define MPU6050_GYRO_LSB_1000 32.8
#define MPU6050_GYRO_LSB_2000 16.4
#if MPU6050_GYRO_FS == MPU6050_GYRO_FS_250
#define MPU6050_GGAIN MPU6050_GYRO_LSB_250
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_500
#define MPU6050_GGAIN MPU6050_GYRO_LSB_500
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_1000
#define MPU6050_GGAIN MPU6050_GYRO_LSB_1000
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_2000
#define MPU6050_GGAIN MPU6050_GYRO_LSB_2000
#endif
#define MPU6050_ACCEL_LSB_2 16384.0
#define MPU6050_ACCEL_LSB_4 8192.0
#define MPU6050_ACCEL_LSB_8 4096.0
#define MPU6050_ACCEL_LSB_16 2048.0
#if MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_2
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_2
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_4
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_4
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_8
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_8
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_16
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_16
#endif
#define MPU6050_CALIBRATEDACCGYRO 1 //set to 1 if is calibrated
#define MPU6050_AXOFFSET 0
#define MPU6050_AYOFFSET 0
#define MPU6050_AZOFFSET 0
#define MPU6050_AXGAIN 16384.0
#define MPU6050_AYGAIN 16384.0
#define MPU6050_AZGAIN 16384.0
#define MPU6050_GXOFFSET -42
#define MPU6050_GYOFFSET 9
#define MPU6050_GZOFFSET -29
//#define MPU6050_GXGAIN 16.4
#define MPU6050_GYGAIN 16.4
//#define MPU6050_GZGAIN 16.4
#define mpu6050_mahonysampleFreq 61.0f // sample frequency in Hz
#define mpu6050_mahonytwoKpDef (2.0f * 0.5f) // 2 * proportional gain
#define mpu6050_mahonytwoKiDef (2.0f * 0.1f) // 2 * integral gain
/*
* read bytes from chip register
*/
int8_t mpu6050_readBytes(uint8_t regAddr, uint8_t length, uint8_t *data)
{
uint8_t i = 0;
int8_t count = 0;
wdogtrig();
if(length > 0)
{
//request register
TWIM_Start(0x68,TWIM_WRITE);
TWIM_Write(regAddr);
delay_us(10);
//read data
TWIM_Start(0x68,TWIM_READ);
for(i=0; i<length; i++)
{
count++;
if(i==length-1)
data[i] = TWIM_ReadNack();
else
data[i] = TWIM_ReadAck();
}
TWIM_Stop();
}
return count;
}
/*
* read 1 byte from chip register
*/
int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data)
{
return mpu6050_readBytes(regAddr, 1, data);
}
/*
* write bytes to chip register
*/
void mpu6050_writeBytes(uint8_t regAddr, uint8_t length, uint8_t* data)
{
uint8_t i;
if(length > 0)
{
//write data
TWIM_Start(0x68,TWIM_WRITE);
TWIM_Write(regAddr); //reg
for (i = 0; i < length; i++)
{
TWIM_Write((uint8_t) data[i]);
}
TWIM_Stop();
}
}
/*
* write 1 byte to chip register
*/
void mpu6050_writeByte(uint8_t regAddr, uint8_t data)
{
mpu6050_writeBytes(regAddr, 1, &data);
}
/*
* read bits from chip register
*/
int8_t mpu6050_readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data)
{
// 01101001 read byte
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 010 masked
// -> 010 shifted
int8_t count = 0;
if(length > 0)
{
uint8_t b;
if ((count = mpu6050_readByte(regAddr, &b)) != 0)
{
uint8_t mask = ((1 << length) – 1) << (bitStart – length + 1);
b &= mask;
b >>= (bitStart – length + 1);
*data = b;
}
}
return count;
}
/*
* read 1 bit from chip register
*/
int8_t mpu6050_readBit(uint8_t regAddr, uint8_t bitNum, uint8_t *data)
{
uint8_t b;
uint8_t count = mpu6050_readByte(regAddr, &b);
*data = b & (1 << bitNum);
return count;
}
/*
* write bit/bits to chip register
*/
void mpu6050_writeBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data)
{
// 010 value to write
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 00011100 mask byte
// 10101111 original value (sample)
// 10100011 original & ~mask
// 10101011 masked | value
//wdogtrig();
if(length > 0)
{
uint8_t b = 0;
if (mpu6050_readByte(regAddr, &b) != 0) //get current data
{
uint8_t mask = ((1 << length) – 1) << (bitStart – length + 1);
data <<= (bitStart – length + 1); // shift data into correct position
data &= mask; // zero all non-important bits in data
b &= ~(mask); // zero all important bits in existing byte
b |= data; // combine data with existing byte
mpu6050_writeByte(regAddr, b);
}
}
}
void mpu6050_writeBit(uint8_t regAddr, uint8_t bitNum, uint8_t data)
{
uint8_t b;
mpu6050_readByte(regAddr, &b);
b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
mpu6050_writeByte(regAddr, b);
}
/*
* set sleep disabled
*/
void mpu6050_setSleepDisabled()
{
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0);
}
/*
* set sleep enabled
*/
void mpu6050_setSleepEnabled()
{
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1);
}
/*
* initialize the accel and gyro
*/
void mpu6050_init()
{
//allow mpu6050 chip clocks to start up
delay_ms(100);
//set sleep disabled
mpu6050_setSleepDisabled();
//wake up delay needed sleep disabled
delay_ms(10);
//set clock source
// it is highly recommended that the device be configured to use one of the gyroscopes (or an external clock source)
// as the clock reference for improved stability
mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO);
//set DLPF bandwidth to 42Hz
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); //OK
//set sampe rate
mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); //1khz / (1 + 4) = 200Hz ///OK
//set gyro range
mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS);
//set accel range
mpu6050_writeBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS);
mpu6050_writeByte(MPU9150_INT_ENABLE, 0b00000001); ////////////////////////enable data ready interrupt
} [11]
//////////Sfârșitul secvenței de cod preluate de pe site-ul https://github.com/Pansenti/MPU9150Lib//////////
Fișierul mpu6050registers.h [11]
/*
MPU6050 lib 0x02
copyright (c) Davide Gironi, 2012
Released under GPLv3.
Please refer to LICENSE file for licensing information.
*/
#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
#define MPU6050_RA_XA_OFFS_L_TC 0x07
#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
#define MPU6050_RA_YA_OFFS_L_TC 0x09
#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
#define MPU6050_RA_ZA_OFFS_L_TC 0x0B
#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
#define MPU6050_RA_XG_OFFS_USRL 0x14
#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
#define MPU6050_RA_YG_OFFS_USRL 0x16
#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
#define MPU6050_RA_ZG_OFFS_USRL 0x18
#define MPU6050_RA_SMPLRT_DIV 0x19
#define MPU6050_RA_CONFIG 0x1A
#define MPU6050_RA_GYRO_CONFIG 0x1B
#define MPU6050_RA_ACCEL_CONFIG 0x1C
#define MPU6050_RA_FF_THR 0x1D
#define MPU6050_RA_FF_DUR 0x1E
#define MPU6050_RA_MOT_THR 0x1F
#define MPU6050_RA_MOT_DUR 0x20
#define MPU6050_RA_ZRMOT_THR 0x21
#define MPU6050_RA_ZRMOT_DUR 0x22
#define MPU6050_RA_FIFO_EN 0x23
#define MPU6050_RA_I2C_MST_CTRL 0x24
#define MPU6050_RA_I2C_SLV0_ADDR 0x25
#define MPU6050_RA_I2C_SLV0_REG 0x26
#define MPU6050_RA_I2C_SLV0_CTRL 0x27
#define MPU6050_RA_I2C_SLV1_ADDR 0x28
#define MPU6050_RA_I2C_SLV1_REG 0x29
#define MPU6050_RA_I2C_SLV1_CTRL 0x2A
#define MPU6050_RA_I2C_SLV2_ADDR 0x2B
#define MPU6050_RA_I2C_SLV2_REG 0x2C
#define MPU6050_RA_I2C_SLV2_CTRL 0x2D
#define MPU6050_RA_I2C_SLV3_ADDR 0x2E
#define MPU6050_RA_I2C_SLV3_REG 0x2F
#define MPU6050_RA_I2C_SLV3_CTRL 0x30
#define MPU6050_RA_I2C_SLV4_ADDR 0x31
#define MPU6050_RA_I2C_SLV4_REG 0x32
#define MPU6050_RA_I2C_SLV4_DO 0x33
#define MPU6050_RA_I2C_SLV4_CTRL 0x34
#define MPU6050_RA_I2C_SLV4_DI 0x35
#define MPU6050_RA_I2C_MST_STATUS 0x36
#define MPU6050_RA_INT_PIN_CFG 0x37
#define MPU6050_RA_INT_ENABLE 0x38
#define MPU6050_RA_DMP_INT_STATUS 0x39
#define MPU6050_RA_INT_STATUS 0x3A
#define MPU6050_RA_ACCEL_XOUT_H 0x3B
#define MPU6050_RA_ACCEL_XOUT_L 0x3C
#define MPU6050_RA_ACCEL_YOUT_H 0x3D
#define MPU6050_RA_ACCEL_YOUT_L 0x3E
#define MPU6050_RA_ACCEL_ZOUT_H 0x3F
#define MPU6050_RA_ACCEL_ZOUT_L 0x40
#define MPU6050_RA_TEMP_OUT_H 0x41
#define MPU6050_RA_TEMP_OUT_L 0x42
#define MPU6050_RA_GYRO_XOUT_H 0x43
#define MPU6050_RA_GYRO_XOUT_L 0x44
#define MPU6050_RA_GYRO_YOUT_H 0x45
#define MPU6050_RA_GYRO_YOUT_L 0x46
#define MPU6050_RA_GYRO_ZOUT_H 0x47
#define MPU6050_RA_GYRO_ZOUT_L 0x48
#define MPU6050_RA_EXT_SENS_DATA_00 0x49
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
#define MPU6050_RA_EXT_SENS_DATA_07 0x50
#define MPU6050_RA_EXT_SENS_DATA_08 0x51
#define MPU6050_RA_EXT_SENS_DATA_09 0x52
#define MPU6050_RA_EXT_SENS_DATA_10 0x53
#define MPU6050_RA_EXT_SENS_DATA_11 0x54
#define MPU6050_RA_EXT_SENS_DATA_12 0x55
#define MPU6050_RA_EXT_SENS_DATA_13 0x56
#define MPU6050_RA_EXT_SENS_DATA_14 0x57
#define MPU6050_RA_EXT_SENS_DATA_15 0x58
#define MPU6050_RA_EXT_SENS_DATA_16 0x59
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
#define MPU6050_RA_EXT_SENS_DATA_23 0x60
#define MPU6050_RA_MOT_DETECT_STATUS 0x61
#define MPU6050_RA_I2C_SLV0_DO 0x63
#define MPU6050_RA_I2C_SLV1_DO 0x64
#define MPU6050_RA_I2C_SLV2_DO 0x65
#define MPU6050_RA_I2C_SLV3_DO 0x66
#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67
#define MPU6050_RA_SIGNAL_PATH_RESET 0x68
#define MPU6050_RA_MOT_DETECT_CTRL 0x69
#define MPU6050_RA_USER_CTRL 0x6A
#define MPU6050_RA_PWR_MGMT_1 0x6B
#define MPU6050_RA_PWR_MGMT_2 0x6C
#define MPU6050_RA_BANK_SEL 0x6D
#define MPU6050_RA_MEM_START_ADDR 0x6E
#define MPU6050_RA_MEM_R_W 0x6F
#define MPU6050_RA_DMP_CFG_1 0x70
#define MPU6050_RA_DMP_CFG_2 0x71
#define MPU6050_RA_FIFO_COUNTH 0x72
#define MPU6050_RA_FIFO_COUNTL 0x73
#define MPU6050_RA_FIFO_R_W 0x74
#define MPU6050_RA_WHO_AM_I 0x75
#define MPU6050_TC_PWR_MODE_BIT 7
#define MPU6050_TC_OFFSET_BIT 6
#define MPU6050_TC_OFFSET_LENGTH 6
#define MPU6050_TC_OTP_BNK_VLD_BIT 0
#define MPU6050_VDDIO_LEVEL_VLOGIC 0
#define MPU6050_VDDIO_LEVEL_VDD 1
#define MPU6050_CFG_EXT_SYNC_SET_BIT 5
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3
#define MPU6050_CFG_DLPF_CFG_BIT 2
#define MPU6050_CFG_DLPF_CFG_LENGTH 3
#define MPU6050_EXT_SYNC_DISABLED 0x0
#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1
#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2
#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7
#define MPU6050_DLPF_BW_256 0x00
#define MPU6050_DLPF_BW_188 0x01
#define MPU6050_DLPF_BW_98 0x02
#define MPU6050_DLPF_BW_42 0x03
#define MPU6050_DLPF_BW_20 0x04
#define MPU6050_DLPF_BW_10 0x05
#define MPU6050_DLPF_BW_5 0x06
#define MPU6050_GCONFIG_FS_SEL_BIT 4
#define MPU6050_GCONFIG_FS_SEL_LENGTH 2
#define MPU6050_GYRO_FS_250 0x00
#define MPU6050_GYRO_FS_500 0x01
#define MPU6050_GYRO_FS_1000 0x02
#define MPU6050_GYRO_FS_2000 0x03
#define MPU6050_ACONFIG_XA_ST_BIT 7
#define MPU6050_ACONFIG_YA_ST_BIT 6
#define MPU6050_ACONFIG_ZA_ST_BIT 5
#define MPU6050_ACONFIG_AFS_SEL_BIT 4
#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2
#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2
#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3
#define MPU6050_ACCEL_FS_2 0x00
#define MPU6050_ACCEL_FS_4 0x01
#define MPU6050_ACCEL_FS_8 0x02
#define MPU6050_ACCEL_FS_16 0x03
#define MPU6050_DHPF_RESET 0x00
#define MPU6050_DHPF_5 0x01
#define MPU6050_DHPF_2P5 0x02
#define MPU6050_DHPF_1P25 0x03
#define MPU6050_DHPF_0P63 0x04
#define MPU6050_DHPF_HOLD 0x07
#define MPU6050_TEMP_FIFO_EN_BIT 7
#define MPU6050_XG_FIFO_EN_BIT 6
#define MPU6050_YG_FIFO_EN_BIT 5
#define MPU6050_ZG_FIFO_EN_BIT 4
#define MPU6050_ACCEL_FIFO_EN_BIT 3
#define MPU6050_SLV2_FIFO_EN_BIT 2
#define MPU6050_SLV1_FIFO_EN_BIT 1
#define MPU6050_SLV0_FIFO_EN_BIT 0
#define MPU6050_MULT_MST_EN_BIT 7
#define MPU6050_WAIT_FOR_ES_BIT 6
#define MPU6050_SLV_3_FIFO_EN_BIT 5
#define MPU6050_I2C_MST_P_NSR_BIT 4
#define MPU6050_I2C_MST_CLK_BIT 3
#define MPU6050_I2C_MST_CLK_LENGTH 4
#define MPU6050_CLOCK_DIV_348 0x0
#define MPU6050_CLOCK_DIV_333 0x1
#define MPU6050_CLOCK_DIV_320 0x2
#define MPU6050_CLOCK_DIV_308 0x3
#define MPU6050_CLOCK_DIV_296 0x4
#define MPU6050_CLOCK_DIV_286 0x5
#define MPU6050_CLOCK_DIV_276 0x6
#define MPU6050_CLOCK_DIV_267 0x7
#define MPU6050_CLOCK_DIV_258 0x8
#define MPU6050_CLOCK_DIV_500 0x9
#define MPU6050_CLOCK_DIV_471 0xA
#define MPU6050_CLOCK_DIV_444 0xB
#define MPU6050_CLOCK_DIV_421 0xC
#define MPU6050_CLOCK_DIV_400 0xD
#define MPU6050_CLOCK_DIV_381 0xE
#define MPU6050_CLOCK_DIV_364 0xF
#define MPU6050_I2C_SLV_RW_BIT 7
#define MPU6050_I2C_SLV_ADDR_BIT 6
#define MPU6050_I2C_SLV_ADDR_LENGTH 7
#define MPU6050_I2C_SLV_EN_BIT 7
#define MPU6050_I2C_SLV_BYTE_SW_BIT 6
#define MPU6050_I2C_SLV_REG_DIS_BIT 5
#define MPU6050_I2C_SLV_GRP_BIT 4
#define MPU6050_I2C_SLV_LEN_BIT 3
#define MPU6050_I2C_SLV_LEN_LENGTH 4
#define MPU6050_I2C_SLV4_RW_BIT 7
#define MPU6050_I2C_SLV4_ADDR_BIT 6
#define MPU6050_I2C_SLV4_ADDR_LENGTH 7
#define MPU6050_I2C_SLV4_EN_BIT 7
#define MPU6050_I2C_SLV4_INT_EN_BIT 6
#define MPU6050_I2C_SLV4_REG_DIS_BIT 5
#define MPU6050_I2C_SLV4_MST_DLY_BIT 4
#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5
#define MPU6050_MST_PASS_THROUGH_BIT 7
#define MPU6050_MST_I2C_SLV4_DONE_BIT 6
#define MPU6050_MST_I2C_LOST_ARB_BIT 5
#define MPU6050_MST_I2C_SLV4_NACK_BIT 4
#define MPU6050_MST_I2C_SLV3_NACK_BIT 3
#define MPU6050_MST_I2C_SLV2_NACK_BIT 2
#define MPU6050_MST_I2C_SLV1_NACK_BIT 1
#define MPU6050_MST_I2C_SLV0_NACK_BIT 0
#define MPU6050_INTCFG_INT_LEVEL_BIT 7
#define MPU6050_INTCFG_INT_OPEN_BIT 6
#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5
#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4
#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3
#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2
#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1
#define MPU6050_INTCFG_CLKOUT_EN_BIT 0
#define MPU6050_INTMODE_ACTIVEHIGH 0x00
#define MPU6050_INTMODE_ACTIVELOW 0x01
#define MPU6050_INTDRV_PUSHPULL 0x00
#define MPU6050_INTDRV_OPENDRAIN 0x01
#define MPU6050_INTLATCH_50USPULSE 0x00
#define MPU6050_INTLATCH_WAITCLEAR 0x01
#define MPU6050_INTCLEAR_STATUSREAD 0x00
#define MPU6050_INTCLEAR_ANYREAD 0x01
#define MPU6050_INTERRUPT_FF_BIT 7
#define MPU6050_INTERRUPT_MOT_BIT 6
#define MPU6050_INTERRUPT_ZMOT_BIT 5
#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4
#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3
#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2
#define MPU6050_INTERRUPT_DMP_INT_BIT 1
#define MPU6050_INTERRUPT_DATA_RDY_BIT 0
// TODO: figure out what these actually do
// UMPL source code is not very obivous
#define MPU6050_DMPINT_5_BIT 5
#define MPU6050_DMPINT_4_BIT 4
#define MPU6050_DMPINT_3_BIT 3
#define MPU6050_DMPINT_2_BIT 2
#define MPU6050_DMPINT_1_BIT 1
#define MPU6050_DMPINT_0_BIT 0
#define MPU6050_MOTION_MOT_XNEG_BIT 7
#define MPU6050_MOTION_MOT_XPOS_BIT 6
#define MPU6050_MOTION_MOT_YNEG_BIT 5
#define MPU6050_MOTION_MOT_YPOS_BIT 4
#define MPU6050_MOTION_MOT_ZNEG_BIT 3
#define MPU6050_MOTION_MOT_ZPOS_BIT 2
#define MPU6050_MOTION_MOT_ZRMOT_BIT 0
#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7
#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4
#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3
#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2
#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1
#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0
#define MPU6050_PATHRESET_GYRO_RESET_BIT 2
#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1
#define MPU6050_PATHRESET_TEMP_RESET_BIT 0
#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5
#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2
#define MPU6050_DETECT_FF_COUNT_BIT 3
#define MPU6050_DETECT_FF_COUNT_LENGTH 2
#define MPU6050_DETECT_MOT_COUNT_BIT 1
#define MPU6050_DETECT_MOT_COUNT_LENGTH 2
#define MPU6050_DETECT_DECREMENT_RESET 0x0
#define MPU6050_DETECT_DECREMENT_1 0x1
#define MPU6050_DETECT_DECREMENT_2 0x2
#define MPU6050_DETECT_DECREMENT_4 0x3
#define MPU6050_USERCTRL_DMP_EN_BIT 7
#define MPU6050_USERCTRL_FIFO_EN_BIT 6
#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5
#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4
#define MPU6050_USERCTRL_DMP_RESET_BIT 3
#define MPU6050_USERCTRL_FIFO_RESET_BIT 2
#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1
#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0
#define MPU6050_PWR1_DEVICE_RESET_BIT 7
#define MPU6050_PWR1_SLEEP_BIT 6
#define MPU6050_PWR1_CYCLE_BIT 5
#define MPU6050_PWR1_TEMP_DIS_BIT 3
#define MPU6050_PWR1_CLKSEL_BIT 2
#define MPU6050_PWR1_CLKSEL_LENGTH 3
#define MPU6050_CLOCK_INTERNAL 0x00
#define MPU6050_CLOCK_PLL_XGYRO 0x01
#define MPU6050_CLOCK_PLL_YGYRO 0x02
#define MPU6050_CLOCK_PLL_ZGYRO 0x03
#define MPU6050_CLOCK_PLL_EXT32K 0x04
#define MPU6050_CLOCK_PLL_EXT19M 0x05
#define MPU6050_CLOCK_KEEP_RESET 0x07
#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7
#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2
#define MPU6050_PWR2_STBY_XA_BIT 5
#define MPU6050_PWR2_STBY_YA_BIT 4
#define MPU6050_PWR2_STBY_ZA_BIT 3
#define MPU6050_PWR2_STBY_XG_BIT 2
#define MPU6050_PWR2_STBY_YG_BIT 1
#define MPU6050_PWR2_STBY_ZG_BIT 0
#define MPU6050_WAKE_FREQ_1P25 0x0
#define MPU6050_WAKE_FREQ_2P5 0x1
#define MPU6050_WAKE_FREQ_5 0x2
#define MPU6050_WAKE_FREQ_10 0x3
#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6
#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5
#define MPU6050_BANKSEL_MEM_SEL_BIT 4
#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5
#define MPU6050_WHO_AM_I_BIT 6
#define MPU6050_WHO_AM_I_LENGTH 6
#define MPU6050_DMP_MEMORY_BANKS 8
#define MPU6050_DMP_MEMORY_BANK_SIZE 256
#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16
Fișierul REG.h
#define MPU9150_SELF_TEST_X 0x0D // R/W
#define MPU9150_SELF_TEST_Y 0x0E // R/W
#define MPU9150_SELF_TEST_X 0x0F // R/W
#define MPU9150_SELF_TEST_A 0x10 // R/W
#define MPU9150_SMPLRT_DIV 0x19 // R/W
#define MPU9150_CONFIG 0x1A // R/W
#define MPU9150_GYRO_CONFIG 0x1B // R/W
#define MPU9150_ACCEL_CONFIG 0x1C // R/W
#define MPU9150_FF_THR 0x1D // R/W
#define MPU9150_FF_DUR 0x1E // R/W
#define MPU9150_MOT_THR 0x1F // R/W
#define MPU9150_MOT_DUR 0x20 // R/W
#define MPU9150_ZRMOT_THR 0x21 // R/W
#define MPU9150_ZRMOT_DUR 0x22 // R/W
#define MPU9150_FIFO_EN 0x23 // R/W
#define MPU9150_I2C_MST_CTRL 0x24 // R/W
#define MPU9150_I2C_SLV0_ADDR 0x25 // R/W
#define MPU9150_I2C_SLV0_REG 0x26 // R/W
#define MPU9150_I2C_SLV0_CTRL 0x27 // R/W
#define MPU9150_I2C_SLV1_ADDR 0x28 // R/W
#define MPU9150_I2C_SLV1_REG 0x29 // R/W
#define MPU9150_I2C_SLV1_CTRL 0x2A // R/W
#define MPU9150_I2C_SLV2_ADDR 0x2B // R/W
#define MPU9150_I2C_SLV2_REG 0x2C // R/W
#define MPU9150_I2C_SLV2_CTRL 0x2D // R/W
#define MPU9150_I2C_SLV3_ADDR 0x2E // R/W
#define MPU9150_I2C_SLV3_REG 0x2F // R/W
#define MPU9150_I2C_SLV3_CTRL 0x30 // R/W
#define MPU9150_I2C_SLV4_ADDR 0x31 // R/W
#define MPU9150_I2C_SLV4_REG 0x32 // R/W
#define MPU9150_I2C_SLV4_DO 0x33 // R/W
#define MPU9150_I2C_SLV4_CTRL 0x34 // R/W
#define MPU9150_I2C_SLV4_DI 0x35 // R
#define MPU9150_I2C_MST_STATUS 0x36 // R
#define MPU9150_INT_PIN_CFG 0x37 // R/W
#define MPU9150_INT_ENABLE 0x38 // R/W
#define MPU9150_INT_STATUS 0x3A // R
#define MPU9150_ACCEL_XOUT_H 0x3B // R
#define MPU9150_ACCEL_XOUT_L 0x3C // R
#define MPU9150_ACCEL_YOUT_H 0x3D // R
#define MPU9150_ACCEL_YOUT_L 0x3E // R
#define MPU9150_ACCEL_ZOUT_H 0x3F // R
#define MPU9150_ACCEL_ZOUT_L 0x40 // R
#define MPU9150_TEMP_OUT_H 0x41 // R
#define MPU9150_TEMP_OUT_L 0x42 // R
#define MPU9150_GYRO_XOUT_H 0x43 // R
#define MPU9150_GYRO_XOUT_L 0x44 // R
#define MPU9150_GYRO_YOUT_H 0x45 // R
#define MPU9150_GYRO_YOUT_L 0x46 // R
#define MPU9150_GYRO_ZOUT_H 0x47 // R
#define MPU9150_GYRO_ZOUT_L 0x48 // R
#define MPU9150_EXT_SENS_DATA_00 0x49 // R
#define MPU9150_EXT_SENS_DATA_01 0x4A // R
#define MPU9150_EXT_SENS_DATA_02 0x4B // R
#define MPU9150_EXT_SENS_DATA_03 0x4C // R
#define MPU9150_EXT_SENS_DATA_04 0x4D // R
#define MPU9150_EXT_SENS_DATA_05 0x4E // R
#define MPU9150_EXT_SENS_DATA_06 0x4F // R
#define MPU9150_EXT_SENS_DATA_07 0x50 // R
#define MPU9150_EXT_SENS_DATA_08 0x51 // R
#define MPU9150_EXT_SENS_DATA_09 0x52 // R
#define MPU9150_EXT_SENS_DATA_10 0x53 // R
#define MPU9150_EXT_SENS_DATA_11 0x54 // R
#define MPU9150_EXT_SENS_DATA_12 0x55 // R
#define MPU9150_EXT_SENS_DATA_13 0x56 // R
#define MPU9150_EXT_SENS_DATA_14 0x57 // R
#define MPU9150_EXT_SENS_DATA_15 0x58 // R
#define MPU9150_EXT_SENS_DATA_16 0x59 // R
#define MPU9150_EXT_SENS_DATA_17 0x5A // R
#define MPU9150_EXT_SENS_DATA_18 0x5B // R
#define MPU9150_EXT_SENS_DATA_19 0x5C // R
#define MPU9150_EXT_SENS_DATA_20 0x5D // R
#define MPU9150_EXT_SENS_DATA_21 0x5E // R
#define MPU9150_EXT_SENS_DATA_22 0x5F // R
#define MPU9150_EXT_SENS_DATA_23 0x60 // R
#define MPU9150_MOT_DETECT_STATUS 0x61 // R
#define MPU9150_I2C_SLV0_DO 0x63 // R/W
#define MPU9150_I2C_SLV1_DO 0x64 // R/W
#define MPU9150_I2C_SLV2_DO 0x65 // R/W
#define MPU9150_I2C_SLV3_DO 0x66 // R/W
#define MPU9150_I2C_MST_DELAY_CTRL 0x67 // R/W
#define MPU9150_SIGNAL_PATH_RESET 0x68 // R/W
#define MPU9150_MOT_DETECT_CTRL 0x69 // R/W
#define MPU9150_USER_CTRL 0x6A // R/W
#define MPU9150_PWR_MGMT_1 0x6B // R/W
#define MPU9150_PWR_MGMT_2 0x6C // R/W
#define MPU9150_FIFO_COUNTH 0x72 // R/W
#define MPU9150_FIFO_COUNTL 0x73 // R/W
#define MPU9150_FIFO_R_W 0x74 // R/W
#define MPU9150_WHO_AM_I 0x75 // R
//MPU9150 Compass
#define MPU9150_CMPS_XOUT_L 0x4A // R
#define MPU9150_CMPS_XOUT_H 0x4B // R
#define MPU9150_CMPS_YOUT_L 0x4C // R
#define MPU9150_CMPS_YOUT_H 0x4D // R
#define MPU9150_CMPS_ZOUT_L 0x4E // R
#define MPU9150_CMPS_ZOUT_H 0x4F // R
#define MPU9150_I2C_ADDRESS 0x68
[1]
#define wdogtrig() #asm("wdr")
#define ACCELEROMETER_SENSITIVITY 8192.0 //////+-2g
#define GYROSCOPE_SENSITIVITY 65.536 /////+-250 /s
#define M_PI 3.14159265359
#define dt 0.01
Codul Telecomenzii
Fișierul main.c
#include <mega16a.h>
#include <delay.h>
#include <stdio.h>
#include "Init.c"
#include "UART.c"
#include "FUNCT.c"
#define wdogtrig() #asm("wdr") // call often if Watchdog timer enabled
void main(void)
{
Init();
// Global enable interrupts
#asm("sei")
LED1 = 1;
while (1)
{
wdogtrig();
}
}
Fișierul Init.c
void Init()
{
// Declare your local variables here
// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;
// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x00;
// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;
// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD=0b00100000; // D.5 needs pull-up resistor
DDRD= 0b01000000; // D.6 is LED
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer1 Stopped
// Mode: Normal top=0xFFFF
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0x00;
//TCCR1B=0x0C;
TCCR1B=0x0D; ////prescaler de 7200
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
//OCR1AH=0xFF;
//OCR1AL=0x80;
OCR1AH=0x01; /// pentru prescaler de 7200 si interrupt la 20 hz
OCR1AL=0x68; ///
OCR1BH=0x00;
OCR1BL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;
// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// INT2: Off
MCUCR=0x00;
MCUCSR=0x00;
// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x10;
// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud Rate: 57600
UCSRA=0x00;
UCSRB=0xD8;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x07;
// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;
// ADC initialization
// ADC disabled
ADCSRA=0x00;
// SPI initialization
// SPI disabled
SPCR=0x00;
// TWI initialization
// TWI disabled
TWCR=0x00;
// Watchdog Timer initialization
// Watchdog Timer Prescaler: OSC/2048k
#pragma optsize-
WDTCR=0x1F;
WDTCR=0x0F;
#ifdef _OPTIMIZE_SIZE_
#pragma optsize+
#endif
}
Fișierul UART.c
#ifndef RXB8
#define RXB8 1
#endif
#ifndef TXB8
#define TXB8 0
#endif
#ifndef UPE
#define UPE 2
#endif
#ifndef DOR
#define DOR 3
#endif
#ifndef FE
#define FE 4
#endif
#ifndef UDRE
#define UDRE 5
#endif
#ifndef RXC
#define RXC 7
#endif
#define FRAMING_ERROR (1<<FE)
#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<DOR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)
// USART Receiver buffer
#define RX_BUFFER_SIZE 8
char rx_buffer[RX_BUFFER_SIZE];
#if RX_BUFFER_SIZE <= 256
unsigned char rx_wr_index,rx_rd_index,rx_counter;
#else
unsigned int rx_wr_index,rx_rd_index,rx_counter;
#endif
// This flag is set on USART Receiver buffer overflow
bit rx_buffer_overflow;
// USART Receiver interrupt service routine
interrupt [USART_RXC] void usart_rx_isr(void)
{
char status,data;
status=UCSRA;
data=UDR;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
{
rx_buffer[rx_wr_index++]=data;
#if RX_BUFFER_SIZE == 256
// special case for receiver buffer size=256
if (++rx_counter == 0) rx_buffer_overflow=1;
#else
if (rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0;
if (++rx_counter == RX_BUFFER_SIZE)
{
rx_counter=0;
rx_buffer_overflow=1;
}
#endif
}
}
#ifndef _DEBUG_TERMINAL_IO_
// Get a character from the USART Receiver buffer
#define _ALTERNATE_GETCHAR_
#pragma used+
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index++];
#if RX_BUFFER_SIZE != 256
if (rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#endif
#asm("cli")
–rx_counter;
#asm("sei")
return data;
}
#pragma used-
#endif
// USART Transmitter buffer
#define TX_BUFFER_SIZE 8
char tx_buffer[TX_BUFFER_SIZE];
#if TX_BUFFER_SIZE <= 256
unsigned char tx_wr_index,tx_rd_index,tx_counter;
#else
unsigned int tx_wr_index,tx_rd_index,tx_counter;
#endif
// USART Transmitter interrupt service routine
interrupt [USART_TXC] void usart_tx_isr(void)
{
if (tx_counter)
{
–tx_counter;
UDR=tx_buffer[tx_rd_index++];
#if TX_BUFFER_SIZE != 256
if (tx_rd_index == TX_BUFFER_SIZE) tx_rd_index=0;
#endif
}
}
#ifndef _DEBUG_TERMINAL_IO_
// Write a character to the USART Transmitter buffer
#define _ALTERNATE_PUTCHAR_
#pragma used+
void putchar(char c)
{
while (tx_counter == TX_BUFFER_SIZE);
#asm("cli")
if (tx_counter || ((UCSRA & DATA_REGISTER_EMPTY)==0))
{
tx_buffer[tx_wr_index++]=c;
#if TX_BUFFER_SIZE != 256
if (tx_wr_index == TX_BUFFER_SIZE) tx_wr_index=0;
#endif
++tx_counter;
}
else
UDR=c;
#asm("sei")
}
#pragma used-
#endif
Fișierul FUNCT.c
#define LED1 PORTD.6 // PORTx is used for output
int i;
int temp;
int a,b,c,d,P,I,D;
int adc_start(int channel) // Function to perform an ADC conversion, Takes 0-8 as input
// to select which input to convert
{
ADCSRA=0X00; // Clear previous ADC results and status flags
ADMUX=0X60; // 0x40 for 10 bits
ADCSRA=0X87; // We have set the ADSC bit to start a conversion, and the
// ADPS bits are set so that the prescaler is 128
ADCSRA=0X80;
ADCH=0x00; // Clear the previous result
i=channel&0x07; // Decide which line to perform ADC conversion on
ADMUX=i|0x60; // Enter which line to perform in the ADC control register
ADCSRA|=1<<ADSC;
while(ADCSRA & (1<<ADSC)); // wait for conv. to complete
temp=ADCH; //unsigned int temp=ADC; for 10 bits
if(temp == 0)
temp = 1;
return temp;
}
interrupt [TIM1_COMPA] void timer1_compa_isr(void)
{
putchar(0);
temp = adc_start(7);
putchar(temp);
temp = adc_start(1);
putchar(temp);
temp = adc_start(2);
putchar(temp);
temp = adc_start(3);
putchar(temp);
temp = adc_start(4);
putchar(temp);
temp = adc_start(5);
putchar(temp);
temp = adc_start(6);
putchar(temp);
LED1 = ~LED1;
}
Anexa B. Scheme suplimentare ale circuitelor utilizate
Figura B.1. Schema circuitului XBee Explorer Regulated [15]
Figura B.2. Schema convertorului de nivele logice [16]
Anexa C. Layout-ul și pozele circuitului dronei
Layout Top View
Layout Bottom View
Figura C.1. Layout-ul circuitului dronei: a) fața superioară b) fața inferioară
văzut de sus
văzut de jos
Figura C.2. Pozele circuitului dronei: a) văzut de sus b) văzut de jos
Anexa D. Schema, lista de componente și pozele pentru circuitul de test
Figura D.1. Schema circuitului de test
Tabelul D.1. Lista de componente pentru circuitul de test
Anexa E. Poze ale telecomenzii și ale quad-copter-ului
Figura E.1. Poza telecomenzii
Figura E.2. Poza quad-copter-ului
Anexa F. Diagrama Gantt
Tabelul F.1. Diagrama Gantt
Bibliografie
[1] http://www.bbc.com/news/technology-25180906
[2] Oscar. Build A Quadcopter From Scratch – Hardware Overview. http://blog.oscarliang.net/build-a-quadcopter-beginners-tutorial-1/, accesat la data de 17.06.2014.
[3] http://copter.ardupilot.com/wiki/px4fmu-only-wiring/, accesat la data de 17.06.2014.
[4] S.P. Bhattacharyya. Modern PID control. Texas A&M University. A Short Course at University of California, Berkeley. http://msc.berkeley.edu/PID/modernPID1-overview.pdf, accesat la data de 19.06.2014.
[5] http://en.wikipedia.org/wiki/PID_controller, accesat la data de 18.06.2014.
[6] http://www.sierra.ro/Quadrocopter-DJI-F450-KIT-Flame-Wheel-(cu-motor–ESC–elice)-p5793p.html, accesat la data de 17.06.2014.
[7] http://ham.elcom.pub.ro/proiect2/files/cpu.pdf, accesat la data de 17.06.2014.
[8] Kenneth J. Jensen. Generalized Nonlinear Complementary Attitude Filter. http://arxiv.org/pdf/1110.0274.pdf, accesat la data de 19.02.2014
[9] http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/IMU/PS-MPU-9150A.pdf, accesat la data de 23.02.2014.
[10] http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/, accesat la data de 07.03.2014.
[11] https://github.com/Pansenti/MPU9150Lib, accesat la data de 19.02.2014.
[12] Pieter Jan. Reading a IMU Without Kalman: The Complementary Filter. http://www.pieter-jan.com/node/11, accesat la data de 24.02.2014.
[13] http://physudo-e.blogspot.ro/2013/09/deutsche-version-using-atmega-as-i2c.html, accesat la data de 12.02.2014.
[14] http://playground.arduino.cc/Main/MPU-9150, accesat la data de 19.02.2014.
[15] http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Wireless/Zigbee/XBee-Regulated-v14.pdf, accesat la data de 22.06.2014.
[16] http://dlnmh9ip6v2uc.cloudfront.net/datasheets/BreakoutBoards/Logic_Level_Bidirectional.pdf, accesat la data de 22.06.2014.
Anexa A. Codul sursă
Codul Quad-copter-ului
Fișierul main.c
#include <mega32.h>
#include <io.h>
#include <stdio.h>
#include <math.h>
#include <delay.h>
#include <string.h>
#include <stdlib.h>
#include "INIT.c"
#include "UART.c"
#include "TWI.c"
#include "FUNCT.c"
#include "REG.h"#define dt 1/20
/*Variabile pentru algoritmul PID*/
float Input, ROLLO, ROLLSET;
float errSum, lastErr;
float kp, ki, kd;
float timechange = 1/20;
float Input2, PITCHO, PITCHSET;
float errSum2, lastErr2;
float kp2, ki2, kd2;
char inter = 0;
interrupt [EXT_INT0] void ext_int0_isr(void)
{
inter = 1;
}
volatile uint8_t buffer[14];
//////////Începutul secvenței de cod preluate de pe site-ul http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction///////////
void Compute_ROLL()
{
float dErr, error;
wdogtrig();
/*Compute all the working error variables*/
error = ROLLSET – Input;
errSum += error;
dErr = error – lastErr;
/*Compute PID Output*/
ROLLO = kp * error + ki * errSum + kd * dErr;
/*Remember some variables for next time*/
lastErr = error;
}
void Compute_PITCH()
{
float dErr, error;
wdogtrig();
/*Compute all the working error variables*/
error = PITCHSET – Input2;
errSum2 += error ;
dErr = error – lastErr2;
/*Compute PID Output*/
PITCHO = kp2 * error + ki2 * errSum2 + kd2 * dErr;
/*Remember some variables for next time*/
lastErr2 = error;
}[10]
//////////Sfârșitul secvenței de cod preluate de pe site-ul http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction///////////
void main(void)
{
long int time;
int ax, ay, az, gx, gy, gz;
float roll, pitch;
unsigned char a,b,c,d,P,I,D,temp;
int forceMagnitudeApprox, rollAcc, pitchAcc;
Init();
TWIM_Init(100000);
delay_us(10);
mpu6050_init();
delay_ms(50);
#asm("sei")
LED1 = 1;
while(1)
{
wdogtrig();
if(rx_counter) // dacă un caracter este prezent pe portul serial
{
temp = getchar();
if(temp == 0)
{
wdogtrig();
a = getchar();
b = getchar();
c = getchar();
d = getchar();
P = getchar();
I = getchar();
D = getchar();
PITCHSET = (float)b*0.25 – 31.75;
ROLLSET = (float)c*0.25 – 31.75;
kp = (float)((I-7)*1.3)/247;
kd = (float)(D-7)*100/247;
kp2 = kp;
kd2 = kd;
if(ROLLSET > 15.0)
LED1 = ~LED1;
if(inter == 1)
{
mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, (uint8_t *)buffer);
ax = (((int16_t)buffer[0]) << 8) | buffer[1];
ay = (((int16_t)buffer[2]) << 8) | buffer[3];
az = (((int16_t)buffer[4]) << 8) | buffer[5];
gx = (((int16_t)buffer[8]) << 8) | buffer[9];
gy = (((int16_t)buffer[10]) << 8) | buffer[11];
gz = (((int16_t)buffer[12]) << 8) | buffer[13];”[9]
„pitch += ((float)gx / MPU6050_GGAIN) *dt;
roll -= ((float)gy / MPU6050_GGAIN) *dt;
forceMagnitudeApprox = abs(ax + ay + az);
if(forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
{
pitchAcc = atan2((float)ay, (float)az)*180/M_PI;
pitch = pitch*0.98 + pitchAcc*0.02;
rollAcc = atan2((float)ax, (float)az)*180/M_PI;
roll = roll*0.98 + rollAcc*0.02;
} [12]
Input = roll;
Compute_ROLL();
Input2 = pitch;
Compute_PITCH();
inter = 0;
}
M1 = a – (unsigned char)(PITCHO/2) + (unsigned char)(ROLLO/2);
M2 = a + (unsigned char)(PITCHO/2) + (unsigned char)(ROLLO/2);
M3 = a – (unsigned char)(PITCHO/2) – (unsigned char)(ROLLO/2);
M4 = a + (unsigned char)(PITCHO/2) – (unsigned char)(ROLLO/2);
}
}
}
}
Fișierul INIT.c
void Init()
{
// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;
// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x08;
// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0b00000000;
// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD=0x00;
DDRD= 0b11110000; // D.6 is LED
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF
// OC0 output: Disconnected
TCCR0=0x6B;
TCNT0=0x00;
OCR0=0x09;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer1 Stopped
// Mode: Normal top=0xFFFF
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=(1<<COM1A1) | (0<<COM1A0) | (1<<COM1B1) | (0<<COM1B0) | (0<<WGM11) | (1<<WGM10);
TCCR1B=(0<<ICNC1) | (0<<ICES1) | (0<<WGM13) | (1<<WGM12) | (0<<CS12) | (1<<CS11) | (1<<CS10);
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x09;
OCR1AL=0x00;
OCR1BH=0x09;
OCR1BL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x6C;
TCNT2=0x00;
OCR2=0x09;
// External Interrupt(s) initialization
// INT0: On
// INT0 Mode: Rising Edge
// INT1: Off
// INT2: Off
GICR|=0x40;
MCUCR=0x03;
MCUCSR=0x00;
GIFR=0x40;
// Timer(s)/Counter(s) Interrupt(s) initialization
//TIMSK=0x10;
TIMSK=0x00;
// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud Rate: 57600
UCSRA=0x00;
UCSRB=0xD8;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x07;
// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;
// ADC initialization
// ADC disabled
ADCSRA=0x00;
// SPI initialization
// SPI disabled
SPCR=0x00;
// TWI initialization
// TWI disabled
TWCR=0x00;
// Watchdog Timer initialization
// Watchdog Timer Prescaler: OSC/2048k
WDTCR=0x0F;
}
Fișierul UART.c
#ifndef RXB8
#define RXB8 1
#endif
#ifndef TXB8
#define TXB8 0
#endif
#ifndef UPE
#define UPE 2
#endif
#ifndef DOR
#define DOR 3
#endif
#ifndef FE
#define FE 4
#endif
#ifndef UDRE
#define UDRE 5
#endif
#ifndef RXC
#define RXC 7
#endif
#define FRAMING_ERROR (1<<FE)
#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<DOR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)
// USART Receiver buffer
#define RX_BUFFER_SIZE 8
char rx_buffer[RX_BUFFER_SIZE];
#if RX_BUFFER_SIZE <= 256
unsigned char rx_wr_index,rx_rd_index,rx_counter;
#else
unsigned int rx_wr_index,rx_rd_index,rx_counter;
#endif
// This flag is set on USART Receiver buffer overflow
bit rx_buffer_overflow;
// USART Receiver interrupt service routine
interrupt [USART_RXC] void usart_rx_isr(void)
{
char status,data;
status=UCSRA;
data=UDR;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
{
rx_buffer[rx_wr_index++]=data;
#if RX_BUFFER_SIZE == 256
// special case for receiver buffer size=256
if (++rx_counter == 0) rx_buffer_overflow=1;
#else
if (rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0;
if (++rx_counter == RX_BUFFER_SIZE)
{
rx_counter=0;
rx_buffer_overflow=1;
}
#endif
}
}
#ifndef _DEBUG_TERMINAL_IO_
// Get a character from the USART Receiver buffer
#define _ALTERNATE_GETCHAR_
#pragma used+
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index++];
#if RX_BUFFER_SIZE != 256
if (rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#endif
#asm("cli")
–rx_counter;
#asm("sei")
return data;
}
#pragma used-
#endif
// USART Transmitter buffer
#define TX_BUFFER_SIZE 8
char tx_buffer[TX_BUFFER_SIZE];
#if TX_BUFFER_SIZE <= 256
unsigned char tx_wr_index,tx_rd_index,tx_counter;
#else
unsigned int tx_wr_index,tx_rd_index,tx_counter;
#endif
// USART Transmitter interrupt service routine
interrupt [USART_TXC] void usart_tx_isr(void)
{
if (tx_counter)
{
–tx_counter;
UDR=tx_buffer[tx_rd_index++];
#if TX_BUFFER_SIZE != 256
if (tx_rd_index == TX_BUFFER_SIZE) tx_rd_index=0;
#endif
}
}
#ifndef _DEBUG_TERMINAL_IO_
// Write a character to the USART Transmitter buffer
#define _ALTERNATE_PUTCHAR_
#pragma used+
void putchar(char c)
{
while (tx_counter == TX_BUFFER_SIZE);
#asm("cli")
if (tx_counter || ((UCSRA & DATA_REGISTER_EMPTY)==0))
{
tx_buffer[tx_wr_index++]=c;
#if TX_BUFFER_SIZE != 256
if (tx_wr_index == TX_BUFFER_SIZE) tx_wr_index=0;
#endif
++tx_counter;
}
else
UDR=c;
#asm("sei")
}
#pragma used-
#endif
Fișierul TWI.c [13]
#include <stdint.h>
#define F_CPU 7372800
#define TRUE 1
#define FALSE 0
/****************************************************************************
TWI State codes
****************************************************************************/
// General TWI Master staus codes
#define TWI_START 0x08 // START has been transmitted
#define TWI_REP_START 0x10 // Repeated START has been transmitted
#define TWI_ARB_LOST 0x38 // Arbitration lost
// TWI Master Transmitter staus codes
#define TWI_MTX_ADR_ACK 0x18 // SLA+W has been tramsmitted and ACK received
#define TWI_MTX_ADR_NACK 0x20 // SLA+W has been tramsmitted and NACK received
#define TWI_MTX_DATA_ACK 0x28 // Data byte has been tramsmitted and ACK received
#define TWI_MTX_DATA_NACK 0x30 // Data byte has been tramsmitted and NACK received
// TWI Master Receiver staus codes
#define TWI_MRX_ADR_ACK 0x40 // SLA+R has been tramsmitted and ACK received
#define TWI_MRX_ADR_NACK 0x48 // SLA+R has been tramsmitted and NACK received
#define TWI_MRX_DATA_ACK 0x50 // Data byte has been received and ACK tramsmitted
#define TWI_MRX_DATA_NACK 0x58 // Data byte has been received and NACK tramsmitted
// TWI Slave Transmitter staus codes
#define TWI_STX_ADR_ACK 0xA8 // Own SLA+R has been received; ACK has been returned
#define TWI_STX_ADR_ACK_M_ARB_LOST 0xB0 // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned
#define TWI_STX_DATA_ACK 0xB8 // Data byte in TWDR has been transmitted; ACK has been received
#define TWI_STX_DATA_NACK 0xC0 // Data byte in TWDR has been transmitted; NOT ACK has been received
#define TWI_STX_DATA_ACK_LAST_BYTE 0xC8 // Last data byte in TWDR has been transmitted (TWEA = “0”); ACK has been received
// TWI Slave Receiver staus codes
#define TWI_SRX_ADR_ACK 0x60 // Own SLA+W has been received ACK has been returned
#define TWI_SRX_ADR_ACK_M_ARB_LOST 0x68 // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned
#define TWI_SRX_GEN_ACK 0x70 // General call address has been received; ACK has been returned
#define TWI_SRX_GEN_ACK_M_ARB_LOST 0x78 // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned
#define TWI_SRX_ADR_DATA_ACK 0x80 // Previously addressed with own SLA+W; data has been received; ACK has been returned
#define TWI_SRX_ADR_DATA_NACK 0x88 // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned
#define TWI_SRX_GEN_DATA_ACK 0x90 // Previously addressed with general call; data has been received; ACK has been returned
#define TWI_SRX_GEN_DATA_NACK 0x98 // Previously addressed with general call; data has been received; NOT ACK has been returned
#define TWI_SRX_STOP_RESTART 0xA0 // A STOP condition or repeated START condition has been received while still addressed as Slave
// TWI Miscellaneous status codes
#define TWI_NO_STATE 0xF8 // No relevant state information available; TWINT = “0”
#define TWI_BUS_ERROR 0x00 // Bus error due to an illegal START or STOP condition
#define TWIM_READ 1
#define TWIM_WRITE 0
/*******************************************************
Public Function: TWIM_Init
Purpose: Initialise the TWI Master Interface
Input Parameter:
– uint16_t TWI_Bitrate (Hz)
Return Value: uint8_t
– FALSE: Bitrate too high
– TRUE: Bitrate OK
*******************************************************/
uint8_t TWIM_Init (uint32_t TWI_Bitrate)
{
/*
** Set TWI bitrate
** If bitrate is too high, then error return
*/
TWBR = ((F_CPU/TWI_Bitrate)-16)/2;
if (TWBR < 11) return FALSE;
return TRUE;
}
/*******************************************************
Public Function: TWIM_Start
Purpose: Start the TWI Master Interface
Input Parameter:
– uint8_t Device address
– uint8_t Type of required Operation:
TWIM_READ: Read data from the slave
TWIM_WRITE: Write data to the slave
Return Value: uint8_t
– TRUE: OK, TWI Master accessible
– FALSE: Error in starting TWI Master
*******************************************************/
uint8_t TWIM_Start (uint8_t Address, uint8_t TWIM_Type)//1 = read, 0 = write
{
uint8_t twst;
/*
** Send START condition
*/
TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
/*
** Wait until transmission completed
*/
while (!(TWCR & (1<<TWINT)));
/*
** Check value of TWI Status Register. Mask prescaler bits.
*/
twst = TWSR & 0xF8;
if ((twst != TWI_START) && (twst != TWI_REP_START)) return FALSE;
/*
** Send device address
*/
TWDR = (Address<<1) + TWIM_Type;
TWCR = (1<<TWINT)|(1<<TWEN);
/*
** Wait until transmission completed and ACK/NACK has been received
*/
while (!(TWCR & (1<<TWINT)));
/*
** Check value of TWI Status Register. Mask prescaler bits.
*/
twst = TWSR & 0xF8;
if ((twst != TWI_MTX_ADR_ACK) && (twst != TWI_MRX_ADR_ACK)) return FALSE;
return TRUE;
}
/*******************************************************
Public Function: TWIM_Stop
Purpose: Stop the TWI Master
Input Parameter: None
Return Value: None
*******************************************************/
void TWIM_Stop (void)
{
/*
** Send stop condition
*/
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);
/*
** Wait until stop condition is executed and bus released
*/
while (TWCR & (1<<TWINT));
}
/*******************************************************
Public Function: TWIM_Write
Purpose: Write a byte to the slave
Input Parameter:
– uint8_t Byte to be sent
Return Value: uint8_t
– TRUE: OK, Byte sent
– FALSE: Error in byte transmission
*******************************************************/
uint8_t TWIM_Write (uint8_t byte)
{
uint8_t twst;
/*
** Send data to the previously addressed device
*/
TWDR = byte;
TWCR = (1<<TWINT)|(1<<TWEN);
/*
** Wait until transmission completed
*/
while (!(TWCR & (1<<TWINT)));
/*
** Check value of TWI Status Register. Mask prescaler bits
*/
twst = TWSR & 0xF8;
if (twst != TWI_MTX_DATA_ACK) return 1;
return 0;
}
/*******************************************************
Public Function: TWIM_ReadAck
Purpose: Read a byte from the slave and request next byte
Input Parameter: None
Return Value: uint8_t
– uint8_t Read byte
*******************************************************/
uint8_t TWIM_ReadAck (void)
{
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWEA);
while (!(TWCR & (1<<TWINT)));
return TWDR;
}
/*******************************************************
Public Function: TWIM_ReadAck
Purpose: Read the last byte from the slave
Input Parameter: None
Return Value: uint8_t
– uint8_t Read byte
*******************************************************/
uint8_t TWIM_ReadNack (void)
{
TWCR = (1<<TWINT)|(1<<TWEN);
while(!(TWCR & (1<<TWINT)));
return TWDR;
}
Fișierul FUNCT.c
#include "mpu6050registers.h"
#include "REG.h"
#include <stdint.h>
#include <delay.h>
#include <io.h>
#include <stdlib.h>
#include <string.h>
#include <math.h> //include libm
#define M1 OCR0
#define M3 OCR1A
#define M4 OCR1B
#define M2 OCR2
#define LED1 PORTD.6 // PORTx is used for output
#define wdogtrig() #asm("wdr") // call often if Watchdog timer enabled
//////////Începutul secvenței de cod preluate de pe site-ul https://github.com/Pansenti/MPU9150Lib//////////
//definitions
#define MPU6050_ADDR 0x68 //device address – 0x68 pin low (GND), 0x69 pin high (VCC)
//definitions for raw data
//gyro and acc scale
#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
#define MPU6050_GYRO_LSB_250 131.0
#define MPU6050_GYRO_LSB_500 65.5
#define MPU6050_GYRO_LSB_1000 32.8
#define MPU6050_GYRO_LSB_2000 16.4
#if MPU6050_GYRO_FS == MPU6050_GYRO_FS_250
#define MPU6050_GGAIN MPU6050_GYRO_LSB_250
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_500
#define MPU6050_GGAIN MPU6050_GYRO_LSB_500
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_1000
#define MPU6050_GGAIN MPU6050_GYRO_LSB_1000
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_2000
#define MPU6050_GGAIN MPU6050_GYRO_LSB_2000
#endif
#define MPU6050_ACCEL_LSB_2 16384.0
#define MPU6050_ACCEL_LSB_4 8192.0
#define MPU6050_ACCEL_LSB_8 4096.0
#define MPU6050_ACCEL_LSB_16 2048.0
#if MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_2
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_2
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_4
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_4
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_8
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_8
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_16
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_16
#endif
#define MPU6050_CALIBRATEDACCGYRO 1 //set to 1 if is calibrated
#define MPU6050_AXOFFSET 0
#define MPU6050_AYOFFSET 0
#define MPU6050_AZOFFSET 0
#define MPU6050_AXGAIN 16384.0
#define MPU6050_AYGAIN 16384.0
#define MPU6050_AZGAIN 16384.0
#define MPU6050_GXOFFSET -42
#define MPU6050_GYOFFSET 9
#define MPU6050_GZOFFSET -29
//#define MPU6050_GXGAIN 16.4
#define MPU6050_GYGAIN 16.4
//#define MPU6050_GZGAIN 16.4
#define mpu6050_mahonysampleFreq 61.0f // sample frequency in Hz
#define mpu6050_mahonytwoKpDef (2.0f * 0.5f) // 2 * proportional gain
#define mpu6050_mahonytwoKiDef (2.0f * 0.1f) // 2 * integral gain
/*
* read bytes from chip register
*/
int8_t mpu6050_readBytes(uint8_t regAddr, uint8_t length, uint8_t *data)
{
uint8_t i = 0;
int8_t count = 0;
wdogtrig();
if(length > 0)
{
//request register
TWIM_Start(0x68,TWIM_WRITE);
TWIM_Write(regAddr);
delay_us(10);
//read data
TWIM_Start(0x68,TWIM_READ);
for(i=0; i<length; i++)
{
count++;
if(i==length-1)
data[i] = TWIM_ReadNack();
else
data[i] = TWIM_ReadAck();
}
TWIM_Stop();
}
return count;
}
/*
* read 1 byte from chip register
*/
int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data)
{
return mpu6050_readBytes(regAddr, 1, data);
}
/*
* write bytes to chip register
*/
void mpu6050_writeBytes(uint8_t regAddr, uint8_t length, uint8_t* data)
{
uint8_t i;
if(length > 0)
{
//write data
TWIM_Start(0x68,TWIM_WRITE);
TWIM_Write(regAddr); //reg
for (i = 0; i < length; i++)
{
TWIM_Write((uint8_t) data[i]);
}
TWIM_Stop();
}
}
/*
* write 1 byte to chip register
*/
void mpu6050_writeByte(uint8_t regAddr, uint8_t data)
{
mpu6050_writeBytes(regAddr, 1, &data);
}
/*
* read bits from chip register
*/
int8_t mpu6050_readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data)
{
// 01101001 read byte
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 010 masked
// -> 010 shifted
int8_t count = 0;
if(length > 0)
{
uint8_t b;
if ((count = mpu6050_readByte(regAddr, &b)) != 0)
{
uint8_t mask = ((1 << length) – 1) << (bitStart – length + 1);
b &= mask;
b >>= (bitStart – length + 1);
*data = b;
}
}
return count;
}
/*
* read 1 bit from chip register
*/
int8_t mpu6050_readBit(uint8_t regAddr, uint8_t bitNum, uint8_t *data)
{
uint8_t b;
uint8_t count = mpu6050_readByte(regAddr, &b);
*data = b & (1 << bitNum);
return count;
}
/*
* write bit/bits to chip register
*/
void mpu6050_writeBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data)
{
// 010 value to write
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 00011100 mask byte
// 10101111 original value (sample)
// 10100011 original & ~mask
// 10101011 masked | value
//wdogtrig();
if(length > 0)
{
uint8_t b = 0;
if (mpu6050_readByte(regAddr, &b) != 0) //get current data
{
uint8_t mask = ((1 << length) – 1) << (bitStart – length + 1);
data <<= (bitStart – length + 1); // shift data into correct position
data &= mask; // zero all non-important bits in data
b &= ~(mask); // zero all important bits in existing byte
b |= data; // combine data with existing byte
mpu6050_writeByte(regAddr, b);
}
}
}
void mpu6050_writeBit(uint8_t regAddr, uint8_t bitNum, uint8_t data)
{
uint8_t b;
mpu6050_readByte(regAddr, &b);
b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
mpu6050_writeByte(regAddr, b);
}
/*
* set sleep disabled
*/
void mpu6050_setSleepDisabled()
{
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0);
}
/*
* set sleep enabled
*/
void mpu6050_setSleepEnabled()
{
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1);
}
/*
* initialize the accel and gyro
*/
void mpu6050_init()
{
//allow mpu6050 chip clocks to start up
delay_ms(100);
//set sleep disabled
mpu6050_setSleepDisabled();
//wake up delay needed sleep disabled
delay_ms(10);
//set clock source
// it is highly recommended that the device be configured to use one of the gyroscopes (or an external clock source)
// as the clock reference for improved stability
mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO);
//set DLPF bandwidth to 42Hz
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); //OK
//set sampe rate
mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); //1khz / (1 + 4) = 200Hz ///OK
//set gyro range
mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS);
//set accel range
mpu6050_writeBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS);
mpu6050_writeByte(MPU9150_INT_ENABLE, 0b00000001); ////////////////////////enable data ready interrupt
} [11]
//////////Sfârșitul secvenței de cod preluate de pe site-ul https://github.com/Pansenti/MPU9150Lib//////////
Fișierul mpu6050registers.h [11]
/*
MPU6050 lib 0x02
copyright (c) Davide Gironi, 2012
Released under GPLv3.
Please refer to LICENSE file for licensing information.
*/
#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
#define MPU6050_RA_XA_OFFS_L_TC 0x07
#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
#define MPU6050_RA_YA_OFFS_L_TC 0x09
#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
#define MPU6050_RA_ZA_OFFS_L_TC 0x0B
#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
#define MPU6050_RA_XG_OFFS_USRL 0x14
#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
#define MPU6050_RA_YG_OFFS_USRL 0x16
#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
#define MPU6050_RA_ZG_OFFS_USRL 0x18
#define MPU6050_RA_SMPLRT_DIV 0x19
#define MPU6050_RA_CONFIG 0x1A
#define MPU6050_RA_GYRO_CONFIG 0x1B
#define MPU6050_RA_ACCEL_CONFIG 0x1C
#define MPU6050_RA_FF_THR 0x1D
#define MPU6050_RA_FF_DUR 0x1E
#define MPU6050_RA_MOT_THR 0x1F
#define MPU6050_RA_MOT_DUR 0x20
#define MPU6050_RA_ZRMOT_THR 0x21
#define MPU6050_RA_ZRMOT_DUR 0x22
#define MPU6050_RA_FIFO_EN 0x23
#define MPU6050_RA_I2C_MST_CTRL 0x24
#define MPU6050_RA_I2C_SLV0_ADDR 0x25
#define MPU6050_RA_I2C_SLV0_REG 0x26
#define MPU6050_RA_I2C_SLV0_CTRL 0x27
#define MPU6050_RA_I2C_SLV1_ADDR 0x28
#define MPU6050_RA_I2C_SLV1_REG 0x29
#define MPU6050_RA_I2C_SLV1_CTRL 0x2A
#define MPU6050_RA_I2C_SLV2_ADDR 0x2B
#define MPU6050_RA_I2C_SLV2_REG 0x2C
#define MPU6050_RA_I2C_SLV2_CTRL 0x2D
#define MPU6050_RA_I2C_SLV3_ADDR 0x2E
#define MPU6050_RA_I2C_SLV3_REG 0x2F
#define MPU6050_RA_I2C_SLV3_CTRL 0x30
#define MPU6050_RA_I2C_SLV4_ADDR 0x31
#define MPU6050_RA_I2C_SLV4_REG 0x32
#define MPU6050_RA_I2C_SLV4_DO 0x33
#define MPU6050_RA_I2C_SLV4_CTRL 0x34
#define MPU6050_RA_I2C_SLV4_DI 0x35
#define MPU6050_RA_I2C_MST_STATUS 0x36
#define MPU6050_RA_INT_PIN_CFG 0x37
#define MPU6050_RA_INT_ENABLE 0x38
#define MPU6050_RA_DMP_INT_STATUS 0x39
#define MPU6050_RA_INT_STATUS 0x3A
#define MPU6050_RA_ACCEL_XOUT_H 0x3B
#define MPU6050_RA_ACCEL_XOUT_L 0x3C
#define MPU6050_RA_ACCEL_YOUT_H 0x3D
#define MPU6050_RA_ACCEL_YOUT_L 0x3E
#define MPU6050_RA_ACCEL_ZOUT_H 0x3F
#define MPU6050_RA_ACCEL_ZOUT_L 0x40
#define MPU6050_RA_TEMP_OUT_H 0x41
#define MPU6050_RA_TEMP_OUT_L 0x42
#define MPU6050_RA_GYRO_XOUT_H 0x43
#define MPU6050_RA_GYRO_XOUT_L 0x44
#define MPU6050_RA_GYRO_YOUT_H 0x45
#define MPU6050_RA_GYRO_YOUT_L 0x46
#define MPU6050_RA_GYRO_ZOUT_H 0x47
#define MPU6050_RA_GYRO_ZOUT_L 0x48
#define MPU6050_RA_EXT_SENS_DATA_00 0x49
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
#define MPU6050_RA_EXT_SENS_DATA_07 0x50
#define MPU6050_RA_EXT_SENS_DATA_08 0x51
#define MPU6050_RA_EXT_SENS_DATA_09 0x52
#define MPU6050_RA_EXT_SENS_DATA_10 0x53
#define MPU6050_RA_EXT_SENS_DATA_11 0x54
#define MPU6050_RA_EXT_SENS_DATA_12 0x55
#define MPU6050_RA_EXT_SENS_DATA_13 0x56
#define MPU6050_RA_EXT_SENS_DATA_14 0x57
#define MPU6050_RA_EXT_SENS_DATA_15 0x58
#define MPU6050_RA_EXT_SENS_DATA_16 0x59
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
#define MPU6050_RA_EXT_SENS_DATA_23 0x60
#define MPU6050_RA_MOT_DETECT_STATUS 0x61
#define MPU6050_RA_I2C_SLV0_DO 0x63
#define MPU6050_RA_I2C_SLV1_DO 0x64
#define MPU6050_RA_I2C_SLV2_DO 0x65
#define MPU6050_RA_I2C_SLV3_DO 0x66
#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67
#define MPU6050_RA_SIGNAL_PATH_RESET 0x68
#define MPU6050_RA_MOT_DETECT_CTRL 0x69
#define MPU6050_RA_USER_CTRL 0x6A
#define MPU6050_RA_PWR_MGMT_1 0x6B
#define MPU6050_RA_PWR_MGMT_2 0x6C
#define MPU6050_RA_BANK_SEL 0x6D
#define MPU6050_RA_MEM_START_ADDR 0x6E
#define MPU6050_RA_MEM_R_W 0x6F
#define MPU6050_RA_DMP_CFG_1 0x70
#define MPU6050_RA_DMP_CFG_2 0x71
#define MPU6050_RA_FIFO_COUNTH 0x72
#define MPU6050_RA_FIFO_COUNTL 0x73
#define MPU6050_RA_FIFO_R_W 0x74
#define MPU6050_RA_WHO_AM_I 0x75
#define MPU6050_TC_PWR_MODE_BIT 7
#define MPU6050_TC_OFFSET_BIT 6
#define MPU6050_TC_OFFSET_LENGTH 6
#define MPU6050_TC_OTP_BNK_VLD_BIT 0
#define MPU6050_VDDIO_LEVEL_VLOGIC 0
#define MPU6050_VDDIO_LEVEL_VDD 1
#define MPU6050_CFG_EXT_SYNC_SET_BIT 5
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3
#define MPU6050_CFG_DLPF_CFG_BIT 2
#define MPU6050_CFG_DLPF_CFG_LENGTH 3
#define MPU6050_EXT_SYNC_DISABLED 0x0
#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1
#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2
#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7
#define MPU6050_DLPF_BW_256 0x00
#define MPU6050_DLPF_BW_188 0x01
#define MPU6050_DLPF_BW_98 0x02
#define MPU6050_DLPF_BW_42 0x03
#define MPU6050_DLPF_BW_20 0x04
#define MPU6050_DLPF_BW_10 0x05
#define MPU6050_DLPF_BW_5 0x06
#define MPU6050_GCONFIG_FS_SEL_BIT 4
#define MPU6050_GCONFIG_FS_SEL_LENGTH 2
#define MPU6050_GYRO_FS_250 0x00
#define MPU6050_GYRO_FS_500 0x01
#define MPU6050_GYRO_FS_1000 0x02
#define MPU6050_GYRO_FS_2000 0x03
#define MPU6050_ACONFIG_XA_ST_BIT 7
#define MPU6050_ACONFIG_YA_ST_BIT 6
#define MPU6050_ACONFIG_ZA_ST_BIT 5
#define MPU6050_ACONFIG_AFS_SEL_BIT 4
#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2
#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2
#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3
#define MPU6050_ACCEL_FS_2 0x00
#define MPU6050_ACCEL_FS_4 0x01
#define MPU6050_ACCEL_FS_8 0x02
#define MPU6050_ACCEL_FS_16 0x03
#define MPU6050_DHPF_RESET 0x00
#define MPU6050_DHPF_5 0x01
#define MPU6050_DHPF_2P5 0x02
#define MPU6050_DHPF_1P25 0x03
#define MPU6050_DHPF_0P63 0x04
#define MPU6050_DHPF_HOLD 0x07
#define MPU6050_TEMP_FIFO_EN_BIT 7
#define MPU6050_XG_FIFO_EN_BIT 6
#define MPU6050_YG_FIFO_EN_BIT 5
#define MPU6050_ZG_FIFO_EN_BIT 4
#define MPU6050_ACCEL_FIFO_EN_BIT 3
#define MPU6050_SLV2_FIFO_EN_BIT 2
#define MPU6050_SLV1_FIFO_EN_BIT 1
#define MPU6050_SLV0_FIFO_EN_BIT 0
#define MPU6050_MULT_MST_EN_BIT 7
#define MPU6050_WAIT_FOR_ES_BIT 6
#define MPU6050_SLV_3_FIFO_EN_BIT 5
#define MPU6050_I2C_MST_P_NSR_BIT 4
#define MPU6050_I2C_MST_CLK_BIT 3
#define MPU6050_I2C_MST_CLK_LENGTH 4
#define MPU6050_CLOCK_DIV_348 0x0
#define MPU6050_CLOCK_DIV_333 0x1
#define MPU6050_CLOCK_DIV_320 0x2
#define MPU6050_CLOCK_DIV_308 0x3
#define MPU6050_CLOCK_DIV_296 0x4
#define MPU6050_CLOCK_DIV_286 0x5
#define MPU6050_CLOCK_DIV_276 0x6
#define MPU6050_CLOCK_DIV_267 0x7
#define MPU6050_CLOCK_DIV_258 0x8
#define MPU6050_CLOCK_DIV_500 0x9
#define MPU6050_CLOCK_DIV_471 0xA
#define MPU6050_CLOCK_DIV_444 0xB
#define MPU6050_CLOCK_DIV_421 0xC
#define MPU6050_CLOCK_DIV_400 0xD
#define MPU6050_CLOCK_DIV_381 0xE
#define MPU6050_CLOCK_DIV_364 0xF
#define MPU6050_I2C_SLV_RW_BIT 7
#define MPU6050_I2C_SLV_ADDR_BIT 6
#define MPU6050_I2C_SLV_ADDR_LENGTH 7
#define MPU6050_I2C_SLV_EN_BIT 7
#define MPU6050_I2C_SLV_BYTE_SW_BIT 6
#define MPU6050_I2C_SLV_REG_DIS_BIT 5
#define MPU6050_I2C_SLV_GRP_BIT 4
#define MPU6050_I2C_SLV_LEN_BIT 3
#define MPU6050_I2C_SLV_LEN_LENGTH 4
#define MPU6050_I2C_SLV4_RW_BIT 7
#define MPU6050_I2C_SLV4_ADDR_BIT 6
#define MPU6050_I2C_SLV4_ADDR_LENGTH 7
#define MPU6050_I2C_SLV4_EN_BIT 7
#define MPU6050_I2C_SLV4_INT_EN_BIT 6
#define MPU6050_I2C_SLV4_REG_DIS_BIT 5
#define MPU6050_I2C_SLV4_MST_DLY_BIT 4
#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5
#define MPU6050_MST_PASS_THROUGH_BIT 7
#define MPU6050_MST_I2C_SLV4_DONE_BIT 6
#define MPU6050_MST_I2C_LOST_ARB_BIT 5
#define MPU6050_MST_I2C_SLV4_NACK_BIT 4
#define MPU6050_MST_I2C_SLV3_NACK_BIT 3
#define MPU6050_MST_I2C_SLV2_NACK_BIT 2
#define MPU6050_MST_I2C_SLV1_NACK_BIT 1
#define MPU6050_MST_I2C_SLV0_NACK_BIT 0
#define MPU6050_INTCFG_INT_LEVEL_BIT 7
#define MPU6050_INTCFG_INT_OPEN_BIT 6
#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5
#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4
#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3
#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2
#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1
#define MPU6050_INTCFG_CLKOUT_EN_BIT 0
#define MPU6050_INTMODE_ACTIVEHIGH 0x00
#define MPU6050_INTMODE_ACTIVELOW 0x01
#define MPU6050_INTDRV_PUSHPULL 0x00
#define MPU6050_INTDRV_OPENDRAIN 0x01
#define MPU6050_INTLATCH_50USPULSE 0x00
#define MPU6050_INTLATCH_WAITCLEAR 0x01
#define MPU6050_INTCLEAR_STATUSREAD 0x00
#define MPU6050_INTCLEAR_ANYREAD 0x01
#define MPU6050_INTERRUPT_FF_BIT 7
#define MPU6050_INTERRUPT_MOT_BIT 6
#define MPU6050_INTERRUPT_ZMOT_BIT 5
#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4
#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3
#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2
#define MPU6050_INTERRUPT_DMP_INT_BIT 1
#define MPU6050_INTERRUPT_DATA_RDY_BIT 0
// TODO: figure out what these actually do
// UMPL source code is not very obivous
#define MPU6050_DMPINT_5_BIT 5
#define MPU6050_DMPINT_4_BIT 4
#define MPU6050_DMPINT_3_BIT 3
#define MPU6050_DMPINT_2_BIT 2
#define MPU6050_DMPINT_1_BIT 1
#define MPU6050_DMPINT_0_BIT 0
#define MPU6050_MOTION_MOT_XNEG_BIT 7
#define MPU6050_MOTION_MOT_XPOS_BIT 6
#define MPU6050_MOTION_MOT_YNEG_BIT 5
#define MPU6050_MOTION_MOT_YPOS_BIT 4
#define MPU6050_MOTION_MOT_ZNEG_BIT 3
#define MPU6050_MOTION_MOT_ZPOS_BIT 2
#define MPU6050_MOTION_MOT_ZRMOT_BIT 0
#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7
#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4
#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3
#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2
#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1
#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0
#define MPU6050_PATHRESET_GYRO_RESET_BIT 2
#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1
#define MPU6050_PATHRESET_TEMP_RESET_BIT 0
#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5
#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2
#define MPU6050_DETECT_FF_COUNT_BIT 3
#define MPU6050_DETECT_FF_COUNT_LENGTH 2
#define MPU6050_DETECT_MOT_COUNT_BIT 1
#define MPU6050_DETECT_MOT_COUNT_LENGTH 2
#define MPU6050_DETECT_DECREMENT_RESET 0x0
#define MPU6050_DETECT_DECREMENT_1 0x1
#define MPU6050_DETECT_DECREMENT_2 0x2
#define MPU6050_DETECT_DECREMENT_4 0x3
#define MPU6050_USERCTRL_DMP_EN_BIT 7
#define MPU6050_USERCTRL_FIFO_EN_BIT 6
#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5
#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4
#define MPU6050_USERCTRL_DMP_RESET_BIT 3
#define MPU6050_USERCTRL_FIFO_RESET_BIT 2
#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1
#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0
#define MPU6050_PWR1_DEVICE_RESET_BIT 7
#define MPU6050_PWR1_SLEEP_BIT 6
#define MPU6050_PWR1_CYCLE_BIT 5
#define MPU6050_PWR1_TEMP_DIS_BIT 3
#define MPU6050_PWR1_CLKSEL_BIT 2
#define MPU6050_PWR1_CLKSEL_LENGTH 3
#define MPU6050_CLOCK_INTERNAL 0x00
#define MPU6050_CLOCK_PLL_XGYRO 0x01
#define MPU6050_CLOCK_PLL_YGYRO 0x02
#define MPU6050_CLOCK_PLL_ZGYRO 0x03
#define MPU6050_CLOCK_PLL_EXT32K 0x04
#define MPU6050_CLOCK_PLL_EXT19M 0x05
#define MPU6050_CLOCK_KEEP_RESET 0x07
#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7
#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2
#define MPU6050_PWR2_STBY_XA_BIT 5
#define MPU6050_PWR2_STBY_YA_BIT 4
#define MPU6050_PWR2_STBY_ZA_BIT 3
#define MPU6050_PWR2_STBY_XG_BIT 2
#define MPU6050_PWR2_STBY_YG_BIT 1
#define MPU6050_PWR2_STBY_ZG_BIT 0
#define MPU6050_WAKE_FREQ_1P25 0x0
#define MPU6050_WAKE_FREQ_2P5 0x1
#define MPU6050_WAKE_FREQ_5 0x2
#define MPU6050_WAKE_FREQ_10 0x3
#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6
#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5
#define MPU6050_BANKSEL_MEM_SEL_BIT 4
#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5
#define MPU6050_WHO_AM_I_BIT 6
#define MPU6050_WHO_AM_I_LENGTH 6
#define MPU6050_DMP_MEMORY_BANKS 8
#define MPU6050_DMP_MEMORY_BANK_SIZE 256
#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16
Fișierul REG.h
#define MPU9150_SELF_TEST_X 0x0D // R/W
#define MPU9150_SELF_TEST_Y 0x0E // R/W
#define MPU9150_SELF_TEST_X 0x0F // R/W
#define MPU9150_SELF_TEST_A 0x10 // R/W
#define MPU9150_SMPLRT_DIV 0x19 // R/W
#define MPU9150_CONFIG 0x1A // R/W
#define MPU9150_GYRO_CONFIG 0x1B // R/W
#define MPU9150_ACCEL_CONFIG 0x1C // R/W
#define MPU9150_FF_THR 0x1D // R/W
#define MPU9150_FF_DUR 0x1E // R/W
#define MPU9150_MOT_THR 0x1F // R/W
#define MPU9150_MOT_DUR 0x20 // R/W
#define MPU9150_ZRMOT_THR 0x21 // R/W
#define MPU9150_ZRMOT_DUR 0x22 // R/W
#define MPU9150_FIFO_EN 0x23 // R/W
#define MPU9150_I2C_MST_CTRL 0x24 // R/W
#define MPU9150_I2C_SLV0_ADDR 0x25 // R/W
#define MPU9150_I2C_SLV0_REG 0x26 // R/W
#define MPU9150_I2C_SLV0_CTRL 0x27 // R/W
#define MPU9150_I2C_SLV1_ADDR 0x28 // R/W
#define MPU9150_I2C_SLV1_REG 0x29 // R/W
#define MPU9150_I2C_SLV1_CTRL 0x2A // R/W
#define MPU9150_I2C_SLV2_ADDR 0x2B // R/W
#define MPU9150_I2C_SLV2_REG 0x2C // R/W
#define MPU9150_I2C_SLV2_CTRL 0x2D // R/W
#define MPU9150_I2C_SLV3_ADDR 0x2E // R/W
#define MPU9150_I2C_SLV3_REG 0x2F // R/W
#define MPU9150_I2C_SLV3_CTRL 0x30 // R/W
#define MPU9150_I2C_SLV4_ADDR 0x31 // R/W
#define MPU9150_I2C_SLV4_REG 0x32 // R/W
#define MPU9150_I2C_SLV4_DO 0x33 // R/W
#define MPU9150_I2C_SLV4_CTRL 0x34 // R/W
#define MPU9150_I2C_SLV4_DI 0x35 // R
#define MPU9150_I2C_MST_STATUS 0x36 // R
#define MPU9150_INT_PIN_CFG 0x37 // R/W
#define MPU9150_INT_ENABLE 0x38 // R/W
#define MPU9150_INT_STATUS 0x3A // R
#define MPU9150_ACCEL_XOUT_H 0x3B // R
#define MPU9150_ACCEL_XOUT_L 0x3C // R
#define MPU9150_ACCEL_YOUT_H 0x3D // R
#define MPU9150_ACCEL_YOUT_L 0x3E // R
#define MPU9150_ACCEL_ZOUT_H 0x3F // R
#define MPU9150_ACCEL_ZOUT_L 0x40 // R
#define MPU9150_TEMP_OUT_H 0x41 // R
#define MPU9150_TEMP_OUT_L 0x42 // R
#define MPU9150_GYRO_XOUT_H 0x43 // R
#define MPU9150_GYRO_XOUT_L 0x44 // R
#define MPU9150_GYRO_YOUT_H 0x45 // R
#define MPU9150_GYRO_YOUT_L 0x46 // R
#define MPU9150_GYRO_ZOUT_H 0x47 // R
#define MPU9150_GYRO_ZOUT_L 0x48 // R
#define MPU9150_EXT_SENS_DATA_00 0x49 // R
#define MPU9150_EXT_SENS_DATA_01 0x4A // R
#define MPU9150_EXT_SENS_DATA_02 0x4B // R
#define MPU9150_EXT_SENS_DATA_03 0x4C // R
#define MPU9150_EXT_SENS_DATA_04 0x4D // R
#define MPU9150_EXT_SENS_DATA_05 0x4E // R
#define MPU9150_EXT_SENS_DATA_06 0x4F // R
#define MPU9150_EXT_SENS_DATA_07 0x50 // R
#define MPU9150_EXT_SENS_DATA_08 0x51 // R
#define MPU9150_EXT_SENS_DATA_09 0x52 // R
#define MPU9150_EXT_SENS_DATA_10 0x53 // R
#define MPU9150_EXT_SENS_DATA_11 0x54 // R
#define MPU9150_EXT_SENS_DATA_12 0x55 // R
#define MPU9150_EXT_SENS_DATA_13 0x56 // R
#define MPU9150_EXT_SENS_DATA_14 0x57 // R
#define MPU9150_EXT_SENS_DATA_15 0x58 // R
#define MPU9150_EXT_SENS_DATA_16 0x59 // R
#define MPU9150_EXT_SENS_DATA_17 0x5A // R
#define MPU9150_EXT_SENS_DATA_18 0x5B // R
#define MPU9150_EXT_SENS_DATA_19 0x5C // R
#define MPU9150_EXT_SENS_DATA_20 0x5D // R
#define MPU9150_EXT_SENS_DATA_21 0x5E // R
#define MPU9150_EXT_SENS_DATA_22 0x5F // R
#define MPU9150_EXT_SENS_DATA_23 0x60 // R
#define MPU9150_MOT_DETECT_STATUS 0x61 // R
#define MPU9150_I2C_SLV0_DO 0x63 // R/W
#define MPU9150_I2C_SLV1_DO 0x64 // R/W
#define MPU9150_I2C_SLV2_DO 0x65 // R/W
#define MPU9150_I2C_SLV3_DO 0x66 // R/W
#define MPU9150_I2C_MST_DELAY_CTRL 0x67 // R/W
#define MPU9150_SIGNAL_PATH_RESET 0x68 // R/W
#define MPU9150_MOT_DETECT_CTRL 0x69 // R/W
#define MPU9150_USER_CTRL 0x6A // R/W
#define MPU9150_PWR_MGMT_1 0x6B // R/W
#define MPU9150_PWR_MGMT_2 0x6C // R/W
#define MPU9150_FIFO_COUNTH 0x72 // R/W
#define MPU9150_FIFO_COUNTL 0x73 // R/W
#define MPU9150_FIFO_R_W 0x74 // R/W
#define MPU9150_WHO_AM_I 0x75 // R
//MPU9150 Compass
#define MPU9150_CMPS_XOUT_L 0x4A // R
#define MPU9150_CMPS_XOUT_H 0x4B // R
#define MPU9150_CMPS_YOUT_L 0x4C // R
#define MPU9150_CMPS_YOUT_H 0x4D // R
#define MPU9150_CMPS_ZOUT_L 0x4E // R
#define MPU9150_CMPS_ZOUT_H 0x4F // R
#define MPU9150_I2C_ADDRESS 0x68
[1]
#define wdogtrig() #asm("wdr")
#define ACCELEROMETER_SENSITIVITY 8192.0 //////+-2g
#define GYROSCOPE_SENSITIVITY 65.536 /////+-250 /s
#define M_PI 3.14159265359
#define dt 0.01
Codul Telecomenzii
Fișierul main.c
#include <mega16a.h>
#include <delay.h>
#include <stdio.h>
#include "Init.c"
#include "UART.c"
#include "FUNCT.c"
#define wdogtrig() #asm("wdr") // call often if Watchdog timer enabled
void main(void)
{
Init();
// Global enable interrupts
#asm("sei")
LED1 = 1;
while (1)
{
wdogtrig();
}
}
Fișierul Init.c
void Init()
{
// Declare your local variables here
// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;
// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x00;
// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;
// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD=0b00100000; // D.5 needs pull-up resistor
DDRD= 0b01000000; // D.6 is LED
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer1 Stopped
// Mode: Normal top=0xFFFF
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0x00;
//TCCR1B=0x0C;
TCCR1B=0x0D; ////prescaler de 7200
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
//OCR1AH=0xFF;
//OCR1AL=0x80;
OCR1AH=0x01; /// pentru prescaler de 7200 si interrupt la 20 hz
OCR1AL=0x68; ///
OCR1BH=0x00;
OCR1BL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;
// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// INT2: Off
MCUCR=0x00;
MCUCSR=0x00;
// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x10;
// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud Rate: 57600
UCSRA=0x00;
UCSRB=0xD8;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x07;
// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;
// ADC initialization
// ADC disabled
ADCSRA=0x00;
// SPI initialization
// SPI disabled
SPCR=0x00;
// TWI initialization
// TWI disabled
TWCR=0x00;
// Watchdog Timer initialization
// Watchdog Timer Prescaler: OSC/2048k
#pragma optsize-
WDTCR=0x1F;
WDTCR=0x0F;
#ifdef _OPTIMIZE_SIZE_
#pragma optsize+
#endif
}
Fișierul UART.c
#ifndef RXB8
#define RXB8 1
#endif
#ifndef TXB8
#define TXB8 0
#endif
#ifndef UPE
#define UPE 2
#endif
#ifndef DOR
#define DOR 3
#endif
#ifndef FE
#define FE 4
#endif
#ifndef UDRE
#define UDRE 5
#endif
#ifndef RXC
#define RXC 7
#endif
#define FRAMING_ERROR (1<<FE)
#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<DOR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)
// USART Receiver buffer
#define RX_BUFFER_SIZE 8
char rx_buffer[RX_BUFFER_SIZE];
#if RX_BUFFER_SIZE <= 256
unsigned char rx_wr_index,rx_rd_index,rx_counter;
#else
unsigned int rx_wr_index,rx_rd_index,rx_counter;
#endif
// This flag is set on USART Receiver buffer overflow
bit rx_buffer_overflow;
// USART Receiver interrupt service routine
interrupt [USART_RXC] void usart_rx_isr(void)
{
char status,data;
status=UCSRA;
data=UDR;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
{
rx_buffer[rx_wr_index++]=data;
#if RX_BUFFER_SIZE == 256
// special case for receiver buffer size=256
if (++rx_counter == 0) rx_buffer_overflow=1;
#else
if (rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0;
if (++rx_counter == RX_BUFFER_SIZE)
{
rx_counter=0;
rx_buffer_overflow=1;
}
#endif
}
}
#ifndef _DEBUG_TERMINAL_IO_
// Get a character from the USART Receiver buffer
#define _ALTERNATE_GETCHAR_
#pragma used+
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index++];
#if RX_BUFFER_SIZE != 256
if (rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#endif
#asm("cli")
–rx_counter;
#asm("sei")
return data;
}
#pragma used-
#endif
// USART Transmitter buffer
#define TX_BUFFER_SIZE 8
char tx_buffer[TX_BUFFER_SIZE];
#if TX_BUFFER_SIZE <= 256
unsigned char tx_wr_index,tx_rd_index,tx_counter;
#else
unsigned int tx_wr_index,tx_rd_index,tx_counter;
#endif
// USART Transmitter interrupt service routine
interrupt [USART_TXC] void usart_tx_isr(void)
{
if (tx_counter)
{
–tx_counter;
UDR=tx_buffer[tx_rd_index++];
#if TX_BUFFER_SIZE != 256
if (tx_rd_index == TX_BUFFER_SIZE) tx_rd_index=0;
#endif
}
}
#ifndef _DEBUG_TERMINAL_IO_
// Write a character to the USART Transmitter buffer
#define _ALTERNATE_PUTCHAR_
#pragma used+
void putchar(char c)
{
while (tx_counter == TX_BUFFER_SIZE);
#asm("cli")
if (tx_counter || ((UCSRA & DATA_REGISTER_EMPTY)==0))
{
tx_buffer[tx_wr_index++]=c;
#if TX_BUFFER_SIZE != 256
if (tx_wr_index == TX_BUFFER_SIZE) tx_wr_index=0;
#endif
++tx_counter;
}
else
UDR=c;
#asm("sei")
}
#pragma used-
#endif
Fișierul FUNCT.c
#define LED1 PORTD.6 // PORTx is used for output
int i;
int temp;
int a,b,c,d,P,I,D;
int adc_start(int channel) // Function to perform an ADC conversion, Takes 0-8 as input
// to select which input to convert
{
ADCSRA=0X00; // Clear previous ADC results and status flags
ADMUX=0X60; // 0x40 for 10 bits
ADCSRA=0X87; // We have set the ADSC bit to start a conversion, and the
// ADPS bits are set so that the prescaler is 128
ADCSRA=0X80;
ADCH=0x00; // Clear the previous result
i=channel&0x07; // Decide which line to perform ADC conversion on
ADMUX=i|0x60; // Enter which line to perform in the ADC control register
ADCSRA|=1<<ADSC;
while(ADCSRA & (1<<ADSC)); // wait for conv. to complete
temp=ADCH; //unsigned int temp=ADC; for 10 bits
if(temp == 0)
temp = 1;
return temp;
}
interrupt [TIM1_COMPA] void timer1_compa_isr(void)
{
putchar(0);
temp = adc_start(7);
putchar(temp);
temp = adc_start(1);
putchar(temp);
temp = adc_start(2);
putchar(temp);
temp = adc_start(3);
putchar(temp);
temp = adc_start(4);
putchar(temp);
temp = adc_start(5);
putchar(temp);
temp = adc_start(6);
putchar(temp);
LED1 = ~LED1;
}
Anexa B. Scheme suplimentare ale circuitelor utilizate
Figura B.1. Schema circuitului XBee Explorer Regulated [15]
Figura B.2. Schema convertorului de nivele logice [16]
Anexa C. Layout-ul și pozele circuitului dronei
Layout Top View
Layout Bottom View
Figura C.1. Layout-ul circuitului dronei: a) fața superioară b) fața inferioară
văzut de sus
văzut de jos
Figura C.2. Pozele circuitului dronei: a) văzut de sus b) văzut de jos
Anexa D. Schema, lista de componente și pozele pentru circuitul de test
Figura D.1. Schema circuitului de test
Tabelul D.1. Lista de componente pentru circuitul de test
Anexa E. Poze ale telecomenzii și ale quad-copter-ului
Figura E.1. Poza telecomenzii
Figura E.2. Poza quad-copter-ului
Anexa F. Diagrama Gantt
Tabelul F.1. Diagrama Gantt
Copyright Notice
© Licențiada.org respectă drepturile de proprietate intelectuală și așteaptă ca toți utilizatorii să facă același lucru. Dacă consideri că un conținut de pe site încalcă drepturile tale de autor, te rugăm să trimiți o notificare DMCA.
Acest articol: Robot Zburator de Tip Quad Copter (ID: 163405)
Dacă considerați că acest conținut vă încalcă drepturile de autor, vă rugăm să depuneți o cerere pe pagina noastră Copyright Takedown.