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);
}
