Go Down

Topic: [Multicotteri] Elettronica : IMU, MCU, Sensori ed algoritmi di controllo (Read 132 times) previous topic - next topic

lesto

Rieccomi!!
per la serie stira & ammira, vi presento la "mia" IMU, basata sulla DCM di S.O.H. Madgwick (ho semplicemente fatto un porting del codice), fastwire di Francesco Ferrara(copia & incolla brutale), codice WMP da mezzo forum di arduino, google & wiibrew.
l'ho estratta stanotte (è dall'1 che ci lavoro) dal resto del codice del quadricottero, ma finamlente ve la posso presentare.
è ancora in versione molto alfa, diciamo che funziona. Non ho ancora integrato la lettura dei comandi via seriale, quindi vi narro brevemente quello che accade:
collegate un WMP via il classico i2c (attenzione le pull-up interne sono attivate, per disattivarle in WMP.cpp, funzione init(), cambiate Fastwire::setup(300, true); in Fastwire::setup(300, false);)
collegate un qualsiasi accelerometro analogico, asse Y al pin 0, X all'1 e Z al 2. A priori dovete sapere il valore medio dell'accelerometro. per la X e la Z è facile, basta leggere il valore analogico a accelerometro pianeggiante. Invece per la Y dovete inclinarlo su un lato di 90°. i valori sono settabili nel file IMUlast.cpp; midA è la Y, midB la X e midC la Z.

ora andate in stabilizzazione.cpp e noterete nella funzione update la riga    float gx=data[2], gy=-data[1], gz=data[0], ax=data[5], ay=-data[4],az=data[3];
i primi 3 valori sono i giroscopi: se state testando l'inclinazione dell'accelerometro, mettete pure questi 3 valori a 0.
ATTENZIONE! gli altri 3 valori sono l'acceleromtro: se volete testare il giroscopio mettete ax e ay = 0 ma AZ=1!!!! è normale vedere l'orientamento che ritorna sui 0 gradi proprio per questo motivo (no, non vale mettere tutti e 3 a 0, incasina l'algoritmo)

Questo codice scrive via seriale il quaternione di rotazione con una frequenza di 50Hz.
già ma chi sa leggere i quaternioni di rotazione, direte voi? semplice, il PC :)

è presente anche il codice processing di un bellissimo rettangolo colorato che mostra l'orientamento della vostra IMU, e che dunque vi aiuterà enormemente in tutti i passaggi di controllo funzionamento e orientazione dei sensori. Il codice originale l'ho sgraffignato a varesano da qualche parte nel suo sito, ma direi che ormai di originale son rimaste solo le funzioni buildBoxShape() e drawCube().
Se non vedete le scritte, o le vedete male, è colpa dei font di processing, dovreste "compilarne" uno, date un'occhiata al sito processing alla funzione "textFont" (che dovrete modificare in draw() )
noterete alcune strane scritte azzurre con texbox, fregatevene che manca un pezzo di codice arduino.
Le scritte bianche invece sono: a sinistra il quaternione, quelle in alto sono l'orientamento in angoli euleriano (i classici x, y, z), al centro in radianti e a destra in gradi.



per ora funziona così e così ve la dovete tenere, ci voglio lavorare sopra ancora un pò per pulire il codice (ci sono certi obbrobri sintattici, logici, e inenarrabili), calibrazione pseudo automatica dell'accelerometro (da gui), attivazione di solo giro, solo acc o entrambi da gui, nonchè attivazione/disattivazione pull-up interne, reset remoto dei sensori e del quaternione, rendere attive le caselle per settare P e I dell'algoritmo via GUI (ma non credo sia necessario)


SAREBBE MOLTO GRADITO UN TEST DA PARTE VOSTRA, E MAGARI UN PROGRAMMA SIMILE CON FILTRO DI KALLMANN PER FARE UN CONFRONTO OGGETTIVO (ma quì mi sa che devo aspettare astro)
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

astrobeed


Rieccomi!!
SAREBBE MOLTO GRADITO UN TEST DA PARTE VOSTRA, E MAGARI UN PROGRAMMA SIMILE CON FILTRO DI KALLMANN PER FARE UN CONFRONTO OGGETTIVO (ma quì mi sa che devo aspettare astro)


Prima di fine settimana non posso mettermi a fare nessun test, però la provo sicuramente e ti faccio sapere, pure io sono curioso di fare un confronto con il Kalman, mi hai appena dato un valido motivo per accelerare i tempi  :)

cavde

@lesto: ma il lavoro che stai facendo è un nuovo codice da sostituire al Multiwii??? comunque complimenti per il lavoraccio

Volevo chiedere una delucidazione sul bus I2C: nel concetto di bus non si ragiona sul fatto che c'è il bus e tutti i dispositivi I2C vanno collegati al bus allo stesso modo? cioè posso mettere in comune SDA del WM+ e SDA dell'ADXL345 e poi collegarlo ad arduino? stessa cosa per SCL 3,3V e GND?
Se gommo tiene,io vince gara! Se gommo non tiene,io come bomba dentro montagna

ratto93

Con l'ADXL 345 devi collegarti nei pin che sarebbero destinati al connettore del nunchuk....
Se corri veloce come un fulmine, ti schianterai come un tuono.

lesto


@lesto: ma il lavoro che stai facendo è un nuovo codice da sostituire al Multiwii??? comunque complimenti per il lavoraccio

non è da sostituire al multiwi... quando ho iniziato a vedere il quadricoso volevo farmi da solo anche il codice. E già che c'ero ho implementato questo nuovo algoritmo (DCM) al posto del "vecchio" (kallmann). Effettivamente non si sà chi il migliore, so solo che la SQRT mi porta via quasi 1/4 di memoria per lo sketch..
grazie per i complimenti, gira e rigira son ormai 2 anni che ci sto dietro :)


Volevo chiedere una delucidazione sul bus I2C: nel concetto di bus non si ragiona sul fatto che c'è il bus e tutti i dispositivi I2C vanno collegati al bus allo stesso modo? cioè posso mettere in comune SDA del WM+ e SDA dell'ADXL345 e poi collegarlo ad arduino? stessa cosa per SCL 3,3V e GND?

si, GND è un caso particolare perchè è il valore di riferimento per il voltaggio. Il resto è come dici te, l'importante è che i componenti NON abbiano lo stesso indirizzo I2C. In alcuni può essere settato usando dei jumper, in altri te le davi cavare in altro modo (come per esempio col WMP e il Nunkuck). I sensori da te citati dovrebbero funzionare tranquillamente insieme.


Con l'ADXL 345 devi collegarti nei pin che sarebbero destinati al connettore del nunchuk....

assolutamente no, il nun è un caso a parte che usa un protocollo proprietario della sony
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Go Up