#include <Kalman.h>
#include <Wire.h>
#include <Math.h>
float fRad2Deg = 57.295779513f; //The multiplier that converts radians to angles
const int MPU = 0x68; //The I2C address of MPU-6050
const int nValCnt = 7; //The number of registers read at one time
const int nCalibTimes = 1000; //Number of readings during calibration
int calibData[nValCnt]; //Calibration data
unsigned long nLastTime = 0; //The time of the last reading
float fLastRoll = 0.0f; //The Roll angle obtained from the last filter
float fLastPitch = 0.0f; //The Pitch angle obtained from the last filter
Kalman kalmanRoll; //Roll angle Filter
Kalman kalmanPitch; //Pitch angle filter
void setup() { Serial.begin(9600); //Initialize the serial port and specify the baud rate Wire.begin(); //Initialize the Wire library WriteMPUReg(0x6B, 0); //Start the MPU6050 device
Calibration(); //Perform calibration
nLastTime = micros(); //Record current time
}
void loop() { int readouts[nValCnt]; ReadAccGyr(readouts); //read the measured value float realVals[7]; Rectify(readouts, realVals); //correct according to the calibration offset
//Calculate the modulus length of the acceleration vector, all in g
float fNorm = sqrt(realVals[0] * realVals[0] + realVals[1] * realVals[1] + realVals[2] * realVals[2]);
float fRoll = GetRoll(realVals, fNorm); //Calculate Roll angle
if (realVals[1]> 0) { fRoll = -fRoll; } float fPitch = GetPitch(realVals, fNorm); //Calculate Pitch angle if (realVals[ 0] <0) { fPitch = -fPitch; }
//Calculate the time interval dt between two measurements, in seconds
unsigned long nCurTime = micros();
float dt = (double)(nCurTime-nLastTime) / 1000000.0;
//Kalman filter
float for Roll and Pitch angles fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt);
float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dt);
//Calculate the angle speed based on the filter value
float fRollRate = (fNewRoll-fLastRoll) / dt;
float fPitchRate = (fNewPitch-fLastPitch) / dt;
//Update Roll and Pitch angles
fLastRoll = fNewRoll;
fLastPitch = fNewPitch;
//Update the time of this measurement
nLastTime = nCurTime;
//Print out Roll and Pitch angles to the serial port, and check
Serial.print("Roll:");
Serial.print(fNewRoll); Serial.print('(');
Serial. print(fRollRate); Serial.print("),\tPitch:");
Serial.print(fNewPitch); Serial.print('(');
Serial.print(fPitchRate); Serial.print(")\n" );
delay(10);
}
//Write one byte of data to MPU6050
//Specify the register address and the value of one byte
void WriteMPUReg(int nReg, unsigned char nVal) { Wire.beginTransmission(MPU); Wire.write(nReg); Wire.write (nVal); Wire.endTransmission(true); }
//Read one byte of data from MPU6050
//Specify the register address and return the read value
unsigned char ReadMPUReg(int nReg) { Wire.beginTransmission(MPU); Wire.write(nReg); Wire.requestFrom(MPU, 1, true); Wire.endTransmission(true); return Wire.read(); }
//Read the three components of the accelerometer, temperature and three angular velocities from MPU6050
//Save in the specified array
void ReadAccGyr(int *pVals) { Wire.beginTransmission(MPU); Wire.write(0x3B); Wire. requestFrom(MPU, nValCnt * 2, true); Wire.endTransmission(true); for (long i = 0; i <nValCnt; ++i) { pVals[i] = Wire.read() << 8 | Wire. read(); } }
// Calculate a large number of readings and calibrate the average offset
void Calibration()
{ float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0}; //seek first And for (int i = 0; i <nCalibTimes; ++i) { int mpuVals[nValCnt]; ReadAccGyr(mpuVals); for (int j = 0; j <nValCnt; ++j) { valSums[j] += mpuVals[j]; } } //Find the average again for (int i = 0; i <nValCnt; ++i) { calibData[i] = int(valSums[i] / nCalibTimes); } calibData[2] += 16384; //Set the Z axis of the chip vertically downward, and set the static working point. }
//Calculate the roll angle. See the documentation for the algorithm.
float GetRoll(float *pRealVals, float fNorm) { float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]); float fCos = fNormXZ / fNorm; return acos(fCos) * fRad2Deg; }
//Calculate the pitch angle. See the documentation for the algorithm.
float GetPitch(float *pRealVals, float fNorm) { float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]); float fCos = fNormYZ / fNorm; return acos(fCos) * fRad2Deg; }
//Correct the reading, eliminate the offset, and convert it to a physical quantity. See the documentation for the formula.
void Rectify(int *pReadout, float *pRealVals) { for (int i = 0; i <3; ++i) { pRealVals[i] = (float)(pReadout[i]-calibData[i]) / 16384.0f ; } pRealVals[3] = pReadout[3] / 340.0f + 36.53; for (int i = 4; i <7; ++i) { pRealVals[i] = (float)(pReadout[i]-calibData[i ]) / 131.0f; } }