Lucrarea de față își propune să realizeze implemenarea unui sistem de monitorizare si control al unor parametri in industria auto. [306312]

INTRODUCERE:

Lucrarea de față își propune să realizeze implemenarea unui sistem de monitorizare si control al unor parametri in industria auto.

Principiul de funcționare al dispozitivuli constă în monitorizarea si control parametrilor unui autovehicul in timp real. Pentru a [anonimizat].

În capitolul 2 [anonimizat]. [anonimizat].

Capitolul 3 dedicat implimentării practice a lucrării de diplomă. [anonimizat], [anonimizat], cu ajutorul căreia se realizeaza conectiunea dintre senzori inertiali si interfata auto. Ca urmare a [anonimizat].

Concluziile trase în urma realizării proiectului asociat lucrării de diplomă sunt evidențiate în capitolul 4. Va fi prezentat felul în care se face corectia unor date primite de la senzori inertiali dar si avantajele folosiri senzorilor inertiali in industria auto.

2. NOȚIUNI TEORETICE:

2.1 Sisteme de monitorizare si control

Se urmărește prezentarea conceptului general al unui sistem de monitorizare si control si analizare unor parametri in timp real. De asemenea se prezinta pe scurt si modul in care se poate face corectia driftului acceleratie.

2.1.1 Generalități

Sistemele de monitorizare si control folosite in industia auto pot avea diferite configurații și funcții care depind de tipul autovehicolului pe care se face testarea. Astfel in functie de autovehicolul testat parametri care pot fi monitorizati sunt:

Debit de aer;

Pozita absoluta a pedalei de acceleratie;

Temperature apei din sistemul de raciere;

Turatia motorului;

Timpul de pornire al motorului;

Consumul de carburant pe termen scurt si lung;

Termperatura ambianta;

Nivelul de carburant.

2.1.2 Rolul și funcționalitatea unui sistem de monitorizare si control

Rolul unui sistem de monitorizare si control este semnalizare cât mai timpurie a apariției unui defect la autovehicolul testat. [anonimizat] (on board diagnostics) sau folosind un instrument de diagnoza auto.

[anonimizat], diferenta majora dintre cele 2 [anonimizat].

2.1.3 Importanța unui sistem de monitorizare si control in industria auto

Deoarece defectiunile care apar la un autovehicul pot fi prevenite detectare acestora este vitala in prevenirea unor posibile accidente. Reducerea pagubelor materiale și eliminarea pierderilor de vieți omenești sunt cele mai importante obiective ale unui sistem de monitorizare si control.

Asigurarea protecției in caza de accident a autovehiculelor si utilizatorizor , reprezintă un obiectiv fundamental în contextul cerințelor și exigențelor actuale.

Având în vedere condițiile specifice existente într-un obiectiv, prin introducerea unui sistem de monitorizare si control al unui autovehicul, se urmărește creșterea gradului de siguranță în exploatarea acestuia, reducându-se astfel posibilitatea de a se produce un incident.

Un astfel de sistem de monitorizare are următoarele funcții:

detectarea timpurie a posibilelor defectiuni care pot aparea la un autovehicul;

detectarea unor probleme de natura electrica;

transmiterea datelor privind consumul autovehiculului precum si numarul de kilometri parcurs de acesta;

semnalizarea automată la distanță a pompierilor, salvare, poliție (opțional)

2.1.4 Cauzele apariției defectiunilor

Oricâte măsuri de prevenire se iau, defectiunile se vor produce tot timpul, asta din cauză că prevenirea lor nu se poate face în totalitate. După cum se poate observa în continuare, cauzele defectiuniilor sunt diverse și nu pot fi prevăzute și prevenite în totalitate.

Diagnoza este depistarea unor defectiuni dereglari functionare anormala, ajungand la cauzele acestora prin analizarea manifestarilor externe ale acestor cauze. Este o succesiune in depistarea unei defectiuni si a cauzelor ei.

1. depistarea simptomului prin care se manifesta defectiunea;
2. intelegerea simptomului ca expresie a unei anumite defectiuni;
3. stabilirea naturii defectiunii;
4. precizarea cauzelor care au dus la aparitia defectiunii si descoperirea organelor defecte;
5. remedierea defectiunii sau luarea unor masuri care sa previna agravarea pana la reparare.

2.2 OBD (On Board Diagnostics)

2.2.1 OBD – Generalitati

OBD sau diagnoza la bord reprezită capacitatea unui automobil de a-și diagnosticadiverse componente care au impact asupra emisiilor poluante. Principalul scop al diagnozei OBD este de a aprinde martorul „Check Engine” în cazul în care s-au detectat probleme de funcționare la componentele care influențează direct sau indirect emisiile poluante. Modul în care trebuie să funcționeze un echipament de diagnosticare este stabilit prin standarde, americane SAE și internaționale ISO.

2.2.2 Evolutia OBD

OBD-1

Începând cu 1988 regulamentele emise de CARB(California Air Resource Board) cer ca toate automobilele noi vândute în statul California (USA) să aibă capabilități de diagnosticare la bord. Aceste cerințe, standarde sunt denumite ca fiind OBD-1 sau prima generație de diagnoză la bordul automobilelor. Succesul acestui regulament, standard nu a fost foarte mare deoarece interfața cu echipametul de diagnosticare nu era standadizată și fiecare constructor a plasat conectorul după bunul plac. De asemenea protocolul de comunicație utilizat nu era standardizat și deci imposibil de a creea un echipament de diagnosticare care să poată fi utilizat pe automobile diferite.

În 1994 CARB emite noile regulamente ce vor fi cunoscute ca OBD-2. Acestea sunt impuse tuturor automobilelor noi ce vor fi vândute în California începând cu anul 1996. Atât conectorul (interfața cu echipamentul de diagnosticare) cât și protocolul de comunicație sunt standardizate, ceea ce face posibilă dezvoltarea de echipament de diagnosticare universale.

EOBD

Versiunea europeană a standardului OBD-2 este denumită EOBD implementarea fiind obligatorie pentru toate automobilele noi produse începând cu 2001 pentru motoarele pe benzină și cu 2004 pentru motoarele diesel.

2.2.3 Prezentarea conectorului OBD II

Termenul OBD-2 implică cerințe standardizate atât pe partea de hardware (electronică, conector) cât și pe partea de software (protocol de comunicație, parametrii măsurați). Pe scurt putem spune că standardul OBD-2 se referă la:

Conector

protocol de comunicație

mod de funcționare (informații/parametrii înregistrați și puși la dispoziția utilizatorului automobilului)

Conectorul OBD-2

Standardul american SAE J1962, echivalent cu ISO 15031-3, prevede dimensiunile conectorului OBD-2 din vehiculului și pentru echipamentul de diagnosticare. De asemenea locația conectorului din vehicul este standardizată, astfel fiecare automobil ce respectă regulamentul/standardul OBD-2 permite accesul utilizatorului.

Figura 2.1: Conector OBD-2 vehicul [2]

Figura 2: Amplasarea conectorului OBD2 intr-un autovehicul[2]

Conectorul trebuie să fie situat în habitaclu, în zona volanului a tabloului de bord sau a consolei centrale. Accesul trebuie să se facă ușor, de pe scaunul conducătorului auto, locația preferată fiind între coloana de direcție și axa longitudinală a vehiculului.

Setul de regulamente SAE și ISO ce definesc OBD-2 prevede o interfață hardware (conector) standard cu 16 pini. Spre deosebire de conectorul OBD-1, care poate fi găsit uneori și în compartimentul motor, conectorul OBD-2 este necesar să fie la o distanță de 0.61 m de volan Numerotarea și definiția pinilor este descrisă după cum urmează:

Figura 2.2: Conector OBD-2 vehicul [2]

Pinii 1, 3, 8, 9, 11, 12, 13 nu sunt explicit definiți de către regulament și sunt la discreția constructorului de automobile. Utilizarea pinilor se face în funcție de protocolul utilizat. Astfel un automobil care utilizează protocolul CAN pentru OBD-2 va avea în conectorul din vehicul pinii 4 (-), 16 (+), 6 și 14. Pe de altă parte un echipament de diagnosticare (scantool) care nu depinde de un anume protocol de comunicație (se poate utiliza indiferent de protocolul automobilului) va avea un conector cu toți pinii, identificare protocolului făcându-se în mod automat.

2.2.4 Protocoale de comunicație utilizate pentru OBD-2

Interfața OBD-2 pentru majoritatea automobilelor poate utiliza unul din cinci protocoale de comunicație. Deducerea protocolului utilizat se poate face prin identificarea pinilor de pe conectorul OBD-2 al automobilului:

SAE J1850 PWM

Protocolul este utilizat în principal de către Ford Motor Company, viteza de transfer a datelor fiind de 41.6 kB/sec. Utilizează pinii 2 (+) și 5 (-) pentru transmiterea semnalelor.

SAE J1850 VPW

Este un protocol standard utilizat de către General Motors, viteza de transfer a datelor fiind între 10.4 și 41.6 kB/sec. De asemenea utilizează pinii 2 (+) și 5 (-) pentru transmiterea semnalelor.

ISO 9141-2

Protocol utilizat cu precădere de către producătorii de automobile europeni, asiatici și Chrysler. Viteza de transfer a datelor este de 10.4 kBaud. Pentru comunicare utilizează pinul 7 (K-line) și opțional pinul 15 (L-line).

ISO 14230 (KWP2000)

Protocol similar cu ISO 9141-2. Pentru comunicare utilizează pinul 7 (K-line) și opțional pinul 15 (L-line). Viteza de transfer a datelor este cuprinsă între 1.2 și 10.4 KBaud.

ISO 15765 (CAN)

Protocol CAN este produsul companiei Bosch și este larg utilizat în industria automobilelor. În funcție de viteza de transfer a datelor, pentru OBD-2, se poate utiliza CAN de 250 kBit/sec sau de 500 kBit/sec. Pentru transmiterea datelor se utilizează pinul 6 (CAN high) și 14 (CAN low). Începând cu 2008, toate vehiculele noi vândute în SUA sunt obligate să utilizeze protocolul CAN pentru OBD-2.

Utilizarea unui protocol sau altul este decisă în principal de norma de poluare pe care o respectă un automobil. Odată cu înăsprirea limitelor de emisii poluante, cerințele OBD-2 sau modificat în sensul creșterii numărului de parametrii măsurați și a testelor efectuate. Aceste reglementări au obligat constructorii auto să utilizeze protocoalele de comunicație mai performante cu viteză de transfer a datelor mai ridicată. Astfel, odată cu normele Euro 4, protocolul de comunicație pentru OBD-2 este CAN, iar pentru vehiculele Euro 3 protocolul poate fi ISO 9141-2 sau ISO 14230 pentru automobilele europene și SAE J1850 pentru cele americane. .[1]

2.2.5 Module de monitorizare al autovehicolelor

Modulele folosite pentru monitorizare parametrilor autoveculelor sunt module care citesc, interpreteaza si pot modifica parametrii unui autovehicol prin intermediul interfetei OBD II.

Tipuri de module:

module de tip “plug and read”;

module wireless.

Modulele de tip “plug and read” sunt module simple care au un conector de tip OBD II

si o interfata hardware prin care se poate realiza citirea codurilor de eroare al masinii direct fara a folosi software-uri aditionale.

Figura 2.3: Modul OBD II[3]

Modulele de tip wireless sunt module care au in component o interfata de tip OBD II (eg: ELM-327) care comunica wireless prin intermediul unui Bluetooth cu un software instalat pe un computer, pe un telefon mobil sau pe o tablet (eg: Torque).

Torque este un software creat special pentru dispozitivele mobile care permite utilizatorului citirea, interpretare parametrilor autovehicului. Aplicatia Torque permite utilizatorului sa creeze interfete personalizate pentru monitorizarea diferiti parametrii.

Figura 2.4: Modul wireless de tip ELM-327

Figura 2.5: Interfata create cu ajutorul aplicatiei “Torque”

2.3 Sisteme inertiale folosite in estimarea directiei si orientarii

2.3.1 Generalitati

Sistemele inertial folosite in estmarea directie si orientarii sunt sisteme care au in componenta lor senzori de pozitie cum ar fi accelerometer, giroscoape si magenetometre. Accelerometrele sunt senzori care masoara acceleratia proprie al unui obiect, magnetometrele sun senzori care masoara pozitia unui obiect fata de nordul magnetic, iar giroscoapele sunt senzori care masoara viteza unghiulara al unui obiect.

2.3.2 Accelerometrul

Accelemetrul este un dispozitiv care masoara acceleratia proprie a unui obiect, de exemplu un accelerometru lasat liber pe suprafata pamantului acesta v-a masura acceleratia gravitationala g=9.81 m/s2.

Conceptual un accelerometru se comporta ca ca un obiect de masa cunoscuta legat de un resort a carui constanta de elasticitate se cunoste, astfel cand aupra obiectului se exercita o acceleratie resortul se alungeste sau se comprima, masurand alungirea sau comprimarea acestuia se poate calcula acceleratia proprie a acelui obiect. La un obiect lasa in cadere libera nu se poate masura acceleratia proprie deoarece aceasta este 0.

Figura 2.7: Modelul fizic al unui accelerometru

In dispozitivele electronice care masoara acceleratia sunt folosite component piezoelectrice, piesorezistive si capacitive pentru a transforma miscarea mecanica in semnal electric. Accelerometre piezoelectrice se bazează pe elemente piezoceramice ( de exemplu, plumb zirconat de titanat ) sau cristale unice ( de exemplu cuarț, turmalină ). Accelerometrele piezorezistive sunt preferate în aplicații de înaltă stare de șoc. Accelerometre capacitive folosesc de obicei un un element din siliciu . Performanța lor este superior în domeniul de frecvență joasă și pot fi exploatate în servomecanisme pentru a atinge stabilitatea ridicată și liniaritate .

Accelerometre moderne sunt adesea sisteme de tip MEMS ( Micro Electrical-Mecanical System ) , și sunt într-adevăr cele mai simple dispozitive de tip MEMS posibil , constând din mai mult decât un obiect cu o masă cunoscuta ( de asemenea , cunoscut sub numele de masă seismică ). Atâta timp cât factorul – Q nu este prea scăzută , amortizare nu are ca rezultat o sensibilitate mai scăzută .

Sub influența accelerațiilor externe masa de proba deviază de la poziția neutră . Acest deformare este măsurată într-o manieră analogica sau digitala . Cel mai frecvent se masoara capacitate dintre un set de grinzi fixe și un set de grinzi atașate la masa probei se măsoară . Această metodă este simplă , fiabilă , și ieftina . [4]

Figura 2.8: Accelerometru MEMS

2.3.3 Magnetometrul

Magnetometrele sunt instrumente de evaluare utilizate pentru două scopuri generale: pentru a măsura magnetizarea unui material magnetic ca un feromagnet, sau pentru a măsura puterea și, în unele cazuri, direcția câmpului magnetic într-un punct în spațiu.

Magnetometrele sunt folosite pentru măsurarea câmpului magnetic al Pamantului și în studii geofizice pentru a detecta anomalile magnetice. Acestea sunt utilizate, de asemenea, de catre armata, pentru a detecta submarine. Prin urmare, unele țări, precum SUA, Canada și Australia clasifica magnetometre mai sensibili ca tehnologie militară, și si tin sub control distributia lor. Magnetometrele pot fi folosite ca detectoare de metal: ele pot detecta metalele magnetice ( feroase ) , dar pot detecta astfel de metale , la o adâncime mult mai mare decât detectoare de metale convenționale ; ele sunt capabile să detecteze obiecte mari , cum ar fi mașini , la zeci de metri , în timp ce un detector de metale conventional rareori poate detecta la un interval mai mare de 2 metrii.

În ultimii ani, magnetometrele au fost miniaturizat în măsura în care acestea pot fi încorporate în circuite integrate la costuri foarte mici și sunt utilizate ca si busole în dispozitive de consum , cum ar fi telefoanele mobile si tablete.

Câmpuri magnetice sunt vectori caracterizate atât puterea și direcția . Puterea unui câmp magnetic in Sistemul International se măsoară in Tesla , și în Gauss în sistemul cgs de unități , 10.000 Gauss sunt egale cu 1 Tesla . Puterea campului magnetic al pamantului se masoara in nanotesla (NT ) , numit , de asemenea, o gama. Câmpul magnetic al Pamantului poate varia de la 20.000 la 80.000 nT , în funcție de locație , fluctuațiile de câmp magnetic al Pamantului sunt de ordinul a 100 nT , și variațiile magnetice ce apar din cauza anomaliilor magnetice pot fi de cateva zeci de picotesla (PT) . Gaussmeterele și teslameterele sunt magnetometer aceastea măsoară în unități de Gauss sau Tesla. În anumite contexte , magnetometrul este termenul utilizat pentru un instrument care măsoară domenii de mai puțin de 1 millitesla ( mT ) și gaussmeter este utilizat pentru măsurarea cei mai mari de 1 mT.

Există două tipuri de bază magnetometre . Magnetometrele vectoriale măsoară componentele vectorului de un câmp magnetic . Magnetometrele scalare măsoara intensitatea campului magnetic . Magnetometrele folosite pentru a studia câmpul magnetic al Pamantului po exprima componentele vectoriale ale câmpului în termeni de declinație ( unghiul dintre componenta orizontală a vector câmp și nordul magnetic ) și înclinația ( unghiul dintre vectorul de teren și suprafața orizontală ).

Magnetometrele absolute măsoara valoarea absolută a vectorui de câmp magnetic , utilizând o calibrare internă sau constante fizice cunoscute ale senzorului magnetic . De asemenea magnetometrele relative sunt folosite pentru a măsura variațiile câmpului magnetic.

Performanța și capacitățile de magnetometrelor sunt descrise prin specificațiile tehnice ale acestora . Specificatiile majore includ:

rata de eșantionare este numărul de citiri date pe secundă. Rata de eșantionare este importantă în magnetometrele mobile; rata de eșantionare și viteza vehiculului determina distanta dintre măsurători.

lățime de bandă caracterizează cat de bine un magnetometru poate percepe schimbări rapide în câmp magnetic. Pentru magnetometre fără prelucrare a semnalului lățime de bandă este determinată de limita Nyquist stabilit de frecvența de eșantionare. Magnetometrele moderne pot efectua nivelare sau mediere peste probele secvențiale.

rezoluția este cea mai mică schimbare în câmp magnetic pe care magnetometru o poate citii. Un magnetometru ar trebui să aibe o rezoluție mai mică decâ cea mai mică schimbare pe care se dorește să se observe, pentru a evita erorile de cuantificare.

eroare absolută este diferența dintre citirile medie a unui magnetometru într-un câmp magnetic constant și câmp magnetic adevărat.

drift-ul este schimbarea eroarii absolute dea lungul timpului.

stabilitatea termică este dependența măsurării de temperatură. Acesta este dat ca un coeficient de temperatură în unități nT pe grad Celsius.

zgomotele sunt fluctuațiile aleatorii generate de senzorul de magnetometru.[5]

2.3.4 Giroscopul

Giroscopul este o roata de tors sau disc în care axa de rotație este liber să-și asume orice orientare. La rotirea, orientarea acestei axe este afectat prin înclinarea în conformitate cu conservarea momentului cinetic. Din acest motiv, giroscoapele sunt utile pentru măsurarea sau menținerea orientării.

Giroscoapele bazate pe alte principii de funcționare, de asemenea există, cum ar fi electronice, MEMS, giroscoape găsite în dispozitive electronice de larg consum giroscoape cu fibra optica, și giroscopul cuantic.

Aplicatiile giroscoapului includ sisteme de navigație inerțiale unde busolele magnetice nu ar funcționa (de exemplu telescopul Hubble) sau nu ar fi suficient de precise (la fel ca în rachete balistice intercontinentale), sau pentru stabilizarea aparatelor de zbor, cum ar fi elicoptere controlate prin telecomanda sau vehicule aeriene fără pilot , precum și bărci și nave comerciale de agrement. Datorită preciziei lor, giroscoapele sunt, de asemenea, utilizate în gyrotheodolites pentru a menține direcția în tunel minerit. giroscoape poate fi folosit pentru a construi girocompas, care completează sau înlocuiesc busole magnetice (în nave, aeronave și nave spațiale, vehicule, în general), pentru a ajuta în stabilitate (Hubble Space Telescope, biciclete, motociclete, și nave) sau pot fi utilizate ca parte a unui sistem de orientare inerțială.

Figura 2.8: Giroscopul clasic

Giroscoapele fabricate cu tehnologia MEMS au devenit disponibile pe scară largă. Acestea sunt create în mod similar cu alte circuite integrate și pot avea ieșiri analogice sau digitale. În multe cazuri, o singura capsula include senzori giroscopici pentru mai multe axe. Unele părți includ mai multe giroscoape și accelerometre (sau giroscoape axa multiple și accelerometre), pentru a obține la ieșire șase grade de libertate complete. Aceste unități sunt numite unități de măsurare inerțial, sau Imus. Panasonic, Robert Bosch GmbH, INVENSENSE, Seiko Epson, Sensonor, STMicroelectronics, Freescale Semiconductor, și Analog Devices sunt principalii producători de giroscoape.

Pe plan intern, giroscoape MEMS utilizează construite lithographically versiuni ale unuia sau mai multora dintre mecanismele (roți vibratoare, sau solide de rezonanta ale diferitelor modele).

Giroscoape MEMS sunt utilizate în industria auto în caz de răsturnare sisteme de prevenire și de airbag, stabilizator de imagine, și au multe alte aplicații.

Senzori de girație folositi in industria auto poate fi construiti pe principiul giroscoapelor vobratoare( VSG). Acestea sunt utilizate pentru a detecta eroare în girație, comparativ cu un răspuns prezis atunci când este conectat ca o intrare la sistemele electronice de control al stabilității, în legătură cu un senzor de pe volan. sisteme avansate ar putea oferi teoretic detectare rasturnarii bazat pe un al doilea VSG, dar este mai ieftin pentru a adăuga accelerometre longitudinale și verticale cu cea laterală existent în acest sens.[6]

Un giroscop cu 3 axe poate măsura rotația în jurul a trei axe: X, Ysi Z. Unele giroscoape sunt construite cu o singura axa sau cu doua, dar giroscoapelor triplu axa într-un singur cip din ce în ce mai mici este, mai puțin costisitoare și mai popular.

Giroscoape sunt adesea folosite pe obiecte care nu se rotesc foarte repede, la toate. Aeronave (sperăm) nu se rotească. În schimb, ei se rotesc cu câteva grade pe fiecare axă. Prin detectarea acestor schimbari mici giroscoape ajuta la stabilizarea zbor a aeronavei. De asemenea, rețineți că viteza de accelerare sau liniară a aeronavei nu afectează măsurarea giroscoapelor. Giroscoape măsura doar viteza unghiulară.[7]

Figura 7: Structura interna a unui giroscop de tip MEMS[8]

2.3.5 Conceptul de Fuziune senzoriala

Fuziunea senzoriala este combinarea datelor senzoriale sau date provenite din datele senzoriale din surse diferite, astfel încât informațiile rezultate au incertitudine de masurare mai mica decât atunci cand sursele ar fi fost folosite individual. Reducerea incertitudinii pe termen lung în acest caz poate însemna masuratori mai precise, mai complete, sau mai de încredere, sau se referă la rezultatul unei vederi în curs de dezvoltare, cum ar fi viziune stereoscopică (calcul de informații aprofundate prin combinarea imaginilor bidimensionale din două camere ).

Sursele de date pentru un proces de fuziune nu sunt specificate să provină de la senzori identici. Se poate distinge de fuziune directă, indirectă si fuziunea dintre cele doua. Fuziune directă este fuziunea de date senzori dintr-un set de senzori eterogeni sau omogeni, senzori moi, și valorile de istorie de date senzor, în timp ce fuziune indirect utilizează surse de informații cum ar fi o cunoaștere a priori despre mediul înconjurător și de intrare umane.

Fuziunea Senzorul este de asemenea cunoscut ca (multisenzor) fuziune a datelor și este un subset a informației de fuziune.

Fuziune senzorială este pur și simplu definit ca unificarea excitațiilor vizuale din imagini retiniene corespunzător într-o percepție vizuală singură o singură imagine vizuală. Viziunea unică este semnul distinctiv al corespondenței retiniene viziune dublă este semnul distinctiv al disparitate retiniene.[8]

Figura 8: conceptul de fuziune senzoriala

Formăm o estimare a orientării care este, de obicei reprezentat de un Quaternion, care este un vector cu patru componente, care conține toate informațiile necesare pentru a specifica o direcție unic în X / Y / Z spațiu. Ieșire W giroscop în radiani pe secundă poate fi considerat ca ratele de schimbare a acestui Quaternion: dQ/dt = 1/2W x Q sau Qt = Qt-1 x (1 + 1/2Wdt) pentru fiecare dintre cele trei axe spațiale. Gândiți-vă la Q ca o estimarea de orientarea senzorului. Acest lucru spune doar că rata de schimbare a quaternion este jumătate din rata de giroscoapelor pentru fiecare axă. Sau, în termeni utili pentru integrarea numerică, noua estimare Quaternion este egal cu cel vechi timpuri o funcție de rotația senzorului într-un interval de timp mic.

Corecția erorilor vine prin compararea estimarea Quaternion a accelerației la care măsurată prin accelerometrul:

Qt = Qt-1 x (1 + 1 / 2Wdt) + (AQ – Ameas) *beta * dt.

Estimarea Qt poate fi reiterat pentru a minimiza acest termen eroare cu parametru reglabil beta controlul rata de convergență la un răspuns stabil. În practică, convergența rezonabilă poate fi realizat în două sau trei iterații ceea ce înseamnă că ne-am dori acest corectare a erorilor sau filtru de fuziune senzoriala să funcționeze la o rată de două sau de trei ori rata mai mare decat ieșirea senzorului. Pentru giroscoape și accelerometru rate de ieșire de 200 Hz, aceasta înseamnă că doriți ratele de actualizare filtrul se apropie de 1 kHz. În scopul acesta se face uz de estimarea noastră Quaternion de orientare este convențional de a transforma vectorului patru componente într-o matrice de rotație de 3 x 3, sau în chiar simple unghiuri de rotație Yaw, Pitch, și roll. Cel mai bun format va fi dictată de aplicarea finală a ieșirii senzorului. Vom folosi Atitudinea standard, rubrica Sistem de referință Perioada de referință aplicabilă la navigare aeriene, dar există și alte cadre de referință care ar putea fi mai adecvate.[8]

Notă simplu termen de corecție a erorilor ne devine roll stabil și valori Pitch dar această estimare orientare vor fi supuse Yaw derivă, indiferent cât de fantezist filtrul este, deoarece nu dispune de un al doilea vector de referință ortogonală pentru a limita Yaw. Prin adăugarea unui magnetometru putem modifica estimarea noastră de orientare sporind corecția erorilor prin compararea estimarii Quaternion de câmp magnetic cu cea măsurată de magnetometru:

Qt = Qt-1 x (1 + 1 / 2Wdt) + [(AQ – Ameas) + (MQ – Mmeas)] x beta x dt.

Acum avem o estimare care converge rapid la o soluție unică, care reprezintă orientarea absoluta a senzor cu privire la un cadru de referință Pământ. Valorile Yaw, Pitch, and roll vor reveni întotdeauna la aceleași valori, indiferent de mișcarea intervenind atunci când senzorul este reorientat înapoi la o anumită direcție.[9]

3. Dispozitivul experimental

3.1 Aspecte generale

Dispozitivul este format din ELM-327 care este un dispozitiv cu interfata de tip OBD II, MPU-9150 care este un dipozitiv ce contine senzori inertiali cum ar fi acceletometru,giroscop si magnetometru si placuta de dezvoltare Arduino.

Software-urile folosite pentru aceasta lucra sunt Arduino Ide – care realizeaza legatura intre partea harware si software si Processing – care este folosit pentru a experimenta moduri de corectie al datelor provenite de la senzori inertiali.

Figura 3.1: Schema bloc a dispozitivului

3.2 Partea Harware

3.2.1 Placuta de dezvoltare Arduino Uno

Arduino este o placă de dezvoltare, cu un singur microcontroler, ce permite dezvoltarea unor proiecte multidisciplinare, mult mai accesibile. Este un dispzitiv uimitor care îți permite să realizezi aproape orice proiect, de la opere de artă interactive, la roboți mobili.

În forma cea mai simplă, o placă de dezvoltare Arduino, poate fi considerată ca un mic calculator căreia îi poți controla intrările și ieșirile de la chip. Placa de bază este alcătuit dintr-un microprocesor, un cistal sau un oscilator și un regulator de 5 V.

Aceasta este realizată dintr-un hardware și software (plăcuță fizică și program). Hardware-ul constă dintr-o placă open-source (sursă deschisă) concepută în jurul unui microcontroler Atmel AVR pe 8 biți sau un Atmel ARM pe 32 de biți. Partea de software, constituită dintr-un compilator cu limbaj propriu de programare executat pe microcontroler.

O placă Arduino constă dintr-un microcontroler și o serie de componente complementare pentru a facilita programarea și încorporarea în alte circuite. În funcție de tipul de plăcuță, acesta poate avea și un conector USB (universal serial bus) pentru a permite conectarea la un calculator. De asemenea, placa dispune de un anumit număr de pini pentru intrare (Input Pins) și pentru ieșire (Output Pins) prin intermediul cărora pot fi conectate alte circuite, cum ar fi senzori, LED-uri, motoare și altele.

Figura 3.2: Arduino uno

Pentru programare se va utiliza Arduino IDE (Integrated Development Enviroment), care este un soft oferit gratuit de această firmă pentru programarea microcontrolerului în limbajul pe care acesta îl înțelege și anume C și C++. Arduino IDE permite scrierea programului pe calculator, care este format dintr-un set de instrucțiuni pas cu pas pe care le încarci apoi în plăcuță. După încărcarea programului propriu-zis, Arduino va efectua instrucțiunile date și va interacționa cu mediul. Arduino numește această programare Sketches (schițe.)

Arduino UNO este o placă de dezvoltare bazată pe microcontroler-ul Atmega328. Acesta are 14 pini de intrare și ieșire digitală, din care șase pot fi utilizate ca ieșiri PWM (puls cu modulare), șase intrări analogice, un oscilator cu cuarț de 16 MHz, o conexiune USB, o mufă de alimentare și un buton de resetare. Însă, alimentarea se mai poate face și direct pe plăcuță, prin intermediul pinilor de alimentare de 5 V respectiv 3,3 V. Pentru a putea folosi Arduino pur și simplu se conectează cu un cablu USB la calculator, se instalează soft-ul oferit gratuit de pe site-ul lor și anume “www.arduino.cc”, se încarcă codul dorit și apoi este gata de execuție. Numele de UNO, care provine din limba italiană și înseamnă „unu” este o versiune standard pentru marca  Arduino.

Ca și parte constructivă, lungimea și lățimea maximă a plăcii este de 7,5 respectiv 5,3 centimetri, care este extinsă dacă este conectat cablul USB și mufa de alimentare. Placa este echipată cu patru găuri pentru a putea fi fixată pe orice suprafață folosind șuruburi.

3.2.2 Interfata OBD II – ELM-327

ELM327 este un microcontroler programat si produs de ELM Electronics pentru traducerea diagnosticare la bord (OBD), interfața găsită în cele mai multe masini moderne. Protocolul de comandă ELM327 este unul dintre cele mai populare standarde de interfață PC-to-OBD și este, de asemenea, pusă în aplicare de alți vânzători.ELM327 este implementat pe microcontroler PIC18F2480 de la Microchip Technology. ELM327 este unul dintr-o familie interfete OBD de la Elm Electronics.

Figura 3.3: Interfata OBD II – ELM 327

Specifitatile tehnice ale Interfetei ELM 32:

Tabel 3.1 : Specificatii tehnice

Modulul ELM 327 se poate conecta la un computer atat wireless cat si prin USB. Daca se foloseste un modul care se poate conecta wireless citirea date se poate face la fel de bine folosind un Smartphone pe care este instalat un software special ca de exemplu Torque.

Figura 3.4: ELM 327 – wireless

Figura 3.5 ELM 327 – USB

Avantajele folosiri modulului ELM 327:

modulul ELM 327 este un modul ieftin si usor de gasit;

este fiabil si usor de folosit;

datorita numarului mare de protocoale de comunicare pe care acest modul le cunoaste acest modul poate fi folosit aproape pe orice autovehicol;

cu ajutoru acestui modul se pot monitoriza mai multi parametri ai auto mobilului:

viteza;

turatia motorului;

consumul;

temperatura motorului;

cantitatea de combustibil ramasa in rezervor;

nivelul de ulei din motor;

3.2.3 Senzori inertiali – MPU 9150

Senzorul de pozitie MPU-9150 este primul dispozitiv din lume conceput pentru puterea scăzută , costul redus, performante ridicate pentru echipamente electronice de consum , smartphone-uri si tablete.

Senzorul MPU-9150 are in componenta lui un accelerometru pe 3 axe, un giroscop pe 3 axe si un magnetometru pe 3 axe.

Figura 3.6: Senzorul inertial MPU 9150

Specificatii tehnice:

Tabelul 3.2: Specificatiile tehnice ale senzorului inertial MPU-9150

Abordarea modernă este de a integra senzori multipli într-un singur pachet mic. Exemple populare ale acestei sunt MPU-9150 de la INVENSENSE și LSM9DS0 de ST Microelectronics, ambele combină un accelerometru / giroscop cu un magnetometru Hall într-o capsula 4 mm x 4 mm.

Figura 3.7: Capsula senzorului MPU-9150

MPU-6050 construit INVENSENSE și constă dintr-un sistem MEMS (sistem microelectro mecanic) de accelerometru și giroscop cu 16 biți convertoare analog digitate pentru o precizie 60 micro g/secunda respectiv 0,01 grad / secundă. Dispozitivul a fost lansat pentru prima dată la sfârșitul lui 2010, dar este încă utilizat în dispozitivul MPU-9150, cu un magnetometru încorporat.MPU6050 a fost reproiectat recent pentru a deveni MPU6500 giroscoapelor / accelerometrele au scăzut cu 40% în mărime, și este utilizat în mai noi cele mai MPU9250 senzorul 9-axe.

MPU-6050 vine pe piata cu o inovatie numita procesor de misca integrat în chip a cărui programare este proprietatea INVENSENSE. Acesta permite celor 6 axe permite procesorului de miscare sa faca fuziuanea senzoriala la o rată fixă de 200 Hz, iar rezultatele livrate la microcontroler sub formă de quaternion, Yaw, Pitch, și Roll. Această capacitate este de o importanță majoră pentru aplicarea principal al acestui cip în piețele de telefoane și tablete.

MPU-9150 foloseste-MPU 6050 ca accelerometru / giroscop combinat cu un magnetometru Asahi Kasei AK8975A. Rezoluția magnetometru este +/- 3 milliGauss cu o gamă de +/- 12 Gauss pe scară largă. Procesorul de miscare din capsula ofera fuziunea senzoriala a celor 6 axe de libertate provenite de la senzorul MPU 6050, dar nu există nici o modalitate de a de a integra date de la magnetometru pentru a obtine cele 9 grade de libertate dorite.Există o bibliotecă cuprinzătoare pentru MPU-6050 și MPU-9150 dezvoltat de Jeff Rowberga numita „I2CdevLib”; majoritati oamenilor li se pare un pic cam greu de folosit, dar ea are un "hack" pentru utilizarea procesorului de miscare care funcționează bine.

Inainte de a trece la modul in care acest dispozit poate fi conectat la un Arduino uno ar trebui sa discut despre orientare in spatiu.

Figura 3.8: Reprezentarea Yaw, Pitch si Roll

Luam ca referinta axa z care este axa gravitatiei.Unghiul Yaw este unghiul (psi în diagrama), dintre fuselajul avionului din imaginea de mai sus si Nordul magnetic al panamtului care este reprezentat de axa x. Unghiul „Pitch” (theta în diagrama) este unghiul pe care il face botul avionului in raport cu cu planu creat de axele x si y. Unghiul „Roll” este unghiul (phi în diagrama) unghiul pe care il face avionu in raport cu axa lui de simetrie.[]

Modulul MPU 9150 este conectat la un arduino ca in figura 3.9:

Figura 3.9: Modul de conectare

Tabelul 3.3: Modul de conectare

3.3 Partea Software

3.3.1 Arduino IDE

Citirea datelor oferite de senzorilor este scrisă în limbajul de programare C și C++ în partea de programare a microcontrolerului Arduino. Programarea în C este destul de flexibilă, o segvență putând fi codificată în mai multe moduri. Așadar putem scrie același program în mai multe feluri, el executând aceași funcție. Permite o scriere compactă, instrucțiunile putând fi scrise pe același rând. Progamul poate fi scris într-o manieră apropiată de pseudocod (gr.pseudo=fals), conceperea algoritmilor fiind mult mai ușoară și eficientă.

C este folosit în general în proiecte de mici dimensiuni unde se pune accent pe performanța codului, ca exemplu, programarea microcontrolelelor. În orice caz limbajul C este un limbaj de programare foarte influent și în același timp foarte popular. Pentru a realiza un program în C este nevoie de un program pentru scrierea codului, un editor de text, și un compilator. Compilatorul este un software care transformă codul scris în C ( limbajul înțeles de noi ) în cod binar ( limbaj înțeles de calculator ). Compilarea unui program constă în verificarea codului, pentru greșelile de sintaxă, transformarea lui în cod binar și crearea unui executabil sau a unui fișier în urma compilării progamului.

După conectarea microcontrolerului prin cablu USB legat la portul serial se va trece la citirea si interpretarea datelor de la senzori.

Citirea datelor de la senzori se face dupa urmatori pasi:

1. Setarea scalei pe care dorim sa efectuam masuratorile se face ca in urmatoarea secventa de program:

enum Ascale {

AFS_2G = 0,

AFS_4G,

AFS_8G,

AFS_16G

};

enum Gscale {

GFS_250DPS = 0,

GFS_500DPS,

GFS_1000DPS,

GFS_2000DPS

};

2. Stocarea datelor neprelucrate de la cele 3 tipuri de sensor se face cu ajutorul uni sir de tip inter pe 16 biti evidentiata in urmatoarea secventa de program.

int16_t accelCount[3];

int16_t gyroCount[3];

int16_t magCount[3];

3. Calibrarea magnetometrului se face cu urmatoare secventa de program si este un pas important de care trebuie tinut cont atunci cand se foloseste acest tip de sensor de aceea se face numa o data atunci cand se foloseste prima data senzorul

if(SerialDebug) {

Serial.println("Magnetometer calibration values: ");

Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2);

Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2);

Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2);

}

4. Citirea datelor neprelucrate de la cei 3 senzori se face :

if (readByte(MPU9150_ADDRESS, INT_STATUS) & 0x01) {

getAres();

// citirea datelor de la accelerometru

ax = (float)accelCount[0]*aRes

ay = (float)accelCount[1]*aRes;

az = (float)accelCount[2]*aRes;

readGyroData(gyroCount); getGres();

// citirea datelor de la giroscop

gx = (float)gyroCount[0]*gRes;

gy = (float)gyroCount[1]*gRes;

gz = (float)gyroCount[2]*gRes;

// dupa calibrarea magnetoetrului se trece la citirea datelor evidentiate dupa cum urmeaza

if (mcount > 200/MagRate) { readMagData(magCount);

mRes = 10.*1229./4096.;

magbias[0] = -5.; // User environmental x-axis correction in milliGauss

magbias[1] = -95.; // User environmental y-axis correction in milliGauss

magbias[2] = -260.; // User environmental z-axis correction in milliGauss

mx = (float)magCount[0]*mRes*magCalibration[0] – magbias[0];

my = (float)magCount[1]*mRes*magCalibration[1] – magbias[1];

mz = (float)magCount[2]*mRes*magCalibration[2] – magbias[2];}

4. Avand toate datele de la senzori putem trece la prelucrarea lor incepand cu calcularea quaternionului si mai apoi integrarea acestuia in calcularea celor 3 unghiuri : Yaw, Pitch si Roll acest lucru este evidentiat in urmatoarea secventa de cod.

//calcularea quaternionului

MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

//calcularea celor 3 unghiuri yaw pitch si roll

yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] – q[2] * q[2] – q[3] * q[3]);

pitch = -asin(2.0f * (q[1] * q[3] – q[0] * q[2]));

roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] – q[1] * q[1] – q[2] * q[2] + q[3] * q[3]);

pitch *= 180.0f / PI;

yaw *= 180.0f / PI;

yaw -= 5.8;// pentru calcularea cu acuratete a unghiului Yaw se tine cont si de declinarea magnetica a pamanatului

roll *= 180.0f / PI;

5. avand cele 3 unghiuri calculate putem trece la interpretarea datelor care se poate realize cu un alt Software numit processing

3.3.1 Processing

Processing este un limbaj de programare open source și mediu de dezvoltare integrat (IDE) construit pentru arte electronice, noul in domeniu mass-media, și in comunitățile de design vizual cu scopul de a preda fundamentele de programare într-un context vizual, și pentru a servi ca bază pentru sketchbooks electronice. Proiectul a fost inițiat în 2001 de către Casey Reas și Benjamin Fry, atât anterior Grupului Estetică și Calcul la MIT Media Lab. Unul dintre obiectivele declarate ale Prelucrarea este de a acționa ca un instrument pentru a obține non-programatori a început cu programarea, prin satisfacerea instantanee de feedback vizual. Limbajul se bazează pe limbajul Java, dar foloseste un model de programare simplificat sintaxă și grafică.

Prelucrarea include un caiet de schite, o alternativă minimă pentru un mediu de dezvoltare integrat (IDE) pentru organizarea de proiecte.

Fiecare schiță de prelucrare este de fapt o subclasă a clasei PApplet Java care implementează cele mai multe dintre caracteristicile de procesare de limbă.

Când programare în prelucrarea, toate clasele suplimentare definite vor fi tratate ca clase interne în cazul în care codul este tradus în pură Java înainte de compilare. Acest lucru înseamnă că utilizarea de variabile și metode din clasele statice este interzisă, cu excepția cazului vă spun în mod explicit de procesare pe care doriți să cod în modul Java pur.

Prelucrarea, de asemenea, permite utilizatorilor să își creeze propriile clase în schița PApplet. Acest lucru permite pentru tipuri de date complexe, care pot include orice număr de argumente și evită limitările numai cu ajutorul tipurilor de date standard, cum ar fi: int (întreg), char (caracter), float (număr real), și de culoare (RGB, ARGB, hex ).

Pentru prelucrarea datelor de la senzori trebuie parcursi niste pasi preliminari cum ar fi:

1. Stabilirea comunicarii seriale intre arduino si processing care se face cu ajutorul

urmatoarei comenzi:

myPort = new Serial(this, Serial.list()[0], 38400);

2. Definirea axelor carteziene :

void drawGraph() {

background(255);

for (int i = 0; i < width; i++) {

stroke(200); // Grey

line(i*10, 0, i*10, height);

line(0, i*10, width, i*10);

}

stroke(0);

for (int i = 1; i <= 3; i++)

line(0, height/2*i, width, height/2*i);

Figura 3.10 Determinarea axelor

3. Citirea datelor de la Arduino

void serialEvent (Serial myPort) {

// get the ASCII string:

stringX= myPort.readStringUntil('\t');

stringY = myPort.readStringUntil('\t');

stringZ = myPort.readStringUntil('\t');

myPort.readStringUntil('\t');

myPort.clear();

drawValues = true;

}

4.Trasarea graficelor in fuctie de datele primate de la arduiono

void drawAxisY() {

/* Draw gyro y-axis */

noFill();

stroke(255, 0, 255); // Purble

// Redraw everything

beginShape();

vertex(0, Y[0]);

for (int i = 1; i < Y.length; i++) {

if ((Y[i] < height/2 && Y[i – 1] > height/2*3) || (Y[i] > height/2*3 && Y[i – 1] < height/2)) {

endShape();

beginShape();

}

vertex(i, Y[i]);

}

endShape();

// Put all data one array back

for (int i = 1; i < Y.length;i++)

Y[i-1] = Y[i];}

void drawAxisZ() {

noFill();

stroke(255, 0, 0); // Roho

// Redraw everything

beginShape();

vertex(0, Z[0]);

for (int i = 1; i < Z.length; i++) {

if ((Z[i] < height/2 && Z[i – 1] > height/2*3) || (Z[i] > height/2*3 && Z[i – 1] < height/2)) {

endShape();

beginShape();

}

vertex(i, Z[i]);

}

endShape();

// Put all data one array back

for (int i = 1; i < Z.length;i++)

Z[i-1] = Z[i];}

Figura 3.11 Pocessing dupa trasarea greaficului

3.4 Rezultate experimentale

Ca prim experiment s-a facut testarea fiecarui senzor din cipu MPU 9150 pe fiecare dintre cele 3 axe pentru a ne asigura ca cei 3 senzori merg cum trebuie.

Figura 3.11 pozitionarea senzorului pe axa x

Figura 3.12 Procesing pe axa x

Figura 3.13 Testarea senzorului pe axa Y

FIgura 3.14Processing pe axa y

Figura 3.15 Testarea senzorului pe axa z

Figura 3.16 Processing pe axa z

Dupa ce s-a realizat testarea senzorilor pe toate axele se trece la calibrarea acestora tinand fix senzorul pe axa z.

Figura 3.17: Pozitionarea senzorului pentru calibrare

Figura 3.18: P rocessing dupa calibrarea senzorilor pe toate cele 3 axele

Dup ace s-a realaizat calibrarea senzorilo se poate trece la calcularea celor 3 unghiuri Yaw, Pitch si Roll necesare pentru determinarea pozitiei obiectului.

Figura 3.19 Procesing realizat dupa calcularea celor 3 unghiuri

3.5 Sistem autovehicul, senzori inertial

Fuziunea de date dintre senzorii inertiali si datele primite de la autovehicul are ca rol determinarea pozitie autovehiculului chiar si in zonele unde nu exista semnal Gps ca de exemplu tunele sau parcari subterane, cu acest system mai putem determina si daca autovehiculul s-a rasturnat pentru a putea declasa aribagurile.

Fuziunea datelor senzorilor inertial cu a datelor primate de la masina ne pot furniza da importante despre pozitionarea exacta a masinii. Aceste sisteme sunt integrate in masinile care au sisteme de navigatie legate la computerul de bord al acesteia.

Figura 3.19: Exemplu de sitiuatie in care Gps-ul nu functioneaza cun trebuie din cauza perturbatiilor electromagnetice

Concluzii

Prin prezenta lucrare, s-a dorit realizarea unui sistem de monitorizare si control in timp real al unor parametrici auto.

Pentru realizarea sistemului de monitorizare s-au folosit urmatoarele component: arduino, senzorul inertial MPU 9150, modulul ELM327 si sofwer-uri precum Processing sau Arduino Ide. Descrierea a putut fi realizată datorită diverselor site-uri de specialitate.

Detectarea unor parametrii ai autovehicului s-a realizat cu ajutor modulului de tip OBD II, s-au studiat parametrii ca de exemplu: viteza, si turatia motorului.

Senzorul inertial MPU 9150 a ajutat la stabilirea pozitie autovehiculului.

Datorită fuziunii senziale dintre mai multi parametrii indicati atat de MPU9150 cat si de ELM327 s-a incercat realizarea unui syistem care reuseste sa stabileasca pozitia exact a autovehiculului

Rezultatele experimentale sunt încurajatoare, sistemul având o precizie ridicată

BIBLIOGRAFIE

[1] http://www.e-automobile.ro/categorie-diagnoza/5-obd-diagnoza-auto.html

[2] http://www.e-automobile.ro/images/articole/diagnoza/obd2-diagnoza/4_conector_obd2_vehicul.JPG

[3]http://www.automation-drive.com/EX/05-15-11/equus-3130-obd-scan-tool.jpg

[4] https://en.wikipedia.org/wiki/accelerometes

[5]https://en.wikipedia.org/wiki/Vibrating_structure_gyroscope#MEMS_gyroscopes

[6] https://learn.sparkfun.com/tutorials/gyroscope/how-a-gyro-works

[7] http://www-me.ccny.cuny.edu/ccnf/Images/MEMS_Gyroscope.JPG

[8] https://en.wikipedia.org/wiki/Sensor_fusion

[9] https://github.com/kriswiner/MPU-6050/wiki/Affordable-9-DoF-Sensor-Fusion

http://www.iobd2.org/wp-content/uploads/2013/10/Super_Mini_ELM327_Interface_Bluetooth_OBD2_Scan_Tool_3509032_a.jpg

ANEXE

Codul pentru MPU 9150

#include "Wire.h"

#include <Adafruit_GFX.h>

#include <Adafruit_PCD8544.h>

// Using NOKIA 5110 monochrome 84 x 48 pixel display

// pin 9 – Serial clock out (SCLK)

// pin 8 – Serial data out (DIN)

// pin 7 – Data/Command select (D/C)

// pin 5 – LCD chip select (CS)

// pin 6 – LCD reset (RST)

Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);

// Define registers per MPU6050, Register Map and Descriptions, Rev 4.2, 08/19/2013 6 DOF Motion sensor fusion device

// Invensense Inc., www.invensense.com

// See also MPU-9150 Register Map and Descriptions, Revision 4.0, RM-MPU-9150A-00, 9/12/2012 for registers not listed in

// above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor

//

//Magnetometer Registers

#define WHO_AM_I_AK8975A 0x00 // should return 0x48

#define INFO 0x01

#define AK8975A_ST1 0x02 // data ready status bit 0

#define AK8975A_ADDRESS 0x0C

#define AK8975A_XOUT_L 0x03 // data

#define AK8975A_XOUT_H 0x04

#define AK8975A_YOUT_L 0x05

#define AK8975A_YOUT_H 0x06

#define AK8975A_ZOUT_L 0x07

#define AK8975A_ZOUT_H 0x08

#define AK8975A_ST2 0x09 // Data overflow bit 3 and data read error status bit 2

#define AK8975A_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0

#define AK8975A_ASTC 0x0C // Self test control

#define AK8975A_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value

#define AK8975A_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value

#define AK8975A_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value

#define XGOFFS_TC 0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD

#define YGOFFS_TC 0x01

#define ZGOFFS_TC 0x02

#define X_FINE_GAIN 0x03 // [7:0] fine gain

#define Y_FINE_GAIN 0x04

#define Z_FINE_GAIN 0x05

#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer

#define XA_OFFSET_L_TC 0x07

#define YA_OFFSET_H 0x08

#define YA_OFFSET_L_TC 0x09

#define ZA_OFFSET_H 0x0A

#define ZA_OFFSET_L_TC 0x0B

#define SELF_TEST_X 0x0D

#define SELF_TEST_Y 0x0E

#define SELF_TEST_Z 0x0F

#define SELF_TEST_A 0x10

#define XG_OFFS_USRH 0x13 // User-defined trim values for gyroscope, populate with calibration routine

#define XG_OFFS_USRL 0x14

#define YG_OFFS_USRH 0x15

#define YG_OFFS_USRL 0x16

#define ZG_OFFS_USRH 0x17

#define ZG_OFFS_USRL 0x18

#define SMPLRT_DIV 0x19

#define CONFIG 0x1A

#define GYRO_CONFIG 0x1B

#define ACCEL_CONFIG 0x1C

#define FF_THR 0x1D // Free-fall

#define FF_DUR 0x1E // Free-fall

#define MOT_THR 0x1F // Motion detection threshold bits [7:0]

#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms

#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]

#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms

#define FIFO_EN 0x23

#define I2C_MST_CTRL 0x24

#define I2C_SLV0_ADDR 0x25

#define I2C_SLV0_REG 0x26

#define I2C_SLV0_CTRL 0x27

#define I2C_SLV1_ADDR 0x28

#define I2C_SLV1_REG 0x29

#define I2C_SLV1_CTRL 0x2A

#define I2C_SLV2_ADDR 0x2B

#define I2C_SLV2_REG 0x2C

#define I2C_SLV2_CTRL 0x2D

#define I2C_SLV3_ADDR 0x2E

#define I2C_SLV3_REG 0x2F

#define I2C_SLV3_CTRL 0x30

#define I2C_SLV4_ADDR 0x31

#define I2C_SLV4_REG 0x32

#define I2C_SLV4_DO 0x33

#define I2C_SLV4_CTRL 0x34

#define I2C_SLV4_DI 0x35

#define I2C_MST_STATUS 0x36

#define INT_PIN_CFG 0x37

#define INT_ENABLE 0x38

#define DMP_INT_STATUS 0x39 // Check DMP interrupt

#define INT_STATUS 0x3A

#define ACCEL_XOUT_H 0x3B

#define ACCEL_XOUT_L 0x3C

#define ACCEL_YOUT_H 0x3D

#define ACCEL_YOUT_L 0x3E

#define ACCEL_ZOUT_H 0x3F

#define ACCEL_ZOUT_L 0x40

#define TEMP_OUT_H 0x41

#define TEMP_OUT_L 0x42

#define GYRO_XOUT_H 0x43

#define GYRO_XOUT_L 0x44

#define GYRO_YOUT_H 0x45

#define GYRO_YOUT_L 0x46

#define GYRO_ZOUT_H 0x47

#define GYRO_ZOUT_L 0x48

#define EXT_SENS_DATA_00 0x49

#define EXT_SENS_DATA_01 0x4A

#define EXT_SENS_DATA_02 0x4B

#define EXT_SENS_DATA_03 0x4C

#define EXT_SENS_DATA_04 0x4D

#define EXT_SENS_DATA_05 0x4E

#define EXT_SENS_DATA_06 0x4F

#define EXT_SENS_DATA_07 0x50

#define EXT_SENS_DATA_08 0x51

#define EXT_SENS_DATA_09 0x52

#define EXT_SENS_DATA_10 0x53

#define EXT_SENS_DATA_11 0x54

#define EXT_SENS_DATA_12 0x55

#define EXT_SENS_DATA_13 0x56

#define EXT_SENS_DATA_14 0x57

#define EXT_SENS_DATA_15 0x58

#define EXT_SENS_DATA_16 0x59

#define EXT_SENS_DATA_17 0x5A

#define EXT_SENS_DATA_18 0x5B

#define EXT_SENS_DATA_19 0x5C

#define EXT_SENS_DATA_20 0x5D

#define EXT_SENS_DATA_21 0x5E

#define EXT_SENS_DATA_22 0x5F

#define EXT_SENS_DATA_23 0x60

#define MOT_DETECT_STATUS 0x61

#define I2C_SLV0_DO 0x63

#define I2C_SLV1_DO 0x64

#define I2C_SLV2_DO 0x65

#define I2C_SLV3_DO 0x66

#define I2C_MST_DELAY_CTRL 0x67

#define SIGNAL_PATH_RESET 0x68

#define MOT_DETECT_CTRL 0x69

#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP

#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode

#define PWR_MGMT_2 0x6C

#define DMP_BANK 0x6D // Activates a specific bank in the DMP

#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank

#define DMP_REG 0x6F // Register in DMP from which to read or to which to write

#define DMP_REG_1 0x70

#define DMP_REG_2 0x71

#define FIFO_COUNTH 0x72

#define FIFO_COUNTL 0x73

#define FIFO_R_W 0x74

#define WHO_AM_I_MPU9150 0x75 // Should return 0x68

// Using the GY-521 breakout board, I set ADO to 0 by grounding through a 4k7 resistor

// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1

#define ADO 0

#if ADO

#define MPU9150_ADDRESS 0x69 // Device address when ADO = 1

#else

#define MPU9150_ADDRESS 0x68 // Device address when ADO = 0

#define AK8975A_ADDRESS 0x0C // Address of magnetometer

#endif

#define AHRS true // set to false for basic data read

#define SerialDebug true // set to true to print serial output for debugging

// Set initial input parameters

enum Ascale {

AFS_2G = 0,

AFS_4G,

AFS_8G,

AFS_16G

};

enum Gscale {

GFS_250DPS = 0,

GFS_500DPS,

GFS_1000DPS,

GFS_2000DPS

};

// Specify sensor full scale

uint8_t Gscale = GFS_250DPS;

uint8_t Ascale = AFS_2G;

float aRes, gRes, mRes; // scale resolutions per LSB for the sensors

// Pin definitions

int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins

#define blinkPin 13 // Blink LED on Teensy or Pro Mini when updating

boolean blinkOn = false;

int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output

int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output

int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output

float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias

float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer

int16_t tempCount; // Stores the raw internal chip temperature counts

float temperature; // temperature in degrees Centigrade

float SelfTest[6];

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)

float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s)

float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)

// There is a tradeoff in the beta parameter between accuracy and response speed.

// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.

// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.

// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!

// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec

// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;

// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.

// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.

float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta

float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value

#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral

#define Ki 0.0f

uint32_t delt_t = 0; // used to control display output rate

uint32_t count = 0; // used to control display output rate

uint32_t mcount = 0; // used to control magnetometer read rate

uint32_t MagRate; // read rate for magnetometer data

float pitch, yaw, roll;

float deltat = 0.0f; // integration interval for both filter schemes

uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval

uint32_t Now = 0; // used to calculate integration interval

float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values

float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion

float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method

void setup()

{

Wire.begin();

Serial.begin(38400);

// Set up the interrupt pin, its set as active high, push-pull

pinMode(intPin, INPUT);

digitalWrite(intPin, LOW);

pinMode(blinkPin, OUTPUT);

digitalWrite(blinkPin, HIGH);

/* display.begin(); // Initialize the display

display.setContrast(55); // Set the contrast

display.setRotation(0); // 0 or 2) width = width, 1 or 3) width = height, swapped etc.

// Start device display with ID of sensor

display.clearDisplay();

display.setTextSize(2);

display.setCursor(0,0); display.print("MPU9150");

display.setTextSize(1);

display.setCursor(0, 20); display.print("9 DOF 16-bit");

display.setCursor(0, 30); display.print("sensor fusion");

display.setCursor(20,40); display.print("AHRS");

display.display();

delay(1000);

// Set up for data display

display.setTextSize(1); // Set text size to normal, 2 is twice normal etc.

display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen

display.clearDisplay(); // clears the screen and buffer

// Read the WHO_AM_I register, this is a good test of communication

uint8_t c = readByte(MPU9150_ADDRESS, WHO_AM_I_MPU9150); // Read WHO_AM_I register for MPU-9150

display.setCursor(20,0); display.print("MPU9150");

display.setCursor(0,10); display.print("I AM");

display.setCursor(0,20); display.print(c, HEX);

display.setCursor(0,30); display.print("I Should Be");

display.setCursor(0,40); display.print(0x68, HEX);

display.display();

delay(1000);

*/

/* if (c == 0x68) // WHO_AM_I should always be 0x68

{

//Serial.println("MPU9150 is online…");

MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values

// Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value");

// Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value");

// Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value");

// Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value");

// Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value");

// Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value");

if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {

/*display.clearDisplay();

display.setCursor(0, 30); display.print("Pass Selftest!");

display.display();

delay(1000);

}*/

calibrateMPU9150(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers

/*display.clearDisplay();

display.setCursor(20, 0); display.print("MPU9150 bias");

display.setCursor(0, 8); display.print(" x y z ");

display.setCursor(0, 16); display.print((int)(1000*accelBias[0]));

display.setCursor(24, 16); display.print((int)(1000*accelBias[1]));

display.setCursor(48, 16); display.print((int)(1000*accelBias[2]));

display.setCursor(72, 16); display.print("mg");

display.setCursor(0, 24); display.print(gyroBias[0], 1);

display.setCursor(24, 24); display.print(gyroBias[1], 1);

display.setCursor(48, 24); display.print(gyroBias[2], 1);

display.setCursor(66, 24); display.print("o/s");

display.display();

delay(1000); */

initMPU9150(); // Inititalize and configure accelerometer and gyroscope

// Serial.println("MPU9150 initialized for active data mode…."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature

// Read the WHO_AM_I register of the magnetometer, this is a good test of communication

uint8_t c = readByte(AK8975A_ADDRESS, WHO_AM_I_AK8975A); // Read WHO_AM_I register for AK8975A

/* display.clearDisplay();

display.setCursor(20,0); display.print("AK8975A");

display.setCursor(0,10); display.print("I AM");

display.setCursor(0,20); display.print(c, HEX);

display.setCursor(0,30); display.print("I Should Be");

display.setCursor(0,40); display.print(0x48, HEX);

display.display();

delay(1000); */

// Get magnetometer calibration from AK8975A ROM

initAK8975A(magCalibration);

if(SerialDebug) {

/*Serial.println("Magnetometer calibration values: ");

Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2);

Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2);

Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2);

}*/

/* display.clearDisplay();

display.setCursor(20,0); display.print("AK8975A");

display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); display.print(magCalibration[0], 2);

display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); display.print(magCalibration[1], 2);

display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); display.print(magCalibration[2], 2);

display.display();

delay(1000); */

MagRate = 10; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values

}

else

{

Serial.print("Could not connect to MPU9150: 0x");

Serial.println(c, HEX);

while(1) ; // Loop forever if communication doesn't happen

}

}

void loop()

{

// If intPin goes high or data ready status is TRUE, all data registers have new data

if (readByte(MPU9150_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt

readAccelData(accelCount); // Read the x/y/z adc values

getAres();

// Now we'll calculate the accleration value into actual g's

ax = (float)accelCount[0]*aRes; // get actual g value, this depends on scale being set

ay = (float)accelCount[1]*aRes;

az = (float)accelCount[2]*aRes;

readGyroData(gyroCount); // Read the x/y/z adc values

getGres();

// Calculate the gyro value into actual degrees per second

gx = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set

gy = (float)gyroCount[1]*gRes;

gz = (float)gyroCount[2]*gRes;

mcount++;

if (mcount > 200/MagRate) { // this is a poor man's way of setting the magnetometer read rate (see below)

readMagData(magCount); // Read the x/y/z adc values

mRes = 10.*1229./4096.; // Conversion from 1229 microTesla full scale (4096) to 12.29 Gauss full scale

// So far, magnetometer bias is calculated and subtracted here manually, should construct an algorithm to do it automatically

// like the gyro and accelerometer biases

magbias[0] = -5.; // User environmental x-axis correction in milliGauss

magbias[1] = -95.; // User environmental y-axis correction in milliGauss

magbias[2] = -260.; // User environmental z-axis correction in milliGauss

// Calculate the magnetometer values in milliGauss

// Include factory calibration per data sheet and user environmental corrections

mx = (float)magCount[0]*mRes*magCalibration[0] – magbias[0]; // get actual magnetometer value, this depends on scale being set

my = (float)magCount[1]*mRes*magCalibration[1] – magbias[1];

mz = (float)magCount[2]*mRes*magCalibration[2] – magbias[2];

mcount = 0;

}

if(!AHRS) {

tempCount = readTempData(); // Read the x/y/z adc values

temperature = ((float) tempCount) / 340. + 36.53; // Temperature in degrees Centigrade

}

}

Now = micros();

deltat = ((Now – lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update

lastUpdate = Now;

// Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer;

// the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro!

// We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter.

// For the MPU-9150, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like

// in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention.

// This is ok by aircraft orientation standards!

// Pass gyro rate as rad/s

MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

// MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

if (!AHRS) {

int delt_t = millis() – count;

if(delt_t > 500) {

digitalWrite(blinkPin, blinkOn);

if(SerialDebug) {

// Print acceleration values in milligs!

Serial.print("X-acceleration: "); Serial.print(1000*ax); Serial.print(" mg ");

Serial.print("Y-acceleration: "); Serial.print(1000*ay); Serial.print(" mg ");

Serial.print("Z-acceleration: "); Serial.print(1000*az); Serial.println(" mg");

// Print gyro values in degree/sec

Serial.print("X-gyro rate: "); Serial.print(gx, 3); Serial.print(" degrees/sec ");

Serial.print("Y-gyro rate: "); Serial.print(gy, 3); Serial.print(" degrees/sec ");

Serial.print("Z-gyro rate: "); Serial.print(gz, 3); Serial.println(" degrees/sec");

// Print mag values in degree/sec

Serial.print("X-mag field: "); Serial.print(mx); Serial.print(" mG ");

Serial.print("Y-mag field: "); Serial.print(my); Serial.print(" mG ");

Serial.print("Z-mag field: "); Serial.print(mz); Serial.println(" mG");

// Print temperature in degrees Centigrade

Serial.print("Temperature is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C

Serial.println("");

}

/* display.clearDisplay();

display.setCursor(0, 0); display.print("MPU9150/AK8975");

display.setCursor(0, 8); display.print(" x y z ");

display.setCursor(0, 16); display.print((int)(1000*ax));

display.setCursor(24, 16); display.print((int)(1000*ay));

display.setCursor(48, 16); display.print((int)(1000*az));

display.setCursor(72, 16); display.print("mg");

display.setCursor(0, 24); display.print((int)(gx));

display.setCursor(24, 24); display.print((int)(gy));

display.setCursor(48, 24); display.print((int)(gz));

display.setCursor(66, 24); display.print("o/s");

display.setCursor(0, 32); display.print((int)(mx));

display.setCursor(24, 32); display.print((int)(my));

display.setCursor(48, 32); display.print((int)(mz));

display.setCursor(72, 32); display.print("mG");

display.setCursor(0, 40); display.print("Gyro T ");

display.setCursor(50, 40); display.print(temperature, 1); display.print(" C");

display.display();*/

blinkOn = ~blinkOn;

count = millis();

}

}

else {

delt_t = millis() – count;

if (delt_t > 500) { // update LCD once per half-second independent of read rate

digitalWrite(blinkPin, blinkOn);*/

if(SerialDebug) {

/* Serial.print("ax = "); Serial.print((int)1000*ax);

Serial.print(" ay = "); Serial.print((int)1000*ay);

Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");

Serial.print("gx = "); Serial.print( gx, 2);

Serial.print(" gy = "); Serial.print( gy, 2);

Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");

Serial.print("mx = "); Serial.print( (int)mx );

Serial.print(" my = "); Serial.print( (int)my );

Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");

Serial.print("q0 = "); Serial.print(q[0]);

Serial.print(" qx = "); Serial.print(q[1]);

Serial.print(" qy = "); Serial.print(q[2]);

Serial.print(" qz = "); Serial.println(q[3]); */

}

yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] – q[2] * q[2] – q[3] * q[3]);

pitch = -asin(2.0f * (q[1] * q[3] – q[0] * q[2]));

roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] – q[1] * q[1] – q[2] * q[2] + q[3] * q[3]);

pitch *= 180.0f / PI;

yaw *= 180.0f / PI;

yaw -= 5.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04

roll *= 180.0f / PI;

if(SerialDebug) {

//Serial.print("Yaw, Pitch, Roll: ");

Serial.print(ax, 2);Serial.print("\t");

//Serial.print(", ");

Serial.print(ay, 2);Serial.print("\t");

//Serial.print(", ");

Serial.print(az, 2);Serial.print("\t");

Serial.println();

/*Serial.print("average rate = "); Serial.print(1.0f/deltat, 2); Serial.println(" Hz");*/

}