陀螺仪加速度计 MPU6050 启用DMP运算角度代码
本人新人——废话不多说了……直接上代码……官网也有这个代码，但是发这里大家用的方便……：）
记得把int接入D2口作为中断
接线和以前一样接arduino 的 I2C接口
SDL SCL两个口
3.3v电源
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu(0x68);
// MPU control/status vars
bool dmpReady = false;// set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // quaternion container
VectorFloat gravity; // gravity vector
float ypr; // yaw/pitch/roll container and gravity vector
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void mpu_setup() {
//Serial.begin(115200); // opens serial port, sets data rate to 9600 bps
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// initialize device
Serial.println("Initializing I2C devices...");
mpu.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
delay(2);
// load and configure the DMP
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println("Enabling DMP...");
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println("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("DMP Initialization failed (code ");
Serial.print(devStatus);
Serial.println(")");
}
}
void mpu_loop()
{
float alpha,omiga;
// if programming failed, don't try to do anything
if (!dmpReady)
return;
// wait for MPU interrupt or extra packet(s) available
if (!mpuInterrupt && fifoCount < packetSize)
return;
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println("FIFO overflow!");
// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
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;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);//从DMP中取出Yaw、Pitch、Roll三个轴的角度，放入数组ypr。单位：弧度
alpha=-ypr * 180/M_PI;
omiga=mpu.getRotationX()/16.4; //配置是16位表示正负2000°/s, 65536/4000
Serial.print("Alpha ");
Serial.print(alpha);//x轴平行偏移角度
Serial.print("\tOmiga ");
Serial.println(omiga);//对应加速度力道
}
}
另外谁研究过dmp的电子罗盘接入？
请分享下 您好:
請問陀螺儀或加速器的中斷口是做什麼用的啊??
謝謝 TTTTTTT33 发表于 2013-9-5 15:19 static/image/common/back.gif
您好:
請問陀螺儀或加速器的中斷口是做什麼用的啊??
中断功能很多，比如fifo缓冲溢出、数据准备完毕，都会使用这个中断进行触发，还有我记得文档内还有mpu的跌落中断，好像是当失控跌落的时候也会产生中断
使用mpu.getIntStatus(); 可以获取到此次中断的状态 有空找个机会测试一下楼主的代码，谢谢楼主分享:P 楼主，试过滚角Roll没？很怪异，数值乱变！ 有空测试一下:) 好多东西没搞明白 尤其是中断 1.6.5版本编译通不过 敢问楼主的库在哪里下载？ DMP 运算角度的精度如何
页:
[1]