这就是传说中的10DOF？ MPU6050 + HMC5883L + BMP180

hi55234 发表于 2014-9-28 11:05:56

问题：
1、偏航初始值不为零怎么破？
2、这个数据一般怎么用？
3、如何把偏航修正为与HMC5883L一致？

连接：全都在arduino的i2c总线上
备注：BMP180用的BMP085的库，也就是最小分辨率只有后者的一半（taobao的JS说这是升级版，其实是降级版啦）
效果：

代码：
//输出控制:

// 实际在四元组件格式
#define OUTPUT_READABLE_QUATERNION

// 欧拉角
#define OUTPUT_READABLE_EULER

// 偏航/俯仰/滚动角（以度为单位）
#define OUTPUT_READABLE_YAWPITCHROLL

//加速度，扣除重力影响
#define OUTPUT_READABLE_REALACCEL

// 加速度，扣除重力影响+参照偏航
#define OUTPUT_READABLE_WORLDACCEL

//输出航向 HMC5883L
#define OUTPUT_HMC5883L

//程序耗时，剩余内存数查询
#define OUTPUT_TIME_AND_MemoryFree

//BMP085
#define OUTPUT_BMP085

/*---------------------输出控制 end---------------------------------*/

//MPU6050:

// I2Cdev和MPU6050必须安装的库
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;// AD0 low = 0x68
//MPU6050 mpu(0x69); // <-- use for AD0 high

/*
科普：
按照posix标准，一般整形对应的*_t类型为：
1字节 uint8_t 近似于 byte
2字节 uint16_t 近似于 int
4字节 uint32_t
8字节 uint64_t
*/

// 控制/状态
bool dmpReady = false; // 设置为true，如果DMP初始化成功
//uint8_t mpuIntStatus; //持有MPU的实际中断状态字节
uint8_t devStatus; // 每个设备运行后返回的状态（0=成功非0=错误）
uint16_t packetSize; // 预计DMP数据包大小（默认为42字节）
uint16_t fifoCount; // 计算当前FIFO中的所有字节
uint8_t fifoBuffer; // FIFO存储缓冲器

// 方向/运动
Quaternion q; // 四元数
VectorInt16 aa; // 加速度传感器测量
VectorInt16 aaReal; // 无重力加速度传感器的测量
VectorInt16 aaWorld; // 世界框架加速度传感器测量
VectorFloat gravity; // 重力矢量
float euler; // 欧拉角容器
float ypr; // 偏航/俯仰/滚动容器和重力矢量

/*---------------------MPU6050 end---------------------------------*/

//HMC5883L

#include "HMC5883L.h"
// class default I2C address is 0x1E
// specific I2C addresses may be passed as a parameter here
// this device only supports one I2C address (0x1E)
HMC5883L mag;
int16_t mx, my, mz;

/*---------------------HMC5883L end---------------------------------*/

//BMP180 用的是BMP085的库，实际最小分辨率只有后者的一半
#include "BMP085.h"
BMP085 barometer;
float temperature;
float pressure;
float altitude;
int32_t lastMicros;
/*---------------------BMP085 end---------------------------------*/

//程序耗时，剩余内存数查询
unsigned long jsq6x;
#include <MemoryFree.h>//剩余内存查询
/*---------------------程序耗时，剩余内存数查询 end---------------------------------*/

// ================================================================
// === 初始设置 ===
// ================================================================

void setup() {

// 加入I2C总线（I2Cdev库没有自动执行此操作）

Wire.begin();
TWBR = 24; // 400kHz的I2C时钟（200kHz的，如果CPU为8MHz）

//初始化串行通信波特率115200
Serial.begin(9600);

// 初始化设备
mpu.initialize();

// 加载和配置DMP
devStatus = mpu.dmpInitialize();

//提供自己的陀螺仪偏置在这里，对于规模最小灵敏度
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688出厂默认为我的测试芯片

// 确保它的工作（返回0，如果这样）
if (devStatus == 0) {

// 打开DMP，现在，它已经准备好
mpu.setDMPEnabled(true);

// 设置我们的DMP Ready标志使主循环（）函数知道它的好来使用它
dmpReady = true;

// 获得预期的DMP数据包大小为以后进行比较
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
//错误！
//1=初始内存加载失败
//2= DMP配置更新失败
//（如果它要打破，通常的代码为1）
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}

/////////////////////////////////////////HMC5883L
mag.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(mag.testConnection() ? "HMC5883L connection successful" : "HMC5883L connection failed");

//////////////////////////////////////////BMP085
barometer.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(barometer.testConnection() ? "BMP085 connection successful" : "BMP085 connection failed");

}

// ================================================================
// === 主程序循环 ===
// ================================================================

void loop() {

#ifdef OUTPUT_TIME_AND_MemoryFree
jsq6x=millis();
#endif

// 如果编程失败，不要尝试做任何事情
if (!dmpReady) return;

//mpuIntStatus = mpu.getIntStatus();

//等待正确可用的数据的长度，应该是一个非常短暂的等待
while (fifoCount < packetSize)fifoCount = mpu.getFIFOCount();

//读取FIFO中的数据包
mpu.getFIFOBytes(fifoBuffer, packetSize);

//轨道先进先出算在这里万一有>1包可
//（这让我们马上了解更多，而无需等待中断）
fifoCount -= packetSize;

#ifdef OUTPUT_READABLE_QUATERNION
// 显示四元素 in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
#endif

#ifdef OUTPUT_READABLE_EULER
// 显示欧拉角的度数
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler * 180/M_PI);
Serial.print("\t");
Serial.print(euler * 180/M_PI);
Serial.print("\t");
Serial.println(euler * 180/M_PI);
#endif

#ifdef OUTPUT_READABLE_YAWPITCHROLL
//偏航/ 从四元数来计算俯仰/滚动角（以度为单位）
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t");
Serial.print(ypr * 180/M_PI);
Serial.print("\t");
Serial.print(ypr * 180/M_PI);
Serial.print("\t");
Serial.println(ypr * 180/M_PI);
#endif

#ifdef OUTPUT_READABLE_REALACCEL
// 显示真正的加速，调整，去掉重力
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Serial.print("areal\t");
Serial.print(aaReal.x);
Serial.print("\t");
Serial.print(aaReal.y);
Serial.print("\t");
Serial.println(aaReal.z);
#endif

#ifdef OUTPUT_READABLE_WORLDACCEL
//显示最初的世界框架的加速，调整以扣除地心引力
//和旋转的基础上，从四元数已知的方向
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
Serial.print("aworld\t");
Serial.print(aaWorld.x);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
#endif

#ifdef OUTPUT_HMC5883L
// 输出航向
// read raw heading measurements from device
mag.getHeading(&mx, &my, &mz);

// To calculate heading in degrees. 0 degree indicates North
float heading = atan2(my, mx);
if(heading < 0)
heading += 2 * M_PI;
Serial.print("heading:\t");
Serial.println(heading * 180/M_PI);
#endif

#ifdef OUTPUT_BMP085

barometer.setControl(BMP085_MODE_TEMPERATURE);

// wait appropriate time for conversion (4.5ms delay)
lastMicros = micros();
while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

// read calibrated temperature value in degrees Celsius
temperature = barometer.getTemperatureC();

// request pressure (3x oversampling mode, high detail, 23.5ms delay)
barometer.setControl(BMP085_MODE_PRESSURE_3);
while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

// read calibrated pressure value in Pascals (Pa)
pressure = barometer.getPressure();

// calculate absolute altitude in meters based on known pressure
// (may pass a second "sea level pressure" parameter here,
// otherwise uses the standard value of 101325 Pa)
altitude = barometer.getAltitude(pressure);

// display measured values if appropriate
Serial.print("T/P/A\t");
Serial.print(temperature); Serial.print("\t");
Serial.print(pressure); Serial.print("\t");
Serial.print(altitude);
Serial.println("");
#endif

#ifdef OUTPUT_TIME_AND_MemoryFree
jsq6x=millis()-jsq6x;
Serial.print("jsq6x=");
Serial.print(jsq6x);
Serial.println("ms");
Serial.print("freeMemory()=");
Serial.println(freeMemory());
Serial.println("/*******************************************/");
Serial.println();
Serial.println();
#endif

// 获取当前FIFO计数
//fifoCount = mpu.getFIFOCount();
//重置，以便我们能继续干净
mpu.resetFIFO();

}

hi55234 发表于 2014-9-29 20:18:44

10DOF（MPU6050 + HMC5883L + BMP180）+ GPS

程序全开，编译后28k
MPU6050仅输出偏航/俯仰/滚动角，编译后24K

//输出控制:

// 实际在四元组件格式
#define OUTPUT_READABLE_QUATERNION

// 欧拉角
#define OUTPUT_READABLE_EULER

// 偏航/俯仰/滚动角（以度为单位）
#define OUTPUT_READABLE_YAWPITCHROLL

//加速度，扣除重力影响
#define OUTPUT_READABLE_REALACCEL

// 加速度，扣除重力影响+参照偏航
#define OUTPUT_READABLE_WORLDACCEL

//输出航向 HMC5883L
#define OUTPUT_HMC5883L

//程序耗时，剩余内存数查询其他测试项目
#define OUTPUT_TIME_other_MemoryFree

//BMP085
#define OUTPUT_BMP085

//GPS
#define OUTPUT_GPS

/*---------------------输出控制 end---------------------------------*/

//MPU6050:

// I2Cdev和MPU6050必须安装的库
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;// AD0 low = 0x68
//MPU6050 mpu(0x69); // <-- use for AD0 high

/*
科普：
按照posix标准，一般整形对应的*_t类型为：
1字节 uint8_t 近似于 byte
2字节 uint16_t 近似于 int
4字节 uint32_t
8字节 uint64_t
*/

// 控制/状态
bool dmpReady = false; // 设置为true，如果DMP初始化成功
//uint8_t mpuIntStatus; //持有MPU的实际中断状态字节
uint8_t devStatus; // 每个设备运行后返回的状态（0=成功非0=错误）
uint16_t packetSize; // 预计DMP数据包大小（默认为42字节）
uint16_t fifoCount; // 计算当前FIFO中的所有字节
uint8_t fifoBuffer; // FIFO存储缓冲器

// 方向/运动
Quaternion q; // 四元数
VectorInt16 aa; // 加速度传感器测量
VectorInt16 aaReal; // 无重力加速度传感器的测量
VectorInt16 aaWorld; // 世界框架加速度传感器测量
VectorFloat gravity; // 重力矢量
float euler; // 欧拉角容器
float ypr; // 偏航/俯仰/滚动容器和重力矢量

/*---------------------MPU6050 end---------------------------------*/

//HMC5883L

#include "HMC5883L.h"
// class default I2C address is 0x1E
// specific I2C addresses may be passed as a parameter here
// this device only supports one I2C address (0x1E)
HMC5883L mag;
int16_t mx, my, mz;

/*---------------------HMC5883L end---------------------------------*/

//BMP180 用的是BMP085的库，实际最小分辨率只有后者的一半
#include "BMP085.h"
BMP085 barometer;
float temperature;
float pressure;
float altitude;
int32_t lastMicros;
/*---------------------BMP085 end---------------------------------*/

//程序耗时，剩余内存数查询
unsigned long jsq6x;
#include <MemoryFree.h>//剩余内存查询
/*---------------------程序耗时，剩余内存数查询 end---------------------------------*/

//GPS
//////////////////////////////////////////////////////////////////////////////////////////////

#include <SoftwareSerial.h>
SoftwareSerial gps(8, 9); // RX, TX
char weidu;//仅适用于00 00.0000N 这种长度（格式）的gps输出！！
char jingdu;//仅适用于000 00.0000E 这种长度（格式）的gps输出！！
char GPRMC;//临时数组1，放gps数据的
unsigned long gpstimes;//gps计时器，如果超过一定时间10ms，则证明gps数据已经发送完毕
//unsigned long quanjujishu;//全局计数，计算1秒之内，循环了多少次，这个次数在一定程度上表示了剩余性能
boolean konghangpanduan;//空行判断，因为在读取串口数据后并没有延迟，完全依赖全局循环，那么，特定程序是否执行，就全靠布尔量来标记了
int jsq1=0;
boolean panduan1=0;
boolean panduan2=0;
String jianyan="";//GPS数据包的ASCII异或校验
int yihuoyunsuan;//异或运算——校验结果
boolean jiaoyanjieguo=0;//校验结果
boolean dingweiok=0;//定位有效
/*---------------------GPS end---------------------------------*/

// ================================================================
// === 初始设置 ===
// ================================================================

void setup() {

// 加入I2C总线（I2Cdev库没有自动执行此操作）

Wire.begin();
TWBR = 24; // 400kHz的I2C时钟（200kHz的，如果CPU为8MHz）

//初始化串行通信波特率115200
Serial.begin(9600);

// 初始化设备
mpu.initialize();

// 加载和配置DMP
devStatus = mpu.dmpInitialize();

//提供自己的陀螺仪偏置在这里，对于规模最小灵敏度
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688出厂默认为我的测试芯片

// 确保它的工作（返回0，如果这样）
if (devStatus == 0) {

// 打开DMP，现在，它已经准备好
mpu.setDMPEnabled(true);

// 设置我们的DMP Ready标志使主循环（）函数知道它的好来使用它
dmpReady = true;

// 获得预期的DMP数据包大小为以后进行比较
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
//错误！
//1=初始内存加载失败
//2= DMP配置更新失败
//（如果它要打破，通常的代码为1）
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}

/////////////////////////////////////////HMC5883L
mag.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(mag.testConnection() ? "HMC5883L connection successful" : "HMC5883L connection failed");

//////////////////////////////////////////BMP085
barometer.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(barometer.testConnection() ? "BMP085 connection successful" : "BMP085 connection failed");

///////////////////////////////////////////GPS
gps.begin(9600);

//////////////////////////////////////////////////////////////////////

}

// ================================================================
// === 主程序循环 ===
// ================================================================

void loop() {

#ifdef OUTPUT_TIME_other_MemoryFree
jsq6x=millis();
#endif

#ifdef OUTPUT_GPS

//无条件接受串口的字符，目的截取GPRMC字符串
while (gps.available() > 0) {
gpstimes=millis();//重置gpstimes为millis()
konghangpanduan=0;//因为串口有数据，所以空行判断为0，即不空行
if(panduan2)gps.read();//采集到所需数据后，舍弃其他串口数据
else GPRMC = gps.read();//软串口读取GPS数据，并保存在临时数组GPRMC中

//delayMicroseconds(50);

//做一个保险，保证数组不会溢出，同时也限制了最大字符串的长度（99）
//为兼容$GPGGA截取、或GPRMC不在最后一行的情况，设定一旦采集完毕，就锁定jsq1的值
if(jsq1<99 && !panduan2) jsq1++;
else if(!panduan2) jsq1=99;

if(jsq1>5 && !panduan1){

if(GPRMC=='M' && GPRMC=='C')panduan1=1;
else{
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
jsq1=5;
}

}

if(panduan1 && GPRMC=='*')panduan2=1;

}

#endif

//如果连续10ms串口都没有收到新的数据，则说明一个周期内（1秒）的GPS数据发送已完毕
//这个空行一个周期只做一次，所以同时加入布尔量konghangpanduan的判断
if(millis()-gpstimes>10 && !konghangpanduan){
konghangpanduan=1;

#ifdef OUTPUT_TIME_other_MemoryFree
Serial.print("jsq1=");
Serial.println(jsq1);
#endif

//异或运算，校验GPS输出字符串的准确性

//经过实测，jsq1的值GPRMC实际有效长度，所以实际亦或运算数=jsq1-4（$ *XX不参与异或运算）
for(int col=1;col<jsq1-3;col++){
if(col==1)yihuoyunsuan=GPRMC;
else yihuoyunsuan=yihuoyunsuan ^ GPRMC;
}
//因为定义int的时候，yihuoyunsuan的结果是以10进制DEC表示的，所以需转换为16进制HEX
//因为GPS校验数据0位也进行了传输，所以在转换的时候有必要将情况分类讨论
//(yihuoyunsuan=0)/(0<yihuoyunsuan<17)/(17<yihuoyunsuan)
if(yihuoyunsuan==0){
//校验数10进制为0，直接填充字符串00
jianyan="00";
}else if(yihuoyunsuan>15){
//此时转换为16进制以后，天然有2位，很理想，不用处理
jianyan = String(yihuoyunsuan,HEX);
}else{
//校验数10进制为1-15时，转换为16进制就只有1位数，所以需要在前面填0
jianyan = "0";
jianyan += String(yihuoyunsuan,HEX);
}
//实践中发现，jianyan中的字符是以小写方式存在，虽然Serial.println(yihuoyunsuan,HEX)会以大写方式打印出来
//但直接Serial.println(jianyan)就是小写的，这是啥情况？
//将其字母转换为大写，否则与GPS传输的校验码对比时大小写问题校验失败（GPS传输的校验码为大写的）
jianyan.toUpperCase();
///////////////////////////////////////////显示异或校验码，方便给GPS编程，虽然还没成功过
#ifdef OUTPUT_TIME_other_MemoryFree
Serial.print("jianyan=");
Serial.println(jianyan);
#endif

if(jianyan==GPRMC && jianyan==GPRMC ){
//一致，则说明数据是有效的，输出校验结果
jiaoyanjieguo=1;
}else{
//不一致
jiaoyanjieguo=0;
}
//对校验数组进行清零
jianyan="";

/*---------------------------异或运算，校验GPS输出字符串的准确性 end--------------------------*/

#ifdef OUTPUT_TIME_other_MemoryFree
Serial.print("jiaoyanjieguo=");
Serial.println(jiaoyanjieguo);
Serial.print("GPRMC=");
Serial.println(GPRMC);
#endif

//解析GPRMC
if(jiaoyanjieguo){
jiaoyanjieguo=0;

if(GPRMC=='A'){
dingweiok=1;//定位有效

//纬度
memcpy(weidu,GPRMC+20, 2);
weidu=176;
memcpy(weidu+3,GPRMC+22, 7);
weidu=GPRMC;

//经度

memcpy(jingdu,GPRMC+32, 3);
jingdu=176;
memcpy(jingdu+4,GPRMC+35, 7);
jingdu=GPRMC;

#ifdef OUTPUT_TIME_other_MemoryFree

Serial.print("weidu=");
Serial.println(weidu);
Serial.print("jingdu=");
Serial.println(jingdu);
#endif

}else dingweiok=0; //定位无效

for(int col=0;col<12;col++)weidu=0;
for(int col=0;col<13;col++)jingdu=0;
}

/*------------解析GPRMC---------------*/

//清空临时数组temp1
for(int col=0;col<99;col++)GPRMC=0;
jsq1=0;
panduan1=0;
panduan2=0;

// 获取当前FIFO计数
//fifoCount = mpu.getFIFOCount();
//重置，以便我们能继续干净
mpu.resetFIFO();

//等待正确可用的数据的长度，应该是一个非常短暂的等待
while (fifoCount < packetSize)fifoCount = mpu.getFIFOCount();

//读取FIFO中的数据包
mpu.getFIFOBytes(fifoBuffer, packetSize);

//轨道先进先出算在这里万一有>1包可
//（这让我们马上了解更多，而无需等待中断）
fifoCount -= packetSize;

#ifdef OUTPUT_READABLE_QUATERNION
// 显示四元素 in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
#endif

#ifdef OUTPUT_READABLE_EULER
// 显示欧拉角的度数
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler * 180/M_PI);
Serial.print("\t");
Serial.print(euler * 180/M_PI);
Serial.print("\t");
Serial.println(euler * 180/M_PI);
#endif

#ifdef OUTPUT_READABLE_YAWPITCHROLL
//偏航/ 从四元数来计算俯仰/滚动角（以度为单位）
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t");
Serial.print(ypr * 180/M_PI);
Serial.print("\t");
Serial.print(ypr * 180/M_PI);
Serial.print("\t");
Serial.println(ypr * 180/M_PI);
#endif

#ifdef OUTPUT_READABLE_REALACCEL
// 显示真正的加速，调整，去掉重力
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Serial.print("areal\t");
Serial.print(aaReal.x);
Serial.print("\t");
Serial.print(aaReal.y);
Serial.print("\t");
Serial.println(aaReal.z);
#endif

#ifdef OUTPUT_READABLE_WORLDACCEL
//显示最初的世界框架的加速，调整以扣除地心引力
//和旋转的基础上，从四元数已知的方向
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
Serial.print("aworld\t");
Serial.print(aaWorld.x);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
#endif

#ifdef OUTPUT_HMC5883L
// 输出航向
// read raw heading measurements from device
mag.getHeading(&mx, &my, &mz);

// To calculate heading in degrees. 0 degree indicates North
float heading = atan2(my, mx);
if(heading < 0)
heading += 2 * M_PI;
Serial.print("heading:\t");
Serial.println(heading * 180/M_PI);
#endif

#ifdef OUTPUT_BMP085

barometer.setControl(BMP085_MODE_TEMPERATURE);

// wait appropriate time for conversion (4.5ms delay)
lastMicros = micros();
while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

// read calibrated temperature value in degrees Celsius
temperature = barometer.getTemperatureC();

// request pressure (3x oversampling mode, high detail, 23.5ms delay)
barometer.setControl(BMP085_MODE_PRESSURE_3);
while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

// read calibrated pressure value in Pascals (Pa)
pressure = barometer.getPressure();

// calculate absolute altitude in meters based on known pressure
// (may pass a second "sea level pressure" parameter here,
// otherwise uses the standard value of 101325 Pa)
altitude = barometer.getAltitude(pressure);

// display measured values if appropriate
Serial.print("T/P/A\t");
Serial.print(temperature); Serial.print("\t");
Serial.print(pressure); Serial.print("\t");
Serial.print(altitude);
Serial.println("");
#endif

#ifdef OUTPUT_TIME_other_MemoryFree
jsq6x=millis()-jsq6x;
Serial.print("jsq6x=");
Serial.print(jsq6x);
Serial.println("ms");
Serial.print("freeMemory()=");
Serial.println(freeMemory());
Serial.println("/*******************************************/");
Serial.println();
Serial.println();
#endif

}

}

hi55234 发表于 2014-9-29 20:44:48

Arduino MPU6050 + HMC5883L 主机从机

http://www.tonylabs.com/tag/mpu6050

// ---------------------------------------------------------------------------
// Receive all measurements from an 9 DOF sensor board.
// ---------------------------------------------------------------------------
//
// Structure:
//
//                                        Sub I2C
//                                     ______^ ______
//                                     |          |
// ----------
//Arduino|             ----------------       -------------
//2560 |- 3.3 V ------ | MPU 6050 |       |HMC5883|
//       |- GND ---------| Acceleration,|---SDA ---|Compass|
//       |- SDA ---------| Gyro, Temp |---SCL ---|       |
//       |- SCL ---------|          |       |       |
//       |             ----------------       -------------
//-----------
//                   |___________________ _______________________|
//                                        V
//                               Integrated IMU sensor
//
// Pull-up resistors are integrated in the sensor board.
//
// IMPORTANT: When I connect the sensor board to a 5V power supply, it was
//          not possible to realize a I2C connection in this case. I made
//          some experiments with additional pull-upps on the I2C but
//          without any results.
//
// ---------------------------------------------------------------------------
//
// It exists a very good library for I2C communication based on Arduino "Wire"
// provided by Jeff Rowberg. It integrates specific controllers as MPU 6050
// and HMC 5883. Take a view on https://github.com/jrowberg/i2cdevlib
//
// The example was implement with i2cdevlib Version and extends the existing
// MPU_6050_raw // example. It uses the code proposed by @muzhig on i2cdevlib
// https://github.com/jrowberg/i2cdevlib/issues/18
// ---------------------------------------------------------------------------

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "HMC5883L.h"

// The default I2C address is defined
// MPU 6050 - 0x68 - MPU6050_DEFAULT_ADDRESS
// HMC5883L - 0x1E - HMC5883L_DEFAULT_ADDRESS
MPU6050 mpu6050;
HMC5883L hmc5883l;

int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;
double temp;

#define LED_PIN 13
bool blinkState = false;

// this method is just used to collect different setSlave operations
void setSlaveControl(uint8_t slaveID){
mpu6050.setSlaveEnabled(slaveID, true);
mpu6050.setSlaveWordByteSwap(slaveID, false);
mpu6050.setSlaveWriteMode(slaveID, false);
mpu6050.setSlaveWordGroupOffset(slaveID, false);
mpu6050.setSlaveDataLength(slaveID, 2);
}

void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// initialize serial communication
Serial.begin(9600);

Serial.println("Initializing I2C devices...");

mpu6050.initialize();
if (mpu6050.testConnection()){
   Serial.println("MPU6050 connection successful");
}
else {
   Serial.println("MPU6050 connection failed");
}

// configuration of the compass module
// activate the I2C bypass to directly access the Sub I2C
mpu6050.setI2CMasterModeEnabled(0);
mpu6050.setI2CBypassEnabled(1);

if (hmc5883l.testConnection()) {
   Serial.println("HMC5883l connection successful");
   hmc5883l.initialize();

   // unfourtunally
   // hmc5883l.setMode(HMC5883L_MODE_CONTINUOUS);
   // does not work correctly. I used the following command to
   // "manually" switch on continouse measurements
   I2Cdev::writeByte(HMC5883L_DEFAULT_ADDRESS,
                     HMC5883L_RA_MODE,
                     HMC5883L_MODE_CONTINUOUS);

   // the HMC5883l is configured now, we switch back to the MPU 6050
   mpu6050.setI2CBypassEnabled(0);

   // X axis word
   mpu6050.setSlaveAddress(0, HMC5883L_DEFAULT_ADDRESS | 0x80);
   mpu6050.setSlaveRegister(0, HMC5883L_RA_DATAX_H);
   setSlaveControl(0);

   // Y axis word
   mpu6050.setSlaveAddress(1, HMC5883L_DEFAULT_ADDRESS | 0x80);
   mpu6050.setSlaveRegister(1, HMC5883L_RA_DATAY_H);
   setSlaveControl(1);

   // Z axis word
   mpu6050.setSlaveAddress(2, HMC5883L_DEFAULT_ADDRESS | 0x80);
   mpu6050.setSlaveRegister(2, HMC5883L_RA_DATAZ_H);
   setSlaveControl(2);

   mpu6050.setI2CMasterModeEnabled(1);
} else {
   Serial.println("HMC5883l connection failed");
}

// activate temperature MPU 6050 sensor
mpu6050.setTempSensorEnabled(true);

// configure Arduino LED for
pinMode(LED_PIN, OUTPUT);
}

void loop() {

// read raw heading measurements from device
mx=mpu6050.getExternalSensorWord(0);
my=mpu6050.getExternalSensorWord(2);
mz=mpu6050.getExternalSensorWord(4);

// To calculate heading in degrees. 0 degree indicates North
float heading = atan2(my, mx);
if(heading < 0)
   heading += 2 * M_PI;

// read raw accel/gyro measurements from device
mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

// see MPU 6050 datasheet page 31 of 47
temp=((double) mpu6050.getTemperature()) /340.0 + 36.53;

Serial.print(ax); Serial.print("\t");
Serial.print(ay); Serial.print("\t");
Serial.print(az); Serial.print("|\t");
Serial.print(gx); Serial.print("\t");
Serial.print(gy); Serial.print("\t");
Serial.print(gz); Serial.print("|\t");
Serial.print(heading * 180/M_PI); Serial.print("|\t");
Serial.println(temp,3);

// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}

hi55234 发表于 2014-9-29 21:44:05

http://segmentfault.com/blog/riodream/1190000000408831

pitch(俯仰），yaw(偏航），roll（滚动）
理解传说中的roll、yaw、pitch
* roll：绕x轴
* pitch：绕y轴
* yaw：绕z轴

roll的意思是翻滚，中文中飞机的翻滚是什么，就是绕着机身所在的那个轴。
yaw:是偏航的意思，如果要改变航向，飞机必定是绕着重力方向为轴。
pitch：有倾斜、坠落的意思。飞机在坠落时，必定会一头栽下去，以翅膀所在的直线为轴。
现在把摄像机看成一个飞机，镜头朝向就是飞机头的朝向，是不是一样？

hi55234 发表于 2014-9-29 22:36:03

MPU6050+HMC5883L+BMP180+GPS -2[纬度、经度数字化，使经纬度有意义]

//输出控制:

// 实际在四元组件格式
#define OUTPUT_READABLE_QUATERNION

// 欧拉角
#define OUTPUT_READABLE_EULER

// 偏航/俯仰/滚动角（以度为单位）
#define OUTPUT_READABLE_YAWPITCHROLL

//加速度，扣除重力影响
#define OUTPUT_READABLE_REALACCEL

// 加速度，扣除重力影响+参照偏航
#define OUTPUT_READABLE_WORLDACCEL

//输出航向 HMC5883L
#define OUTPUT_HMC5883L

//程序耗时，剩余内存数查询其他测试项目
#define OUTPUT_TIME_other_MemoryFree

//BMP085
#define OUTPUT_BMP085

//GPS
#define OUTPUT_GPS

/*---------------------输出控制 end---------------------------------*/

//MPU6050:

// I2Cdev和MPU6050必须安装的库
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;// AD0 low = 0x68
//MPU6050 mpu(0x69); // <-- use for AD0 high

/*
科普：
按照posix标准，一般整形对应的*_t类型为：
1字节 uint8_t 近似于 byte
2字节 uint16_t 近似于 int
4字节 uint32_t
8字节 uint64_t
*/

// 控制/状态
bool dmpReady = false; // 设置为true，如果DMP初始化成功
//uint8_t mpuIntStatus; //持有MPU的实际中断状态字节
uint8_t devStatus; // 每个设备运行后返回的状态（0=成功非0=错误）
uint16_t packetSize; // 预计DMP数据包大小（默认为42字节）
uint16_t fifoCount; // 计算当前FIFO中的所有字节
uint8_t fifoBuffer; // FIFO存储缓冲器

// 方向/运动
Quaternion q; // 四元数
VectorInt16 aa; // 加速度传感器测量
VectorInt16 aaReal; // 无重力加速度传感器的测量
VectorInt16 aaWorld; // 世界框架加速度传感器测量
VectorFloat gravity; // 重力矢量
float euler; // 欧拉角容器
float ypr; // 偏航/俯仰/滚动容器和重力矢量

/*---------------------MPU6050 end---------------------------------*/

//HMC5883L

#include "HMC5883L.h"
// class default I2C address is 0x1E
// specific I2C addresses may be passed as a parameter here
// this device only supports one I2C address (0x1E)
HMC5883L mag;
int16_t mx, my, mz;

/*---------------------HMC5883L end---------------------------------*/

//BMP180 用的是BMP085的库，实际最小分辨率只有后者的一半
#include "BMP085.h"
BMP085 barometer;
float temperature;
float pressure;
float altitude;
int32_t lastMicros;
/*---------------------BMP085 end---------------------------------*/

//程序耗时，剩余内存数查询
unsigned long jsq6x;
#include <MemoryFree.h>//剩余内存查询
/*---------------------程序耗时，剩余内存数查询 end---------------------------------*/

//GPS
//////////////////////////////////////////////////////////////////////////////////////////////

#include <SoftwareSerial.h>
SoftwareSerial gps(8, 9); // RX, TX
char tempx;
int weiduA,weiduB,weiduC;//把GPS格式纬度，以度、分、分的小数部分进行拆分
int jingduA,jingduB,jingduC;//把GPS格式经度，以度、分、分的小数部分进行拆分

char GPRMC;//临时数组1，放gps数据的
unsigned long gpstimes;//gps计时器，如果超过一定时间10ms，则证明gps数据已经发送完毕
//unsigned long quanjujishu;//全局计数，计算1秒之内，循环了多少次，这个次数在一定程度上表示了剩余性能
boolean konghangpanduan;//空行判断，因为在读取串口数据后并没有延迟，完全依赖全局循环，那么，特定程序是否执行，就全靠布尔量来标记了
int jsq1=0;
boolean panduan1=0;
boolean panduan2=0;
String jianyan="";//GPS数据包的ASCII异或校验
int yihuoyunsuan;//异或运算——校验结果
boolean jiaoyanjieguo=0;//校验结果
boolean dingweiok=0;//定位有效

/*---------------------GPS end---------------------------------*/

// ================================================================
// === 初始设置 ===
// ================================================================

void setup() {

// 加入I2C总线（I2Cdev库没有自动执行此操作）

Wire.begin();
TWBR = 24; // 400kHz的I2C时钟（200kHz的，如果CPU为8MHz）

//初始化串行通信波特率115200
Serial.begin(9600);

// 初始化设备
mpu.initialize();

// 加载和配置DMP
devStatus = mpu.dmpInitialize();

//提供自己的陀螺仪偏置在这里，对于规模最小灵敏度
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688出厂默认为我的测试芯片

// 确保它的工作（返回0，如果这样）
if (devStatus == 0) {

// 打开DMP，现在，它已经准备好
mpu.setDMPEnabled(true);

// 设置我们的DMP Ready标志使主循环（）函数知道它的好来使用它
dmpReady = true;

// 获得预期的DMP数据包大小为以后进行比较
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
//错误！
//1=初始内存加载失败
//2= DMP配置更新失败
//（如果它要打破，通常的代码为1）
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}

/////////////////////////////////////////HMC5883L
mag.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(mag.testConnection() ? "HMC5883L connection successful" : "HMC5883L connection failed");

//////////////////////////////////////////BMP085
barometer.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(barometer.testConnection() ? "BMP085 connection successful" : "BMP085 connection failed");

///////////////////////////////////////////GPS
gps.begin(9600);

//////////////////////////////////////////////////////////////////////

}

// ================================================================
// === 主程序循环 ===
// ================================================================

void loop() {

#ifdef OUTPUT_TIME_other_MemoryFree
jsq6x=millis();
#endif

#ifdef OUTPUT_GPS

//无条件接受串口的字符，目的截取GPRMC字符串
while (gps.available() > 0) {
gpstimes=millis();//重置gpstimes为millis()
konghangpanduan=0;//因为串口有数据，所以空行判断为0，即不空行
if(panduan2)gps.read();//采集到所需数据后，舍弃其他串口数据
else GPRMC = gps.read();//软串口读取GPS数据，并保存在临时数组GPRMC中

//delayMicroseconds(50);

//做一个保险，保证数组不会溢出，同时也限制了最大字符串的长度（99）
//为兼容$GPGGA截取、或GPRMC不在最后一行的情况，设定一旦采集完毕，就锁定jsq1的值
if(jsq1<99 && !panduan2) jsq1++;
else if(!panduan2) jsq1=99;

if(jsq1>5 && !panduan1){

if(GPRMC=='M' && GPRMC=='C')panduan1=1;
else{
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
jsq1=5;
}

}

if(panduan1 && GPRMC=='*')panduan2=1;

}

#endif

//如果连续10ms串口都没有收到新的数据，则说明一个周期内（1秒）的GPS数据发送已完毕
//这个空行一个周期只做一次，所以同时加入布尔量konghangpanduan的判断
if(millis()-gpstimes>10 && !konghangpanduan){
konghangpanduan=1;

#ifdef OUTPUT_TIME_other_MemoryFree
Serial.print("jsq1=");
Serial.println(jsq1);
#endif

//异或运算，校验GPS输出字符串的准确性

//经过实测，jsq1的值GPRMC实际有效长度，所以实际亦或运算数=jsq1-4（$ *XX不参与异或运算）
for(int col=1;col<jsq1-3;col++){
if(col==1)yihuoyunsuan=GPRMC;
else yihuoyunsuan=yihuoyunsuan ^ GPRMC;
}
//因为定义int的时候，yihuoyunsuan的结果是以10进制DEC表示的，所以需转换为16进制HEX
//因为GPS校验数据0位也进行了传输，所以在转换的时候有必要将情况分类讨论
//(yihuoyunsuan=0)/(0<yihuoyunsuan<17)/(17<yihuoyunsuan)
if(yihuoyunsuan==0){
//校验数10进制为0，直接填充字符串00
jianyan="00";
}else if(yihuoyunsuan>15){
//此时转换为16进制以后，天然有2位，很理想，不用处理
jianyan = String(yihuoyunsuan,HEX);
}else{
//校验数10进制为1-15时，转换为16进制就只有1位数，所以需要在前面填0
jianyan = "0";
jianyan += String(yihuoyunsuan,HEX);
}
//实践中发现，jianyan中的字符是以小写方式存在，虽然Serial.println(yihuoyunsuan,HEX)会以大写方式打印出来
//但直接Serial.println(jianyan)就是小写的，这是啥情况？
//将其字母转换为大写，否则与GPS传输的校验码对比时大小写问题校验失败（GPS传输的校验码为大写的）
jianyan.toUpperCase();
///////////////////////////////////////////显示异或校验码，方便给GPS编程，虽然还没成功过
#ifdef OUTPUT_TIME_other_MemoryFree
Serial.print("jianyan=");
Serial.println(jianyan);
#endif

if(jianyan==GPRMC && jianyan==GPRMC ){
//一致，则说明数据是有效的，输出校验结果
jiaoyanjieguo=1;
}else{
//不一致
jiaoyanjieguo=0;
}
//对校验数组进行清零
jianyan="";

/*---------------------------异或运算，校验GPS输出字符串的准确性 end--------------------------*/

#ifdef OUTPUT_TIME_other_MemoryFree
Serial.print("jiaoyanjieguo=");
Serial.println(jiaoyanjieguo);
Serial.print("GPRMC=");
Serial.println(GPRMC);
#endif

//解析GPRMC
if(jiaoyanjieguo){
jiaoyanjieguo=0;

if(GPRMC=='A'){
dingweiok=1;//定位有效

//纬度数字化
for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+20, 2);
jianyan =tempx;
weiduA=jianyan.toInt();

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+22, 2);
jianyan =tempx;
weiduB=jianyan.toInt();

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+25, 4);
jianyan =tempx;
weiduC=jianyan.toInt();

//纬度数字化
for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+32, 3);
jianyan =tempx;
jingduA=jianyan.toInt();

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+35, 2);
jianyan =tempx;
jingduB=jianyan.toInt();

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+38, 4);
jianyan =tempx;
jingduC=jianyan.toInt();

jianyan="";

#ifdef OUTPUT_TIME_other_MemoryFree
Serial.print("weidu=");
Serial.print(weiduA);
Serial.print(weiduB);
Serial.print(".");
Serial.println(weiduC);

Serial.print("weidu=");
Serial.print(jingduA);
Serial.print(jingduB);
Serial.print(".");
Serial.println(jingduC);

#endif

}else dingweiok=0; //定位无效

}

/*------------解析GPRMC---------------*/

//清空临时数组temp1
for(int col=0;col<99;col++)GPRMC=0;
jsq1=0;
panduan1=0;
panduan2=0;

// 获取当前FIFO计数
//fifoCount = mpu.getFIFOCount();
//重置，以便我们能继续干净
mpu.resetFIFO();

//等待正确可用的数据的长度，应该是一个非常短暂的等待
while (fifoCount < packetSize)fifoCount = mpu.getFIFOCount();

//读取FIFO中的数据包
mpu.getFIFOBytes(fifoBuffer, packetSize);

//轨道先进先出算在这里万一有>1包可
//（这让我们马上了解更多，而无需等待中断）
fifoCount -= packetSize;

#ifdef OUTPUT_READABLE_QUATERNION
// 显示四元素 in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
#endif

#ifdef OUTPUT_READABLE_EULER
// 显示欧拉角的度数
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler * 180/M_PI);
Serial.print("\t");
Serial.print(euler * 180/M_PI);
Serial.print("\t");
Serial.println(euler * 180/M_PI);
#endif

#ifdef OUTPUT_READABLE_YAWPITCHROLL
//偏航/ 从四元数来计算俯仰/滚动角（以度为单位）
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t");
Serial.print(ypr * 180/M_PI);
Serial.print("\t");
Serial.print(ypr * 180/M_PI);
Serial.print("\t");
Serial.println(ypr * 180/M_PI);
#endif

#ifdef OUTPUT_READABLE_REALACCEL
// 显示真正的加速，调整，去掉重力
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Serial.print("areal\t");
Serial.print(aaReal.x);
Serial.print("\t");
Serial.print(aaReal.y);
Serial.print("\t");
Serial.println(aaReal.z);
#endif

#ifdef OUTPUT_READABLE_WORLDACCEL
//显示最初的世界框架的加速，调整以扣除地心引力
//和旋转的基础上，从四元数已知的方向
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
Serial.print("aworld\t");
Serial.print(aaWorld.x);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
#endif

#ifdef OUTPUT_HMC5883L
// 输出航向
// read raw heading measurements from device
mag.getHeading(&mx, &my, &mz);

// To calculate heading in degrees. 0 degree indicates North
float heading = atan2(my, mx);
if(heading < 0)
heading += 2 * M_PI;
Serial.print("heading:\t");
Serial.println(heading * 180/M_PI);
#endif

#ifdef OUTPUT_BMP085

barometer.setControl(BMP085_MODE_TEMPERATURE);

// wait appropriate time for conversion (4.5ms delay)
lastMicros = micros();
while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

// read calibrated temperature value in degrees Celsius
temperature = barometer.getTemperatureC();

// request pressure (3x oversampling mode, high detail, 23.5ms delay)
barometer.setControl(BMP085_MODE_PRESSURE_3);
while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

// read calibrated pressure value in Pascals (Pa)
pressure = barometer.getPressure();

// calculate absolute altitude in meters based on known pressure
// (may pass a second "sea level pressure" parameter here,
// otherwise uses the standard value of 101325 Pa)
altitude = barometer.getAltitude(pressure);

// display measured values if appropriate
Serial.print("T/P/A\t");
Serial.print(temperature); Serial.print("\t");
Serial.print(pressure); Serial.print("\t");
Serial.print(altitude);
Serial.println("");
#endif

#ifdef OUTPUT_TIME_other_MemoryFree
jsq6x=millis()-jsq6x;
Serial.print("jsq6x=");
Serial.print(jsq6x);
Serial.println("ms");
Serial.print("freeMemory()=");
Serial.println(freeMemory());
Serial.println("/*******************************************/");
Serial.println();
Serial.println();
#endif

}

}

hi55234 发表于 2014-10-1 18:04:24

看波动范围的程序

heading: 303.21
heading: 303.21
heading: 303.21
heading: 303.3
heading: 303.3 HMC5883L
heading: 303.34 静止波动max
heading: 303.43 0.26
heading: 303.43
heading: 303.43
heading: 303.47
heading: 303.47
heading: 303.51
heading: 303.56
T/P/A 27 98034 277.67 BMP180
T/P/A 27 98037 277.42 静止波动max
T/P/A 27 98037 277.42 气压pa
T/P/A 27 98038 277.33
T/P/A 27.1 98039 277.25 13
T/P/A 27 98039 277.25
T/P/A 27.1 98039 277.25
T/P/A 27 98040 277.16
T/P/A 27 98040 277.16
T/P/A 27.1 98040 277.16
T/P/A 27 98041 277.08
T/P/A 27.1 98041 277.08
T/P/A 27.1 98046 276.65
T/P/A 27.1 98047 276.56
ypr -107.09 3.28 0.18 MPU6050
ypr -107.09 3.28 0.18 静止波动max
ypr -107.09 3.29 0.18 0.01
ypr -107.09 3.28 0.17
ypr -107.09 3.29 0.18
ypr -107.09 3.28 0.17

/*
缺点在于频繁而无效的串口传输数据，拟改为仅当GPS定位后经纬度（yrp、航向、气压）有变换时再输出串口信息
*/

//输出控制:

// 偏航/俯仰/滚动角（以度为单位）
#define OUTPUT_READABLE_YAWPITCHROLL

//输出航向 HMC5883L
#define OUTPUT_HMC5883L

//其他测试项目
//#define OUTPUT_other_test

//程序耗时，剩余内存数查询
//#define OUTPUT_TIME_AND_MemoryFree

//BMP085
#define OUTPUT_BMP085

//GPS
#define OUTPUT_GPS

/*---------------------输出控制 end---------------------------------*/

//MPU6050:

// I2Cdev和MPU6050必须安装的库
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;// AD0 low = 0x68
//MPU6050 mpu(0x69); // <-- use for AD0 high

/*
科普：
按照posix标准，一般整形对应的*_t类型为：
1字节 uint8_t 近似于 byte
2字节 uint16_t 近似于 int
4字节 uint32_t
8字节 uint64_t
*/

// 控制/状态
bool dmpReady = false; // 设置为true，如果DMP初始化成功
//uint8_t mpuIntStatus; //持有MPU的实际中断状态字节
uint8_t devStatus; // 每个设备运行后返回的状态（0=成功非0=错误）
uint16_t packetSize; // 预计DMP数据包大小（默认为42字节）
uint16_t fifoCount; // 计算当前FIFO中的所有字节
uint8_t fifoBuffer; // FIFO存储缓冲器

// 方向/运动
Quaternion q; // 四元数
VectorInt16 aa; // 加速度传感器测量
VectorInt16 aaReal; // 无重力加速度传感器的测量
VectorInt16 aaWorld; // 世界框架加速度传感器测量
VectorFloat gravity; // 重力矢量
float euler; // 欧拉角容器
float ypr; // 偏航/俯仰/滚动容器和重力矢量

float ychangertest,pchangertest,rchangertest;
boolean ychanger,pchanger,rchanger;
boolean yprchanger;
/*---------------------MPU6050 end---------------------------------*/

//HMC5883L

#include "HMC5883L.h"
// class default I2C address is 0x1E
// specific I2C addresses may be passed as a parameter here
// this device only supports one I2C address (0x1E)
HMC5883L mag;
int16_t mx, my, mz;
float heading;

float headingchangertest;
boolean headingchanger;
/*---------------------HMC5883L end---------------------------------*/

//BMP180 用的是BMP085的库，实际最小分辨率只有后者的一半
#include "BMP085.h"
BMP085 barometer;
float temperature;
float pressure;
float altitude;
int32_t lastMicros;

float highchangertest;
boolean highchanger;
/*---------------------BMP085 end---------------------------------*/

//程序耗时，剩余内存数查询
unsigned long jsq6x;
#include <MemoryFree.h>//剩余内存查询
/*---------------------程序耗时，剩余内存数查询 end---------------------------------*/

//GPS
//////////////////////////////////////////////////////////////////////////////////////////////

#include <SoftwareSerial.h>
SoftwareSerial gps(8, 9); // RX, TX
char tempx;
int gpschangertest;
boolean gpschanger;
int weiduA,weiduB,weiduC;//把GPS格式纬度，以度、分、分的小数部分进行拆分
int jingduA,jingduB,jingduC;//把GPS格式经度，以度、分、分的小数部分进行拆分

char GPRMC;//临时数组1，放gps数据的
unsigned long gpstimes;//gps计时器，如果超过一定时间10ms，则证明gps数据已经发送完毕
//unsigned long quanjujishu;//全局计数，计算1秒之内，循环了多少次，这个次数在一定程度上表示了剩余性能
boolean konghangpanduan;//空行判断，因为在读取串口数据后并没有延迟，完全依赖全局循环，那么，特定程序是否执行，就全靠布尔量来标记了
int jsq1=0;
boolean panduan1=0;
boolean panduan2=0;
String jianyan="";//GPS数据包的ASCII异或校验
int yihuoyunsuan;//异或运算——校验结果
boolean jiaoyanjieguo=0;//校验结果
boolean dingweiok=0;//定位有效

/*---------------------GPS end---------------------------------*/

// ================================================================
// === 初始设置 ===
// ================================================================

void setup() {

// 加入I2C总线（I2Cdev库没有自动执行此操作）

Wire.begin();
TWBR = 24; // 400kHz的I2C时钟（200kHz的，如果CPU为8MHz）

//初始化串行通信波特率115200
Serial.begin(19200);

// 初始化设备
mpu.initialize();

// 加载和配置DMP
devStatus = mpu.dmpInitialize();

//提供自己的陀螺仪偏置在这里，对于规模最小灵敏度
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688出厂默认为我的测试芯片

// 确保它的工作（返回0，如果这样）
if (devStatus == 0) {

// 打开DMP，现在，它已经准备好
mpu.setDMPEnabled(true);

// 设置我们的DMP Ready标志使主循环（）函数知道它的好来使用它
dmpReady = true;

// 获得预期的DMP数据包大小为以后进行比较
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
//错误！
//1=初始内存加载失败
//2= DMP配置更新失败
//（如果它要打破，通常的代码为1）
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}

/////////////////////////////////////////HMC5883L
mag.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(mag.testConnection() ? "HMC5883L connection successful" : "HMC5883L connection failed");

//////////////////////////////////////////BMP085
barometer.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(barometer.testConnection() ? "BMP085 connection successful" : "BMP085 connection failed");

///////////////////////////////////////////GPS
gps.begin(19200);

//////////////////////////////////////////////////////////////////////

}

// ================================================================
// === 主程序循环 ===
// ================================================================

void loop() {

#ifdef OUTPUT_TIME_AND_MemoryFree
jsq6x=millis();
#endif

#ifdef OUTPUT_GPS

//无条件接受串口的字符，目的截取GPRMC字符串
while (gps.available() > 0) {
gpstimes=millis();//重置gpstimes为millis()
konghangpanduan=0;//因为串口有数据，所以空行判断为0，即不空行
if(panduan2)gps.read();//采集到所需数据后，舍弃其他串口数据
else GPRMC = gps.read();//软串口读取GPS数据，并保存在临时数组GPRMC中

//delayMicroseconds(50);

//做一个保险，保证数组不会溢出，同时也限制了最大字符串的长度（99）
//为兼容$GPGGA截取、或GPRMC不在最后一行的情况，设定一旦采集完毕，就锁定jsq1的值
if(jsq1<99 && !panduan2) jsq1++;
else if(!panduan2) jsq1=99;

if(jsq1>5 && !panduan1){

if(GPRMC=='M' && GPRMC=='C')panduan1=1;
else{
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
jsq1=5;
}

}

if(panduan1 && GPRMC=='*'){
panduan2=1;

}
}

#endif

//如果连续10ms串口都没有收到新的数据，则说明一个周期内（1秒）的GPS数据发送已完毕
//这个空行一个周期只做一次，所以同时加入布尔量konghangpanduan的判断
if(millis()-gpstimes>10 && !konghangpanduan){
konghangpanduan=1;

gpsdechuli();

//清空临时数组temp1
for(int col=0;col<99;col++)GPRMC=0;
jsq1=0;
panduan1=0;
panduan2=0;

}

// 获取当前FIFO计数
//fifoCount = mpu.getFIFOCount();
//重置，以便我们能继续干净
mpu.resetFIFO();

//等待正确可用的数据的长度，应该是一个非常短暂的等待
while (fifoCount < packetSize)fifoCount = mpu.getFIFOCount();

//读取FIFO中的数据包
mpu.getFIFOBytes(fifoBuffer, packetSize);

//轨道先进先出算在这里万一有>1包可
//（这让我们马上了解更多，而无需等待中断）
fifoCount -= packetSize;

#ifdef OUTPUT_READABLE_YAWPITCHROLL
//偏航/ 从四元数来计算俯仰/滚动角（以度为单位）
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

if(ychangertest!=ypr){
ychangertest=ypr;
ychanger=1;
}

if(pchangertest!=ypr){
pchangertest=ypr;
pchanger=1;
}

if(rchangertest!=ypr){
rchangertest=ypr;
rchanger=1;
}

//Y、P、R依次进行异或运算，确保只要有一个值为1，则yprchanger为1
yprchanger=ychanger^pchanger;
yprchanger=yprchanger^rchanger;

//仅当YPR改变时，才打印新的YPR信息

if(yprchanger){

ychanger=0;
pchanger=0;
rchanger=0;

Serial.print("ypr\t");
Serial.print(ypr * 180/M_PI);
Serial.print("\t");
Serial.print(ypr * 180/M_PI);
Serial.print("\t");
Serial.println(ypr * 180/M_PI);
}
#endif

#ifdef OUTPUT_HMC5883L
// 输出航向
// read raw heading measurements from device
mag.getHeading(&mx, &my, &mz);

// To calculate heading in degrees. 0 degree indicates North
headingchangertest = atan2(my, mx);
if(headingchangertest < 0)
headingchangertest += 2 * M_PI;

if(headingchangertest!=heading){
heading=headingchangertest;
headingchanger=1;
}

if(headingchanger){
headingchanger=0;

Serial.print("heading:\t");
Serial.println(heading * 180/M_PI);
}

#endif

#ifdef OUTPUT_BMP085

barometer.setControl(BMP085_MODE_TEMPERATURE);

// wait appropriate time for conversion (4.5ms delay)
lastMicros = micros();
while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

// read calibrated temperature value in degrees Celsius
temperature = barometer.getTemperatureC();

// request pressure (3x oversampling mode, high detail, 23.5ms delay)
barometer.setControl(BMP085_MODE_PRESSURE_3);
while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

// read calibrated pressure value in Pascals (Pa)
pressure = barometer.getPressure();

// calculate absolute altitude in meters based on known pressure
// (may pass a second "sea level pressure" parameter here,
// otherwise uses the standard value of 101325 Pa)
highchangertest = barometer.getAltitude(pressure);
if(highchangertest!=altitude){
altitude=highchangertest;
highchanger=1;
}

if(highchanger){

highchanger=0;
// display measured values if appropriate
Serial.print("T/P/A\t");
Serial.print(temperature); Serial.print("\t");
Serial.print(pressure); Serial.print("\t");
Serial.print(altitude);
Serial.println("");

}
#endif

//仅当GPS的经纬度改变时，才打印新的经纬度信息
if(gpschanger){
gpschanger=0;

Serial.print("weidu=");
Serial.print(weiduA);
Serial.print(weiduB);
Serial.print(".");
Serial.println(weiduC);

Serial.print("jingdu=");
Serial.print(jingduA);
Serial.print(jingduB);
Serial.print(".");
Serial.println(jingduC);
}

#ifdef OUTPUT_TIME_AND_MemoryFree
jsq6x=millis()-jsq6x;
Serial.print("jsq6x=");
Serial.println(jsq6x);
//Serial.println("ms");
//Serial.print("freeMemory()=");
//Serial.println(freeMemory());
//Serial.println("/*******************************************/");
//Serial.println();
Serial.println();
#endif

}

void gpsdechuli()
{

#ifdef OUTPUT_other_test
Serial.print("jsq1=");
Serial.println(jsq1);
#endif

//异或运算，校验GPS输出字符串的准确性

//经过实测，jsq1的值GPRMC实际有效长度，所以实际亦或运算数=jsq1-4（$ *XX不参与异或运算）
for(int col=1;col<jsq1-3;col++){
if(col==1)yihuoyunsuan=GPRMC;
else yihuoyunsuan=yihuoyunsuan ^ GPRMC;
}
//因为定义int的时候，yihuoyunsuan的结果是以10进制DEC表示的，所以需转换为16进制HEX
//因为GPS校验数据0位也进行了传输，所以在转换的时候有必要将情况分类讨论
//(yihuoyunsuan=0)/(0<yihuoyunsuan<17)/(17<yihuoyunsuan)
if(yihuoyunsuan==0){
//校验数10进制为0，直接填充字符串00
jianyan="00";
}else if(yihuoyunsuan>15){
//此时转换为16进制以后，天然有2位，很理想，不用处理
jianyan = String(yihuoyunsuan,HEX);
}else{
//校验数10进制为1-15时，转换为16进制就只有1位数，所以需要在前面填0
jianyan = "0";
jianyan += String(yihuoyunsuan,HEX);
}
//实践中发现，jianyan中的字符是以小写方式存在，虽然Serial.println(yihuoyunsuan,HEX)会以大写方式打印出来
//但直接Serial.println(jianyan)就是小写的，这是啥情况？
//将其字母转换为大写，否则与GPS传输的校验码对比时大小写问题校验失败（GPS传输的校验码为大写的）
jianyan.toUpperCase();
///////////////////////////////////////////显示异或校验码，方便给GPS编程，虽然还没成功过
#ifdef OUTPUT_other_test
Serial.print("jianyan=");
Serial.println(jianyan);
#endif

if(jianyan==GPRMC && jianyan==GPRMC ){
//一致，则说明数据是有效的，输出校验结果
jiaoyanjieguo=1;
}else{
//不一致
jiaoyanjieguo=0;
}
//对校验数组进行清零
jianyan="";

/*---------------------------异或运算，校验GPS输出字符串的准确性 end--------------------------*/

#ifdef OUTPUT_other_test
Serial.print("jiaoyanjieguo=");
Serial.println(jiaoyanjieguo);
Serial.print("GPRMC=");
Serial.println(GPRMC);
#endif

//解析GPRMC
if(jiaoyanjieguo){
jiaoyanjieguo=0;

if(GPRMC=='A'){
dingweiok=1;//定位有效

//纬度数字化
for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+20, 2);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(weiduA!=gpschangertest){
weiduA=gpschangertest;
gpschanger=1;
}

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+22, 2);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(weiduB!=gpschangertest){
weiduB=gpschangertest;
gpschanger=1;
}

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+25, 4);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(weiduC!=gpschangertest){
weiduC=gpschangertest;
gpschanger=1;
}

//经度数字化
for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+32, 3);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(jingduA!=gpschangertest){
jingduA=gpschangertest;
gpschanger=1;
}

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+35, 2);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(jingduB!=gpschangertest){
jingduB=gpschangertest;
gpschanger=1;
}

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+38, 4);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(jingduC!=gpschangertest){
jingduC=gpschangertest;
gpschanger=1;
}

jianyan="";

}else {
dingweiok=0; //定位无效
weiduA=0;
weiduB=0;
weiduC=0;
jingduA=0;
jingduB=0;
jingduC=0;
}

/*------------解析GPRMC---------------*/

}

}

hi55234 发表于 2014-10-1 21:26:22

减少串口的数据输出，变化小于一定范围则忽略此次变化

本帖最后由 hi55234 于 2014-10-1 21:34 编辑

/*
为改善频繁而无效的串口传输数据，
仅当GPS定位后经纬度（yrp、航向、气压）有变换时再输出串口信息

遗留问题：滤波

此程序可以看出各个数据的波动范围，因为这个程序中，只有波动才会输出

*/

//输出控制:

// 偏航/俯仰/滚动角（以度为单位）
#define OUTPUT_READABLE_YAWPITCHROLL

//输出航向 HMC5883L
#define OUTPUT_HMC5883L

//其他测试项目
//#define OUTPUT_other_test

//程序耗时，剩余内存数查询
//#define OUTPUT_TIME_AND_MemoryFree

//BMP085
#define OUTPUT_BMP085

//GPS
#define OUTPUT_GPS

/*---------------------输出控制 end---------------------------------*/

//MPU6050:

// I2Cdev和MPU6050必须安装的库
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;// AD0 low = 0x68
//MPU6050 mpu(0x69); // <-- use for AD0 high

/*
科普：
按照posix标准，一般整形对应的*_t类型为：
1字节 uint8_t 近似于 byte
2字节 uint16_t 近似于 int
4字节 uint32_t
8字节 uint64_t
*/

// 控制/状态
bool dmpReady = false; // 设置为true，如果DMP初始化成功
//uint8_t mpuIntStatus; //持有MPU的实际中断状态字节
uint8_t devStatus; // 每个设备运行后返回的状态（0=成功非0=错误）
uint16_t packetSize; // 预计DMP数据包大小（默认为42字节）
uint16_t fifoCount; // 计算当前FIFO中的所有字节
uint8_t fifoBuffer; // FIFO存储缓冲器

// 方向/运动
Quaternion q; // 四元数
VectorInt16 aa; // 加速度传感器测量
VectorInt16 aaReal; // 无重力加速度传感器的测量
VectorInt16 aaWorld; // 世界框架加速度传感器测量
VectorFloat gravity; // 重力矢量
float euler; // 欧拉角容器
float ypr; // 偏航/俯仰/滚动容器和重力矢量

float ychangertest,pchangertest,rchangertest;
boolean ychanger,pchanger,rchanger;
boolean yprchanger;
/*---------------------MPU6050 end---------------------------------*/

//HMC5883L

#include "HMC5883L.h"
// class default I2C address is 0x1E
// specific I2C addresses may be passed as a parameter here
// this device only supports one I2C address (0x1E)
HMC5883L mag;
int16_t mx, my, mz;
float heading;

float headingchangertest;
boolean headingchanger;
/*---------------------HMC5883L end---------------------------------*/

//BMP180 用的是BMP085的库，实际最小分辨率只有后者的一半
#include "BMP085.h"
BMP085 barometer;
float temperature;
float pressure;
float altitude;
int32_t lastMicros;

float pressurechangertest;
boolean pressurechanger;
/*---------------------BMP085 end---------------------------------*/

//程序耗时，剩余内存数查询
unsigned long jsq6x;
#include <MemoryFree.h>//剩余内存查询
/*---------------------程序耗时，剩余内存数查询 end---------------------------------*/

//GPS
//////////////////////////////////////////////////////////////////////////////////////////////

#include <SoftwareSerial.h>
SoftwareSerial gps(8, 9); // RX, TX
char tempx;
int gpschangertest;
boolean gpschanger;
int weiduA,weiduB,weiduC;//把GPS格式纬度，以度、分、分的小数部分进行拆分
int jingduA,jingduB,jingduC;//把GPS格式经度，以度、分、分的小数部分进行拆分

char GPRMC;//临时数组1，放gps数据的
unsigned long gpstimes;//gps计时器，如果超过一定时间10ms，则证明gps数据已经发送完毕
//unsigned long quanjujishu;//全局计数，计算1秒之内，循环了多少次，这个次数在一定程度上表示了剩余性能
boolean konghangpanduan;//空行判断，因为在读取串口数据后并没有延迟，完全依赖全局循环，那么，特定程序是否执行，就全靠布尔量来标记了
int jsq1=0;
boolean panduan1=0;
boolean panduan2=0;
String jianyan="";//GPS数据包的ASCII异或校验
int yihuoyunsuan;//异或运算——校验结果
boolean jiaoyanjieguo=0;//校验结果
boolean dingweiok=0;//定位有效

/*---------------------GPS end---------------------------------*/
//滤波限幅
float lvboxianfu;

// ================================================================
// === 初始设置 ===
// ================================================================

void setup() {

// 加入I2C总线（I2Cdev库没有自动执行此操作）

Wire.begin();
TWBR = 24; // 400kHz的I2C时钟（200kHz的，如果CPU为8MHz）

//初始化串行通信波特率115200
Serial.begin(19200);

// 初始化设备
mpu.initialize();

// 加载和配置DMP
devStatus = mpu.dmpInitialize();

//提供自己的陀螺仪偏置在这里，对于规模最小灵敏度
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688出厂默认为我的测试芯片

// 确保它的工作（返回0，如果这样）
if (devStatus == 0) {

// 打开DMP，现在，它已经准备好
mpu.setDMPEnabled(true);

// 设置我们的DMP Ready标志使主循环（）函数知道它的好来使用它
dmpReady = true;

// 获得预期的DMP数据包大小为以后进行比较
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
//错误！
//1=初始内存加载失败
//2= DMP配置更新失败
//（如果它要打破，通常的代码为1）
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}

/////////////////////////////////////////HMC5883L
mag.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(mag.testConnection() ? "HMC5883L connection successful" : "HMC5883L connection failed");

//////////////////////////////////////////BMP085
barometer.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(barometer.testConnection() ? "BMP085 connection successful" : "BMP085 connection failed");

///////////////////////////////////////////GPS
gps.begin(19200);

//////////////////////////////////////////////////////////////////////

}

// ================================================================
// === 主程序循环 ===
// ================================================================

void loop() {

#ifdef OUTPUT_TIME_AND_MemoryFree
jsq6x=millis();
#endif

#ifdef OUTPUT_GPS

//无条件接受串口的字符，目的截取GPRMC字符串
while (gps.available() > 0) {
gpstimes=millis();//重置gpstimes为millis()
konghangpanduan=0;//因为串口有数据，所以空行判断为0，即不空行
if(panduan2)gps.read();//采集到所需数据后，舍弃其他串口数据
else GPRMC = gps.read();//软串口读取GPS数据，并保存在临时数组GPRMC中

//delayMicroseconds(50);

//做一个保险，保证数组不会溢出，同时也限制了最大字符串的长度（99）
//为兼容$GPGGA截取、或GPRMC不在最后一行的情况，设定一旦采集完毕，就锁定jsq1的值
if(jsq1<99 && !panduan2) jsq1++;
else if(!panduan2) jsq1=99;

if(jsq1>5 && !panduan1){

if(GPRMC=='M' && GPRMC=='C')panduan1=1;
else{
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
GPRMC=GPRMC;
jsq1=5;
}

}

if(panduan1 && GPRMC=='*'){
panduan2=1;

}
}

#endif

//如果连续10ms串口都没有收到新的数据，则说明一个周期内（1秒）的GPS数据发送已完毕
//这个空行一个周期只做一次，所以同时加入布尔量konghangpanduan的判断
if(millis()-gpstimes>10 && !konghangpanduan){
konghangpanduan=1;

gpsdechuli();

//清空临时数组temp1
for(int col=0;col<99;col++)GPRMC=0;
jsq1=0;
panduan1=0;
panduan2=0;

}

// 获取当前FIFO计数
//fifoCount = mpu.getFIFOCount();
//重置，以便我们能继续干净
mpu.resetFIFO();

//等待正确可用的数据的长度，应该是一个非常短暂的等待
while (fifoCount < packetSize)fifoCount = mpu.getFIFOCount();

//读取FIFO中的数据包
mpu.getFIFOBytes(fifoBuffer, packetSize);

//轨道先进先出算在这里万一有>1包可
//（这让我们马上了解更多，而无需等待中断）
fifoCount -= packetSize;

#ifdef OUTPUT_READABLE_YAWPITCHROLL
//偏航/ 从四元数来计算俯仰/滚动角（以度为单位）
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

if(ychangertest!=ypr){

lvboxianfu=ychangertest-ypr;
if(lvboxianfu<0)lvboxianfu=-lvboxianfu;
if(lvboxianfu>0.0004){

ychanger=1;//0.0002弧度=0.0114592 度(°)
ychangertest=ypr;
}
}

if(pchangertest!=ypr){

lvboxianfu=pchangertest-ypr;
if(lvboxianfu<0)lvboxianfu=-lvboxianfu;
if(lvboxianfu>0.0004) {
pchanger=1;
pchangertest=ypr;
}

}

if(rchangertest!=ypr){

lvboxianfu=rchangertest-ypr;
if(lvboxianfu<0)lvboxianfu=-lvboxianfu;
if(lvboxianfu>0.0004) {

rchanger=1;
rchangertest=ypr;
}

}
//Y、P、R依次进行异或运算，确保只要有一个值为1，则yprchanger为1
yprchanger=ychanger^pchanger;
yprchanger=yprchanger^rchanger;

//仅当YPR改变时，才打印新的YPR信息

if(yprchanger){

ychanger=0;
pchanger=0;
rchanger=0;

Serial.print("ypr\t");
Serial.print(ypr * 180/M_PI);
Serial.print("\t");
Serial.print(ypr * 180/M_PI);
Serial.print("\t");
Serial.println(ypr * 180/M_PI);
}
#endif

#ifdef OUTPUT_HMC5883L
// 输出航向
// read raw heading measurements from device
mag.getHeading(&mx, &my, &mz);

// To calculate heading in degrees. 0 degree indicates North
headingchangertest = atan2(my, mx);
if(headingchangertest < 0)
headingchangertest += 2 * M_PI;

if(headingchangertest!=heading){

lvboxianfu=headingchangertest-heading;
if(lvboxianfu<0)lvboxianfu=-lvboxianfu;
if(lvboxianfu>0.015){
headingchanger=1;

heading=headingchangertest;
}
}

if(headingchanger){
headingchanger=0;

Serial.print("heading:\t");
Serial.println(heading * 180/M_PI);
}

#endif

#ifdef OUTPUT_BMP085

barometer.setControl(BMP085_MODE_TEMPERATURE);

// wait appropriate time for conversion (4.5ms delay)
lastMicros = micros();
while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

// read calibrated temperature value in degrees Celsius
temperature = barometer.getTemperatureC();

// request pressure (3x oversampling mode, high detail, 23.5ms delay)
barometer.setControl(BMP085_MODE_PRESSURE_3);
while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

// read calibrated pressure value in Pascals (Pa)
pressurechangertest = barometer.getPressure();

if(pressurechangertest!=pressure){

lvboxianfu=pressurechangertest-pressure;
if(lvboxianfu<0)lvboxianfu=-lvboxianfu;

if(lvboxianfu>10){

pressurechanger=1;
pressure=pressurechangertest;
}

}

if(pressurechanger){

pressurechanger=0;

// calculate absolute altitude in meters based on known pressure
// (may pass a second "sea level pressure" parameter here,
// otherwise uses the standard value of 101325 Pa)
altitude= barometer.getAltitude(pressure);

// display measured values if appropriate
Serial.print("T/P/A\t");
Serial.print(temperature); Serial.print("\t");
Serial.print(pressure); Serial.print("\t");
Serial.print(altitude);
Serial.println("");

}
#endif

//仅当GPS的经纬度改变时，才打印新的经纬度信息
if(gpschanger){
gpschanger=0;

Serial.print("weidu=");
Serial.print(weiduA);
Serial.print(weiduB);
Serial.print(".");
Serial.println(weiduC);

Serial.print("jingdu=");
Serial.print(jingduA);
Serial.print(jingduB);
Serial.print(".");
Serial.println(jingduC);
}

#ifdef OUTPUT_TIME_AND_MemoryFree
jsq6x=millis()-jsq6x;
Serial.print("jsq6x=");
Serial.println(jsq6x);
//Serial.println("ms");
//Serial.print("freeMemory()=");
//Serial.println(freeMemory());
//Serial.println("/*******************************************/");
//Serial.println();
Serial.println();
#endif

}

void gpsdechuli()
{

#ifdef OUTPUT_other_test
Serial.print("jsq1=");
Serial.println(jsq1);
#endif

//异或运算，校验GPS输出字符串的准确性

//经过实测，jsq1的值GPRMC实际有效长度，所以实际亦或运算数=jsq1-4（$ *XX不参与异或运算）
for(int col=1;col<jsq1-3;col++){
if(col==1)yihuoyunsuan=GPRMC;
else yihuoyunsuan=yihuoyunsuan ^ GPRMC;
}
//因为定义int的时候，yihuoyunsuan的结果是以10进制DEC表示的，所以需转换为16进制HEX
//因为GPS校验数据0位也进行了传输，所以在转换的时候有必要将情况分类讨论
//(yihuoyunsuan=0)/(0<yihuoyunsuan<17)/(17<yihuoyunsuan)
if(yihuoyunsuan==0){
//校验数10进制为0，直接填充字符串00
jianyan="00";
}else if(yihuoyunsuan>15){
//此时转换为16进制以后，天然有2位，很理想，不用处理
jianyan = String(yihuoyunsuan,HEX);
}else{
//校验数10进制为1-15时，转换为16进制就只有1位数，所以需要在前面填0
jianyan = "0";
jianyan += String(yihuoyunsuan,HEX);
}
//实践中发现，jianyan中的字符是以小写方式存在，虽然Serial.println(yihuoyunsuan,HEX)会以大写方式打印出来
//但直接Serial.println(jianyan)就是小写的，这是啥情况？
//将其字母转换为大写，否则与GPS传输的校验码对比时大小写问题校验失败（GPS传输的校验码为大写的）
jianyan.toUpperCase();
///////////////////////////////////////////显示异或校验码，方便给GPS编程，虽然还没成功过
#ifdef OUTPUT_other_test
Serial.print("jianyan=");
Serial.println(jianyan);
#endif

if(jianyan==GPRMC && jianyan==GPRMC ){
//一致，则说明数据是有效的，输出校验结果
jiaoyanjieguo=1;
}else{
//不一致
jiaoyanjieguo=0;
}
//对校验数组进行清零
jianyan="";

/*---------------------------异或运算，校验GPS输出字符串的准确性 end--------------------------*/

#ifdef OUTPUT_other_test
Serial.print("jiaoyanjieguo=");
Serial.println(jiaoyanjieguo);
Serial.print("GPRMC=");
Serial.println(GPRMC);
#endif

//解析GPRMC
if(jiaoyanjieguo){
jiaoyanjieguo=0;

if(GPRMC=='A'){
dingweiok=1;//定位有效

//纬度数字化
for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+20, 2);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(weiduA!=gpschangertest){
weiduA=gpschangertest;
gpschanger=1;
}

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+22, 2);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(weiduB!=gpschangertest){
weiduB=gpschangertest;
gpschanger=1;
}

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+25, 4);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(weiduC!=gpschangertest){
weiduC=gpschangertest;
gpschanger=1;
}

//经度数字化
for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+32, 3);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(jingduA!=gpschangertest){
jingduA=gpschangertest;
gpschanger=1;
}

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+35, 2);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(jingduB!=gpschangertest){
jingduB=gpschangertest;
gpschanger=1;
}

for(int col=0;col<7;col++)tempx=0;
memcpy(tempx,GPRMC+38, 4);
jianyan =tempx;
gpschangertest=jianyan.toInt();
if(jingduC!=gpschangertest){
jingduC=gpschangertest;
gpschanger=1;
}

jianyan="";

}else {
dingweiok=0; //定位无效
weiduA=0;
weiduB=0;
weiduC=0;
jingduA=0;
jingduB=0;
jingduC=0;
}

/*------------解析GPRMC---------------*/

}

}

duanliangcong 发表于 2014-10-3 20:59:16

楼主，你是火星来的吗？太厉害了

Albert，_Einste 发表于 2014-10-29 21:53:18

求助，大神能不能留下联系方式？

g86690 发表于 2014-11-3 02:47:21

太神了!
請問若要取X軸做動態傾角測試,這有動態加減速及離心力,是否無法用pitch(俯仰），yaw(偏航），roll（滚动)的一軸?要用無重力分量的"OUTPUT_READABLE_REALACCEL""OUTPUT_READABLE_WORLDACCEL"組合?謝謝

4463424 发表于 2014-11-6 15:07:25

如果能加个卡尔曼滤波就更利害了！！！！！！！！！！！

xingyun1024 发表于 2014-12-29 10:42:02

:dizzy::dizzy::dizzy::dizzy::dizzy::dizzy::dizzy::dizzy:

suoma 发表于 2014-12-29 19:28:32

谢谢分享，学习一下

雨轩发表于 2015-6-19 11:46:00

我头好晕:dizzy:

wdjkzym 发表于 2015-6-19 13:23:14

怎么连一起的?

页: [1] 2

极客工坊's Archiver