MPU6050 con HMC5883L magnetometro compensato per le inclinazioni

Ciao a tutti, ho la IMU come da titolo:
http://www.drotek.com/shop/en/home/62-imu-10dof-mpu6050-hmc5883-ms5611.html
che ho usato per costruire un quadricottero, poi distrutto e cannibalizzato, volevo usarla all’interno del ROV che sto cercando di “accrocchiare”, così ho cercato una libreria già fatta, visto che sono un maestro nello scopiazzare!! Ora, prima di tutto ho cercato se ci fosse un modo per non appesantire arduino per calcolare roll e pitch, ed ho trovato questa libreria,

ho eliminato dal codice di esempio gli interrupt, perchè… non avevo voglia di saldare il pin!
Visto che Pitch e Roll erano molto precisi e Yaw anche, senza drift, ho cercato di scopiazzare una parte di codice per attivare e leggere i dati del magnetometro, (vorrei fare apparire sul monitor del rov una barra con le indicazioni approssimative della direzione, o almeno i gradi, tipo la foto in allegato.
La lettura del magnetometro è avvenuta con successo, ma funzionava solo se il sensore rimaneva in piano, così ho cercato in lungo ed il largo per compensarne la lettura con l’inclinazione, alla fine, qui,

ho letto un po di teoria, ma le formule non funzionavano, così da qui,

http://hobbylogs.me.pn/?p=17
alla risposta 21, ho scopiazzato la formula e cambiato un segno, non chiedetemi perché, ed ora funziona! Ho dei dubbi però, il codice occupa quasi 20 kb su arduino, saprebbe qualcuno aiutarmi a capire se si può diciamo semplificare? Senza interrupt cosa rischio? Ho capito che c’è una coda, FIFO, rischio, aggiungendo per esempio il codice per l’osd max7456, di perdermi dei “pezzi” o sfasare la lettura in qualche modo?
Grazie

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu(0x69);

#define HMC5883L_DEFAULT_ADDRESS    0x1E
#define HMC5883L_RA_DATAX_H         0x03
#define HMC5883L_RA_DATAZ_H         0x05
#define HMC5883L_RA_DATAY_H         0x07


bool dmpReady = false;  
uint8_t devStatus;      
uint16_t packetSize;    
uint16_t fifoCount;     
uint8_t fifoBuffer[64]; 

// orientation/motion vars
Quaternion q;           
VectorInt16 aa;         
int16_t mx, my, mz;    
VectorInt16 aaReal;  
VectorInt16 aaWorld;  
VectorFloat gravity;  
float ypr[3];          
float heading;          


double mxMin = 0;
double mxMax = 0;
double myMin = 0;
double myMax = 0;
double mzMin = 0;
double mzMax = 0;

void setup() {
  
   
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; 
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif
    
    Serial.begin(115200);
    
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // wait for ready
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); 
    while (!Serial.available());                 // 
    while (Serial.available() && Serial.read());

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(-6);
    mpu.setYGyroOffset(-35);
    mpu.setZGyroOffset(-19);
    mpu.setXAccelOffset(-3699);
    mpu.setYAccelOffset(1113);
    mpu.setZAccelOffset(1377); // 1688 factory default for my test chip
    
    // Magnetometer configuration

  mpu.setI2CMasterModeEnabled(0);
  mpu.setI2CBypassEnabled(1);

  Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS);
  Wire.write(0x02); 
  Wire.write(0x00);  // Set continuous mode
  Wire.endTransmission();
  delay(5);

  Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS);
  Wire.write(0x00);
  Wire.write(B00011000);  // 75Hz
  Wire.endTransmission();
  delay(5);

  mpu.setI2CBypassEnabled(0);

  // X axis word
  mpu.setSlaveAddress(0, HMC5883L_DEFAULT_ADDRESS | 0x80); // 0x80 turns 7th bit ON, according to datasheet, 7th bit controls Read/Write direction
  mpu.setSlaveRegister(0, HMC5883L_RA_DATAX_H);
  mpu.setSlaveEnabled(0, true);
  mpu.setSlaveWordByteSwap(0, false);
  mpu.setSlaveWriteMode(0, false);
  mpu.setSlaveWordGroupOffset(0, false);
  mpu.setSlaveDataLength(0, 2);

  // Y axis word
  mpu.setSlaveAddress(1, HMC5883L_DEFAULT_ADDRESS | 0x80);
  mpu.setSlaveRegister(1, HMC5883L_RA_DATAY_H);
  mpu.setSlaveEnabled(1, true);
  mpu.setSlaveWordByteSwap(1, false);
  mpu.setSlaveWriteMode(1, false);
  mpu.setSlaveWordGroupOffset(1, false);
  mpu.setSlaveDataLength(1, 2);

  // Z axis word
  mpu.setSlaveAddress(2, HMC5883L_DEFAULT_ADDRESS | 0x80);
  mpu.setSlaveRegister(2, HMC5883L_RA_DATAZ_H);
  mpu.setSlaveEnabled(2, true);
  mpu.setSlaveWordByteSwap(2, false);
  mpu.setSlaveWriteMode(2, false);
  mpu.setSlaveWordGroupOffset(2, false);
  mpu.setSlaveDataLength(2, 2);

  mpu.setI2CMasterModeEnabled(1);

    
    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        //Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
 
}


void loop() {
    
    if (!dmpReady) return;
    
    if (!compassCalibrated) compassCalibration();
    
    while (fifoCount < packetSize) {fifoCount = mpu.getFIFOCount();}
        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            
            //Read magnetometer measures
            mx=mpu.getExternalSensorWord(0);
            my=mpu.getExternalSensorWord(2);
            mz=mpu.getExternalSensorWord(4);
           
  double Xh = mx * cos(ypr[1]) + my * sin(ypr[2]) * sin(ypr[1]) - mz * cos(ypr[2])*sin(ypr[1]);
  double Yh = my * cos(ypr[2]) - mz * sin(ypr[2]);
  float heading = atan2 ( Yh, Xh );
  if(heading < 0) heading += 2*PI;
  if(heading > 2*PI) heading -= 2*PI;
  float headingDegrees = heading * 180/M_PI; 
  
  float heading2 = atan2(my, mx);
  if(heading2 < 0) heading2 += 2 * M_PI;

            Serial.print("\t");
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[2] * 180/M_PI);
            Serial.print("\t h:\t");
            Serial.print(heading2 * 180/M_PI);
            Serial.print("\t");
            Serial.print("mag : ");
            Serial.print(headingDegrees);

            
}

Cattura.JPG

L'interrupt è di fondamentale importante quando usi gli MPU6050, non puoi non usarlo, sopratutto se intendi fare altre cose oltre all'acquisizione dei dati dalla IMU.

Ciao, scusami, ma per esempio multiwii non usa gli interrupt, perché quando ho costruito il quadricottero non li/lo ho connesso! Eppure fa molte altre cose oltre a leggere l'IMU. Stasera provo a unire gli sketch e ti dico cosa succede!
Grazie

vinsub:
Ciao, scusami, ma per esempio multiwii non usa gli interrupt,

Multiwii è nato utilizzando i sensori del Nintendo Wii, da cui il nome, come software è stra obsoleto e molto limitato, anche se gli hanno aggiunto gli MPU6050 tra i sensori utilizzabili non vuol dire che li gestisce nel giusto modo.

astrobeed:
L'interrupt è di fondamentale importante quando usi gli MPU6050, non puoi non usarlo, sopratutto se intendi fare altre cose oltre all'acquisizione dei dati dalla IMU.

Scusa, volevi dire "quando usi il DMP del MPU6050"? Mi pare che se volessi i dati dei sensori non avrei bisogno di code giusto? Se io leggo il buffer alla stessa velocità o più veloce del suo "riempimento" non dovrei avere problemi, potrei svuotarlo ogni tanto e perdere magari qualcosa?

Come giustamente diceva Astrobeed, appena ho unito il codice dell'osd le letture dell'imu sono diventate incomprensibili, così ho deciso di resettare il buffer ad ogni lettura, ed ora funziona! Il mio problema è ora scrivere una routine per la calibrazione degli offset e della non, diciamo così, circolarità delle letture. Per gli offsets ho chiaro il metodo:offset_x = (x_max_mag + x_min_mag)/2, quindi x_mag = x_mag - offset_x. Giusto? Per x y e z. Per l'ampiezza delle letture invece potrei usare map per normalizzare i valori e renderli simili nel range?
Per la non circolarità?
:o
A proposito di circolarità poi, vorrei fare scorrere un'array contenente un testo, "N---NE---E---SE---S---SO---O---NO---", tipo buffer circolare, ho provato con una serie di if, partendo dal carattere che starà al centro nell'array di testo nuova che andrà stampata, ma ci vuole un if per ogni carattere, c'è un metodo più consono?
Grazie

Funziona, ho calibrato la bussola con un altro codice, solo per l’hard iron, per il soft iron ancora no, ma sono soddisfatto! Ora devo aggiungere la lettura del pressostato, quella dei volt batteria e l’easy transfer per l’rs485, vi posterò gli aggiornamenti!
link al video

OSD___DMP.ino (13.1 KB)

Ciao, ho aggiunto la visualizzazione del beccheggio!
Volete fare un commento!? :smiley:
video OSD con visualizzazione beccheggio

@Astrobeed
@Etemenanki
Volendo comandare i 2 motori che svolgeranno la "trazione" e si occuperanno anche della rotazione con un joystick, parlando solo di algoritmo, avevo pensato che fosse semplice, portando la leva in avanti naturalmente il rov andrà in avanti, quindi i motori riceveranno un comando proporzionale al valore assunto da yJoystick, + o - il valore assunto da xJoystick, fino ad arrivare al tutto destra o tutto sinistra, quando yJoystick sarà a "0", (intorno a 512), quindi i 2 motori andranno rispettivamente o tutto avanti e tutto indietro o il contrario. Ma quando Y assumerà valori inferiori allo "0"? Devo invertire tutto?
Grazie

vinsub:
Ma quando Y assumerà valori inferiori allo "0"? Devo invertire tutto?

Non è chiara la tua domanda, ovviamente se vuoi un controllo tramite joy devi prevedere che i comandi variano tra +/- Max con la relativa inversione del verso di rotazione dei motori quando cambia il segno.
Molto dipende da quale joystick usi, quelli economici sfruttano solo una piccola parte del potenziometro, tipicamente un 30%, questo vuol dire che leggendoli con l'ADC di Arduino ottieni una variazione compresa tra circa 360 e 660 cont, con 512 per il centro.
Se usi joystick professionali, i più semplice costano circa 25-30 Euro, la variazione resistiva è del 99%, p.e. uno da 10k varia tra 100 e 990 ohm, 5k al centro, dal punto di vista Arduino ottieni una variazione tra circa 15 e 1010 count con tutti i benefici del caso per la risoluzione del comando.

Grazie!
Nella situazione in cui il joystick si trovi per esempio tutto a destra, i motori dovrebbero andare, quello a sinistra tutto avanti, quello a destra tutto indietro, per fare ruotare il rov sul posto, nel momento in cui io spostassi il joy leggermente in avanti, il motore di sinistra rimarrebbe tutto avanti, quello di destra dovrebbe diminuire la velocità fino ad invertire la rotazione in avanti, il mio dubbio è, nel caso spostassi invece il joy indietro, il rov dovrebbe ruotare al contrario per logica, quindi dovrei invertire la rotazione dei motori, giusto?

vinsub:
quindi dovrei invertire la rotazione dei motori, giusto?

Ovviamente si, in pratica hai due componenti provenienti dal joy, la velocità di avanzamento data dall'asse Y e quella di rotazione data dall'asse X.
Se Y vale 0, 512 per l'ADC, il rov è fermo e se sposti l'asse X ruota su se stesso a velocità variabile in verso CW se sposti X come valori positivi e CCW se sposti X come valori negativi, stiamo assumendo che gli ipotetici assi X e Y valgono 0 quando il joy è al centro (512,512 per l'ADC).
Quando se in movimento asse Y positivo se avanzi, negativo se indietreggi, l'asse X diventa l'angolo di sterzata, dal punto di vista matematico cambia poco perché comunque dovrai sommare sottrarre al controllo dei motori un valore in modo che uno accelera e l'altro decelera, la differenza rispetto alla condizione di fermo è che i motori girano nello stesso verso invece di girare con versi opposti.
Dato che non hai encoder su i motori, per determinare e imporre la velocità di rotazione degli stessi tramite un pid, non è il caso di mettersi a fare conti con la distanza tra le ruote e il loro diametro per ottimizzare la variazione della velocità di rotazione per le curve, fai prima a trovarla in modo empirico con dei test.
Se vuoi fare dei conti precisi, per la variazione della velocità di rotazione dei motori, per sterzare ecco un estratto dal libro "Art of Robotics" che sto scrivendo (a breve i primi capitoli saranno disponibili).

La differenza di velocità tra le due ruote è data :

Nel caso in cui il raggio di sterzata è maggiore di 1/2 interasse ruote

Dvel = Vm * IntA/raggio

Nel caso in cui il raggio di sterzata è minore di 1/2 interasse ruote

DVel  = Vm * Raggio/IntA

Vm è la velocità di percorrenza della curva riferita alla mezzaria del robot.
Raggio è il raggio della curva da percorrere.
IntA è la metà dell'interasse tra le ruote.
Dvel è la velocità da sommare e sottrare alle ruote per effettuare la sterzata, il calcolo vale solo se Raggio > 0

Il rov ha le eliche, stasera scrivo qualcosa e ti sarei grato se lo correggessi! Grazie!

Ciao, pardon per l’assenza, ho pensato a questo, molto elementare, pseudo codice:

asse_Y_joystick compreso da -100 e 100 (0-1023)
asse_X_Joystick come sopra
motore_destra compreso da -100 e 100 (1000-2000 "writeMicroseconds")
motore_sinistra come sopra

if asse_Y_joystick > 0
   { 
     motore_destra = asse_Y_joystick - asse_X_Joystick
     motore_sinistra = asse_Y_joystick + asse_X_Joystick
    }

else if asse_Y_joystick = 0 (range da stabilire in base al joystick, tipo (-10 < asse_Y_joystick com < 10)
   {
     motore_destra = - asse_X_Joystick
     motore_sinistra = asse_X_Joystick
    }
else if asse_Y_joystick < 0
   {
     motore_destra = asse_Y_joystick + asse_X_Joystick
     motore_sinistra = asse_Y_joystick - asse_X_Joystick
    }

Secondo te non funzionerebbe? Sarebbe troppo repentina la variazione di velocità tra y = 0 ed un’altra situazione?