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

铁血㊣罡风 发表于 2013-9-6 09:58:43

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

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


http://v.youku.com/v_show/id_XNjA1Mjc3NjQw.html

#include "SoftwareSerial.h"
#include "TinyGPS.h"
#include "LiquidCrystal.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu(0x68);
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);
TinyGPS gps;
SoftwareSerial nss(4, 5);

byte up = {
B00100,
B01010,
B10101,
B00100,
B00100,
B00100,
B00100,
};
byte down = {
B00100,
B00100,
B00100,
B00100,
B10101,
B01010,
B00100,
};
bool dmpReady = false;// set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;   // count of all bytes currently in FIFO
uint8_t fifoBuffer; // FIFO storage buffer

Quaternion q;         //          quaternion container
VectorFloat gravity;    //             gravity vector
float ypr;         //    yaw/pitch/roll container and gravity vector
volatile bool mpuInterrupt = false;   // indicates whether MPU interrupt pin has gone high

static void gpsdump(TinyGPS &gps);
static bool feedgps();
static void print_float(float val, float invalid, int len, int prec);
static void print_int(unsigned long val, unsigned long invalid, int len);
static void print_str(const char *str, int len);
//int kk=0;
void dmpDataReady()
{
mpuInterrupt = true;
}

void setup()
{
lcd.createChar(1, up);
lcd.createChar(2, down);
Wire.begin();
nss.begin(9600);
lcd.begin(24, 2);
//pinMode(3,INPUT);
lcd.print ("---Storm GPS---");
lcd.setCursor(0, 1);
lcd.print ("Initializing......");
mpu.initialize();
delay(5);
lcd.clear();
devStatus = mpu.dmpInitialize();
if (devStatus == 0)
{
    mpu.setDMPEnabled(true);
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
}   
}

void loop()
{
    float pitch,roll;
/***************MPU-6050*****************/
if (!dmpReady)
    return;
if (!mpuInterrupt && fifoCount < packetSize)
    return;
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
    mpu.resetFIFO();
}
else if (mpuIntStatus & 0x02)
{
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    fifoCount -= packetSize;

    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);//从DMP中取出Yaw、Pitch、Roll三个轴的角度，放入数组ypr。单位：弧度
    pitch=ypr * 180/M_PI;
    roll=ypr * 180/M_PI;
/***************GPS*****************/
bool newdata = false;
unsigned long start = millis();
// Every second we print an update
while (millis() - start < 250)
{
    if (feedgps())
      newdata = true;
}   
   /***************Display****************/
    gpsdump(gps);
    tly_lcd(pitch,1);
    tly_lcd(roll,2);         
}
}
void tly_lcd(float tly,int k_tly)//(tly)：MPU6050传递的pitch\roll值，先要转换成int；(k_tly)1为pitch; 2为roll;
{
   int i_tly;
   i_tly=(int)tly;
   switch (k_tly)
   {
   case 1:
              lcd.setCursor(0,0);
        if(i_tly<0)
          {lcd.write (2);}
      else
          {lcd.write (1);}      
      if(abs(i_tly)<10)
              {
              lcd.print("0");
              lcd.print(abs(i_tly));        
              }
            else
              {
              lcd.print(abs(i_tly));
              }
        if(i_tly<0)
          {lcd.write (2);}
      else
          {lcd.write (1);}
      break;
      case 2:
              lcd.setCursor(0,1);
        if(i_tly<0)
          {lcd.print("<");}
      else
          {lcd.print(">");}   
      if(abs(i_tly)<10)
              {
              lcd.print("0");
              lcd.print(abs(i_tly));        
              }
            else
              {
              lcd.print(abs(i_tly));
              }
        if(i_tly<0)
          {lcd.print("<");}
      else
          {lcd.print(">");}
      break;
   }        
}

static void gpsdump(TinyGPS &gps)
{
float flat, flon;
unsigned long age, date, time, chars = 0;

//print_int(gps.satellites(), TinyGPS::GPS_INVALID_SATELLITES, 5);
//print_int(gps.hdop(), TinyGPS::GPS_INVALID_HDOP, 5);
gps.f_get_position(&flat, &flon, &age);

lcd_ref(8,0,2);   
lcd.setCursor(8, 0);
lcd.print ("*");   
//print_int(gps.satellites(), TinyGPS::GPS_INVALID_SATELLITES, 3);

lcd_ref(5,0,3);
lcd.setCursor(5, 0);
print_str(gps.f_course() == TinyGPS::GPS_INVALID_F_ANGLE ? "*** " : TinyGPS::cardinal(gps.f_course()), 2);

lcd_ref(5,1,4);
lcd.setCursor(5, 1);
print_float(gps.f_course(), TinyGPS::GPS_INVALID_F_ANGLE, 3, 0);

lcd_ref(10,0,9);
lcd.setCursor(10, 0);
lcd.print ("N");
print_float(flat, TinyGPS::GPS_INVALID_F_ANGLE, 6, 4);

lcd_ref(19,0,4);
lcd.setCursor(19, 0);   
print_float(gps.f_altitude(), TinyGPS::GPS_INVALID_F_ALTITUDE, 4, 0);
lcd.print ("M");



lcd_ref(9,1,9);
lcd.setCursor(9, 1);       
lcd.print ("E");
print_float(flon, TinyGPS::GPS_INVALID_F_ANGLE, 7, 45);

lcd_ref(19,1,3);
lcd.setCursor(19, 1);
print_float(gps.f_speed_kmph(), TinyGPS::GPS_INVALID_F_SPEED, 3, 0);
lcd.print ("KM");

//print_int(age, TinyGPS::GPS_INVALID_AGE, 5);
//print_date(gps);
}

void lcd_ref(int x,int y,int z)
{
lcd.setCursor(x, y);
for (int k=0;k<z;k++)
{
    lcd.print (" ");   
}
}

static void print_int(unsigned long val, unsigned long invalid, int len)
{
char sz;
if (val == invalid)
    strcpy(sz, "*******");
else
    sprintf(sz, "%ld", val);
sz = 0;
for (int i=strlen(sz); i<len; ++i)
    sz = ' ';
if (len > 0)
    sz = ' ';
lcd.print(sz);
feedgps();
}

static void print_float(float val, float invalid, int len, int prec)
{
char sz;
if (val == invalid)
{
    strcpy(sz, "*******");
    sz = 0;
      if (len > 0)
          sz = ' ';
    for (int i=7; i<len; ++i)
      sz = ' ';
    lcd.print(sz);
}
else
{
    int vi = abs((int)val);
    int flen = prec + (val < 0.0 ? 2 : 1);
    flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;   
    for (int i=flen; i<len; ++i)
      {lcd.print(" ");}
    lcd.print(val, prec);
}
feedgps();
}

static void print_str(const char *str, int len)
{
int slen = strlen(str);
for (int i=0; i<len; ++i)
    lcd.print(i<slen ? str : ' ');
feedgps();
}

static bool feedgps()
{
while (nss.available())
{
    if (gps.encode(nss.read()))
      return true;
}
return false;
}

迷你强 发表于 2013-9-6 10:50:04

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

铁血㊣罡风 发表于 2013-9-6 11:09:04

迷你强 发表于 2013-9-6 10:50 static/image/common/back.gif
貌似很亮骚，有使用视频么？

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

qptimus 发表于 2013-9-6 17:04:19

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

学慧放弃 发表于 2013-9-6 21:03:45

好厉害啊 啊啊啊啊啊啊啊

duanliangcong 发表于 2013-9-6 22:15:14

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

agaonet 发表于 2013-9-6 23:34:18

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

Randy 发表于 2013-9-7 09:10:33

强大而不失华丽！

gold8 发表于 2014-5-28 09:55:45

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

鸿子子 发表于 2014-12-17 14:50:52

亲有四元数的库么

SproutME 发表于 2014-12-17 16:16:19

鸿子子 发表于 2014-12-17 14:50 static/image/common/back.gif
亲有四元数的库么

找X－IMU官网

查看完整版本: 做的GPS和陀螺仪的越野E表 请各位帮忙优化下代码