做的GPS和陀螺仪的越野E表 请各位帮忙优化下代码

铁血㊣罡风

本人新手一枚，从今年7月刚开始接触单片机，喜欢旅游，突发奇想做一个车载的陀螺仪，搜遍淘宝，只有一种越野E表，价格不菲，1960大米{:soso_e141:} ；又看HUD，淘宝上没有能够显示坡度的HUD，有的只是读取OBD中的速度、油耗等信息。
下定决心，自己做个出来，接下来就是买各种模块，尤其是各种显示模块，先后买了数码管、LCD1602、LCD2402、LCD4002,又买了两块9位的VFD，可投到前挡玻璃上还是差强人意，晚上看清晰无比，白天狗P看不到{:soso_e118:} ，只能用LCD正向显示。
现在又看好了VFD的2002,还没入手，各位前辈有什么建议，不妨给个！
下面是面包板接线图和视频。代码是东拼西凑的，哪位前辈有时间给优化下，在此表示感谢！

1. 
   
2. #include "SoftwareSerial.h"
   
3. #include "TinyGPS.h"
   
4. #include "LiquidCrystal.h"
   
5. #include "Wire.h"
   
6. #include "I2Cdev.h"
   
7. #include "MPU6050_6Axis_MotionApps20.h"
   
8. MPU6050 mpu(0x68);
   
9. LiquidCrystal lcd(12, 11, 10, 9, 8, 7);
   
10. TinyGPS gps;
    
11. SoftwareSerial nss(4, 5);
    
12. 
    
13. byte up[8] = {
    
14.   B00100,
    
15.   B01010,
    
16.   B10101,
    
17.   B00100,  
    
18.   B00100,
    
19.   B00100,
    
20.   B00100,
    
21. };
    
22. byte down[8] = {
    
23.   B00100,
    
24.   B00100,
    
25.   B00100,
    
26.   B00100,
    
27.   B10101,
    
28.   B01010,
    
29.   B00100,
    
30. };
    
31. bool dmpReady = false;  // set true if DMP init was successful
    
32. uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
    
33. uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
    
34. uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
    
35. uint16_t fifoCount;     // count of all bytes currently in FIFO
    
36. uint8_t fifoBuffer[64]; // FIFO storage buffer
    
37. 
    
38. Quaternion q;           // [w, x, y, z]         quaternion container
    
39. VectorFloat gravity;    // [x, y, z]            gravity vector
    
40. float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
    
41. volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
    
42. 
    
43. static void gpsdump(TinyGPS &gps);
    
44. static bool feedgps();
    
45. static void print_float(float val, float invalid, int len, int prec);
    
46. static void print_int(unsigned long val, unsigned long invalid, int len);
    
47. static void print_str(const char *str, int len);
    
48. //int kk=0;
    
49. void dmpDataReady()
    
50. {
    
51.   mpuInterrupt = true;
    
52. }
    
53. 
    
54. void setup()
    
55. {
    
56.   lcd.createChar(1, up);
    
57.   lcd.createChar(2, down);
    
58.   Wire.begin();
    
59.   nss.begin(9600);
    
60.   lcd.begin(24, 2);
    
61.   //pinMode(3,INPUT);
    
62.   lcd.print ("---Storm GPS---");
    
63.   lcd.setCursor(0, 1);
    
64.   lcd.print ("Initializing......");
    
65.   mpu.initialize();
    
66.   delay(5);
    
67.   lcd.clear();
    
68.   devStatus = mpu.dmpInitialize();
    
69.   if (devStatus == 0)
    
70.   {
    
71.     mpu.setDMPEnabled(true);
    
72.     attachInterrupt(0, dmpDataReady, RISING);
    
73.     mpuIntStatus = mpu.getIntStatus();
    
74.     dmpReady = true;
    
75.     packetSize = mpu.dmpGetFIFOPacketSize();
    
76.   }   
    
77. }
    
78. 
    
79. void loop()
    
80. {
    
81.     float pitch,roll;
    
82.   /***************MPU-6050*****************/
    
83.   if (!dmpReady)  
    
84.     return;
    
85.   if (!mpuInterrupt && fifoCount < packetSize)
    
86.     return;
    
87.   mpuInterrupt = false;
    
88.   mpuIntStatus = mpu.getIntStatus();
    
89.   fifoCount = mpu.getFIFOCount();
    
90.   if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    
91.   {
    
92.     mpu.resetFIFO();
    
93.   }
    
94.   else if (mpuIntStatus & 0x02)
    
95.   {
    
96.     while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    
97.     mpu.getFIFOBytes(fifoBuffer, packetSize);
    
98.     fifoCount -= packetSize;
    
99. 
    
100.     mpu.dmpGetQuaternion(&q, fifoBuffer);
     
101.     mpu.dmpGetGravity(&gravity, &q);
     
102.     mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);  //从DMP中取出Yaw、Pitch、Roll三个轴的角度，放入数组ypr。单位：弧度
     
103.     pitch=ypr[1] * 180/M_PI;
     
104.     roll=ypr[2] * 180/M_PI;
     
105.   /***************GPS*****************/
     
106.   bool newdata = false;
     
107.   unsigned long start = millis();
     
108.   // Every second we print an update
     
109.   while (millis() - start < 250)
     
110.   {
     
111.     if (feedgps())
     
112.       newdata = true;
     
113.   }   
     
114.    /***************Display****************/
     
115.     gpsdump(gps);
     
116.     tly_lcd(pitch,1);
     
117.     tly_lcd(roll,2);         
     
118.   }
     
119. }
     
120. void tly_lcd(float tly,int k_tly)  //(tly)：MPU6050传递的pitch\roll值，先要转换成int；(k_tly)1为pitch; 2为roll;
     
121. {
     
122.    int i_tly;
     
123.    i_tly=(int)tly;
     
124.    switch (k_tly)
     
125.    {
     
126.      case 1:
     
127.               lcd.setCursor(0,0);
     
128.         if(i_tly<0)
     
129.           {lcd.write (2);}
     
130.         else
     
131.           {lcd.write (1);}      
     
132.         if(abs(i_tly)<10)
     
133.               {
     
134.                 lcd.print("0");
     
135.                 lcd.print(abs(i_tly));        
     
136.               }
     
137.             else
     
138.               {
     
139.                 lcd.print(abs(i_tly));
     
140.               }
     
141.         if(i_tly<0)
     
142.           {lcd.write (2);}
     
143.         else
     
144.           {lcd.write (1);}  
     
145.         break;
     
146.         case 2:
     
147.               lcd.setCursor(0,1);
     
148.         if(i_tly<0)
     
149.           {lcd.print("<");}
     
150.         else
     
151.           {lcd.print(">");}     
     
152.         if(abs(i_tly)<10)
     
153.               {
     
154.                 lcd.print("0");
     
155.                 lcd.print(abs(i_tly));        
     
156.               }
     
157.             else
     
158.               {
     
159.                 lcd.print(abs(i_tly));
     
160.               }
     
161.         if(i_tly<0)
     
162.           {lcd.print("<");}
     
163.         else
     
164.           {lcd.print(">");}  
     
165.         break;
     
166.    }        
     
167. }
     
168. 
     
169. static void gpsdump(TinyGPS &gps)
     
170. {
     
171.   float flat, flon;
     
172.   unsigned long age, date, time, chars = 0;
     
173.   
     
174.   //print_int(gps.satellites(), TinyGPS::GPS_INVALID_SATELLITES, 5);
     
175.   //print_int(gps.hdop(), TinyGPS::GPS_INVALID_HDOP, 5);
     
176.   gps.f_get_position(&flat, &flon, &age);
     
177. 
     
178.   lcd_ref(8,0,2);     
     
179.   lcd.setCursor(8, 0);
     
180.   lcd.print ("*");   
     
181.   //print_int(gps.satellites(), TinyGPS::GPS_INVALID_SATELLITES, 3);
     
182.   
     
183.   lcd_ref(5,0,3);
     
184.   lcd.setCursor(5, 0);
     
185.   print_str(gps.f_course() == TinyGPS::GPS_INVALID_F_ANGLE ? "*** " : TinyGPS::cardinal(gps.f_course()), 2);
     
186.   
     
187.   lcd_ref(5,1,4);
     
188.   lcd.setCursor(5, 1);
     
189.   print_float(gps.f_course(), TinyGPS::GPS_INVALID_F_ANGLE, 3, 0);
     
190. 
     
191.   lcd_ref(10,0,9);
     
192.   lcd.setCursor(10, 0);  
     
193.   lcd.print ("N");  
     
194.   print_float(flat, TinyGPS::GPS_INVALID_F_ANGLE, 6, 4);
     
195. 
     
196.   lcd_ref(19,0,4);
     
197.   lcd.setCursor(19, 0);   
     
198.   print_float(gps.f_altitude(), TinyGPS::GPS_INVALID_F_ALTITUDE, 4, 0);
     
199.   lcd.print ("M");
     
200. 
     
201. 
     
202. 
     
203.   lcd_ref(9,1,9);
     
204.   lcd.setCursor(9, 1);       
     
205.   lcd.print ("E");
     
206.   print_float(flon, TinyGPS::GPS_INVALID_F_ANGLE, 7, 45);
     
207. 
     
208.   lcd_ref(19,1,3);
     
209.   lcd.setCursor(19, 1);
     
210.   print_float(gps.f_speed_kmph(), TinyGPS::GPS_INVALID_F_SPEED, 3, 0);
     
211.   lcd.print ("KM");
     
212. 
     
213.   //print_int(age, TinyGPS::GPS_INVALID_AGE, 5);
     
214.   //print_date(gps);
     
215. }
     
216. 
     
217. void lcd_ref(int x,int y,int z)
     
218. {
     
219.   lcd.setCursor(x, y);
     
220.   for (int k=0;k<z;k++)
     
221.   {
     
222.     lcd.print (" ");   
     
223.   }
     
224. }
     
225. 
     
226. static void print_int(unsigned long val, unsigned long invalid, int len)
     
227. {
     
228.   char sz[32];
     
229.   if (val == invalid)
     
230.     strcpy(sz, "*******");
     
231.   else
     
232.     sprintf(sz, "%ld", val);
     
233.   sz[len] = 0;
     
234.   for (int i=strlen(sz); i<len; ++i)
     
235.     sz[i] = ' ';
     
236.   if (len > 0)
     
237.     sz[len-1] = ' ';
     
238.   lcd.print(sz);
     
239.   feedgps();
     
240. }
     
241. 
     
242. static void print_float(float val, float invalid, int len, int prec)
     
243. {
     
244.   char sz[32];
     
245.   if (val == invalid)
     
246.   {
     
247.     strcpy(sz, "*******");
     
248.     sz[len] = 0;
     
249.         if (len > 0)
     
250.           sz[len-1] = ' ';
     
251.     for (int i=7; i<len; ++i)
     
252.         sz[i] = ' ';
     
253.     lcd.print(sz);
     
254.   }
     
255.   else
     
256.   {
     
257.     int vi = abs((int)val);
     
258.     int flen = prec + (val < 0.0 ? 2 : 1);
     
259.     flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;   
     
260.     for (int i=flen; i<len; ++i)
     
261.       {lcd.print(" ");}
     
262.     lcd.print(val, prec);
     
263.   }
     
264.   feedgps();
     
265. }
     
266. 
     
267. static void print_str(const char *str, int len)
     
268. {
     
269.   int slen = strlen(str);
     
270.   for (int i=0; i<len; ++i)
     
271.     lcd.print(i<slen ? str[i] : ' ');
     
272.   feedgps();
     
273. }
     
274. 
     
275. static bool feedgps()
     
276. {
     
277.   while (nss.available())
     
278.   {
     
279.     if (gps.encode(nss.read()))
     
280.       return true;
     
281.   }
     
282.   return false;
     
283. }

复制代码

迷你强

貌似很亮骚，有使用视频么？

铁血㊣罡风

迷你强 发表于 2013-9-6 10:50
貌似很亮骚，有使用视频么？

欢迎领导参观指导，视频上面有啊

qptimus

这个是想把信息投到挡风玻璃上么，参考一下战斗机的抬头显示吧

学慧放弃

好厉害啊 啊啊啊啊啊啊啊

duanliangcong

历害历害，这程序太牛B了。我一个都看不懂

agaonet

哈哈，超级赞！很有创意！想当年BBS才流行的时候，不就是文字符号的天下么！楼主加油！

Randy

强大而不失华丽！

gold8

太强了，太强了，这个真是太强了

鸿子子

亲有四元数的库么

SproutME

鸿子子 发表于 2014-12-17 14:50
亲有四元数的库么

找X－IMU官网