极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 7571|回复: 15

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

[复制链接]
发表于 2014-9-28 11:05:56 | 显示全部楼层 |阅读模式
问题:
1、偏航初始值不为零怎么破?
2、这个数据一般怎么用?
3、如何把偏航修正为与HMC5883L一致?

连接:全都在arduino的i2c总线上
备注:BMP180用的BMP085的库,也就是最小分辨率只有后者的一半(taobao的JS说这是升级版,其实是降级版啦)
效果:
捕获5.PNG
代码:
  1. //输出控制:

  2. // 实际在四元组件[W,X,Y,Z]格式
  3. #define OUTPUT_READABLE_QUATERNION

  4. // 欧拉角
  5. #define OUTPUT_READABLE_EULER

  6. // 偏航/俯仰/滚动角(以度为单位)
  7. #define OUTPUT_READABLE_YAWPITCHROLL

  8. //加速度,扣除重力影响
  9. #define OUTPUT_READABLE_REALACCEL

  10. // 加速度,扣除重力影响+参照偏航
  11. #define OUTPUT_READABLE_WORLDACCEL

  12. //输出航向 HMC5883L
  13. #define OUTPUT_HMC5883L

  14. //程序耗时,剩余内存数查询
  15. #define OUTPUT_TIME_AND_MemoryFree

  16. //BMP085
  17. #define OUTPUT_BMP085

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


  19. //MPU6050:

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

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

  26. /*
  27. 科普:
  28. 按照posix标准,一般整形对应的*_t类型为:
  29. 1字节 uint8_t 近似于 byte
  30. 2字节 uint16_t 近似于 int
  31. 4字节 uint32_t
  32. 8字节 uint64_t
  33. */

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

  41. // 方向/运动
  42. Quaternion q; // [w, x, y, z] 四元数
  43. VectorInt16 aa; // [x, y, z] 加速度传感器测量
  44. VectorInt16 aaReal; // [x, y, z] 无重力加速度传感器的测量
  45. VectorInt16 aaWorld; // [x, y, z] 世界框架加速度传感器测量
  46. VectorFloat gravity; // [x, y, z] 重力矢量
  47. float euler[3]; // [psi, theta, phi] 欧拉角容器
  48. float ypr[3]; // [yaw, pitch, roll] 偏航/俯仰/滚动容器和重力矢量

  49. /*---------------------MPU6050 end---------------------------------*/


  50. //HMC5883L

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

  57. /*---------------------HMC5883L end---------------------------------*/

  58. //BMP180 用的是BMP085的库,实际最小分辨率只有后者的一半
  59. #include "BMP085.h"
  60. BMP085 barometer;
  61. float temperature;
  62. float pressure;
  63. float altitude;
  64. int32_t lastMicros;
  65. /*---------------------BMP085 end---------------------------------*/


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

  70. // ================================================================
  71. // === 初始设置 ===
  72. // ================================================================

  73. void setup() {



  74. // 加入I2C总线(I2Cdev库没有自动执行此操作)

  75. Wire.begin();
  76. TWBR = 24; // 400kHz的I2C时钟(200kHz的,如果CPU为8MHz)


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

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


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

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

  88. // 确保它的工作(返回0,如果这样)
  89. if (devStatus == 0) {

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

  92. // 设置我们的DMP Ready标志使主循环()函数 知道它的好来使用它
  93. dmpReady = true;

  94. // 获得预期的DMP数据包大小为以后进行比较
  95. packetSize = mpu.dmpGetFIFOPacketSize();
  96. } else {
  97. //错误!
  98. //1=初始内存加载失败
  99. //2= DMP配置更新失败
  100. //(如果它要打破,通常的代码为1)
  101. Serial.print(F("DMP Initialization failed (code "));
  102. Serial.print(devStatus);
  103. Serial.println(F(")"));
  104. }

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

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

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

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

  115. }



  116. // ================================================================
  117. // === 主程序循环 ===
  118. // ================================================================

  119. void loop() {

  120. #ifdef OUTPUT_TIME_AND_MemoryFree
  121. jsq6x=millis();
  122. #endif


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

  125. //mpuIntStatus = mpu.getIntStatus();

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

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

  130. //轨道先进先出算在这里万一有>1包可
  131. //(这让我们马上了解更多,而无需等待中断)
  132. fifoCount -= packetSize;


  133. #ifdef OUTPUT_READABLE_QUATERNION
  134. // 显示四元素 in easy matrix form: w x y z
  135. mpu.dmpGetQuaternion(&q, fifoBuffer);
  136. Serial.print("quat\t");
  137. Serial.print(q.w);
  138. Serial.print("\t");
  139. Serial.print(q.x);
  140. Serial.print("\t");
  141. Serial.print(q.y);
  142. Serial.print("\t");
  143. Serial.println(q.z);
  144. #endif

  145. #ifdef OUTPUT_READABLE_EULER
  146. // 显示欧拉角的度数
  147. mpu.dmpGetQuaternion(&q, fifoBuffer);
  148. mpu.dmpGetEuler(euler, &q);
  149. Serial.print("euler\t");
  150. Serial.print(euler[0] * 180/M_PI);
  151. Serial.print("\t");
  152. Serial.print(euler[1] * 180/M_PI);
  153. Serial.print("\t");
  154. Serial.println(euler[2] * 180/M_PI);
  155. #endif

  156. #ifdef OUTPUT_READABLE_YAWPITCHROLL
  157. //偏航/ 从四元数来计算俯仰/滚动角(以度为单位)
  158. mpu.dmpGetQuaternion(&q, fifoBuffer);
  159. mpu.dmpGetGravity(&gravity, &q);
  160. mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  161. Serial.print("ypr\t");
  162. Serial.print(ypr[0] * 180/M_PI);
  163. Serial.print("\t");
  164. Serial.print(ypr[1] * 180/M_PI);
  165. Serial.print("\t");
  166. Serial.println(ypr[2] * 180/M_PI);
  167. #endif

  168. #ifdef OUTPUT_READABLE_REALACCEL
  169. // 显示真正的加速,调整,去掉重力
  170. mpu.dmpGetQuaternion(&q, fifoBuffer);
  171. mpu.dmpGetAccel(&aa, fifoBuffer);
  172. mpu.dmpGetGravity(&gravity, &q);
  173. mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  174. Serial.print("areal\t");
  175. Serial.print(aaReal.x);
  176. Serial.print("\t");
  177. Serial.print(aaReal.y);
  178. Serial.print("\t");
  179. Serial.println(aaReal.z);
  180. #endif

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



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

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


  207. #ifdef OUTPUT_BMP085


  208.     barometer.setControl(BMP085_MODE_TEMPERATURE);
  209.    
  210.     // wait appropriate time for conversion (4.5ms delay)
  211.     lastMicros = micros();
  212.     while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

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

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

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

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

  224.     // display measured values if appropriate
  225.     Serial.print("T/P/A\t");
  226.     Serial.print(temperature); Serial.print("\t");
  227.     Serial.print(pressure); Serial.print("\t");
  228.     Serial.print(altitude);
  229.     Serial.println("");
  230. #endif


  231. #ifdef OUTPUT_TIME_AND_MemoryFree
  232. jsq6x=millis()-jsq6x;
  233. Serial.print("jsq6x=");
  234. Serial.print(jsq6x);
  235. Serial.println("ms");
  236. Serial.print("freeMemory()=");
  237. Serial.println(freeMemory());
  238. Serial.println("/*******************************************/");
  239. Serial.println();
  240. Serial.println();
  241. #endif


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



  246. }
复制代码
回复

使用道具 举报

 楼主| 发表于 2014-9-29 20:18:44 | 显示全部楼层

10DOF(MPU6050 + HMC5883L + BMP180)+ GPS

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

  1. //输出控制:

  2. // 实际在四元组件[W,X,Y,Z]格式
  3. #define OUTPUT_READABLE_QUATERNION

  4. // 欧拉角
  5. #define OUTPUT_READABLE_EULER

  6. // 偏航/俯仰/滚动角(以度为单位)
  7. #define OUTPUT_READABLE_YAWPITCHROLL

  8. //加速度,扣除重力影响
  9. #define OUTPUT_READABLE_REALACCEL

  10. // 加速度,扣除重力影响+参照偏航
  11. #define OUTPUT_READABLE_WORLDACCEL

  12. //输出航向 HMC5883L
  13. #define OUTPUT_HMC5883L

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

  16. //BMP085
  17. #define OUTPUT_BMP085

  18. //GPS
  19. #define OUTPUT_GPS


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


  21. //MPU6050:

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

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

  28. /*
  29. 科普:
  30. 按照posix标准,一般整形对应的*_t类型为:
  31. 1字节 uint8_t 近似于 byte
  32. 2字节 uint16_t 近似于 int
  33. 4字节 uint32_t
  34. 8字节 uint64_t
  35. */

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

  43. // 方向/运动
  44. Quaternion q; // [w, x, y, z] 四元数
  45. VectorInt16 aa; // [x, y, z] 加速度传感器测量
  46. VectorInt16 aaReal; // [x, y, z] 无重力加速度传感器的测量
  47. VectorInt16 aaWorld; // [x, y, z] 世界框架加速度传感器测量
  48. VectorFloat gravity; // [x, y, z] 重力矢量
  49. float euler[3]; // [psi, theta, phi] 欧拉角容器
  50. float ypr[3]; // [yaw, pitch, roll] 偏航/俯仰/滚动容器和重力矢量

  51. /*---------------------MPU6050 end---------------------------------*/


  52. //HMC5883L

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

  59. /*---------------------HMC5883L end---------------------------------*/

  60. //BMP180 用的是BMP085的库,实际最小分辨率只有后者的一半
  61. #include "BMP085.h"
  62. BMP085 barometer;
  63. float temperature;
  64. float pressure;
  65. float altitude;
  66. int32_t lastMicros;
  67. /*---------------------BMP085 end---------------------------------*/


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


  72. //GPS
  73. //////////////////////////////////////////////////////////////////////////////////////////////

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

  90. // ================================================================
  91. // === 初始设置 ===
  92. // ================================================================

  93. void setup() {



  94. // 加入I2C总线(I2Cdev库没有自动执行此操作)

  95. Wire.begin();
  96. TWBR = 24; // 400kHz的I2C时钟(200kHz的,如果CPU为8MHz)


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

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


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

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

  108. // 确保它的工作(返回0,如果这样)
  109. if (devStatus == 0) {

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

  112. // 设置我们的DMP Ready标志使主循环()函数 知道它的好来使用它
  113. dmpReady = true;

  114. // 获得预期的DMP数据包大小为以后进行比较
  115. packetSize = mpu.dmpGetFIFOPacketSize();
  116. } else {
  117. //错误!
  118. //1=初始内存加载失败
  119. //2= DMP配置更新失败
  120. //(如果它要打破,通常的代码为1)
  121. Serial.print(F("DMP Initialization failed (code "));
  122. Serial.print(devStatus);
  123. Serial.println(F(")"));
  124. }

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

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

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

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


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



  137. //////////////////////////////////////////////////////////////////////


  138. }



  139. // ================================================================
  140. // === 主程序循环 ===
  141. // ================================================================

  142. void loop() {

  143. #ifdef OUTPUT_TIME_other_MemoryFree
  144. jsq6x=millis();
  145. #endif

  146. #ifdef OUTPUT_GPS



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


  153. //delayMicroseconds(50);

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


  158. if(jsq1>5 && !panduan1){

  159. if(GPRMC[jsq1-2]=='M' && GPRMC[jsq1-1]=='C')panduan1=1;
  160. else{
  161. GPRMC[jsq1-6]=GPRMC[jsq1-5];
  162. GPRMC[jsq1-5]=GPRMC[jsq1-4];
  163. GPRMC[jsq1-4]=GPRMC[jsq1-3];
  164. GPRMC[jsq1-3]=GPRMC[jsq1-2];
  165. GPRMC[jsq1-2]=GPRMC[jsq1-1];
  166. jsq1=5;
  167. }

  168. }

  169. if(panduan1 && GPRMC[jsq1-3]=='*')panduan2=1;

  170. }



  171. #endif

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

  176. #ifdef OUTPUT_TIME_other_MemoryFree
  177. Serial.print("jsq1=");
  178. Serial.println(jsq1);
  179. #endif

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

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

  209. if(jianyan[0]==GPRMC[jsq1-2] && jianyan[1]==GPRMC[jsq1-1] ){
  210. //一致,则说明数据是有效的,输出校验结果
  211. jiaoyanjieguo=1;
  212. }else{
  213. //不一致
  214. jiaoyanjieguo=0;
  215. }
  216. //对校验数组进行清零
  217. jianyan="";

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

  219. #ifdef OUTPUT_TIME_other_MemoryFree
  220. Serial.print("jiaoyanjieguo=");
  221. Serial.println(jiaoyanjieguo);
  222. Serial.print("GPRMC=");
  223. Serial.println(GPRMC);
  224. #endif

  225. //解析GPRMC
  226. if(jiaoyanjieguo){
  227. jiaoyanjieguo=0;



  228. if(GPRMC[18]=='A'){
  229. dingweiok=1;//定位有效



  230. //纬度
  231. memcpy(weidu,GPRMC+20, 2);
  232. weidu[2]=176;
  233. memcpy(weidu+3,GPRMC+22, 7);
  234. weidu[10]=GPRMC[30];

  235. //经度

  236. memcpy(jingdu,GPRMC+32, 3);
  237.   jingdu[3]=176;
  238. memcpy(jingdu+4,GPRMC+35, 7);
  239. jingdu[11]=GPRMC[43];

  240. #ifdef OUTPUT_TIME_other_MemoryFree

  241. Serial.print("weidu=");
  242. Serial.println(weidu);
  243. Serial.print("jingdu=");
  244. Serial.println(jingdu);
  245. #endif

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

  247. for(int col=0;col<12;col++)weidu[col]=0;
  248. for(int col=0;col<13;col++)jingdu[col]=0;
  249. }

  250. /*------------解析GPRMC---------------*/



  251. //清空临时数组temp1
  252. for(int col=0;col<99;col++)GPRMC[col]=0;
  253. jsq1=0;
  254. panduan1=0;
  255. panduan2=0;

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


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

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

  264. //轨道先进先出算在这里万一有>1包可
  265. //(这让我们马上了解更多,而无需等待中断)
  266. fifoCount -= packetSize;


  267. #ifdef OUTPUT_READABLE_QUATERNION
  268. // 显示四元素 in easy matrix form: w x y z
  269. mpu.dmpGetQuaternion(&q, fifoBuffer);
  270. Serial.print("quat\t");
  271. Serial.print(q.w);
  272. Serial.print("\t");
  273. Serial.print(q.x);
  274. Serial.print("\t");
  275. Serial.print(q.y);
  276. Serial.print("\t");
  277. Serial.println(q.z);
  278. #endif

  279. #ifdef OUTPUT_READABLE_EULER
  280. // 显示欧拉角的度数
  281. mpu.dmpGetQuaternion(&q, fifoBuffer);
  282. mpu.dmpGetEuler(euler, &q);
  283. Serial.print("euler\t");
  284. Serial.print(euler[0] * 180/M_PI);
  285. Serial.print("\t");
  286. Serial.print(euler[1] * 180/M_PI);
  287. Serial.print("\t");
  288. Serial.println(euler[2] * 180/M_PI);
  289. #endif

  290. #ifdef OUTPUT_READABLE_YAWPITCHROLL
  291. //偏航/ 从四元数来计算俯仰/滚动角(以度为单位)
  292. mpu.dmpGetQuaternion(&q, fifoBuffer);
  293. mpu.dmpGetGravity(&gravity, &q);
  294. mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  295. Serial.print("ypr\t");
  296. Serial.print(ypr[0] * 180/M_PI);
  297. Serial.print("\t");
  298. Serial.print(ypr[1] * 180/M_PI);
  299. Serial.print("\t");
  300. Serial.println(ypr[2] * 180/M_PI);
  301. #endif

  302. #ifdef OUTPUT_READABLE_REALACCEL
  303. // 显示真正的加速,调整,去掉重力
  304. mpu.dmpGetQuaternion(&q, fifoBuffer);
  305. mpu.dmpGetAccel(&aa, fifoBuffer);
  306. mpu.dmpGetGravity(&gravity, &q);
  307. mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  308. Serial.print("areal\t");
  309. Serial.print(aaReal.x);
  310. Serial.print("\t");
  311. Serial.print(aaReal.y);
  312. Serial.print("\t");
  313. Serial.println(aaReal.z);
  314. #endif

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



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

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


  341. #ifdef OUTPUT_BMP085


  342.     barometer.setControl(BMP085_MODE_TEMPERATURE);
  343.    
  344.     // wait appropriate time for conversion (4.5ms delay)
  345.     lastMicros = micros();
  346.     while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

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

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

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

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

  358.     // display measured values if appropriate
  359.     Serial.print("T/P/A\t");
  360.     Serial.print(temperature); Serial.print("\t");
  361.     Serial.print(pressure); Serial.print("\t");
  362.     Serial.print(altitude);
  363.     Serial.println("");
  364. #endif




  365. #ifdef OUTPUT_TIME_other_MemoryFree
  366. jsq6x=millis()-jsq6x;
  367. Serial.print("jsq6x=");
  368. Serial.print(jsq6x);
  369. Serial.println("ms");
  370. Serial.print("freeMemory()=");
  371. Serial.println(freeMemory());
  372. Serial.println("/*******************************************/");
  373. Serial.println();
  374. Serial.println();
  375. #endif

  376. }





  377. }
复制代码
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-9-29 20:44:48 | 显示全部楼层

Arduino MPU6050 + HMC5883L 主机从机

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

  1. // ---------------------------------------------------------------------------
  2. // Receive all measurements from an 9 DOF sensor board.
  3. // ---------------------------------------------------------------------------
  4. //
  5. // Structure:
  6. //                                         
  7. //                                           Sub I2C
  8. //                                        ______^ ______
  9. //                                       |              |
  10. // ----------
  11. //  Arduino  |               ----------------          -------------
  12. //  2560     |- 3.3 V ------ | MPU 6050     |          |  HMC5883  |
  13. //           |- GND ---------| Acceleration,|---SDA ---|  Compass  |
  14. //           |- SDA ---------| Gyro, Temp   |---SCL ---|           |
  15. //           |- SCL ---------|              |          |           |
  16. //           |               ----------------          -------------
  17. //-----------
  18. //                       |___________________ _______________________|
  19. //                                           V
  20. //                                   Integrated IMU sensor
  21. //
  22. // Pull-up resistors are integrated in the sensor board.
  23. //
  24. // IMPORTANT: When I connect the sensor board to a 5V power supply, it was
  25. //            not possible to realize a I2C connection in this case. I made
  26. //            some experiments with additional pull-upps on the I2C but
  27. //            without any results.
  28. //
  29. // ---------------------------------------------------------------------------
  30. //
  31. // It exists a very good library for I2C communication based on Arduino "Wire"
  32. // provided by Jeff Rowberg. It integrates specific controllers as MPU 6050
  33. // and HMC 5883. Take a view on [url]https://github.com/jrowberg/i2cdevlib[/url]
  34. //
  35. // The example was implement with i2cdevlib Version and extends the existing
  36. // MPU_6050_raw // example. It uses the code proposed by @muzhig on i2cdevlib
  37. // [url]https://github.com/jrowberg/i2cdevlib/issues/18[/url]
  38. // ---------------------------------------------------------------------------

  39. #include "Wire.h"
  40. #include "I2Cdev.h"
  41. #include "MPU6050.h"
  42. #include "HMC5883L.h"

  43. // The default I2C address is defined
  44. // MPU 6050 - 0x68 - MPU6050_DEFAULT_ADDRESS
  45. // HMC5883L - 0x1E - HMC5883L_DEFAULT_ADDRESS   
  46. MPU6050 mpu6050;
  47. HMC5883L hmc5883l;

  48. int16_t ax, ay, az;
  49. int16_t gx, gy, gz;
  50. int16_t mx, my, mz;
  51. double temp;

  52. #define LED_PIN 13
  53. bool blinkState = false;

  54. // this method is just used to collect different setSlave operations
  55. void setSlaveControl(uint8_t slaveID){
  56.     mpu6050.setSlaveEnabled(slaveID, true);
  57.     mpu6050.setSlaveWordByteSwap(slaveID, false);
  58.     mpu6050.setSlaveWriteMode(slaveID, false);
  59.     mpu6050.setSlaveWordGroupOffset(slaveID, false);
  60.     mpu6050.setSlaveDataLength(slaveID, 2);
  61. }

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

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

  68.     mpu6050.initialize();
  69.     if (mpu6050.testConnection()){
  70.       Serial.println("MPU6050 connection successful");
  71.     }
  72.     else {
  73.       Serial.println("MPU6050 connection failed");
  74.     }

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

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

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

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

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

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

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

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

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

  109.     // configure Arduino LED for
  110.     pinMode(LED_PIN, OUTPUT);
  111. }

  112. void loop() {

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

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

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

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

  125.    Serial.print(ax); Serial.print("\t");
  126.    Serial.print(ay); Serial.print("\t");
  127.    Serial.print(az); Serial.print("|\t");
  128.    Serial.print(gx); Serial.print("\t");
  129.    Serial.print(gy); Serial.print("\t");
  130.    Serial.print(gz); Serial.print("|\t");
  131.    Serial.print(heading * 180/M_PI); Serial.print("|\t");
  132.    Serial.println(temp,3);

  133.     // blink LED to indicate activity
  134.     blinkState = !blinkState;
  135.     digitalWrite(LED_PIN, blinkState);
  136. }
复制代码
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-9-29 21:44:05 | 显示全部楼层
http://segmentfault.com/blog/riodream/1190000000408831

pitch(俯仰),yaw(偏航),roll(滚动)
理解传说中的roll、yaw、pitch
* roll:绕x轴
* pitch:绕y轴
* yaw:绕z轴
Fig11-5_YawPitchRoll_usingCameraUpDir.png

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

使用道具 举报

 楼主| 发表于 2014-9-29 22:36:03 | 显示全部楼层

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

  1. //输出控制:

  2. // 实际在四元组件[W,X,Y,Z]格式
  3. #define OUTPUT_READABLE_QUATERNION

  4. // 欧拉角
  5. #define OUTPUT_READABLE_EULER

  6. // 偏航/俯仰/滚动角(以度为单位)
  7. #define OUTPUT_READABLE_YAWPITCHROLL

  8. //加速度,扣除重力影响
  9. #define OUTPUT_READABLE_REALACCEL

  10. // 加速度,扣除重力影响+参照偏航
  11. #define OUTPUT_READABLE_WORLDACCEL

  12. //输出航向 HMC5883L
  13. #define OUTPUT_HMC5883L

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

  16. //BMP085
  17. #define OUTPUT_BMP085

  18. //GPS
  19. #define OUTPUT_GPS


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


  21. //MPU6050:

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

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

  28. /*
  29. 科普:
  30. 按照posix标准,一般整形对应的*_t类型为:
  31. 1字节 uint8_t 近似于 byte
  32. 2字节 uint16_t 近似于 int
  33. 4字节 uint32_t
  34. 8字节 uint64_t
  35. */

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

  43. // 方向/运动
  44. Quaternion q; // [w, x, y, z] 四元数
  45. VectorInt16 aa; // [x, y, z] 加速度传感器测量
  46. VectorInt16 aaReal; // [x, y, z] 无重力加速度传感器的测量
  47. VectorInt16 aaWorld; // [x, y, z] 世界框架加速度传感器测量
  48. VectorFloat gravity; // [x, y, z] 重力矢量
  49. float euler[3]; // [psi, theta, phi] 欧拉角容器
  50. float ypr[3]; // [yaw, pitch, roll] 偏航/俯仰/滚动容器和重力矢量

  51. /*---------------------MPU6050 end---------------------------------*/


  52. //HMC5883L

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

  59. /*---------------------HMC5883L end---------------------------------*/

  60. //BMP180 用的是BMP085的库,实际最小分辨率只有后者的一半
  61. #include "BMP085.h"
  62. BMP085 barometer;
  63. float temperature;
  64. float pressure;
  65. float altitude;
  66. int32_t lastMicros;
  67. /*---------------------BMP085 end---------------------------------*/


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


  72. //GPS
  73. //////////////////////////////////////////////////////////////////////////////////////////////

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

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


  90. /*---------------------GPS end---------------------------------*/

  91. // ================================================================
  92. // === 初始设置 ===
  93. // ================================================================

  94. void setup() {



  95. // 加入I2C总线(I2Cdev库没有自动执行此操作)

  96. Wire.begin();
  97. TWBR = 24; // 400kHz的I2C时钟(200kHz的,如果CPU为8MHz)


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

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


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

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

  109. // 确保它的工作(返回0,如果这样)
  110. if (devStatus == 0) {

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

  113. // 设置我们的DMP Ready标志使主循环()函数 知道它的好来使用它
  114. dmpReady = true;

  115. // 获得预期的DMP数据包大小为以后进行比较
  116. packetSize = mpu.dmpGetFIFOPacketSize();
  117. } else {
  118. //错误!
  119. //1=初始内存加载失败
  120. //2= DMP配置更新失败
  121. //(如果它要打破,通常的代码为1)
  122. Serial.print(F("DMP Initialization failed (code "));
  123. Serial.print(devStatus);
  124. Serial.println(F(")"));
  125. }

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

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

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

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


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



  138. //////////////////////////////////////////////////////////////////////


  139. }



  140. // ================================================================
  141. // === 主程序循环 ===
  142. // ================================================================

  143. void loop() {

  144. #ifdef OUTPUT_TIME_other_MemoryFree
  145. jsq6x=millis();
  146. #endif

  147. #ifdef OUTPUT_GPS



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


  154. //delayMicroseconds(50);

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


  159. if(jsq1>5 && !panduan1){

  160. if(GPRMC[jsq1-2]=='M' && GPRMC[jsq1-1]=='C')panduan1=1;
  161. else{
  162. GPRMC[jsq1-6]=GPRMC[jsq1-5];
  163. GPRMC[jsq1-5]=GPRMC[jsq1-4];
  164. GPRMC[jsq1-4]=GPRMC[jsq1-3];
  165. GPRMC[jsq1-3]=GPRMC[jsq1-2];
  166. GPRMC[jsq1-2]=GPRMC[jsq1-1];
  167. jsq1=5;
  168. }

  169. }

  170. if(panduan1 && GPRMC[jsq1-3]=='*')panduan2=1;

  171. }



  172. #endif

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

  177. #ifdef OUTPUT_TIME_other_MemoryFree
  178. Serial.print("jsq1=");
  179. Serial.println(jsq1);
  180. #endif

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

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

  210. if(jianyan[0]==GPRMC[jsq1-2] && jianyan[1]==GPRMC[jsq1-1] ){
  211. //一致,则说明数据是有效的,输出校验结果
  212. jiaoyanjieguo=1;
  213. }else{
  214. //不一致
  215. jiaoyanjieguo=0;
  216. }
  217. //对校验数组进行清零
  218. jianyan="";

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

  220. #ifdef OUTPUT_TIME_other_MemoryFree
  221. Serial.print("jiaoyanjieguo=");
  222. Serial.println(jiaoyanjieguo);
  223. Serial.print("GPRMC=");
  224. Serial.println(GPRMC);
  225. #endif

  226. //解析GPRMC
  227. if(jiaoyanjieguo){
  228. jiaoyanjieguo=0;



  229. if(GPRMC[18]=='A'){
  230. dingweiok=1;//定位有效



  231. //纬度数字化
  232. for(int col=0;col<7;col++)tempx[col]=0;
  233. memcpy(tempx,GPRMC+20, 2);
  234. jianyan =tempx;
  235. weiduA=jianyan.toInt();

  236. for(int col=0;col<7;col++)tempx[col]=0;
  237. memcpy(tempx,GPRMC+22, 2);
  238. jianyan =tempx;
  239. weiduB=jianyan.toInt();

  240.   for(int col=0;col<7;col++)tempx[col]=0;
  241. memcpy(tempx,GPRMC+25, 4);
  242. jianyan =tempx;
  243. weiduC=jianyan.toInt();


  244. //纬度数字化
  245. for(int col=0;col<7;col++)tempx[col]=0;
  246. memcpy(tempx,GPRMC+32, 3);
  247. jianyan =tempx;
  248. jingduA=jianyan.toInt();

  249. for(int col=0;col<7;col++)tempx[col]=0;
  250. memcpy(tempx,GPRMC+35, 2);
  251. jianyan =tempx;
  252. jingduB=jianyan.toInt();

  253.   for(int col=0;col<7;col++)tempx[col]=0;
  254. memcpy(tempx,GPRMC+38, 4);
  255. jianyan =tempx;
  256. jingduC=jianyan.toInt();

  257. jianyan="";

  258. #ifdef OUTPUT_TIME_other_MemoryFree
  259. Serial.print("weidu=");
  260. Serial.print(weiduA);
  261. Serial.print(weiduB);
  262. Serial.print(".");
  263. Serial.println(weiduC);

  264. Serial.print("weidu=");
  265. Serial.print(jingduA);
  266. Serial.print(jingduB);
  267. Serial.print(".");
  268. Serial.println(jingduC);

  269. #endif

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



  271. }

  272. /*------------解析GPRMC---------------*/



  273. //清空临时数组temp1
  274. for(int col=0;col<99;col++)GPRMC[col]=0;
  275. jsq1=0;
  276. panduan1=0;
  277. panduan2=0;

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


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

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

  286. //轨道先进先出算在这里万一有>1包可
  287. //(这让我们马上了解更多,而无需等待中断)
  288. fifoCount -= packetSize;


  289. #ifdef OUTPUT_READABLE_QUATERNION
  290. // 显示四元素 in easy matrix form: w x y z
  291. mpu.dmpGetQuaternion(&q, fifoBuffer);
  292. Serial.print("quat\t");
  293. Serial.print(q.w);
  294. Serial.print("\t");
  295. Serial.print(q.x);
  296. Serial.print("\t");
  297. Serial.print(q.y);
  298. Serial.print("\t");
  299. Serial.println(q.z);
  300. #endif

  301. #ifdef OUTPUT_READABLE_EULER
  302. // 显示欧拉角的度数
  303. mpu.dmpGetQuaternion(&q, fifoBuffer);
  304. mpu.dmpGetEuler(euler, &q);
  305. Serial.print("euler\t");
  306. Serial.print(euler[0] * 180/M_PI);
  307. Serial.print("\t");
  308. Serial.print(euler[1] * 180/M_PI);
  309. Serial.print("\t");
  310. Serial.println(euler[2] * 180/M_PI);
  311. #endif

  312. #ifdef OUTPUT_READABLE_YAWPITCHROLL
  313. //偏航/ 从四元数来计算俯仰/滚动角(以度为单位)
  314. mpu.dmpGetQuaternion(&q, fifoBuffer);
  315. mpu.dmpGetGravity(&gravity, &q);
  316. mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  317. Serial.print("ypr\t");
  318. Serial.print(ypr[0] * 180/M_PI);
  319. Serial.print("\t");
  320. Serial.print(ypr[1] * 180/M_PI);
  321. Serial.print("\t");
  322. Serial.println(ypr[2] * 180/M_PI);
  323. #endif

  324. #ifdef OUTPUT_READABLE_REALACCEL
  325. // 显示真正的加速,调整,去掉重力
  326. mpu.dmpGetQuaternion(&q, fifoBuffer);
  327. mpu.dmpGetAccel(&aa, fifoBuffer);
  328. mpu.dmpGetGravity(&gravity, &q);
  329. mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  330. Serial.print("areal\t");
  331. Serial.print(aaReal.x);
  332. Serial.print("\t");
  333. Serial.print(aaReal.y);
  334. Serial.print("\t");
  335. Serial.println(aaReal.z);
  336. #endif

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



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

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


  363. #ifdef OUTPUT_BMP085


  364.     barometer.setControl(BMP085_MODE_TEMPERATURE);
  365.    
  366.     // wait appropriate time for conversion (4.5ms delay)
  367.     lastMicros = micros();
  368.     while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds());

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

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

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

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

  380.     // display measured values if appropriate
  381.     Serial.print("T/P/A\t");
  382.     Serial.print(temperature); Serial.print("\t");
  383.     Serial.print(pressure); Serial.print("\t");
  384.     Serial.print(altitude);
  385.     Serial.println("");
  386. #endif




  387. #ifdef OUTPUT_TIME_other_MemoryFree
  388. jsq6x=millis()-jsq6x;
  389. Serial.print("jsq6x=");
  390. Serial.print(jsq6x);
  391. Serial.println("ms");
  392. Serial.print("freeMemory()=");
  393. Serial.println(freeMemory());
  394. Serial.println("/*******************************************/");
  395. Serial.println();
  396. Serial.println();
  397. #endif

  398. }





  399. }
复制代码
回复 支持 反对

使用道具 举报

 楼主| 发表于 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       



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

  4. //输出控制:



  5. // 偏航/俯仰/滚动角(以度为单位)
  6. #define OUTPUT_READABLE_YAWPITCHROLL


  7. //输出航向 HMC5883L
  8. #define OUTPUT_HMC5883L

  9. //其他测试项目
  10. //#define OUTPUT_other_test

  11. //程序耗时,剩余内存数查询
  12. //#define OUTPUT_TIME_AND_MemoryFree

  13. //BMP085
  14. #define OUTPUT_BMP085

  15. //GPS
  16. #define OUTPUT_GPS


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


  18. //MPU6050:

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

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

  25. /*
  26. 科普:
  27. 按照posix标准,一般整形对应的*_t类型为:
  28. 1字节 uint8_t 近似于 byte
  29. 2字节 uint16_t 近似于 int
  30. 4字节 uint32_t
  31. 8字节 uint64_t
  32. */

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

  40. // 方向/运动
  41. Quaternion q; // [w, x, y, z] 四元数
  42. VectorInt16 aa; // [x, y, z] 加速度传感器测量
  43. VectorInt16 aaReal; // [x, y, z] 无重力加速度传感器的测量
  44. VectorInt16 aaWorld; // [x, y, z] 世界框架加速度传感器测量
  45. VectorFloat gravity; // [x, y, z] 重力矢量
  46. float euler[3]; // [psi, theta, phi] 欧拉角容器
  47. float ypr[3]; // [yaw, pitch, roll] 偏航/俯仰/滚动容器和重力矢量

  48. float ychangertest,pchangertest,rchangertest;
  49. boolean ychanger,pchanger,rchanger;
  50. boolean yprchanger;
  51. /*---------------------MPU6050 end---------------------------------*/


  52. //HMC5883L

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

  60.   float headingchangertest;
  61.   boolean headingchanger;
  62. /*---------------------HMC5883L end---------------------------------*/

  63. //BMP180 用的是BMP085的库,实际最小分辨率只有后者的一半
  64. #include "BMP085.h"
  65. BMP085 barometer;
  66. float temperature;
  67. float pressure;
  68. float altitude;
  69. int32_t lastMicros;

  70.   float highchangertest;
  71.   boolean highchanger;
  72. /*---------------------BMP085 end---------------------------------*/


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


  77. //GPS
  78. //////////////////////////////////////////////////////////////////////////////////////////////

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

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


  97. /*---------------------GPS end---------------------------------*/

  98. // ================================================================
  99. // === 初始设置 ===
  100. // ================================================================

  101. void setup() {



  102. // 加入I2C总线(I2Cdev库没有自动执行此操作)

  103. Wire.begin();
  104. TWBR = 24; // 400kHz的I2C时钟(200kHz的,如果CPU为8MHz)


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

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


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

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

  116. // 确保它的工作(返回0,如果这样)
  117. if (devStatus == 0) {

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

  120. // 设置我们的DMP Ready标志使主循环()函数 知道它的好来使用它
  121. dmpReady = true;

  122. // 获得预期的DMP数据包大小为以后进行比较
  123. packetSize = mpu.dmpGetFIFOPacketSize();
  124. } else {
  125. //错误!
  126. //1=初始内存加载失败
  127. //2= DMP配置更新失败
  128. //(如果它要打破,通常的代码为1)
  129. Serial.print(F("DMP Initialization failed (code "));
  130. Serial.print(devStatus);
  131. Serial.println(F(")"));
  132. }

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

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

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

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


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



  145. //////////////////////////////////////////////////////////////////////


  146. }



  147. // ================================================================
  148. // === 主程序循环 ===
  149. // ================================================================

  150. void loop() {

  151. #ifdef OUTPUT_TIME_AND_MemoryFree
  152. jsq6x=millis();
  153. #endif

  154. #ifdef OUTPUT_GPS



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


  161. //delayMicroseconds(50);

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


  166. if(jsq1>5 && !panduan1){

  167. if(GPRMC[jsq1-2]=='M' && GPRMC[jsq1-1]=='C')panduan1=1;
  168. else{
  169. GPRMC[jsq1-6]=GPRMC[jsq1-5];
  170. GPRMC[jsq1-5]=GPRMC[jsq1-4];
  171. GPRMC[jsq1-4]=GPRMC[jsq1-3];
  172. GPRMC[jsq1-3]=GPRMC[jsq1-2];
  173. GPRMC[jsq1-2]=GPRMC[jsq1-1];
  174. jsq1=5;
  175. }

  176. }

  177. if(panduan1 && GPRMC[jsq1-3]=='*'){
  178. panduan2=1;




  179. }
  180. }



  181. #endif

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

  186. gpsdechuli();






  187. //清空临时数组temp1
  188. for(int col=0;col<99;col++)GPRMC[col]=0;
  189. jsq1=0;
  190. panduan1=0;
  191. panduan2=0;

  192. }


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


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

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

  201. //轨道先进先出算在这里万一有>1包可
  202. //(这让我们马上了解更多,而无需等待中断)
  203. fifoCount -= packetSize;






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

  209. if(ychangertest!=ypr[0]){
  210. ychangertest=ypr[0];
  211. ychanger=1;
  212. }

  213. if(pchangertest!=ypr[1]){
  214. pchangertest=ypr[1];
  215. pchanger=1;
  216. }

  217. if(rchangertest!=ypr[2]){
  218. rchangertest=ypr[2];
  219. rchanger=1;
  220. }

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

  224. //仅当YPR改变时,才打印新的YPR信息

  225. if(yprchanger){

  226. ychanger=0;
  227. pchanger=0;
  228. rchanger=0;

  229. Serial.print("ypr\t");
  230. Serial.print(ypr[0] * 180/M_PI);
  231. Serial.print("\t");
  232. Serial.print(ypr[1] * 180/M_PI);
  233. Serial.print("\t");
  234. Serial.println(ypr[2] * 180/M_PI);
  235. }
  236. #endif







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

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

  245.   if(headingchangertest!=heading){
  246.   heading=headingchangertest;
  247.   headingchanger=1;
  248.   }


  249. if(headingchanger){
  250. headingchanger=0;

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

  254. #endif


  255. #ifdef OUTPUT_BMP085


  256.     barometer.setControl(BMP085_MODE_TEMPERATURE);

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

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

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

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

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

  275. if(highchanger){
  276.    
  277.          highchanger=0;
  278.         // display measured values if appropriate
  279.     Serial.print("T/P/A\t");
  280.     Serial.print(temperature); Serial.print("\t");
  281.     Serial.print(pressure); Serial.print("\t");
  282.     Serial.print(altitude);
  283.     Serial.println("");
  284.        
  285.         }
  286. #endif


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

  290. Serial.print("weidu=");
  291. Serial.print(weiduA);
  292. Serial.print(weiduB);
  293. Serial.print(".");
  294. Serial.println(weiduC);

  295. Serial.print("jingdu=");
  296. Serial.print(jingduA);
  297. Serial.print(jingduB);
  298. Serial.print(".");
  299. Serial.println(jingduC);
  300. }


  301. #ifdef OUTPUT_TIME_AND_MemoryFree
  302. jsq6x=millis()-jsq6x;
  303. Serial.print("jsq6x=");
  304. Serial.println(jsq6x);
  305. //Serial.println("ms");
  306. //Serial.print("freeMemory()=");
  307. //Serial.println(freeMemory());
  308. //Serial.println("/*******************************************/");
  309. //Serial.println();
  310. Serial.println();
  311. #endif






  312. }


  313. void gpsdechuli()
  314. {

  315. #ifdef OUTPUT_other_test
  316. Serial.print("jsq1=");
  317. Serial.println(jsq1);
  318. #endif

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

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

  348. if(jianyan[0]==GPRMC[jsq1-2] && jianyan[1]==GPRMC[jsq1-1] ){
  349. //一致,则说明数据是有效的,输出校验结果
  350. jiaoyanjieguo=1;
  351. }else{
  352. //不一致
  353. jiaoyanjieguo=0;
  354. }
  355. //对校验数组进行清零
  356. jianyan="";

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

  358. #ifdef OUTPUT_other_test
  359. Serial.print("jiaoyanjieguo=");
  360. Serial.println(jiaoyanjieguo);
  361. Serial.print("GPRMC=");
  362. Serial.println(GPRMC);
  363. #endif

  364. //解析GPRMC
  365. if(jiaoyanjieguo){
  366. jiaoyanjieguo=0;



  367. if(GPRMC[18]=='A'){
  368. dingweiok=1;//定位有效




  369. //纬度数字化
  370. for(int col=0;col<7;col++)tempx[col]=0;
  371. memcpy(tempx,GPRMC+20, 2);
  372. jianyan =tempx;
  373. gpschangertest=jianyan.toInt();
  374. if(weiduA!=gpschangertest){
  375. weiduA=gpschangertest;
  376. gpschanger=1;
  377. }


  378. for(int col=0;col<7;col++)tempx[col]=0;
  379. memcpy(tempx,GPRMC+22, 2);
  380. jianyan =tempx;
  381. gpschangertest=jianyan.toInt();
  382. if(weiduB!=gpschangertest){
  383. weiduB=gpschangertest;
  384. gpschanger=1;
  385. }

  386.   for(int col=0;col<7;col++)tempx[col]=0;
  387. memcpy(tempx,GPRMC+25, 4);
  388. jianyan =tempx;
  389. gpschangertest=jianyan.toInt();
  390. if(weiduC!=gpschangertest){
  391. weiduC=gpschangertest;
  392. gpschanger=1;
  393. }


  394. //经度数字化
  395. for(int col=0;col<7;col++)tempx[col]=0;
  396. memcpy(tempx,GPRMC+32, 3);
  397. jianyan =tempx;
  398. gpschangertest=jianyan.toInt();
  399. if(jingduA!=gpschangertest){
  400. jingduA=gpschangertest;
  401. gpschanger=1;
  402. }

  403. for(int col=0;col<7;col++)tempx[col]=0;
  404. memcpy(tempx,GPRMC+35, 2);
  405. jianyan =tempx;
  406. gpschangertest=jianyan.toInt();
  407. if(jingduB!=gpschangertest){
  408. jingduB=gpschangertest;
  409. gpschanger=1;
  410. }

  411.   for(int col=0;col<7;col++)tempx[col]=0;
  412. memcpy(tempx,GPRMC+38, 4);
  413. jianyan =tempx;
  414. gpschangertest=jianyan.toInt();
  415. if(jingduC!=gpschangertest){
  416. jingduC=gpschangertest;
  417. gpschanger=1;
  418. }

  419. jianyan="";

  420. }else {
  421. dingweiok=0; //定位无效
  422.   weiduA=0;
  423.    weiduB=0;
  424.     weiduC=0;
  425.   jingduA=0;
  426.    jingduB=0;
  427.     jingduC=0;
  428. }

  429. /*------------解析GPRMC---------------*/

  430. }

  431. }
复制代码
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-10-1 21:26:22 | 显示全部楼层

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

本帖最后由 hi55234 于 2014-10-1 21:34 编辑
  1. /*
  2. 为改善频繁而无效的串口传输数据,
  3. 仅当GPS定位后 经纬度(yrp、航向、气压)有变换时再输出串口信息

  4. 遗留问题:滤波

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

  6. */

  7. //输出控制:



  8. // 偏航/俯仰/滚动角(以度为单位)
  9. #define OUTPUT_READABLE_YAWPITCHROLL


  10. //输出航向 HMC5883L
  11. #define OUTPUT_HMC5883L

  12. //其他测试项目
  13. //#define OUTPUT_other_test

  14. //程序耗时,剩余内存数查询
  15. //#define OUTPUT_TIME_AND_MemoryFree

  16. //BMP085
  17. #define OUTPUT_BMP085

  18. //GPS
  19. #define OUTPUT_GPS


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


  21. //MPU6050:

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

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

  28. /*
  29. 科普:
  30. 按照posix标准,一般整形对应的*_t类型为:
  31. 1字节 uint8_t 近似于 byte
  32. 2字节 uint16_t 近似于 int
  33. 4字节 uint32_t
  34. 8字节 uint64_t
  35. */

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

  43. // 方向/运动
  44. Quaternion q; // [w, x, y, z] 四元数
  45. VectorInt16 aa; // [x, y, z] 加速度传感器测量
  46. VectorInt16 aaReal; // [x, y, z] 无重力加速度传感器的测量
  47. VectorInt16 aaWorld; // [x, y, z] 世界框架加速度传感器测量
  48. VectorFloat gravity; // [x, y, z] 重力矢量
  49. float euler[3]; // [psi, theta, phi] 欧拉角容器
  50. float ypr[3]; // [yaw, pitch, roll] 偏航/俯仰/滚动容器和重力矢量

  51. float ychangertest,pchangertest,rchangertest;
  52. boolean ychanger,pchanger,rchanger;
  53. boolean yprchanger;
  54. /*---------------------MPU6050 end---------------------------------*/


  55. //HMC5883L

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

  63.   float headingchangertest;
  64.   boolean headingchanger;
  65. /*---------------------HMC5883L end---------------------------------*/

  66. //BMP180 用的是BMP085的库,实际最小分辨率只有后者的一半
  67. #include "BMP085.h"
  68. BMP085 barometer;
  69. float temperature;
  70. float pressure;
  71. float altitude;
  72. int32_t lastMicros;

  73.   float pressurechangertest;
  74.   boolean pressurechanger;
  75. /*---------------------BMP085 end---------------------------------*/


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


  80. //GPS
  81. //////////////////////////////////////////////////////////////////////////////////////////////

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

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


  100. /*---------------------GPS end---------------------------------*/
  101. //滤波限幅
  102. float lvboxianfu;

  103. // ================================================================
  104. // === 初始设置 ===
  105. // ================================================================

  106. void setup() {



  107. // 加入I2C总线(I2Cdev库没有自动执行此操作)

  108. Wire.begin();
  109. TWBR = 24; // 400kHz的I2C时钟(200kHz的,如果CPU为8MHz)


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

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


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

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

  121. // 确保它的工作(返回0,如果这样)
  122. if (devStatus == 0) {

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

  125. // 设置我们的DMP Ready标志使主循环()函数 知道它的好来使用它
  126. dmpReady = true;

  127. // 获得预期的DMP数据包大小为以后进行比较
  128. packetSize = mpu.dmpGetFIFOPacketSize();
  129. } else {
  130. //错误!
  131. //1=初始内存加载失败
  132. //2= DMP配置更新失败
  133. //(如果它要打破,通常的代码为1)
  134. Serial.print(F("DMP Initialization failed (code "));
  135. Serial.print(devStatus);
  136. Serial.println(F(")"));
  137. }

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

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

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

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


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



  150. //////////////////////////////////////////////////////////////////////


  151. }



  152. // ================================================================
  153. // === 主程序循环 ===
  154. // ================================================================

  155. void loop() {

  156. #ifdef OUTPUT_TIME_AND_MemoryFree
  157. jsq6x=millis();
  158. #endif

  159. #ifdef OUTPUT_GPS



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


  166. //delayMicroseconds(50);

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


  171. if(jsq1>5 && !panduan1){

  172. if(GPRMC[jsq1-2]=='M' && GPRMC[jsq1-1]=='C')panduan1=1;
  173. else{
  174. GPRMC[jsq1-6]=GPRMC[jsq1-5];
  175. GPRMC[jsq1-5]=GPRMC[jsq1-4];
  176. GPRMC[jsq1-4]=GPRMC[jsq1-3];
  177. GPRMC[jsq1-3]=GPRMC[jsq1-2];
  178. GPRMC[jsq1-2]=GPRMC[jsq1-1];
  179. jsq1=5;
  180. }

  181. }

  182. if(panduan1 && GPRMC[jsq1-3]=='*'){
  183. panduan2=1;




  184. }
  185. }



  186. #endif

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

  191. gpsdechuli();






  192. //清空临时数组temp1
  193. for(int col=0;col<99;col++)GPRMC[col]=0;
  194. jsq1=0;
  195. panduan1=0;
  196. panduan2=0;

  197. }


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


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

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

  206. //轨道先进先出算在这里万一有>1包可
  207. //(这让我们马上了解更多,而无需等待中断)
  208. fifoCount -= packetSize;






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

  214. if(ychangertest!=ypr[0]){

  215. lvboxianfu=ychangertest-ypr[0];
  216. if(lvboxianfu<0)lvboxianfu=-lvboxianfu;
  217. if(lvboxianfu>0.0004){

  218. ychanger=1;//0.0002弧度=0.0114592 度(°)
  219. ychangertest=ypr[0];
  220. }
  221. }

  222. if(pchangertest!=ypr[1]){

  223. lvboxianfu=pchangertest-ypr[1];
  224. if(lvboxianfu<0)lvboxianfu=-lvboxianfu;
  225. if(lvboxianfu>0.0004) {
  226. pchanger=1;
  227. pchangertest=ypr[1];
  228. }

  229. }

  230. if(rchangertest!=ypr[2]){

  231. lvboxianfu=rchangertest-ypr[2];
  232. if(lvboxianfu<0)lvboxianfu=-lvboxianfu;
  233. if(lvboxianfu>0.0004) {

  234. rchanger=1;
  235. rchangertest=ypr[2];
  236. }

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

  241. //仅当YPR改变时,才打印新的YPR信息

  242. if(yprchanger){

  243. ychanger=0;
  244. pchanger=0;
  245. rchanger=0;

  246. Serial.print("ypr\t");
  247. Serial.print(ypr[0] * 180/M_PI);
  248. Serial.print("\t");
  249. Serial.print(ypr[1] * 180/M_PI);
  250. Serial.print("\t");
  251. Serial.println(ypr[2] * 180/M_PI);
  252. }
  253. #endif







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

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

  262.   if(headingchangertest!=heading){
  263.   
  264.    lvboxianfu=headingchangertest-heading;
  265. if(lvboxianfu<0)lvboxianfu=-lvboxianfu;
  266. if(lvboxianfu>0.015){
  267. headingchanger=1;
  268.   
  269.   heading=headingchangertest;
  270. }
  271.   }


  272. if(headingchanger){
  273. headingchanger=0;

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

  277. #endif


  278. #ifdef OUTPUT_BMP085


  279.     barometer.setControl(BMP085_MODE_TEMPERATURE);

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

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

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

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


  290. if(pressurechangertest!=pressure){

  291.     lvboxianfu=pressurechangertest-pressure;
  292. if(lvboxianfu<0)lvboxianfu=-lvboxianfu;

  293. if(lvboxianfu>10){

  294. pressurechanger=1;
  295. pressure=pressurechangertest;
  296. }

  297. }



  298. if(pressurechanger){
  299.    
  300.          pressurechanger=0;
  301.          
  302.               // calculate absolute altitude in meters based on known pressure
  303.     // (may pass a second "sea level pressure" parameter here,
  304.     // otherwise uses the standard value of 101325 Pa)
  305.     altitude= barometer.getAltitude(pressure);
  306.        
  307.         // display measured values if appropriate
  308.     Serial.print("T/P/A\t");
  309.     Serial.print(temperature); Serial.print("\t");
  310.     Serial.print(pressure); Serial.print("\t");
  311.     Serial.print(altitude);
  312.     Serial.println("");
  313.        
  314.         }
  315. #endif


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

  319. Serial.print("weidu=");
  320. Serial.print(weiduA);
  321. Serial.print(weiduB);
  322. Serial.print(".");
  323. Serial.println(weiduC);

  324. Serial.print("jingdu=");
  325. Serial.print(jingduA);
  326. Serial.print(jingduB);
  327. Serial.print(".");
  328. Serial.println(jingduC);
  329. }


  330. #ifdef OUTPUT_TIME_AND_MemoryFree
  331. jsq6x=millis()-jsq6x;
  332. Serial.print("jsq6x=");
  333. Serial.println(jsq6x);
  334. //Serial.println("ms");
  335. //Serial.print("freeMemory()=");
  336. //Serial.println(freeMemory());
  337. //Serial.println("/*******************************************/");
  338. //Serial.println();
  339. Serial.println();
  340. #endif






  341. }


  342. void gpsdechuli()
  343. {

  344. #ifdef OUTPUT_other_test
  345. Serial.print("jsq1=");
  346. Serial.println(jsq1);
  347. #endif

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

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

  377. if(jianyan[0]==GPRMC[jsq1-2] && jianyan[1]==GPRMC[jsq1-1] ){
  378. //一致,则说明数据是有效的,输出校验结果
  379. jiaoyanjieguo=1;
  380. }else{
  381. //不一致
  382. jiaoyanjieguo=0;
  383. }
  384. //对校验数组进行清零
  385. jianyan="";

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

  387. #ifdef OUTPUT_other_test
  388. Serial.print("jiaoyanjieguo=");
  389. Serial.println(jiaoyanjieguo);
  390. Serial.print("GPRMC=");
  391. Serial.println(GPRMC);
  392. #endif

  393. //解析GPRMC
  394. if(jiaoyanjieguo){
  395. jiaoyanjieguo=0;



  396. if(GPRMC[18]=='A'){
  397. dingweiok=1;//定位有效




  398. //纬度数字化
  399. for(int col=0;col<7;col++)tempx[col]=0;
  400. memcpy(tempx,GPRMC+20, 2);
  401. jianyan =tempx;
  402. gpschangertest=jianyan.toInt();
  403. if(weiduA!=gpschangertest){
  404. weiduA=gpschangertest;
  405. gpschanger=1;
  406. }


  407. for(int col=0;col<7;col++)tempx[col]=0;
  408. memcpy(tempx,GPRMC+22, 2);
  409. jianyan =tempx;
  410. gpschangertest=jianyan.toInt();
  411. if(weiduB!=gpschangertest){
  412. weiduB=gpschangertest;
  413. gpschanger=1;
  414. }

  415.   for(int col=0;col<7;col++)tempx[col]=0;
  416. memcpy(tempx,GPRMC+25, 4);
  417. jianyan =tempx;
  418. gpschangertest=jianyan.toInt();
  419. if(weiduC!=gpschangertest){
  420. weiduC=gpschangertest;
  421. gpschanger=1;
  422. }


  423. //经度数字化
  424. for(int col=0;col<7;col++)tempx[col]=0;
  425. memcpy(tempx,GPRMC+32, 3);
  426. jianyan =tempx;
  427. gpschangertest=jianyan.toInt();
  428. if(jingduA!=gpschangertest){
  429. jingduA=gpschangertest;
  430. gpschanger=1;
  431. }

  432. for(int col=0;col<7;col++)tempx[col]=0;
  433. memcpy(tempx,GPRMC+35, 2);
  434. jianyan =tempx;
  435. gpschangertest=jianyan.toInt();
  436. if(jingduB!=gpschangertest){
  437. jingduB=gpschangertest;
  438. gpschanger=1;
  439. }

  440.   for(int col=0;col<7;col++)tempx[col]=0;
  441. memcpy(tempx,GPRMC+38, 4);
  442. jianyan =tempx;
  443. gpschangertest=jianyan.toInt();
  444. if(jingduC!=gpschangertest){
  445. jingduC=gpschangertest;
  446. gpschanger=1;
  447. }

  448. jianyan="";

  449. }else {
  450. dingweiok=0; //定位无效
  451.   weiduA=0;
  452.    weiduB=0;
  453.     weiduC=0;
  454.   jingduA=0;
  455.    jingduB=0;
  456.     jingduC=0;
  457. }

  458. /*------------解析GPRMC---------------*/

  459. }

  460. }
复制代码
回复 支持 反对

使用道具 举报

发表于 2014-10-3 20:59:16 | 显示全部楼层
楼主,你是火星来的吗?太厉害了
回复 支持 反对

使用道具 举报

发表于 2014-10-29 21:53:18 | 显示全部楼层
求助,大神能不能留下联系方式?
回复 支持 反对

使用道具 举报

发表于 2014-11-3 02:47:21 | 显示全部楼层
太神了!
請問若要取X軸做動態傾角測試,這有動態加減速及離心力,是否無法用pitch(俯仰),yaw(偏航),roll(滚动)的一軸?要用無重力分量的"OUTPUT_READABLE_REALACCEL""OUTPUT_READABLE_WORLDACCEL"組合?謝謝
回复 支持 反对

使用道具 举报

发表于 2014-11-6 15:07:25 | 显示全部楼层
如果能加个卡尔曼滤波就更利害了!!!!!!!!!!!
回复 支持 反对

使用道具 举报

发表于 2014-12-29 10:42:02 | 显示全部楼层
回复 支持 反对

使用道具 举报

发表于 2014-12-29 19:28:32 | 显示全部楼层
谢谢分享,学习一下
回复 支持 反对

使用道具 举报

发表于 2015-6-19 11:46:00 | 显示全部楼层
我头好晕
回复 支持 反对

使用道具 举报

发表于 2015-6-19 13:23:14 | 显示全部楼层
怎么连一起的?
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

Archiver|联系我们|极客工坊 ( 浙ICP备09023225号 )

GMT+8, 2019-3-22 23:16 , Processed in 0.054700 second(s), 27 queries .

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

快速回复 返回顶部 返回列表