Hi everybody,
I'm developing an arduino app to calibrate a gyro. I tried it on a arduino UNO and it works fine, and as same as in the DUE, but now I have a very strange trouble only with the DUE: The gyro (which employs i2C protocol) always reads +500 º/s, but only into X and Y axis!!!, that is crazy, and I have no idea about what can be happening. Please, Does anyone have the same problem?.
Sensor:
IMU -> MPU6050 based GY521 IMU.
Main code:
// Arduino IMU lector - Adaptado a necesidades
#include <Wire.h>
/* Constantes a leer */
int accX, accY, accZ;
int gyroX, gyroY, gyroZ;
int tempRaw;
double bgX, bgY, bgZ;
/* Variables para timer y buffer */
uint8_t i2cData[14]; // Buffer for I2C data
void setup() {
// Inicializo IMU
Serial.begin(9600);
Wire.begin();
Wire.flush();
//TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x06; // Disable FSYNC and set 5 Hz Acc filtering, 5 Hz Gyro filtering, 1 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
bgX = -2.075921122383778;
bgY = -0.072308549354311;
bgZ = 0.547682427661292;
}
void loop() {
// put your main code here, to run repeatedly:
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]); // Aceleración eje X (parte alta y parte baja)
accY = ((i2cData[2] << 8) | i2cData[3]); // Aceleración eje Y (parte alta y parte baja)
accZ = ((i2cData[4] << 8) | i2cData[5]); // Aceleración eje Z (parte alta y parte baja)
tempRaw = (i2cData[6] << 8) | i2cData[7]; // Temperatura
gyroX = (i2cData[8] << 8) | i2cData[9]; // Ratio de giro eje X
gyroY = (i2cData[10] << 8) | i2cData[11]; // Ratio de giro eje Y
gyroZ = (i2cData[12] << 8) | i2cData[13]; // Ratio de giro eje Z
Serial.print((double) gyroX/131.0-bgX,10); Serial.print(","); Serial.print((double) gyroY/131.0-bgY,10);Serial.print(",");
Serial.print((double) gyroZ/131.0-bgZ,10);
Serial.print("\r\n");
delay(10); // Espero 10 mili segundos (si pongo menos, matlab falla)
}
i2C library:
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
*/
const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
}
uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data, length);
uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
if (rcode) {
Serial.print(F("i2cWrite failed: "));
Serial.println(rcode);
}
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}
uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
uint32_t timeOutTimer;
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
if (rcode) {
Serial.print(F("i2cRead failed: "));
Serial.println(rcode);
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}
Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
for (uint8_t i = 0; i < nbytes; i++) {
if (Wire.available())
data[i] = Wire.read();
else {
timeOutTimer = micros();
while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
if (Wire.available())
data[i] = Wire.read();
else {
Serial.println(F("i2cRead timeout"));
return 5; // This error value is not already taken by endTransmission
}
}
}
return 0; // Success
}
Note: bias (bgX, bgY, bgZ) has been obtained previously using Matlab with the arduino UNO and the same code.
Thanks everyone.