QuantumJump:
Oh, well that's that. Thank you anyway.
But I'm not trying to get an orientation as Euler angles or a quaternion based on his work, that I already do myself. I'm just trying to handle the device properly without a library.
While I never quite figured out the factory self-test I have developed a calibration routine that uses PID rather than averages which I find lacking. The Calibration routine works with MPU's 6050 9150 9250 9255
It is quite fast.
If you want to know the offsets it created they can be read from the offset buffers once it's done.
take it for a spin:
//***************************************************************************************
//********************** Calibration Routines **********************
//***************************************************************************************
/**
@brief Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings
*/
#define MPU6050_RA_WHO_AM_I 0x75
#define MPU6050_WHO_AM_I_BIT 6
#define MPU6050_WHO_AM_I_LENGTH 6
uint8_t getDeviceID(){
readBits(MPU9250_ADDRESS, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
}
void CalibrateGyro(uint8_t Loops ) {
double kP = 0.3;
double kI = 90;
float x;
x = (100 - map(Loops, 1, 5, 20, 0)) * .01;
kP *= x;
kI *= x;
PID( 0x43, kP, kI, Loops);
}
/**
@brief Fully calibrate Accel from ZERO in about 6-7 Loops 600-700 readings
*/
void CalibrateAccel(uint8_t Loops ) {
float kP = 0.3;
float kI = 20;
float x;
x = (100 - map(Loops, 1, 5, 20, 0)) * .01;
kP *= x;
kI *= x;
PID( 0x3B, kP, kI, Loops);
}
void PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops) {
uint8_t SaveAddress = (ReadAddress == 0x3B) ? ((getDeviceID() < 0x38 ) ? 0x06 : 0x77) : 0x13;
int16_t Data;
float Reading;
int16_t BitZero[3];
uint8_t shift = (SaveAddress == 0x77) ? 3 : 2;
float Error, PTerm, ITerm[3];
int16_t eSample;
uint32_t eSum ;
Serial.write('>');
for (int i = 0; i < 3; i++) {
ReadWords(MPU9250_ADDRESS, SaveAddress + (i * shift), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word)
Reading = Data;
if (SaveAddress != 0x13) {
BitZero[i] = Data & 1; // Capture Bit Zero to properly handle Accelerometer calibration
ITerm[i] = ((float)Reading) * 8;
} else {
ITerm[i] = Reading * 4;
}
}
for (int L = 0; L < Loops; L++) {
eSample = 0;
for (int c = 0; c < 100; c++) {// 100 PI Calculations
eSum = 0;
for (int i = 0; i < 3; i++) {
ReadWords(MPU9250_ADDRESS, ReadAddress + (i * 2), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word)
Reading = Data;
if ((ReadAddress == 0x3B) && (i == 2)) Reading -= 16384; //remove Gravity
Error = -Reading;
eSum += abs(Reading);
PTerm = kP * Error;
ITerm[i] += (Error * 0.001) * kI; // Integral term 1000 Calculations a second = 0.001
if (SaveAddress != 0x13) {
Data = round((PTerm + ITerm[i] ) / 8); //Compute PID Output
Data = ((Data) & 0xFFFE) | BitZero[i]; // Insert Bit0 Saved at beginning
} else Data = round((PTerm + ITerm[i] ) / 4); //Compute PID Output
WriteWords(MPU9250_ADDRESS, SaveAddress + (i * shift), 1, (uint16_t *)&Data);
}
if ((c == 99) && eSum > 1000) { // Error is still to great to continue
c = 0;
Serial.write('*');
}
if ((eSum * ((ReadAddress == 0x3B)?.05 : 1)) < 5) eSample++; // Successfully found offsets prepare to advance
if ((eSum < 100) && (c > 10) && (eSample >= 10)) break; // Advance to next Loop
delay(1);
}
Serial.write('.');
kP *= .75;
kI *= .75;
for (int i = 0; i < 3; i++) {
if (SaveAddress != 0x13) {
Data = round((ITerm[i] ) / 8); //Compute PID Output
Data = ((Data) & 0xFFFE) | BitZero[i]; // Insert Bit0 Saved at beginning
} else Data = round((ITerm[i]) / 4);
WriteWords(MPU9250_ADDRESS, SaveAddress + (i * shift), 1, (uint16_t *)&Data);
}
}
}
void ReadWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data) {
int8_t count = 0;
uint32_t t1 = millis();
for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.write(regAddr);
Wire.endTransmission();
Wire.beginTransmission(devAddr);
Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
bool msb = true; // starts with MSB, then LSB
for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) {
if (msb) {
// first byte is bits 15-8 (MSb=15)
data[count] = Wire.read() << 8;
} else {
// second byte is bits 7-0 (LSb=0)
data[count] |= Wire.read();
count++;
}
msb = !msb;
}
Wire.endTransmission();
}
}
void WriteWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) {
Wire.beginTransmission(devAddr);
Wire.write(regAddr); // send address
for (uint8_t i = 0; i < length; i++) {
Wire.write((uint8_t)(data[i] >> 8)); // send MSB
Wire.write((uint8_t)data[i]); // send LSB
status = Wire.endTransmission();
}
}
void ReadBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) {
// 01101001 read byte
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 010 masked
// -> 010 shifted
uint8_t b;
Wire.beginTransmission(devAddr);
Wire.write(regAddr);
Wire.endTransmission();
Wire.beginTransmission(devAddr);
Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH));
if (Wire.available()) {
b = Wire.read();
}
status = Wire.endTransmission();
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
b &= mask;
b >>= (bitStart - length + 1);
*data = b;
}
}
Z