以下是SparkFun和Adafruit的代码片段，它使用MPU-9150将3轴的加速度，陀螺仪和罗盘输出到显示器。我试图修改代码，因此我打印到串口，因为我没有要使用的显示器。但是，无论筹码位置或动作如何，我总能获得相同的输出。
输出：
ax = 0.00 ay = 0.00 az = 0.00 mg
gx = 0.00 gy = 0.00 gz = 0.00 deg/s
mx = 0 my = 0 mz = 0 mG
q0 = 1.00 qx = 0.00 qy = 0.00 qz = 0.00
Yaw, Pitch, Roll: 0.00, 0.00, 0.00
rate = 778.82 Hz
x y z 0
0
0
mg
0
0
0
o/s
0
0
0
mG
0
0
0
ypr
代码：
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050_9Axis_MotionApps41.h"
//#include <Adafruit_GFX.h>
//#include <Adafruit_PCD8544.h>
// Using NOKIA 5110 monochrome 84 x 48 pixel display
// pin 9 - Serial clock out (SCLK)
// pin 8 - Serial data out (DIN)
// pin 7 - Data/Command select (D/C)
// pin 5 - LCD chip select (CS)
// pin 6 - LCD reset (RST)
//Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
// Declare device MPU6050 class
MPU6050 mpu;
// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
#define GyroMeasError PI * (40.0f / 180.0f) // gyroscope measurement error in rads/s (shown as 3 deg/s)
#define GyroMeasDrift PI * (0.0f / 180.0f) // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s)
// There is a tradeoff in the beta parameter between accuracy and response speed.
// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
#define beta sqrt(3.0f / 4.0f) * GyroMeasError // compute beta
#define zeta sqrt(3.0f / 4.0f) * GyroMeasDrift // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 0.0f
int16_t a1, a2, a3, g1, g2, g3, m1, m2, m3; // raw data arrays reading
uint16_t count = 0; // used to control display output rate
uint16_t delt_t = 0; // used to control display output rate
uint16_t mcount = 0; // used to control display output rate
uint8_t MagRate; // read rate for magnetometer data
float pitch, yaw, roll;
float deltat = 0.0f; // integration interval for both filter schemes
uint16_t lastUpdate = 0; // used to calculate integration interval
uint16_t now = 0; // used to calculate integration interval
float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method
void setup()
{
Serial.begin(38400); // Start serial at 38400 bps
//display.begin(); // Initialize the display
//display.setContrast(58); // Set the contrast
//display.setRotation(0); // 0 or 2) width = width, 1 or 3) width = height, swapped etc.
// Start device display with ID of sensor
/*
display.clearDisplay();
display.setTextSize(2);
display.setCursor(0,0); display.print("MPU9150");
display.setTextSize(1);
display.setCursor(0, 20); display.print("9 DOF sensor");
display.setCursor(0, 30); display.print("data fusion");
display.setCursor(20, 40); display.print("AHRS");
display.display();
delay(2000);
// Set up for data display
display.setTextSize(1); // Set text size to normal, 2 is twice normal etc.
display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen
display.clearDisplay(); // clears the screen and buffer
display.display();
*/
// initialize MPU6050 device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU9150 connection successful") : F("MPU9150 connection failed"));
// Set up the accelerometer, gyro, and magnetometer for data output
mpu.setRate(7); // set gyro rate to 8 kHz/(1 * rate) shows 1 kHz, accelerometer ODR is fixed at 1 KHz
MagRate = 10; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values
// Digital low pass filter configuration.
// It also determines the internal sampling rate used by the device as shown in the table below.
// The accelerometer output rate is fixed at 1kHz. This means that for a Sample
// Rate greater than 1kHz, the same accelerometer sample may be output to the
// FIFO, DMP, and sensor registers more than once.
/*
* | ACCELEROMETER | GYROSCOPE
* DLPF_CFG | Bandwidth | Delay | Bandwidth | Delay | Sample Rate
* ---------+-----------+--------+-----------+--------+-------------
* 0 | 260Hz | 0ms | 256Hz | 0.98ms | 8kHz
* 1 | 184Hz | 2.0ms | 188Hz | 1.9ms | 1kHz
* 2 | 94Hz | 3.0ms | 98Hz | 2.8ms | 1kHz
* 3 | 44Hz | 4.9ms | 42Hz | 4.8ms | 1kHz
* 4 | 21Hz | 8.5ms | 20Hz | 8.3ms | 1kHz
* 5 | 10Hz | 13.8ms | 10Hz | 13.4ms | 1kHz
* 6 | 5Hz | 19.0ms | 5Hz | 18.6ms | 1kHz
*/
mpu.setDLPFMode(4); // set bandwidth of both gyro and accelerometer to ~20 Hz
// Full-scale range of the gyro sensors:
// 0 = +/- 250 degrees/sec, 1 = +/- 500 degrees/sec, 2 = +/- 1000 degrees/sec, 3 = +/- 2000 degrees/sec
mpu.setFullScaleGyroRange(0); // set gyro range to 250 degrees/sec
// Full-scale accelerometer range.
// The full-scale range of the accelerometer: 0 = +/- 2g, 1 = +/- 4g, 2 = +/- 8g, 3 = +/- 16g
mpu.setFullScaleAccelRange(0); // set accelerometer to 2 g range
mpu.setIntDataReadyEnabled(true); // enable data ready interrupt
}
void loop()
{
if(mpu.getIntDataReadyStatus() == 1) { // wait for data ready status register to update all data registers
mcount++;
// read the raw sensor data
mpu.getAcceleration ( &a1, &a2, &a3 );
ax = a1*2.0f/32768.0f; // 2 g full range for accelerometer
ay = a2*2.0f/32768.0f;
az = a3*2.0f/32768.0f;
mpu.getRotation ( &g1, &g2, &g3 );
gx = g1*250.0f/32768.0f; // 250 deg/s full range for gyroscope
gy = g2*250.0f/32768.0f;
gz = g3*250.0f/32768.0f;
// The gyros and accelerometers can in principle be calibrated in addition to any factory calibration but they are generally
// pretty accurate. You can check the accelerometer by making sure the reading is +1 g in the positive direction for each axis.
// The gyro should read zero for each axis when the sensor is at rest. Small or zero adjustment should be needed for these sensors.
// The magnetometer is a different thing. Most magnetometers will be sensitive to circuit currents, computers, and
// other both man-made and natural sources of magnetic field. The rough way to calibrate the magnetometer is to record
// the maximum and minimum readings (generally achieved at the North magnetic direction). The average of the sum divided by two
// should provide a pretty good calibration offset. Don't forget that for the MPU9150, the magnetometer x- and y-axes are switched
// compared to the gyro and accelerometer!
if (mcount > 1000/MagRate) { // this is a poor man's way of setting the magnetometer read rate (see below)
mpu.getMag ( &m1, &m2, &m3 );
mx = m1*10.0f*1229.0f/4096.0f + 18.0f; // milliGauss (1229 microTesla per 2^12 bits, 10 mG per microTesla)
my = m2*10.0f*1229.0f/4096.0f + 70.0f; // apply calibration offsets in mG that correspond to your environment and magnetometer
mz = m3*10.0f*1229.0f/4096.0f + 270.0f;
mcount = 0;
}
}
now = micros();
deltat = ((now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = now;
// Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer;
// the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro!
// We have to make some allowance for this orientationmismatch in feeding the output to the quaternion filter.
// For the MPU-9150, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like
// in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention.
// This is ok by aircraft orientation standards!
// Pass gyro rate as rad/s
MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
// MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
// Serial print and/or display at 0.5 s rate independent of data rates
delt_t = millis() - count;
if (delt_t > 500) { // update LCD once per half-second independent of read rate
Serial.print("ax = "); Serial.print((int)1000*ax);
Serial.print(" ay = "); Serial.print((int)1000*ay);
Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
Serial.print("gx = "); Serial.print( gx, 2);
Serial.print(" gy = "); Serial.print( gy, 2);
Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
Serial.print("mx = "); Serial.print( (int)mx );
Serial.print(" my = "); Serial.print( (int)my );
Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");
Serial.print("q0 = "); Serial.print(q[0]);
Serial.print(" qx = "); Serial.print(q[1]);
Serial.print(" qy = "); Serial.print(q[2]);
Serial.print(" qz = "); Serial.println(q[3]);
// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI - 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
roll *= 180.0f / PI;
Serial.print("Yaw, Pitch, Roll: ");
Serial.print(yaw, 2);
Serial.print(", ");
Serial.print(pitch, 2);
Serial.print(", ");
Serial.println(roll, 2);
Serial.print("rate = "); Serial.print((float)1.0f/deltat, 2); Serial.println(" Hz");
/*
display.clearDisplay();
display.setCursor(0, 0); display.print(" x y z ");
display.setCursor(0, 8); display.print((int)(1000*ax));
display.setCursor(24, 8); display.print((int)(1000*ay));
display.setCursor(48, 8); display.print((int)(1000*az));
display.setCursor(72, 8); display.print("mg");
display.setCursor(0, 16); display.print((int)(gx));
display.setCursor(24, 16); display.print((int)(gy));
display.setCursor(48, 16); display.print((int)(gz));
display.setCursor(66, 16); display.print("o/s");
display.setCursor(0, 24); display.print((int)(mx));
display.setCursor(24, 24); display.print((int)(my));
display.setCursor(48, 24); display.print((int)(mz));
display.setCursor(72, 24); display.print("mG");
display.setCursor(0, 32); display.print((int)(yaw));
display.setCursor(24, 32); display.print((int)(pitch));
display.setCursor(48, 32); display.print((int)(roll));
display.setCursor(66, 32); display.print("ypr");
// With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and
// >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz.
// The filter update rate is determined mostly by the mathematical steps in the respective algorithms,
// the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR:
// an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces
// filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively.
// This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.
// This filter update rate should be fast enough to maintain accurate platform orientation for
// stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
// produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
// The 3.3 V 8 MHz Pro Mini is doing pretty well!
display.setCursor(0, 40); display.print("rt: "); display.print((1/deltat)); display.print(" Hz");
display.display();
count = millis();
*/
Serial.print(" x y z ");
Serial.println((int)(1000*ax));
Serial.println((int)(1000*ay));
Serial.println((int)(1000*az));
Serial.println("mg");
Serial.println((int)(gx));
Serial.println((int)(gy));
Serial.println((int)(gz));
Serial.println("o/s");
Serial.println((int)(mx));
Serial.println((int)(my));
Serial.println((int)(mz));
Serial.println("mG");
Serial.println((int)(yaw));
Serial.println((int)(pitch));
Serial.println((int)(roll));
Serial.println("ypr");
}
}
// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
norm = 1.0f/norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * deltat;
q2 += qDot2 * deltat;
q3 += qDot3 * deltat;
q4 += qDot4 * deltat;
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f/norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}
// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
// measured ones.
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float pa, pb, pc;
// Auxiliary variables to avoid repeated arithmetic
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
bx = sqrt((hx * hx) + (hy * hy));
bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
// Estimated direction of gravity and magnetic field
vx = 2.0f * (q2q4 - q1q3);
vy = 2.0f * (q1q2 + q3q4);
vz = q1q1 - q2q2 - q3q3 + q4q4;
wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
// Error is cross product between estimated direction and measured direction of gravity
ex = (ay * vz - az * vy) + (my * wz - mz * wy);
ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
if (Ki > 0.0f)
{
eInt[0] += ex; // accumulate integral error
eInt[1] += ey;
eInt[2] += ez;
}
else
{
eInt[0] = 0.0f; // prevent integral wind up
eInt[1] = 0.0f;
eInt[2] = 0.0f;
}
// Apply feedback terms
gx = gx + Kp * ex + Ki * eInt[0];
gy = gy + Kp * ey + Ki * eInt[1];
gz = gz + Kp * ez + Ki * eInt[2];
// Integrate rate of change of quaternion
pa = q2;
pb = q3;
pc = q4;
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
// Normalise quaternion
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}
答案 0 :(得分：1)
这里有一些代码可供您查看：http://playground.arduino.cc/Main/MPU-9150 另外，请看一下：https://github.com/zeran/MPU9150Lib 这对你来说似乎很有趣。