|
楼主 |
发表于 2014-9-29 22:36:03
|
显示全部楼层
MPU6050+HMC5883L+BMP180+GPS -2[纬度、经度数字化,使经纬度有意义]
- //输出控制:
- // 实际在四元组件[W,X,Y,Z]格式
- #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[64]; // FIFO存储缓冲器
- // 方向/运动
- Quaternion q; // [w, x, y, z] 四元数
- VectorInt16 aa; // [x, y, z] 加速度传感器测量
- VectorInt16 aaReal; // [x, y, z] 无重力加速度传感器的测量
- VectorInt16 aaWorld; // [x, y, z] 世界框架加速度传感器测量
- VectorFloat gravity; // [x, y, z] 重力矢量
- float euler[3]; // [psi, theta, phi] 欧拉角容器
- float ypr[3]; // [yaw, pitch, roll] 偏航/俯仰/滚动容器和重力矢量
- /*---------------------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[7];
- int weiduA,weiduB,weiduC;//把GPS格式纬度,以度、分、分的小数部分进行拆分
- int jingduA,jingduB,jingduC;//把GPS格式经度,以度、分、分的小数部分进行拆分
- char GPRMC[100];//临时数组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[jsq1] = 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[jsq1-2]=='M' && GPRMC[jsq1-1]=='C')panduan1=1;
- else{
- GPRMC[jsq1-6]=GPRMC[jsq1-5];
- GPRMC[jsq1-5]=GPRMC[jsq1-4];
- GPRMC[jsq1-4]=GPRMC[jsq1-3];
- GPRMC[jsq1-3]=GPRMC[jsq1-2];
- GPRMC[jsq1-2]=GPRMC[jsq1-1];
- jsq1=5;
- }
- }
- if(panduan1 && GPRMC[jsq1-3]=='*')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[col];
- else yihuoyunsuan=yihuoyunsuan ^ GPRMC[col];
- }
- //因为定义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[0]==GPRMC[jsq1-2] && jianyan[1]==GPRMC[jsq1-1] ){
- //一致,则说明数据是有效的,输出校验结果
- 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[18]=='A'){
- dingweiok=1;//定位有效
- //纬度数字化
- for(int col=0;col<7;col++)tempx[col]=0;
- memcpy(tempx,GPRMC+20, 2);
- jianyan =tempx;
- weiduA=jianyan.toInt();
-
- for(int col=0;col<7;col++)tempx[col]=0;
- memcpy(tempx,GPRMC+22, 2);
- jianyan =tempx;
- weiduB=jianyan.toInt();
-
- for(int col=0;col<7;col++)tempx[col]=0;
- memcpy(tempx,GPRMC+25, 4);
- jianyan =tempx;
- weiduC=jianyan.toInt();
-
- //纬度数字化
- for(int col=0;col<7;col++)tempx[col]=0;
- memcpy(tempx,GPRMC+32, 3);
- jianyan =tempx;
- jingduA=jianyan.toInt();
-
- for(int col=0;col<7;col++)tempx[col]=0;
- memcpy(tempx,GPRMC+35, 2);
- jianyan =tempx;
- jingduB=jianyan.toInt();
-
- for(int col=0;col<7;col++)tempx[col]=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[col]=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[0] * 180/M_PI);
- Serial.print("\t");
- Serial.print(euler[1] * 180/M_PI);
- Serial.print("\t");
- Serial.println(euler[2] * 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[0] * 180/M_PI);
- Serial.print("\t");
- Serial.print(ypr[1] * 180/M_PI);
- Serial.print("\t");
- Serial.println(ypr[2] * 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
- }
- }
复制代码 |
|