Go Down

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

astrobeed


Perche' se non e' cosi'  ti dico che i tempi di eseguzione del fixedpoint, suffragato da LUT


Una coordinata GPS ha risoluzione al milionesimo di grado, anche di più in modelli di classe superiore, e ovviamente i vari seni/coseni devono fornire valori di pari risoluzione, anzi superiore se possibile, come pensi di gestire una LUT da 90.000.000 ( 90.000000°) di valori a 64 bit (= 720 MegaByte) ?



lesto


Io i test li ho fatti anche sul campo, ovvero gps in mano collegato al pc e muovendomi con controllo incrociato tra valori calcolati e visualizzazione cartografica (Autoroute Microsoft).
Se vuoi delle coppie di coordinate gps puoi ricavarle da google maps, per esempio prendi l'inizio e la fine di una strada.


eh, ma poi mi mancano i valori di controllo.

cmq è interessante vedere come la funzione di calcolo della distanza ottimizzata per perdere poca precisione dia risultati più veritieri, e la miglio sia la più veloce:
Code: [Select]
Lesto2:
bearing: 55.69
time:928


codice:
Code: [Select]
c=mod(atan2(sin(lon1-lon2)*cos(lat2), cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon1-lon2)), 2*M_PI);

Quote
No è il tuo codice che esegue la for solo una volta, se non ci credi impostala a 100 e avrai sempre lo stesso tempo, non puoi definire una variabile all'interno di una for, mettila tra le globali, o locale nella funzione, e vedrai che il tempo diventa 10 volte maggiore.

mannaggia, il bello che lo so pure....

ora vedo come si può ottimizzare (con i float) e vediamo dove scendo, comunque si può pensare di usare un secondo arduino, anzi magari un tiny, per fare questi conti
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

astrobeed


ora vedo come si può ottimizzare (con i float) e vediamo dove scendo, comunque si può pensare di usare un secondo arduino, anzi magari un tiny, per fare questi conti


Non puoi farlo con i float a 32 bit, è impossibile perché non  hai abbastanza risoluzione su i decimali, per non parlare dei vari arrotondamenti durante i calcoli.
Mettiti il cuore in pace, per fare i calcoli con le coordinate del GPS è obbligatorio usare la matematica a 64 bit se vuoi la risoluzione al singolo metro, poi se ti accontenti delle centinaia di metri allora puoi farlo anche con la 32 bit.

astrobeed


eh, ma poi mi mancano i valori di controllo.


Che vuol dire che ti mancano i valori di controllo ?

lesto

sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

astrobeed


Infatti tu hai pensato subito alle quadratiche pure, iterative magari (lente per definizione di convergenza), l'ho capito al volo da come hai risposto


No io ho pensato a una lut full table che è il modo più rapido in assoluto per fare i calcoli, però costa molto a livello di memoria, nel caso specifico ben 720 MegaByte esatti per tabellare i 90000000 di seni/coseni precalcolati a 64 bit, poi è lo stesso valore angolare, convertito come long int, a fare da indice per la tabella, una manciata di cicli macchina e hai il valore pronto senza dover fare nessun calcolo reale.
Comunque non vedo per quale motivo devo complicarmi la vita per fare un calcolo complesso su una piccola mcu 8 bit, che a stento arriva a 10 mips medi reali e non ha nessun supporto matematico, quando con un paio di Euro in più posso avere un Cortex M3 , o M4, che non ha problemi per fare calcoli a 64 bit e tutto il conteggio lo fa in meno di 200 microsecondi (a 64 bit), se poi aggiungo ancora un  Euro ci metto un Cortex M4 con FPU ed ecco che il conto richiede meno di 50 us.

astrobeed


la bearing reale tra i due punti


Distanza e angolo li trovi con uno dei vari calcolatori online che fanno il calcolo oppure ti diverti con la calcolatrice scientifica e te li fai da solo, se ne hai una programmabile ti puoi pure divertire a scrivere il programmino che ti fa il conto in automatico  :)

astrobeed

Ho fatto un test al volo con un modulo mBed, rammento che usa un core Cortex M3 @96 MHz, tutto il calcolo della distanza e dell'angolo a 64 bit richiede circa 280 us, i valori ottenuti sono distanza 171.89 metri e angolo 55.73°, esattamente gli stessi che ottengo con un programma in C sul desktop.
Il programma di test usato è il seguente, è in C ANSI e salvo le funzioni di inizializzazione dell'ambiente hardware e quelle di stampa sulla seriale, specifiche per l'mBed, è utilizzabile direttamente con qualunque altro micro e compilatore C.

Code: [Select]


/*
 
  TEST distanza e bearing tra coordinate GPS

  mBed Cortex M3 @ 96MHz

*/

// declarations
#include "mbed.h"

#define RADS 57.2957795131
#define Metri_Per_Rad 6366692,0724

double a,b,c;
double Lat1,Lat2,Lon1,Lon2;

// servono per inizializzare i quattro led di stato del modulo, non fanno parte dei pin a disposizione.   
DigitalOut LD1(LED1);
DigitalOut LD2(LED2);
DigitalOut LD3(LED3);
DigitalOut LD4(LED4);

// inizializza la Vcom USB integrata sul modulo
Serial pc(USBTX, USBRX); // tx, rx

// Inizializza una seriale hardware per collegarci il GPS, per il momento non utilizzata
// Serial gps(p9,p10);// tx, rx

int main() {
    pc.baud(115200);
    pc.printf("Start TEST\n");
    LD1 = 1; 

    Lat1 = 42.857818;
    Lon1 = 12.563545;
    Lat2 = 42.858689;
    Lon2 = 12.561801;
 
    while (1)
{
  LD2 = 1; // accende il led 2, viene usato per misurare la velocità di esecuzione tramite DSO
    a = acos(sin(Lat1/RADS)*sin(Lat2/RADS) + cos(Lat1/RADS)*cos(Lat2/RADS)*cos((Lon1-Lon2)/RADS));
          b = a * Metri_Per_Rad;
          c = acos((sin(Lat2/RADS) - sin(Lat1/RADS)* cos(a)) / (cos(Lat1/RADS) * sin(a)));
          c *= RADS;
  LD2 = 0; // spegne il led 2
     
  pc.printf("Distanza : %3.2f \n",b);
  pc.printf("Angolo   : %3.2f \n",c);
   
      }
}

Federico

Federico - Sideralis
Arduino &C: http://www.sideralis.org
Foto: http://blackman.amicofigo.com

astrobeed


Ma il risultato del test?


L'ho scritto, circa 280 us per eseguire il calcolo completo a 64 bit e valori ottenuti sono quelli attesi.

lesto

stavo pensando: se il GPS funzona a 10Hz (ed è tanto), abbiamo 100ms per elaborare input e tutto prima che sia "vecchio".
Se noi scompoiniamo il calcolo in modo tale che ogni loop eseguiamo solo un'operazione sin/cos/atan, il tempo di elaborazione complessiva magari sale a 10 ms, ma almeno non blocchiamo il loop, anzi sfruttiamo i tempi morti causati dalla frequenza massima di lavoro dei giroscopi.

Astro, che ne dici di questa idea?
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

astrobeed


Astro, che ne dici di questa idea?


Sicuramente puoi pure scomporre il conto in operazioni singole da fare durante i vari cicli in modo da non gravare troppo, però non è che una volta che hai la distanza e l'angolo hai finito, devi usarli per correggere la rotta, e questo comporta altro lavoro per la cpu, comunque c'è sempre il piccolo problema della matematica a 64 bit che non hai a disposizione.

lesto

prova a usare la mia formul che non passa dalla distanza:
Quote
c=mod(atan2(sin(lon1-lon2)*cos(lat2), cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon1-lon2)), 2*M_PI);


nei miei test a 32bit è qulla che più si è avvicinata al risultato corretto (errore di 3 gradi se ricordo bene, contro gli oltre 10 degli altri 2 metodi)

se invece vuoi seguire la strada per la distanza, almeno usa il codice per la distanza che ho postato "a prova di virgola mobile" :smiley-mr-green:

edit: questa:
Quote

//i dati sono già in radianti
da = sin((lat1-lat2)/2);
    db = cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2));
    d=2*asin(sqrt( da*da+db*db ));  //distanza in radianti
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

astrobeed


se invece vuoi seguire la strada per la distanza, almeno usa il codice per la distanza che ho postato "a prova di virgola mobile" :smiley-mr-green:


Non puoi gestire calcoli trigonometrici con valori angolari a sei cifre decimali con i float a 32 bit senza perdere tantissima risoluzione/precisione.
Qui si parla di apprezzare variazioni di non più di un metro di percorso, ovvero poco meno di 10 milionesimi di grado inteso come distanza angolare sulla sfera terrestre.
Le tue formule le ho provate, forniscono risultati totalmente sbagliati sulla distanza  se applicate a 32 bit, non le ho provate a 64 bit, l'angolo è meno critico da calcolare e forse si riesce a farlo pure a 32 bit, ma senza la distanza non serve a nulla.

lesto

finalmente un pò di tempo libero, e mi son dedicato alla programmazione della IMU.

ecco alcuni dati:

allora, ho i due sensori ADXL345 e ITG3200, con cui comunico senza problemi.
ora, con la libreria wire "normale", ovvero TWI_FREQ = 100000L, leggo i sensori a queste velocità:
solo giro ~ 1kHz
solo acc ~ 1kHz
giro + acc ~ 0.5kHz

come potete notare il problema grosso è la banda disponibile di comunicazione, dato che il giroscopio è settato per campionare a 8kHz e l'accelerometro a 3.2kHz.

allora, dato che entrambi i sensori lo supportano, ho usato il fast wire, alzando il TWI_FREQ = 400000L
risultati:
solo giro ~ 2.8kHz
solo acc ~ 2.8kHz
giro + acc ~ 1.2kHz

che dire? l'i2c da questo punto di vista fa alquanto pena. (edit: bisogna passare a SPI, ma non c'è sulla breakout che uso)

ecco i dati raw che ho usato:
colonna 1: numero di update del gyro, colonna2: numero di update consecutivi con differenti valori (per testare la velocità di campionamento), colenna 3 e 4 stessa cosa ma dell'accelerometro, colonna 5 numero di millis di raccolta dati (1 secondo)

Code: [Select]
1288 1272 1288 1279 1000
1285 1267 1285 1275 1000
1287 1275 1287 1283 1000
1286 1268 1286 1277 1000
1289 1267 1289 1282 1000
1286 1258 1286 1277 1000
1288 1273 1288 1281 1000
1285 1264 1285 1281 1000
1287 1273 1287 1285 1000
1285 1272 1285 1280 1000
1287 1264 1287 1281 1000
2843 2622 0 0 1000
2855 2732 0 0 1000
2848 2705 0 0 1000
2853 2796 0 0 1000
2851 2725 0 0 1000
2843 2618 0 0 1000
2853 2749 0 0 1000
2843 2663 0 0 1000
2850 2692 0 0 1000
2842 2630 0 0 1000
0 0 2290 2287 1000
0 0 2285 2281 1000
0 0 2287 2286 1000
0 0 2282 2278 1000
0 0 2286 2286 1000
0 0 2281 2276 1000
0 0 2284 2282 1000
0 0 2288 2287 1000
0 0 2284 2281 1000
0 0 2285 2283 1000


edit2: bisognerebbe iniziare a fare una board che usa SPI...
ma mi sono accorto che il giroscopio NON supporta lo SPI... com'è possibile? cioè, allora a che serve la capacita di campionare a più di 3kHz?
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Go Up