Developing a self-balancing quadcopter with STNucleo microcontroller

1. Università degli Studi di Trieste
Dipartimento di Ingegneria e Architettura
Corso di Studi in Ingegneria Elettronica ed Informatica
Curriculum Informatica
Sviluppo del sistema di controllo dell'as-
setto di un quadricottero con processore
STNucleo
Tesi di Laurea Triennale
Candidato:
Andrea Gulberti
Relatore:
Prof. Ing. Stefano Marsi
Correlatore:
Prof. Ing. Sergio Carrato
Anno Accademico 2016-2017

3. Ai miei genitori, che mi hanno sempre supportato e hanno sempre
approvato ogni mia decisione
A mia sorella, che ha saputo guidarmi e ispirarmi
A mio zio, stimolo per la mia crescita professionale
Ai miei amici, perché ridere è la parte più importante della vita

5. Indice
1 Introduzione ........................................................................................................ 2
2 Il protocollo I2C................................................................................................... 6
2.1 La libreria integrata i2c.......................................................................... 7
2.2 La libreria i2c_hw.................................................................................... 8
2.2.1 WriteBytes.................................................................................... 9
2.2.2 ReadBytes................................................................................... 10
3 I componenti...................................................................................................... 11
3.1 Gli E.S.C. ............................................................................................... 11
3.1.1 La programmazione................................................................... 13
3.1.2 Codice Utilizzato per la programmazione ................................ 15
3.1.3 Libreria “esc”.............................................................................. 17
3.2 Il Ricevitore ad Infrarossi (IR).............................................................. 19
3.2.1 La Libreria “ReceiverIR” ........................................................... 20
3.3 MPU6050 ............................................................................................... 23
3.3.1 Il Sensore.................................................................................... 23
3.3.2 La FIFO...................................................................................... 24
3.3.3 Giroscopio ed Accelerometro...................................................... 25
3.3.4 Digital Motion Processor ........................................................... 27
3.3.5 Il Clocking .................................................................................. 28
4 La Libreria MPU6050....................................................................................... 29
4.1 Initialize................................................................................................. 29
4.2 SetClockSource...................................................................................... 30
4.3 setFullScaleGyroRange......................................................................... 31
4.4 SetFullSCaleAccelRange....................................................................... 31

6. 4.5 GetMotion6 ............................................................................................ 32
4.6 MeanValues ........................................................................................... 33
4.7 SetXAccelOffset ..................................................................................... 33
4.8 OffsetCalc............................................................................................... 34
5 Codice finale...................................................................................................... 37
5.1 Le Macro ................................................................................................ 37
5.2 Variabili globali ..................................................................................... 39
5.3 Catch_ir.................................................................................................. 42
5.4 Check...................................................................................................... 44
5.5 Il Setup................................................................................................... 45
5.6 Loop........................................................................................................ 46
5.6.1 PID Roll e Pitch.......................................................................... 47
5.6.2 PID Yaw ..................................................................................... 48
5.6.3 Calcolo delle velocità.................................................................. 49
6 Considerazioni sull’efficienza........................................................................... 52
7 Sitografia e Bibliografia.................................................................................... 56

7. 1

8. 2
1 Introduzione
Questo progetto ha come obiettivo l’assemblaggio e lo sviluppo del controllo della
stabilità di un quadricottero, utilizzando un giroscopio e un accelerometro a tre
assi, l’MPU-6050. Dati gli output dei sensori disponibili, un filtro di Kalman e
un filtro complementare sono stati implementati per ottenere gli angoli di Eu-
lero del drone, poi utilizzati come input per i controllori PID.
Il microcontrollore utilizzato è un STM32L053R8, le cui caratteristiche princi-
pali sono riportate in seguito:
• CPU ARM 32-bit Cortex-M0+;
• 32 MHz di frequenza massima della CPU;
• Bassa tensione di funzionamento (dagli 1.8V fino ai 3.6V);
• 64 KB di memoria Flash;
• 8 KB di memoria RAM [1].
Le schede STM32 appartengono alla famiglia dei microcontrollori di STMicroe-
lectronics, azienda franco-italiana con sede a Ginevra, leader nel settore di pro-
duzione di componenti elettronici a semiconduttore. Sono particolarmente indi-
cate per applicazioni/progetti sia a scopo d’insegnamento, sia in ambito indu-
striale, data la enorme offerta in termini di prestazioni e prezzi.
Per la programmazione del microcontrollore è stata utilizzata Mbed1: una piat-
taforma online che agisce da ambiente di sviluppo, permettendo di scrivere co-
dice, salvarlo all’interno di un cloud dedicato e compilarlo (tramite il cross-com-
piling integrato in Mbed).
In questo ambiente di sviluppo è presente inoltre la libreria “mbed.h”, creata
dagli sviluppatori ufficiali e contenente le classi principali e, in linea di mas-
sima, più utilizzate dai consumatori:
• AnalogIn, per la gestione dei segnali analogici che arrivano ai pin della
1 https://www.mbed.com/

9. 3
scheda
• AnalogOut, per i segnali analogici inviati dalla scheda
• DigitalIn, per gestire i segnali digitali in ingresso
• DigitalOut, per gestire i segnali digitali in uscita
• InterruptIn, per la gestione di un interrupt, ovvero un evento che con-
sente l’interruzione di un processo qualora si verifichi un determinato
evento definito dall’utente
• PwmOut, per inviare da un pin digitale un segnale PWM
• Serial, grazie alla quale si riesce a configurare la scheda programmabile
per la comunicazione seriale di tipo RS232 con altri dispositivi
• Timer, che inizializza un semplice conteggio temporale
• I2C, libreria che permette di implementare l’omonimo protocollo. Questa
verrà presentata successivamente, con un maggior grado di dettaglio e
precisione.
Utilizzando l’ambiente ST-Nucleo, insieme a mbed, le librerie a disposizione
sono molto limitate e la qualità di quelle presenti è discutibile, essendo spesso
costruite da non professionisti. Per questo si è scelto di scrivere autonomamente
gran parte del codice. Il codice sorgente dell’intero progetto è disponibile su Gi-
tHub2.
Per iniziare, si è rivelata necessaria la scrittura per intero di una libreria I2C,
più avanzata di quella fornita di default da mbed, seguita dalla libreria MPU,
per l’interfacciamento con il relativo sensore. Entrambe sono state prodotte
prendendo ispirazione dalla famosa libreria per Arduino I2Cdev3, di Jeff Row-
berg.
La configurazione dell’MPU, ha preso molto del tempo dedicato all’intero pro-
getto: a partire dal interfacciamento con il microcontrollore, tutto effettuato
2 https://github.com/gulbo/dronone.git
3 https://www.i2cdevlib.com/

10. 4
senza librerie di terzi, passando poi per il settaggio degli offsets (che permettono
la taratura del sensore), per la quale è stata costruita una sofisticata funzione
ad hoc, fino all’utilizzo del DMP4, il processore integrato dell’MPU. L’MPU e
tutti i suoi componenti verranno presentati nel capitolo 3.3.
La configurazione degli ESC5, dei complicati dispositivi elettronici con il compito
di regolare la velocità di rotazione di un motore di tipo brushless in funzione di
un segnale di tipo PWM fornito in ingresso, è stata svolta tramite un’apposita
funzione. Verrà esposta nel capitolo 3.1, insieme alle semplici funzioni che sono
state scritte per l’effettivo utilizzo in runtime dei motori.
Per il calcolo degli angoli, avendo a disposizione accelerazione inerziale e velo-
cità angolare dall’MPU, sono stati impiegati sia un filtro complementare che un
filtro di Kalman. L’impiego del DMP per il calcolo degli angoli è stato scartato
dopo diversi insuccessi, ripiegando appunto sull’utilizzo dei filtri. In seguito sono
state fatte delle analisi per verificare l’effettiva superiorità del filtro di Kalman
sul complementare, nella particolare fattispecie del controllo della stabilità del
drone. È risultato che, a fronte di piccole inclinazioni (inferiori ai 30 gradi), il
filtro complementare sia sufficiente per ottenere un buon risultato.
Infine, gli angoli ottenuti sono stati utilizzati all’interno di due controllori PID,
per il controllo degli assi X e Y, mentre un controllore solo proporzionale con-
trolla l’imbardata (asse Z).
Per il controllo remoto del drone è stato utilizzato un sensore infrarossi, che
quindi ha richiesto la relativa libreria, abbinato sia ad un telecomando IR che
ad un’applicazione Android, limitata ai telefoni con emettitore IR.
L’elaborato, scritto da Andrea Gulberti e Simone Fini, è stato suddiviso in due
parti, cercando di separare la parte di assemblaggio e studio dell’hardware dal
puro sviluppo del software.
La prima parte, presentata da Fini, tratta quindi:
4 Digital Motion Processor
5 Electronic Speed Control

11. 5
• il microcontrollore ST-Nucleo
• gli ESC e i motori utilizzati, da un punto di vista tecnico-elettronico
• il ricevitore infrarossi e il protocollo NEC
• MPU-6050, accelerometro e giroscopio
• i filtri complementari e di Kalman, con le considerazioni sul loro impiego
• l’applicazione Android per il controllo del drone
La presente invece si concentrerà sulle librerie utilizzate per il controllo dei mo-
tori, per il ricevitore infrarossi, per la comunicazione con l’MPU-6050 in I2C e
infine il codice principale caricato sul microcontrollore, con la descrizione detta-
gliata del codice C++, linguaggio utilizzato per ogni funzione esposta.
Mentre per molti degli argomenti trattati in questo elaborato ne verranno ri-
prese le nozioni fondamentali, senza rimandare quindi alla prima parte, ciò non
avverrà per i filtri complementare e di Kalman: essi infatti rappresentano un
intero capitolo, il 2 della tesi di Fini, e quindi vi si rimanda il lettore per even-
tuali approfondimenti.
Figura 1: il drone

12. 6
2 Il protocollo I2C
Il bus I2C67 (da Inter Integrated Circuit) è un sistema di comunicazione seriale
bifilare tra circuiti integrati. Disegnato da Philips all’inizio degli anni ‘80 per
facilitare la comunicazione tra componenti, ha subito diversi aggiornamenti fino
al 2006, quanto è stato abbandonato da Philips in favore di NXP. Da quella data
è possibile utilizzare I2C liberamente. La versione originale del protocollo sup-
portava un massimo di 100kbit al secondo in standard mode, per applicazioni
quindi che non richiedessero trasmissioni veloci. È disponibile inoltre il fast
mode per trasferimenti fino a 400kbit/s. Dal 1998 è disponibile lo standard HS
I2C8, o high speed mode, per comunicazioni fino a 3.4Mbit al secondo. Ovvia-
mente tutti i dispositivi della rete devono essere highspeed-enabled per poter
usufruire del nuovo protocollo. L’MPU 6050 supporta al massimo il fast mode,
ma in questo progetto (e di conseguenza nelle librerie allegate) non sono stati
trattati i registri per la configurazione di I2C del sensore. Di conseguenza viene
utilizzato lo standard mode, non ritendendo particolarmente vantaggiosa una
comunicazione più veloce.
In questa tesi non viene trattata l’implementazione hardware di I2C, data per
scontata. E’ comunque facilmente reperibile su internet [2] [3] e ai fini del pro-
getto non è indispensabile. Come si vedrà nel seguente paragrafo, e si è già ac-
cennato nell’introduzione, il protocollo è implementato da una libreria integrata
in mbed, che si chiama anch’essa i2c.
6 http://www.nxp.com/documents/user_manual/UM10204.pdf
7 https://www.i2c-bus.org/
8 https://www.i2c-bus.org/highspeed/

13. 7
2.1 La libreria integrata i2c
I2c_hw, interamente scritta dai canditati, è liberamente ispirata alla famosis-
sima i2dev9 di Jeff Rowberg, utilizzata in centinaia di applicazioni in ambiente
Arduinio. Purtroppo i2cdev, al momento della scrittura di questa tesi, non è di-
sponibile in mbed. Tuttavia, sul sito ufficiale della libreria, quest’ultimo è pre-
sente (insieme a stm32) tra le upcoming platforms.
I2cdev si appoggia alla libreria wire10 di Arduino, che rappresenta la libreria
integrata per l’utilizzo di I2C su tale piattaforma. In mbed la corrispettiva libre-
ria si chiama i2c, come il protocollo stesso. L’intera documentazione della libre-
ria i2c non è inclusa in questa tesi, ma è facilmente reperibile sul sito ufficiale
[4]. Qui verranno esposti solo i due metodi principali, utilizzati poi in i2c_hw:
read() e write().
È molto importante notare che questa libreria richiede come parametri indirizzi
a 8 bit. Essendo gli indirizzi di I2C a 7 bit, prima di passarli come parametri
vanno sempre traslati a sinistra di un bit, in modo da lasciare il bit meno signi-
ficativo libero. Quest’ultimo, per come è definito I2C, andrà a determinare se si
tratta di un’operazione di scrittura (0) o di lettura (1).
Figura 2: Schema libreria i2c
9 https://www.i2cdevlib.com/
10 https://www.arduino.cc/en/Reference/Wire
i2c
read write

14. 8
Read
Write
Si può facilmente notare quanto sia limitante utilizzare questa libreria per ap-
plicazioni non puramente elementari: permette infatti di leggere o scrivere una
generica sequenza di bit, senza presentare quindi il concetto di registro interno
a uno slave. Per questo si è scelto di costruire i2c_hw.
2.2 La libreria i2c_hw
I2c_hw possiede quattro metodi di scrittura e quattro di lettura. Ogni metodo si
differenzia dall’altro unicamente per la mole di dati che è in grado di gestire: in
ordine di grandezza un bit, più bits, un byte, più bytes. In realtà, tuttavia, i
primi tre metodi derivano dal metodo più grande, writeBytes (o readBytes nel
caso della lettura), limitandosi a scartare la parte in eccesso fino ad arrivare alla

15. 9
dimensione voluta. Per questo motivo si riporta e si analizza unicamente questi
ultimi due.
Figura 3: Schema libreria i2c_hw
2.2.1 WriteBytes
Prende come parametri l’indirizzo di un dispositivo devAddr, l’indirizzo di un
registro a 8 bit regAddr, il numero di bytes da scrivere length e l’array di bytes
da scrivere data.
Per prima cosa unisce in un array temp il numero di registro (in prima posizione)
e i bytes da scrivere, dopo di che lancia la funzione write della libreria i2c, pas-
sando come parametro l’indirizzo del dispositivo traslato di un bit. Il motivo di
tale traslazione è stato esposto in precendenza.
i2c_hw
writeBytes
writeWord
writeBit
writeBits
readBytes
readWord
readBit
readBits
constructor

16. 10
2.2.2 ReadBytes
Prende come parametri l’indirizzo di un dispositivo devAddr, l’indirizzo di un
registro a 8 bit regAddr, il numero di bytes da leggere length e l’array di bytes
data dove memorizzare i dati letti. Dopo aver allocato in memoria un array di
bytes readData di dimensione length, si avvia un’operazione i2c di scrittura,
avente come unica informazione il registro da cui leggere regAddr. A questo
punto con un read verrà letta e memorizzata in readData la stringa ricevuta.
Basta infine copiare readData in data.

17. 11
3 I componenti
In questo capitolo vengono ripresi alcuni dei componenti già esposti nella prima
parte, ma ora integrati con le librerie create e con la relativa spiegazione, in
modo da rendere il codice ampliabile e riutilizzabile.
3.1 Gli E.S.C.
Figura 4: gli ESC collegati al microcontrollore
Gli ESC, acronimo di Electronic Speed Control, sono dei complicati dispositivi
elettronici che hanno il compito di regolare la velocità di rotazione di un motore
di tipo brushless, collegato attraverso tre morsetti per la corrente trifase. Sono
in grado quindi, attraverso un microprocessore interno, di convertire un segnale
di tipo PWM in ingresso in una corrente trifase proporzionale allo stesso valore
d’entrata, che andrà a variare il numero di giri al minuto del motore brushless.
Infine, oltre alla trifase per i motori, forniscono una tensione regolata a 5V che

18. 12
può essere utilizzata per ulteriori scopi. Nel corso del progetto si è provato ad
alimentare StNucleo con tale tensione, ma il regolatore degli ESC in dotazione
si è rivelato di bassa qualità e spesso ha causato il riavvio della scheda. Quindi,
come estensivamente spiegato nella tesi di Fini, si è optato per utilizzare il re-
golatore interno al microcontrollore, fornendogli direttamente i 12V delle batte-
rie, piuttosto che il regolatore degli ESC.
Figura 5: schema di un ESC
La parte fondamentale per quanto riguarda gli ESC risiede però nella loro pro-
grammazione: ogni tipologia, marca o modello è programmabile. Per inciso esi-
stono tre diverse modalità di programmazione: i più costosi vengono spesso for-
niti insieme ad un programma Windows/OS compatibile e i parametri possono
essere impostati direttamente da lì. Ci sono poi gli ESC dotati di una particolare
tastiera, che va collegata per entrare in modalità programmazione. Infine, i più
semplici ma anche i più diffusi (e utilizzati in questo progetto), che presentano
una particolare sequenza di programmazione che si interfaccia verso il mondo
esterno attraverso una serie di suoni emessi dai motori collegati.
In questa sezione dapprima verrà spiegata la procedura per programmare gli
ESC utilizzati nel corso del progetto, procedura che varia a seconda della marca
e del numero di funzioni degli stessi. Dopo di che verrà presentata una funzione

19. 13
appositamente sviluppata per permettere un rapido settaggio degli ESC tramite
la scheda ST-Nucleo. Infine verrà esposta la libreria ESC costruita per la ge-
stione dei motori in runtime.
Per la spiegazione delle diverse funzionalità presenti negli ESC e per ottenere
informazioni sulle loro caratteristiche fisiche, come la risposta dei motori in base
al PWM fornito e la relativa portanza generata, si rimanda al capitolo 1.3 della
tesi di Fini.
3.1.1 La programmazione
Per programmare gli E.S.C. (guida ufficiale [5]) innanzitutto si deve entrare in
modalità programmazione, impostando il throttle del segnale PWM al massimo
(ovvero un valore del duty cycle che, come verrà descritto in seguito, è definito
dall’utente), accendendo il sistema ed aspettando, con questa impostazione,
circa sette secondi, dopo i quali verrà emesso un suono speciale.
Successivamente, per selezionare il parametro che si vuole andare a modificare,
il sistema emetterà 8 diversi suoni in sequenza, ognuno dei quali corrisponde ad
una particolare funzione:
Suono Modalità
* Modalità break
** Tipo di batteria
*** Modalità di protezione alla bassa tensione
****
Modalità di protezione all’abbassamento della tensione della
batteria
_ Modalità di startup
_* Timing
** Impostazione di tutti i parametri ai valori di default
__ Uscita dalla modalità di programmazione

20. 14
Tabella 1: corrispondenza tra suoni e funzioni. Con * si intende suono corto, con _ suono
lungo
In corrispondenza della funzione che interessa (e quindi del relativo suono) si
deve settare il throttle al minimo entro tre secondi, in modo tale da accedere a
tutte le varie possibilità di settaggio per quella determinata funzione che, anche
in questo caso, verranno identificate attraverso dei suoni:
Modalità * ** ***
Brake Disabled Enabled
Tipo di batteria Li-xx Ni-xx
Modalità di protezione alla bassa tensione
Soft Cut-
Off
Cut-Off
Modalità di protezione all’abbassamento
della tensione della batteria
Low Medium High
Modalità di startup Normal Soft
Super-
Soft
Timing Low Medium High
Tabella 2: corrispondenza tra suoni e valori possibili per ogni modalità. Con * si intende
suono corto
Per scegliere un determinato valore, si re-imposta il valore del throttle al mas-
simo, fino a quando non verrà emesso un tono speciale: in questo modo si ha una
conferma che il valore scelto è stato applicato.
A questo punto, senza modificare il throttle, si viene riportati nel menu princi-
pale e si possono andare a modificare altre modalità; altrimenti se si porta il
throttle al valore minimo, si esce dalla modalità di programmazione e gli ESC
saranno pronti a comandare i motori.
Prima di fare tutto ciò, però, siccome il controllo della programmazione avviene
essenzialmente alternando il valore del throttle da massimo a minimo e vice-
versa, si devono impostare questi due valori, o più tecnicamente, fornire al mi-
croprocessore il full scale range del segnale di comando. Per fare ciò si deve im-
postare, sul microcontrolore, il PWM al valore che si vuole memorizzare come

21. 15
massimo, accendere gli ESC ed aspettare circa due secondi, dopo i quali verrà
emesso un doppio suono corto (**), che sarà la conferma dell’avvenuta memoriz-
zazione. Fatto ciò si andrà a abbassare il livello d’ingresso, in modo da salvare
il valore minimo; in questo caso però prima verranno emessi una serie di suoni
corti (*) in base alla tipologia di batteria che alimenta il sistema, poi un suono
lungo (_) che avverte l’utilizzatore che il range di valori è stato correttamente
impostato.
3.1.2 Codice Utilizzato per la programmazione
Come già accennato, si è sviluppato un semplice programma “universale” per la
programmazione degli ESC, in modo tale da poter sia settare i valori massimi e
minimi del throttle, che consentire di andare a scorrere tutti i menù degli ESC
con facilità. Una volta caricato il codice in questione sul microcontrollore, ba-
sterà cliccare il bottone presente su ST-Nucleo per selezionare il menù o la fun-
zione di cui in quel momento l’ESC sta riproducendo il relativo suono.
Inoltre, all’avvio degli ESC, il codice provvede anche a settare i valori massimi
e minimi accettati dagli stessi ESC, quasi completamente in automatico.
Di seguito le istruzioni per l’utilizzo e lo schema di set-up del collegamento tra
ST-Nucleo e gli ESC.
Figura 6: schema per la configurazione degli ESC
Dopo aver effettuato i collegamenti sopraindicati, per prima cosa andrà acceso
il microcontrollore, dopo di che gli ESC. A questo punto in automatico alla loro

22. 16
accensione verrà impostato il valore massimo del PWM, gli ESC emetteranno
quindi un suono (**, come sopra esposto, dopo tre secondi), e a questo punto si
dovrà cliccare il bottone della scheda per impostare anche il valore minimo. Un
altro suono segnalerà l’avvenuta ricezione dei massimi e dei minimi. Ora, ri-
cliccando il bottone sulla scheda, il PWM tornerà al massimo e dopo sette secondi
inizieranno i suoni che corrispondono al menù di configurazione, già esposti pre-
cedentemente. Per selezionare una funzione o un valore basterà cliccare sempre
il bottone della scheda.
Il codice utilizzato è il seguente.
Come prima cosa si imposta il segnale PWM a livello logico uno, e fino a che il
pin D2 collegato all’ESC non riceve un impulso che implichi l’accensione del si-
stema, si aspetta, in modo tale da essere sicuri che il primo segnale che verrà
mandato all’ESC sia alto. Il pin D2 non è altro che il regolatore 5V degli ESC,
che quindi qui viene utilizzato come segnale dell’accensione degli ESC.

23. 17
A questo punto la presenza di un interrupt sullo user_button fa in modo (aggan-
ciando il segnale alla funzione invert) di cambiare il valore della variabile state
ogni qualvolta venga premuto lo il pulsante della scheda. Infine si entra in un
ciclo infinito, in cui in base al valore booleano si imposta il PWM alto o basso.
In questo modo, premendo semplicemente un bottone si è in grado di alternare
i valori del throttle senza passare per stati intermedi (non “1” o “0” logici, ma
valori transitori), che potrebbero portare a degli errori nelle impostazioni di con-
figurazione degli ESC.
3.1.3 Libreria “esc”
Di seguito viene riportata l’intera libreria indispensabile per l’utilizzo degli ESC,
e quindi il controllo dei motori. Questa libreria è stata interamente scritta dai
canditati. Consta di un metodo costruttore e due metodi per settare il valore del
throttle dei motori, uno con solo un parametro ed uno con 4 parametri.
Figura 7: Schema libreria esc
ESC
setThrottle setThrottle
constructor

24. 18
Costruttore
Con il codice appena esposto si genera un oggetto di tipo ESC e si imposta il
periodo del PWM, cioè il valore del throttle, al minimo (per il funzionamento del
drone si è scelto 1100us, ovvero 1.1ms).
SetThrottle con un parametro
Il primo metodo setThrottle imposta (ammesso che il valore passatogli come pa-
rametro sia adeguato, ovvero compreso tra 0 ed 1) la durata dell’impulso del
PWM, mappandola però tra il range settato. In questo caso, ad ogni motore verrà
data la stessa velocità, in quanto tutti gli ESC sono comandati dallo stesso va-
lore di throttle.

25. 19
SetThrottle con quattro parametri
Il secondo metodo setThrottle funziona in maniera molto simile al precedente,
tranne per il fatto che è possibile impostare un diverso valore di throttle per ogni
ESC; in tal modo, ogni motore potrà avere una velocità diversa dagli altri tre.
Questo sarà il metodo da utilizzare per l’implementazione del sistema atto a
garantire il controllo automatico della stabilità.
3.2 Il Ricevitore ad Infrarossi (IR)
La comunicazione infrarossa tra due dispositivi elettronici è una tra le più sem-
plici ed economiche alternative per l’utilizzo della trasmissione wireless di dati
digitali (binari). Bastano infatti soltanto due componenti:
• un trasmettitore, ovvero un led che funziona come un normale diodo ad
emissione luminosa, tranne per il fatto che l’onda emanata è invisibile
all’occhio umano;
• un ricevitore, ovvero un foto-sensore che invia l’informazione estrapolata
dalla sequenza luminosa al dispositivo a cui è collegato.
Il funzionamento della trasmissione e il protocollo NEC, utilizzato nella libreria
ReceiverIR che verrà esposta in seguito, sono stati trattati ampiamente nella
tesi di Fini, capitolo 1.4. Qui, come le altre volte, verrà invece analizzato il codice
utilizzato nel progetto.

26. 20
3.2.1 La Libreria “ReceiverIR”
ReceiverIR [6], scritta da Shinichiro Nakamura, è una delle librerie più utiliz-
zate in mbed per la trasmissione infrarossi ed è stata importata nel progetto
senza alcuna modifica.
Di seguito vengono riportati i metodi utilizzati, con una breve spiegazione.
Figura 8: Schema libreria ReceiverIR
getState
Metodo utilizzato per determinare lo stato, definito come un’enumerazione, del
ricevitore: i tre possibili valori sono:
• Idle: ovvero “inattivo”, corrispondente al valore 0;
• Receiving: ovvero “in corso di ricezione”, corrispondente al valore 1;
• Receiver: ovvero “ricevuto”, corrispondente al valore 2.
LOCK e UNLOCK sono metodi che bloccano o sbloccano la ricezione di ulteriori
ReceiverIR
getState getData
receive
decode_data
constructor

27. 21
byte tramite infrarossi.
getData
Metodo che salva all’interno di un array di byte, che deve essere fornito come
parametro, un pacchetto di dati ricevuto. Come prima cosa, viene bloccata la
ricezione e controllato che la dimensione dell’array sia adatta alla mole dell’in-
formazione; se lo è, viene calcolato il numero di bit e dei byte della stessa e,
attraverso un ciclo for, memorizzata. Il sensore poi viene resettato a riabilitato
alla ricezione.
receive

28. 22
Estensione del metodo getData; in questo caso, prima di salvare il dato tra-
smesso, viene fatto un controllo temporale: infatti se la ricezione non avviene
entro un tempo limite, fornito come parametro, la stessa viene saltata.
decode_data
Metodo utilizzato per la codifica dei dati ricevuti; come prima cosa viene utiliz-
zato il metodo receive, e se l’array viene riempito, ovvero se è stato captato e
salvato qualcosa, viene calcolato il numero di byte del dato. Attraverso un ciclo
for infine, i vari byte vengono uniti, traslando ogni volta il risultato parziale di
8 bit e facendo l’and logico con l’elemento [i] dell’array.

29. 23
3.3 MPU6050
Si vuole introdurre l’argomento con una definizione: un sistema di navigazione
inerziale è un ausilio alla navigazione di un mezzo, è composto da computer e
sensori con il fine di stimare la posizione, la velocità e l’orientamento della vet-
tura senza la necessità di riferimenti esterni [7]. Il principale componente di tale
sistema è l’IMU (i.e. Inertial Measurement Unit), un dispositivo elettronico in
grado di misurare l’accelerazione inerziale e la velocità angolare di una massa.
A seconda dei casi, può anche essere capace di calcolare il campo magnetico in
cui il corpo è immerso e la sua altitudine.
I sensori IMU sono diventati ai giorni d’oggi uno dei congegni più impiegati in
ogni sorta di sistema elettronico: dagli smartphones, agli APR (Aeromobili a Pi-
lotaggio Remoto, o UAVs ), ai robot autostabilizzanti.
Per effettuare tutte queste misure, un IMU è composto da più parti:
• uno o più accelerometri;
• uno o più giroscopi;
• un magnetometro (o bussola);
• un altimetro
Si veda ora più nel dettaglio il funzionamento del sensore utilizzato nel corso del
progetto.
3.3.1 Il Sensore
Il dispositivo utilizzato in questa tesi è l’InvenSense MPU-6050 [8] che contiene,
riuniti in un singolo circuito integrato, un accelerometro e un giroscopio, insieme
ad altri elementi che verranno visti in seguito. È un sensore a sei assi, o con sei
gradi di libertà, che quindi fornisce sei valori come output. Impiega il protocollo
I2C per la comunicazione con l’esterno. Si tratta di un IMU economico ma piut-
tosto affidabile e preciso, ed è tra i più diffusi per progetti di questo tipo. In

30. 24
questa tesi viene utilizzato con il modulo GY-521, così da avere l’MPU-6050 già
pronto per l’interfacciamento con la scheda programmabile. Inoltre nel modulo
è presente un regolatore di tensione, che permette l’alimentazione da 3.3V fino
a 5V. Oltre ad accelerometro e giroscopio, l’MPU contiene anche un Digital Mo-
tion Processor (DMP), di cui si parlerà in seguito, una FIFO e un sensore di
temperatura. Tutte le informazioni qui riportate sono state recuperate dal da-
tasheet disponibile presso il sito della InvenSense [9].
Tramite I2C è possibile configurare molte impostazioni dell’MPU, scrivendo sui
suoi registri. Molti di questi però non sono ben documentati e quindi non è asso-
lutamente scontato capire il loro utilizzo. La mappa dei registri con le relative
descrizioni è disponibile presso il sito della InvenSense [10]. Va segnalato tutta-
via che l’utilizzo dei registri non è banale, per questo è stata costruita una libre-
ria ad hoc che implementi le funzionalità più importanti, che verrà esposta suc-
cessivamente.
3.3.2 La FIFO
È possibile impostare l’MPU affinché scriva i dati dell’accelerometro e del giro-
scopio nella FIFO, oppure nei registri dedicati, perdendo però così la sicurezza
di leggere tutti i dati generati dal sensore. È tuttavia possibile impostare un
interrupt (uscita INT) che segnali la disponibilità di nuovi dati nei registri, in
modo tale da andare a leggerli prima che vengano sovrascritti.
La FIFO ha una dimensione di 1024 bytes ed è accessibile, ovviamente, tramite
Figura 13: componenti dell'MPU6050

31. 25
interfaccia seriale. I registri di configurazione della FIFO determinano i dati che
vengono scritti al suo interno (giroscopio, accelerometro, temperatura, DMP).
Una comodità derivante dall’utilizzo della FIFO, oltre che, come già detto, la
certezza di non perdere dati, è il burst read. Questa funzione permette di leggere
tutti i dati della coda in un breve lasso di tempo, fino a svuotarla, per poi aspet-
tare che si torni a riempire. Così facendo è possibile lasciare il processore della
scheda logica in low-power mode mentre l’MPU riempie la FIFO.
3.3.3 Giroscopio ed Accelerometro
I campioni forniti da giroscopio ed accelerometro sono raw, grezzi: vanno elabo-
rati (con una semplice divisione), dopo essere stati prelevati dai registri o dalla
FIFO, per ottenere le velocità angolari e le accelerazioni. Il divisore dipende
dalla sensibilità impostata, come si vedrà in seguito. Inoltre, i dati raw sono
rappresentati da variabili di 2 byte per ogni asse, essendo prodotti dai sei con-
vertitori AD a 16 bit presenti sull’MPU, che permettono il campionamento si-
multaneo di accelerometro e giroscopio su ogni asse. Tuttavia, sono memorizzati
in registri da un byte e, per questo motivo, dopo aver letto i registri bisognerà
eseguire una concatenazione dei bit: si veda a riguardo il capitolo 0.
A questo punto, dopo aver prelevato i dati raw, un calcolo va eseguito per con-
vertire velocità e accelerazioni in angoli. Dal punto di vista matematico/fisico
una integrazione (singola o doppia) sarebbe sufficiente, ma la quantità di ru-
more presente nei dati dell’MPU impedirebbe di ottenere dei risultati accetta-
bili. Per risolvere ciò sono stati utilizzati due filtri: uno complementare e l’altro
di Kalman, che sono stati ampiamente trattati nella prima parte dell’esposto, al
capitolo 2.
Il giroscopio ha una frequenza di campionamento configurabile da 4Hz a 8kHz
(standard 8kHz) e un filtro passa-basso programmabile da 5Hz a 256Hz. L’acce-
lerometro ha una frequenza di campionamento configurabile da 4Hz a 1kHz
(standard 1kHz), con un filtro passa-basso da 5Hz a 260Hz. I filtri low pass non
sono stati trattati nella libreria utilizzata per il progetto.

32. 26
L’interfaccia I2C è configurabile in fast-mode per lavorare a 400kHz e in stan-
dard-mode a 100kHz. Considerando che ogni byte trasmesso richiede 9 clocks (8
bits più l’acknowledge), alla frequenza standard di 100kHz si trasmettono i 12
bytes di campioni con una frequenza di circa 900Hz (considerando che si prele-
vino sia i dati dell’accelerometro che quelli del giroscopio, senza però la tempe-
ratura). Questo è un limite teorico che non viene mai raggiunto, in quanto in
ogni trasmissione I2C va considerato anche l’invio dell’indirizzo dello slave con
cui comunicare e del registro da cui leggere/scrivere (ulteriori due bytes).
Sia il giroscopio che l’accelerometro sono configurabili su diversi livelli di sensi-
bilità, come riportato nella tabella sottostante. Ovviamente a seconda della sen-
sibilità impostata ogni LSB assume un peso diverso.
GYRO FULL
SCALE RANGE
(°/SEC)
GYRO SENSIBI-
LITY (LSB/°/SEC)
ACCEL FULL
SCALE RANGE
(G)
ACCEL SENSI-
BILITY (LSB/G)
±250 131 ±2 16384
±500 65.5 ±4 8192
±1000 32.8 ±8 4096
±2000 16.4 ±16 2048
Tabella 3: Full Scale Range di accelerometro e giroscopio
Con queste informazioni è possibile trasformare i dati raw forniti dai sensori in
velocità angolari e accelerazioni. Ad esempio un valore raw di 2048 rappresenta
1g con la sensibilità minima (±16g), 1/2 g con la sensibilità ±8g, 1/4 g con ±4g e
infine 1/8 g con la sensibilità massima (±2g).
Come già sottolineato, l’ulteriore passaggio per arrivare agli angoli di Eulero ha
richiesto un ulteriore studio, che per la sua complessità ha richiesto un capitolo
interamente dedicato nella tesi di Fini: il capitolo 2.

33. 27
3.3.4 Digital Motion Processor
Il DMP (Digital Motion Processor), processore integrato nell’MPU, è in grado di
elaborare tramite avanzati algoritmi di integrazione i dati grezzi a sei assi for-
niti dai sensori. La sua implementazione è però tenuta segreta dalla InvenSense
e, sebbene per Arduino si trovino in abbondanza sul web librerie che lo trattino,
per ST-Nucleo la situazione è più complicata.
Nel corso del progetto si è provato a fare un porting della libreria più famosa
disponibile in ambiente Arduino: MPU6050 e I2Cdev di Jeff Rowberg, disponi-
bile su GitHub [11]. Purtroppo, sebbene funzioni in maniera soddisfacente sotto
gran parte degli aspetti, non si può dire lo stesso per la parte di implementazione
del DMP. Sul web sono disponibili molti esempi [12] di bug rilevati nella libreria
quando si utilizza il DMP, e non ci sono modi per capire come questo processore
operi e quindi correggerli. Per questo motivo, utilizzarlo per droni e altre appli-
cazioni di questo genere non è consigliato. In caso di freeze e crash inaspettati
potrebbe diventare veramente pericoloso.
Nonostante ciò, si è cercato di utilizzare il DMP leggendo l’output dalla FIFO.
Si è avuto modo di notare l’incredibile precisione degli algoritmi che esegue, gli
angoli di Eulero ottenuti risultavano molto più precisi di tutti i filtri fino ad
allora utilizzati. Interessante notare che il DMP è in grado di calcolare anche la
rotazione sull’asse z (yaw o imbardata), cosa impossibile da fare con i filtri com-
plementari o di Kalman. Purtroppo dopo pochi test è subito emerso un problema:
l’MPU dopo qualche minuto andava in freeze, senza apparente motivo. Si è cer-
cato di capire se potesse essere un problema di overflow della FIFO, senza risul-
tato.
Infine, una funzione integrata del DMP permette l’autocalibrazione del girosco-
pio, dopo 10 secondi di stazionarietà. Si è cercato allora di calcolare e inserire
manualmente gli offsets per il DMP, ritendendo decisamente lunga l’attesa di
10 secondi ad ogni riavvio della scheda. Purtroppo non è affatto chiaro come
funzionino gli offsets del DMP (e non sono gli stessi offsets presenti per il giro-
scopio e l’accelerometro): la documentazione relativa è molto approssimativa se

34. 28
non inesistente. Potrebbe essere interessante un approfondimento a riguardo,
deviando l’output del DMP sui registri dedicati invece che sulla FIFO: si sem-
plificherebbe così in maniera consistente l’operazione di lettura dei dati (che al-
trimenti richiederebbe anche la gestione dell’interrupt) e forse si risolverebbe il
problema del freeze. In ogni caso, premettendo che bisognerebbe verificare l’af-
fidabilità dell’MPU con DMP attivo (anche dopo aver risolto i problemi di freeze
riscontrati), diversi utenti su internet hanno segnalato una sostanziale indiffe-
renza nel comportamento dei loro velivoli con l’utilizzo del DMP o dei filtri [13].
Per questo motivo in questo progetto si è scelto di abbandonare il processore
integrato e proseguire utilizzando i filtri.
3.3.5 Il Clocking
L’MPU 6050 permette diverse sorgenti di clock, esterne o interne:
• un oscillatore interno da 8MHz
• uno degli oscillatori MEMS del giroscopio (asse x, y o z)
• una sorgente esterna da 32,768kHz (onda quadra)
• una sorgente esterna da 19,2MHz (onda quadra)
All’accensione il dispositivo usa l’oscillatore interno per operare, finché non
viene programmato diversamente. Nel datasheet del MPU è consigliato di sele-
zionare il giroscopio come fonte di clock, operazione che fornisce maggiore stabi-
lità . Tuttavia in casi in cui ad esempio il consumo di energia è il parametro
principale, è possibile disattivare i giroscopi e utilizzare l’oscillatore interno
come riferimento.

35. 29
4 La Libreria MPU6050
Qui si espone la libreria realizzata ai fini del progetto, non per intero ma solo
nelle funzioni che meritano maggiore attenzione. Va sottolineato che il codice è
stato ben commentato, quindi non dovrebbe essere difficile capire il loro funzio-
namento anche solo leggendo direttamente il codice sorgente. Qui verrà esposta
l’implementazione delle funzioni, senza però riportare i commenti introduttivi,
che sono sempre presenti nel codice prima di ogni metodo e che lo descrivono
sinteticamente. Ogni parola scritta in CAPSLOCK è definita (per il precompila-
tore) negli header della libreria.
Innanzitutto va segnalato che in questa libreria sono presenti due definizioni
importanti per il precompilatore: DEFAULT_OFFSETS e useDebugSerial. Il
primo verrà spiegato insieme con la funzione offsetCalc(), mentre il secondo
serve per attivare o disabilitare tutti i print presenti nella libreria. A differenza
che in altre situazioni, mantenere i print attivi in questa libreria non altera la
velocità del loop principale del drone (impostata a 250Hz).
4.1 Initialize
Dev’essere la prima ad essere lanciata, dopo ovviamente il costruttore dell’og-
getto MPU6050. Imposta come riferimento del clock il giroscopio, ottenendo così
maggiore stabilità, come già accennato. Configura poi la sensibilità del girosco-
pio a 250deg/sec e quella dell’accelerometro a ±4g. Infine disattiva lo stato di

36. 30
sleep, terminando l’inizializzazione.
4.2 SetClockSource
Tramite questa funzione (di cui é presente anche il relativo get) è possibile cam-
biare la sorgente di clock dell’MPU. Per fare ciò è stato sufficiente seguire le
indicazioni del datasheet, e quindi direttamente lanciare una funzione di scrit-
tura in I2C. Il parametro source deve corrispondere a una delle seguenti defini-
zioni:
SOURCE PARAMETER CLK_SEL CLOCK SOURCE
MPU6050_CLOCK_INTERNAL 0 Internal oscillator
MPU6050_CLOCK_PLL_XGYRO 1
PLL with X Gyro refer-
ence
MPU6050_CLOCK_PLL_YGYRO 2
PLL with Y Gyro refer-
ence
MPU6050_CLOCK_PLL_ZGYRO 3
PLL with Z Gyro refer-
ence
MPU6050_CLOCK_PLL_EXT32K 4
External 32.768kHz ref-
erence
MPU6050_CLOCK_PLL_EXT19M 5
External 19.2MHz refer-
ence
Tabella 4: corrispondenza tra parametri del codice e valori di clock

37. 31
4.3 setFullScaleGyroRange
Tramite questo metodo (di cui è presente anche il relativo get) si imposta la sen-
sibilità del giroscopio. Il parametro range deve assumere uno dei seguenti valori:
RANGE PARAMETER SENSITIVITY (DEG/SEC)
MPU6050_GYRO_FS_250 250
MPU6050_GYRO_FS_500 500
MPU6050_GYRO_FS_1000 1000
MPU6050_GYRO_FS_2000 2000
Tabella 5: corrispondenza tra parametri del codice e sensitività del giroscopio
4.4 SetFullSCaleAccelRange
Tramite questo metodo (di cui è presente anche il relativo get) si imposta la sen-
sibilità dell’accelerometro. Il parametro range deve assumere uno dei seguenti
valori:

38. 32
RANGE PARAMETER SENSITIVITY (G)
MPU6050_ACCEL_FS_2 2
MPU6050_ACCEL_FS_4 4
MPU6050_ACCEL_FS_8 8
MPU6050_ACCEL_FS_16 16
Tabella 6: corrispondenza tra parametri del codice e sensitività dell'accelerometro
4.5 GetMotion6
Questo metodo ritorna sei interi che rappresentano i dati raw forniti da accele-
rometro e giroscopio. Consiste in una semplice lettura di 14 bytes a partire dal
registro dell’asse x dell’accelerometro. Di questi, i primi 6 bytes rappresentano,
a due a due, i dati raw dell’accelerometro (assi x,y e z in quest’ordine). Dopo di
che vengono scartati 2 bytes e i successivi 6 rappresentano i dati raw del giro-
scopio. Per concatenare le coppie di bytes in un'unica variabile da 16 bit è ba-
stato traslare di 8 bit il primo byte e in seguito unirlo con il secondo byte con un
OR (il simbolo |).
Essendo questi dati grezzi, come già sottolineato, vanno elaborati prima di otte-
nere la velocità angolare e l’accelerazione. Questo va fatto in base alla sensibilità
impostata.

39. 33
4.6 MeanValues
Questa funzione è utilizzata solo all’interno di offsetCalc, ha unicamente lo scopo
di effettuare 1000 letture da giroscopio ed accelerometro e farne una media.
Getta via le prime 100 letture per una migliore accuratezza, assumendo che i
sensori possano ancora rilevare dell’inerzia dovuta a un precedente movimento.
4.7 SetXAccelOffset
Tramite questo metodo (di cui è presente anche il relativo get ed è disponibile in
diverse versioni per tutti gli assi dell’accelerometro del giroscopio, dove diventa
ad esempio setYGyroOffset) è possibile impostare gli offsets direttamente agendo
sui registri dell’MPU. Gli offsets sono dei valori che, al loro variare, fanno cam-
biare i valori letti dal giroscopio e dall’accelerometro. Agendo su di loro è quindi
possibile calibrare l’MPU per ottenere i valori desiderati, auspicabilmente pros-
simi allo zero in caso di stazionarietà. Essendo i registri dell’MPU da un byte, il

40. 34
parametro da 16 bit preso in input dalla funzione va diviso in due variabili da 8
bit (nel codice zeroToSeven e eightToFifteen) in modo da poter lanciare la fun-
zione writeBytes con le due variabili da un byte come parametro (“impacchet-
tate” in un array arr).
4.8 OffsetCalc

41. 35
L’esecuzione di questa funzione varia significativamente in base alla definizione
della macro DEFAULT_OFFSETS. In caso quest’ultima sia definita, il metodo
imposta con delle semplici scritture I2C gli offsets di default. Altrimenti, per
prima cosa verifica le impostazioni di sensibilità attuali dell’MPU, dopo di che
azzera gli offsets, in modo da operare su dati raw vergini. A questo punto viene
avviato un ciclo nel quale viene ogni volta calcolata la media dei valori letti (con
il metodo meanValues, descritto precedentemente) e viene sottratto agli offsets
un quarto del valore della media calcolata. In questo modo ad ogni ciclo gli off-
sets vengono incrementati (in modulo) proporzionalmente (fattore un quarto) al
valore medio dei dati letti. Facilmente si capisce che al tendere all’infinito del
numero di cicli, la media dei valori letti tende a zero. Nel caso dell’asse z dell’ac-
celerometro viene effettuata una leggera correzione: è sempre sottratto agli off-
sets un quarto della media dei valori letti, però non viene contata nella media
l’accelerazione di gravità. Così facendo la media dei dati raw dell’asse z non

42. 36
tende a zero all’aumentare dei cicli, ma tende al valore che è l’accelerazione di
gravità con la sensibilità selezionata. Il ciclo termina solo dopo che la media
calcolata per ogni asse sia minore di, rispettivamente, giro_deadzone e ac-
cel_deadzone (impostati di default a 1 e 8).
Questa è l’unica maniera per impostare degli offsets nell’MPU. Una soluzione
alternativa, sebbene meno elegante, potrebbe essere: calcolare durante il setup
(dopo la funzione initialize ovviamente) il valore medio di un migliaio di letture,
memorizzarlo in una variabile globale in ST-Nucleo, e ogni qual volta viene lan-
ciata una funzione di lettura dei dati raw (come ad esempio getMotion6, già trat-
tata) sottrarre ai dati letti la media. Durante questo progetto sono state provate
entrambe le modalità, cioè offsets integrati nell’MPU o offsets calcolati a run-
time, e non sono state notate significative differenze. Va tenuto conto, infatti,
che una media di valori non ragionevolmente prossima allo zero, come ad esem-
pio una media di 40, rimane lo stesso un’ottima approssimazione: perfino con la
sensibilità massima (131 LSB/deg/sec come da Tabella 5), 40 unità rappresen-
tano una velocità angolare di meno di un terzo di grado al secondo. È stato scelto
tuttavia lo stesso di implementare la calibrazione diretta dell’MPU, che garan-
tisce una media dei valori dell’accelerometro minore uguale a nove, e minore
uguale a due per i valori del giroscopio.

43. 37
5 Codice finale
In questo capitolo verrà esposto il codice sorgente che è stato caricato sul drone.
Qui verranno utilizzate tutte le librerie precedentemente analizzate.
Per comprendere questo capitolo finale è consigliato aver compreso in partico-
lare il capitolo 2 della prima parte, sui filtri software. Ovviamente anche la co-
noscenza di tutte le altre librerie è consigliata, tuttavia i2c_hw, mpu6050 e Re-
moteIR operano a un livello di astrazione inferiore e per questo, come per ogni
libreria ben costruita, è sufficiente conoscere i loro metodi di interfacciamento
con l’esterno, utilizzandoli a “scatola chiusa” senza ben conoscerne il vero e pro-
prio funzionamento.
5.1 Le Macro
Per prima cosa, di vitale importanza è capire il funzionamento della macro PC.
Essa infatti va definita quando il drone è connesso al pc, in modo da disattivare
diverse funzioni che potrebbero altrimenti essere pericolose (come ad esempio il
test dei motori in fase di setup, oppure con una semplice modifica si potrebbe
disattivare i motori completamente). Ha inoltre una funzione di debug: che de-
finita porta ad attivare tutti i print presenti nel codice, che permettono di avere
una visione più chiara del comportamento del drone.
Figura 9: La macro PC

44. 38
Altrettanto importante, tuttavia, è disabilitare la macro se il drone non è effet-
tivamente connesso via USB. Questo perché in tal caso i print, lasciati attivi,
andrebbero a rallentare rovinosamente il loop. Andando più nel dettaglio: il co-
dice è stato scritto in modo che la frequenza di esecuzione del loop sia fissata
250Hz. Per questo motivo, tutte le istruzioni che il processore si accolla vanno
eseguite entro il tempo limite di 4ms. Se questo non si verifica per un numero di
volte non trascurabile (il problema ovviamente non si pone se un ciclo sfora il
tempo massimo solo saltuariamente) il processo di integrazione, che utilizza il
periodo del loop come base su cui effettuare i calcoli, risulta compromesso.
Le stampe a terminale sono infatti risultate essere un problema non indiffe-
rente, in quanto capaci di occupare anche diverse decine di ms. Come prima so-
luzione si è pensato di alzare la frequenza di trasmissione dei dati sulla porta
seriale, da 9600 a 57.600 bits al secondo. Tuttavia il problema non è stato com-
pletamente risolto, in quanto certi print potevano risultare piuttosto lunghi:
stampare i sei dati raw ottenuti dall’MPU significa infatti inviare 12 byte (senza
contare la formattazione che richiede almeno un carattere di separazione tra un
dato l’altro), che a 57.600bits/sec richiede un tempo quasi di 2 ms, ovvero la metà
del tempo disponibile per eseguire tutto il codice.
A questo punto si è cercata allora un'altra soluzione, rappresentata da questa
parte di codice:
Figura 10: Loop count
Come si può facilmente dedurre, è stato creato un contatore, loop_count, che
conta i cicli e quindi solo una volta ogni 100 (2,5Hz) stampa, in questo caso, le
velocitá dei rotori. Va ricordato che PRINTF è una macro, creata appositamente,
attiva solo nel caso in cui PC sia definita (vedi Figura 11: Tutte le macroFigura
11).
Grazie a questo stratagemma si è potuto osservare, tramite stampe a terminale,

45. 39
il comportamento del PID, del calcolo degli angoli e in generale del drone stesso
durante il suo normale funzionamento, senza che quest’ultimo venga alterato
significativamente dall’esecuzione delle funzioni “print”.
Figura 11: Tutte le macro
Come si nota dalla Figura 11, è possibile anche definire una durata diversa del
loop e, ovviamente, definire i pin a cui sono stati collegati tutti i componenti.
LED_IR è un led posizionato rialzato e quindi visibile anche mentre il drone è
in volo. Come si vedrà successivamente, è stato utilizzato per diversi scopi, primi
tra tutti la comunicazione di un problema verificatosi nel corso del setup e l’av-
venuta ricezione di un segnale IR.
5.2 Variabili globali
Figura 12: Oggetti
Nella Figura 12 si possono notare tutti gli oggetti che sono stati costruiti per il
funzionamento del drone. Si può facilmente notare che i primi quattro rappre-

46. 40
sentano dei veri e propri dispositivi hardware, ognuno con una sua libreria ap-
posita. Click è stato implementato senza essere mai effettivamente utilizzato:
rappresenta il bottone incluso sulla board STNucleo. Può rivelarsi utile in ap-
plicazioni future. Led_ir è già stato accenanto, mentre led_running rappresenta
il led integrato nella scheda, e in questo progetto è stato utilizzato per segnalare
che il loop è ancora attivo. Questo si è rivelato utile quando, inizialmente, si era
cercato di utilizzare il DMP. I blocchi del sistema che si verificavano di tanto in
tanto erano quindi facili da identificare notando che in tale condizione il led ri-
maneva spento. Infine, time_count è un timer che viene inizializzato nel setup e
serve a temporizzare la ricezione dei segnali IR, come si vedrà nelle prossime
pagine.
Figura 13: Altre variabili globali
Le variabili globali in Figura 13 definiscono interamente lo stato del drone in un
dato momento. Angles rappresenta l’array contenente di angoli YPR, altitude è
la quota calcolata dal sensore ad ultrasuoni e speed rappresenta la velocità di
base dei quattro rotori, sulla quale poi interverrà il PID con le sue correzioni.
Viene modificata tramite segnali infrarossi.
Figura 14: Variabili globali del PID

47. 41
Infine, nella Figura 14 si può notare le variabili che servono per il funziona-
mento del PID. Le prime tre rappresentano i relativi coefficienti dei controllori
per il pitch e per il roll, che ovviamente si equivalgono. La seconda terna, invece,
serve per il controllore dello yaw. Non potendo avere però dei riferimenti assoluti
certi per l’imbardata, come spiegato estensivamente nel capitolo 2 della tesi di
Fini, implementare un PID completo per controllarla sarebbe eccessivo. Di con-
seguenza è sufficiente solo il coefficiente proporzionale, per impedire al drone di
girare vorticosamente. Senza una bussola, tuttavia, è impossibile riuscire a
mantenere il velivolo fermo rispetto all’asse Z. Potrebbe essere interessante, in
questo caso, l’utilizzo del DMP: come già affermato, infatti, risulta in grado di
fornire anche i dati per l’imbardata. Tuttavia, per quanto possano essere efficaci
gli algoritmi proprietari della InvenSense, dovrebbe essere impossibile per loro
riuscire a evitare lo shift dell’angolo Z senza un riferimento assoluto come una
bussola.
Le ultime due variabili globali in figura, max_pid e max_pid_integral, rappre-
sentano rispettivamente gli estremi per l’ouput del PID e per la sola componente
integrale. Più avanti, parlando del PID, verrà spiegato il loro utilizzo e il perché
del “/3”.

48. 42
5.3 Catch_ir
Figura 15: Catch_ir
Questo metodo va eseguito il più spesso possibile, il suo scopo è rilevare un se-
gnale IR e prendere di conseguenza una particolare azione.
Le prime righe verranno spiegate alla fine. La funzione decode_data della libre-
ria ReceiverIR svolge la maggior parte del lavoro, restituendo il codice corrispon-
dente al segnale ricevuto, oppure -1 nel caso di nessuna ricezione. A questo
punto, se effettivamente si è ricevuto qualcosa, si salva in now il momento at-
tuale. Qualora dall’ultima ricezione siano passati meno di 300ms (da notare che
alla fine del codice si salva il valore di now nella variabile statica last_time), lo
si interpreta come lo stesso segnale ricevuto in precedenza e quindi termina la
funzione.

49. 43
In caso contrario si prosegue allo switch, dove a seconda del segnale ricevuto
viene eseguita una operazione diversa. In Figura 15 sono scritti i codici inviati
dall’applicazione Android, che fanno aumentare o diminuire la velocità base dei
motori. Tuttavia è possibile modificare il comportamento di tali segnali, ad
esempio per far aumentare o diminuire un dato coefficiente del PID senza dover
entrare in modalità debugging (quindi con cavo USB collegato e macro PC at-
tiva). Questa funzione in particolare si è rivelata molto utile nel corso del pro-
getto.
Un terzo pulsante, presente sull’applicazione, ha la funzione di resettare il
drone, ad esempio in caso di emergenza. Per il funzionamento dell’applicazione
Android si veda il capitolo 2 della prima parte. Per aggiungere ulteriori segnali,
come ad esempio quelli di un telecomando, è sufficiente stampare a terminale il
risultato della funzione decode_data in seguito alla pressione dei diversi tasti, e
incollare i codici così ottenuti formando ulteriori case.
Alla fine della funzione, oltre che, come già detto, salvare il tempo dell’ultima
ricezione, si accende il led_ir. Questo infatti segnala all’esterno che il drone ha
ricevuto il segnale correttamente. Tornando all’inizio del metodo, si nota che
l’istante attuale viene confrontato con il tempo di ultima ricezione, qualora sia
superiore a 200ms si spegne il led. Questo fa sì che il led venga acceso
nell’istante di ricezione di un segnale, e venga spento 200ms dopo. Dopo altri
100ms si potrà ricevere un ulteriore segnale.

50. 44
5.4 Check
Figura 16: Check
Questo metodo serve a verificare il corretto funzionamento di tutte le periferiche
presenti sul drone. Esso ha un comportamento differente a seconda che sia at-
tiva la modalità debugging o meno. Nel primo caso si limita a verificare se l’MPU
è presente, con la funzione testConnection. Nel secondo caso invece prepara il
drone per il volo, verifica quindi il corretto funzionamento del ricevitore a infra-
rossi e invia un piccolo impulso ad ogni motore. Una miglioria applicabile a que-
sta funzione potrebbe essere quella di cercare di verificare il corretto funziona-
mento del sensore ad ultrasuoni, ad esempio verificando la sensatezza della

51. 45
quota restituita in quel momento (che per inciso dovrebbe essere prossima allo
zero se il drone parte da terra).
Per facilitare la risoluzione dei problemi, ad ogni messaggio riscontrato è stata
associata una diversa sequenza di accensione del led:
PERIODO DI LAMPEGGIO MESSAGGIO
30ms Waiting for IR signal
200ms MPU6050 undetected
5.5 Il Setup
Figura 17: Il metodo Main
Come si può facilmente notare, in questo progetto si è scelto di strutturare il
main in modo da replicare l’ambiente Arduino. Il setup viene quindi eseguito
solo una volta, all’avvio, mentre il loop viene eseguito all’interno di un ciclo.
Figura 18: Setup
Dopo aver eseguito check, viene inizializzato l’MPU con initialize, già analizzato

52. 46
nella sua libreria, dopo di che viene avviato il sensore ad ultrasuoni e viene at-
tivato il filtro selezionato. LOOP_TIME, KALMAN e COMPLEMENTARY sono
macro definite nella libreria MPU. Tramite getAngles si fa in modo che la varia-
bile angles punti agli angoli calcolati dall’MPU. Infine si lancia offsetCalc, sem-
pre appartenente alla libreria MPU.
5.6 Loop
Figura 19: Loop I parte
Le righe iniziali sono già state spiegate: la prima nel paragrafo 5.1 e la seconda
nel 5.2. Dopo di che vengono lanciate delle funzioni, anch’esse già analizzate. In
pratica, dopo aver eseguito questa porzione di codice, il drone ha già calcolato la
sua posizione attraverso gli angoli YPR e l’altitudine, e ha già impostato la ve-
locità base dei motori, configurabile tramite IR.
Rimane solo il calcolo del PID.

53. 47
5.6.1 PID Roll e Pitch
Figura 20: Loop II parte, PID Pitch e Roll
Dopo aver impostato i setpoints, che ovviamente sono tutti zero, si prosegue con
il controllore per il roll. La metodologia per il calcolo del pitch seque una logica
identica e pertanto non verrà discussa in modo approfondito.
Per prima cosa viene calcolato l’errore, ottenuto dalla differenza tra l’angolo at-
tuale e l’angolo che si vuole ottenere, cioè il setpoint. Ovviamente, come riportato
in Figura 20, l’errore è un angolo. A questo punto viene calcolato l’errore inte-
grale e questo viene limitato con max_pid_integral. Come di consueto, infine,
l’output del controllore è la combinazione lineare delle tre componenti: propor-
zionale, integrale e differenziale. Questo viene infine limitato da max_pid, e ag-
giornato il valore di last_roll_differential_error.

54. 48
5.6.2 PID Yaw
Figura 21: Loop II parte, PID Yaw
Le differenze essenziali di questo controllore rispetto al precedente sono due:
innanzitutto questo non opera più su angoli, ma su velocità angolari. Infatti,
come visto nel capitolo 2, prima parte, sui filtri software, gli strumenti impiegati
in questo progetto non sono in grado di calcolare l’angolo di imbardata. Per que-
sto in angles[2] viene semplicemente riportato il valore dei dati raw calcolati dal
giroscopio.
La seconda differenza è che non è richiesta una grande precisione nel processo
di controllo. Non avendo un riferimento assoluto, si può solo cercare di non far
ruotare eccessivamente il drone su se stesso, sapendo però che una leggera ro-
tazione potrà sempre essere presente. Per questo motivo, cercare di annullare
completamente la velocità angolare sull’asse Z, non è necessario: nella realtà
non si raggiungerà mai un risultato eccellente. Un controllore solo proporzionale
è pertanto più che sufficiente per limitare detta rotazione. Nel codice, tuttavia,
è stato lo stesso implementato un PID, ma ponendo i coefficienti differenziali e
integrali a zero.

55. 49
Come input di questo controllore non viene data la velocità angolare pura calco-
lata dal giroscopio, ma viene effettuato un filtro IIR11: un filtro digitale la cui
uscita è ottenuta come combinazione lineare di precedenti ingressi ed uscite. Da
qui deriva la sua risposta “infinita” all’impulso. Questo filtro è stato aggiunto
per ridurre le alte frequenze e quindi aumentare la stabilità della funzione. Si
ricordi che per passare dai dati raw alla velocità angolare bisogna dividere per
un coefficiente, dato dalla tabella di sensibilità (Tabella 3). In questo progetto si
è usato il range di 250deg/sec, a cui corrisponde il coefficiente 131 che si nota in
Figura 21: Loop II parte, PID Yaw. La parte successiva del PID è identica a
quello visto in precedenza.
5.6.3 Calcolo delle velocità
Figura 22: Le velocità dei rotori
A questo punto, dopo aver ottenuto tutti gli output dei PID, rimane da attuare i
risultati sui motori. Prima di tutto vengono calcolati gli output totali di ogni
11 Infinite Impulse Filter

56. 50
motore, in modo da ottenere le quattro variabili che determinano come i control-
lori agiscano su ogni rotore. È facile capire che ognuno di questi valori è limitato
in [ -max_pid*3 , max_pid*3 ]. I segni con cui abbiamo sommato i diversi output
dei singoli PID dipendono da come il drone viene costruito.
Figura 23: Drone
Prendondo ouput_yaw positivo come la necessità di far ruotare il drone in senso
orario, si nota facilmente che in tal caso va diminuita la potenza ai motori che
girano in senso orario, cioè up_rx e dw_lx, e aumentata agli altri due.
Considerando ora output_roll positivo come la necessità di rollare verso sinistra,
in tal caso va aumentata la potenza di dw_rx e dw_lx, e diminuita agli altri due.
Infine, con ouput_pitch positivo inteso come la necessità di cabrare (impennarsi),
bisognerà aumentare i girid di up_rx e dw_rx, diminuendo gli altri.
Manca ora solo da sommare gli output finali calcolati per ogni motore alla velo-

57. 51
cità base, stabilita tramite controllo remoto (telecomando o app Android), e li-
mitare la somma affinché sia tra 0 e 1, come richiesto dalla libreria ESC. La
funzione limitSpeed non è stata analizzata perché il suo funzionamento è ba-
nale.
Figura 24: Fine loop
Qui si conclude il loop, con l’effettiva attuazione dei valori calcolati come velocità
dei diversi rotori.

58. 52
6 Considerazioni sull’efficienza
La tesi si conclude con un capitolo sull’efficienza di un drone: come si potrebbe
estendere l’autonomia di volo, quali sono le caratteristiche che influenzano tale
proprietà, quali sono gli svantaggi legati a tali soluzioni.
Purtroppo tutto il progetto è stato svolto con un’alimentazione a 12.6V ottenuta
tramite un generatore di tensione, non avendo avuto delle batterie a disposi-
zione. Per questo non è stato possibile fare esperimenti sull’autonomia di volo e
perciò tutte le considerazioni qui esposte provengono da basilari concetti fisici,
oppure da esperimenti effettuati da terzi.
Gran parte dei droni in commercio hanno un’autonomia di volo compresa tra i
10 e i 20 minuti, ma è possibile estenderla selezionando attentamente i compo-
nenti utilizzati. Innanzitutto, aumentando il numero di propulsori, ad esempio
a sei o otto, si aumenta la capacità di carico, a discapito dell’efficienza. Questo
perché all’aumentare del numero di rotori, senza aumentarne le dimensioni, il
frame aumenta in maniera più che proporzionale, come si può notare dalla Fi-
gura 25. Di conseguenza, nonostante aumenti la portanza totale, diminuisce il
TWR12. Per questo un quadricottero rappresenta la configurazione più efficiente,
e motori aggiuntivi hanno senso solo se serve maggiore capacità di carico oppure
fault-tolerance [14].
Un’altra possibilità per aumentare la capacità di volo è aumentare la tensione
delle batterie: sistemi a bassa corrente ed alta tensione, ovviamente, sono in
grado di generare la stessa potenza di sistemi ad alta corrente e bassa tensione,
con la differenza però di consumare, appunto, una quantità inferiore di corrente,
prolungando quindi la durata delle batterie. Un esempio è mostrato in Figura
26, dove si può apprezzare come lo stesso motore, alimentato a tensioni differenti
(evidenziate in rosso) può generare una potenza superiore utilizzando meno cor-
rente. Questo è particolarmente utile in quanto la capacità delle batterie LiPo
12 Thrust-Weight Ratio

59. 53
si misura in mAHs (milliampere-ora), dove 1 AH indica l’abilità della batteria
di fornire 1A per un ora. Per questo motivo, abbassando la corrente, si prolunga
l’autonomia, a patto ovviamente di usare una batteria che fornisca una tensione
maggiore (ad esempio una LiPo 6s, che fornisce 22.2V).
Figura 25: Quadricottero vs ottocottero
Figura 26: Consumi con diverse tensioni di batteria
Ovviamente anche le caratteristiche dei motori giocano un ruolo importante:

60. 54
grandi o piccole eliche richiedono diversi tipi di motori. Andando leggermente
più nel dettaglio, la potenza assorbita da un motore può essere considerata come
il prodotto tra la coppia che genera e i suoi RPM. Questi ultimi sono essenzial-
mente equivalenti al parametro KV, caratteristica fondamentale di ogni motore:
esso indica gli RPM costanti del motore a fronte di una tensione di 1V, in assenza
di carico. Per esempio un motore da 1000KV (da non confondere con kilo-volt)
alimentato a 12V teoricamente sarà in grado di raggiungere 12,000 RPM. Ov-
viamente maggiori RPM rappresentano anche maggiori attriti, e quindi minore
efficienza. Grazie al fatto che motori con bassi KV a parità di potenza generano
coppie superiori, allora tali motori possono pilotare eliche più grandi, e quindi
generare ugualmente la stessa portanza di motori con maggiori RPM, con però
una maggiore efficienza.
A questo punto potrebbe sembrare che usare i rotori il più grandi possibile, ab-
binati a motori con KV molto bassi e batterie ad altissimo voltaggio, fornisca il
setup più efficiente. In realtà non è così semplice: ogni componente ha un punto
ottimo di lavoro, e bisogna trovare il compromesso migliore per tutti i compo-
nenti, per avere una buona efficienza. Ad esempio mentre setup ad alti voltaggi
sono in generale migliori, di solito sono anche più pesanti a causa delle batterie
e i motori maggiorati. Non è quindi possibile continuare il trend all’infinito, in
quanto prima o poi arriverà un momento in cui il peso sarà troppo e quindi ci
saranno più svantaggi che vantaggi. Lo stesso vale per la dimensione dei rotori:
non si può solo montare i più grandi disponibili, nel nome dell’efficienza. Infatti
grandi rotori assorbono anche più corrente, e quindi minore autonomia di volo.
Per concludere, sebbene questi principi forniti siano generalmente validi, biso-
gna anche considerare altri fattori, come il matching tra i componenti e la qua-
lità degli stessi.

61. 55

62. 56
7 Sitografia e Bibliografia
[1] «STMicroElectronics,» [Online]. Available:
http://www.st.com/content/st_com/en/products/evaluation-
tools/product-evaluation-tools/mcu-eval-tools/stm32-mcu-eval-
tools/stm32-mcu-nucleo/nucleo-l053r8.html.
[2] «Wikipedia I2C,» [Online]. Available:
https://it.wikipedia.org/wiki/I%C2%B2C. [Consultato il giorno 2017].
[3] «i2c-bus,» 2017. [Online]. Available: i2c-bus.org.
[4] Arduino, «Arduino,» [Online]. Available:
https://www.arduino.cc/en/Reference/Wire. [Consultato il giorno
2017].
[5] Anon., «sarkanyellato,» [Online]. Available:
http://www.sarkanyellato.hu/wp-content/uploads/2011/10/RC-Timer-
10.18.30.40A-ESC-Instruction.pdf.
[6] S. Nakamura, «Mbed RemoteIR,» [Online]. Available:
https://os.mbed.com/users/shintamainjp/code/RemoteIR/. [Consultato
il giorno 2017].
[7] «Tampere University of Technology,» [Online]. Available:
http://aerostudents.com/files/avionics/InertialNavigationSystems.pdf
.
[8] «InvenSense,» [Online]. Available:
https://www.invensense.com/products/motion-tracking/6-axis/mpu-
6050.
[9] Invensense, «Invensense,» [Online]. Available:
https://www.invensense.com/wp-content/uploads/2015/02/MPU-
6000-Datasheet1.pdf. [Consultato il giorno 2017].

63. 57
[10] Invensense, «Invensense,» 2017. [Online]. Available:
https://www.invensense.com/wp-content/uploads/2015/02/MPU-
6000-Register-Map1.pdf.
[11] J. Rowberg, «Github,» [Online]. Available:
https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
. [Consultato il giorno 2017].
[12] batata004, «Github,» [Online]. Available:
https://github.com/jrowberg/i2cdevlib/issues/252. [Consultato il
giorno 2017].
[13] «GeekMomProjects,» [Online]. Available:
http://www.geekmomprojects.com/mpu-6050-redux-dmp-data-fusion-
vs-complementary-filter/.
[14] Flyingtech, «Efficiency vs Performance: how to build drone long
flight time,» [Online]. Available:
https://www.flyingtech.co.uk/blog/efficiency-vs-performance-how-
build-drone-long-flight-time. [Consultato il giorno 2017].
[15] D. Beninato. [Online]. Available:
http://www.dei.unipd.it/~ieeesb/PIC/PresentazionePIC_01.pdf.
[16] [Online]. Available:
https://www.techopedia.com/definition/5299/jumper.
[17] L. Tanganelli. [Online]. Available:
http://www.settorezero.com/wordpress/alla-scoperta-delle-schede-di-
sviluppo-stm32-nucleo/.
[18] L. Frosini, «Università degli Studi di Pavia,» [Online]. Available:
http://http://www-
3.unipv.it/dmae/materiale_didattico/Costruzioni_13.pdf.
[19] L. Frosini, «Università degli Studi di Pavia,» [Online]. Available:
http://http://www-

64. 58
3.unipv.it/dmae/materiale_didattico/Costruzioni_22.pdf.
[20] A. d. Gerlando, «Politecnico di Milano,» [Online]. Available:
http://docenti.etec.polimi.it/IND32/Didattica/ME_professionalizzante
/materiale_didattico/dispense/5_MS.pdf.
[21] S. Keeping, «Digikey,» [Online]. Available:
https://www.digikey.it/it/articles/techzone/2013/mar/an-introduction-
to-brushless-dc-motor-control.
[22] RC Timer, [Online]. Available: http://www.sarkanyellato.hu/wp-
content/uploads/2011/10/RC-Timer-10.18.30.40A-ESC-
Instruction.pdf.
[23] FutureElectronics, «FutureElectronics,» [Online]. Available:
http://www.futureelectronics.com/en/optoelectronics/infrared-
receivers.aspx.
[24] «SBProjects,» [Online]. Available:
http://www.sbprojects.com/knowledge/ir/index.php.
[25] D. Tan, «irq5.io,» [Online]. Available:
http://irq5.io/2012/07/27/infrared-remote-control-protocols-part-1/.
[26] G. Singh, «CircuitValley,» [Online]. Available:
http://www.circuitvalley.com/2013/09/nec-protocol-ir-infrared-
remote-control.html.
[27] «Vishay Semiconductors,» [Online]. Available:
http://www.vishay.com/docs/80071/dataform.pdf.
[28] G. Wetzstein, «Stanford University,» [Online]. Available:
https://stanford.edu/class/ee267/lectures/lecture10.pdf.
[29] B. Bona, «Politecnico di Torino,» [Online]. Available:
http://www.ladispe.polito.it/corsi/meccatronica/01GTG/2008-
09/Slides/Quaternioni.pdf.
[30] «Geek Mom Projects,» [Online]. Available:

65. 59
http://www.geekmomprojects.com/mpu-6050-dmp-data-from-
i2cdevlib/.
[31] «Instructables,» [Online]. Available:
http://www.instructables.com/id/PCB-Quadrotor-
Brushless/step15/IMU-Part-2-Complementary-Filter/.
[32] «Robottini,» [Online]. Available:
http://robottini.altervista.org/kalman-filter-vs-complementary-
filter?doing_wp_cron=1381307547.0642869472503662109375.
[33] M. Agozzino, «Università degli Studi di Bologna,» [Online].
Available:
http://amslaurea.unibo.it/12064/1/Tracciamento%20di%20particelle
%20con%20filtro%20di%20Kalman.pdf.
[34] «ApogeoOnline,» [Online]. Available:
http://www.apogeonline.com/2002/libri/88-7303-864-
6/ebook/pdf/864_appendice1.pdf.
[35] A. P. A. Mohinder S. Grewal, Kalman Filtering-Theory and Practice
Using MATLAB, A John Wiley & Sons, Inc., Publication, 2008.
[36] P. Medici, «Università degli Studi di Parma,» [Online]. Available:
http://www.ce.unipr.it/people/medici/geometry/node52.html.
[37] «DroneBase,» [Online]. Available:
http://www.dronebase.it/prodotto/drone-per-fotogrammetria-x-cam-
700/.