randi ritvaldi / Mbed 2 deprecated gps-fix

oooooo

Dependencies:   gps_settings_venus mbed

main.cpp@0:c6c878661d4e, 2014-06-04 (annotated)

Committer:
ritvaldirandi
Date:
Wed Jun 04 16:01:08 2014 +0000
Revision:
0:c6c878661d4e
mopimn

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ritvaldirandi 0:c6c878661d4e 1 #include "mbed.h"
ritvaldirandi 0:c6c878661d4e 2 #include "gps_stg_venus.h"
ritvaldirandi 0:c6c878661d4e 3 #include "string.h"
ritvaldirandi 0:c6c878661d4e 4
ritvaldirandi 0:c6c878661d4e 5 Serial pc(USBTX,USBRX);
ritvaldirandi 0:c6c878661d4e 6 Serial gps(p13,p14);
ritvaldirandi 0:c6c878661d4e 7 GPSVenus venus(gps);
ritvaldirandi 0:c6c878661d4e 8 DigitalOut led1(LED1);
ritvaldirandi 0:c6c878661d4e 9 char data_gps[256],ns,ew,sampah[256];
ritvaldirandi 0:c6c878661d4e 10 int g=0, cek_gps,lock,satelit;
ritvaldirandi 0:c6c878661d4e 11 float waktu,latx,longx, laty, longy,dilution, altitude;
ritvaldirandi 0:c6c878661d4e 12 double lat_target,long_target;
ritvaldirandi 0:c6c878661d4e 13 double degrees, minutes, seconds;
ritvaldirandi 0:c6c878661d4e 14 int lattitude[3], longitude[3];
ritvaldirandi 0:c6c878661d4e 15
ritvaldirandi 0:c6c878661d4e 16 void initialize_gps()
ritvaldirandi 0:c6c878661d4e 17 {
ritvaldirandi 0:c6c878661d4e 18 venus.setBaud_115200();
ritvaldirandi 0:c6c878661d4e 19 wait(0.5);
ritvaldirandi 0:c6c878661d4e 20 gps.baud(115200);
ritvaldirandi 0:c6c878661d4e 21 venus.setUpdateRate(20);
ritvaldirandi 0:c6c878661d4e 22 wait(0.5);
ritvaldirandi 0:c6c878661d4e 23 venus.setNmeaMessages(true, false, false, false, false, false);
ritvaldirandi 0:c6c878661d4e 24 }
ritvaldirandi 0:c6c878661d4e 25
ritvaldirandi 0:c6c878661d4e 26 void gps_interrupt()
ritvaldirandi 0:c6c878661d4e 27 {
ritvaldirandi 0:c6c878661d4e 28 // char temp = gps.getc();
ritvaldirandi 0:c6c878661d4e 29 // if(g==0)
ritvaldirandi 0:c6c878661d4e 30 // { led1=0;
ritvaldirandi 0:c6c878661d4e 31 // data_gps[g]=temp;
ritvaldirandi 0:c6c878661d4e 32 // if(data_gps[g]=='$')
ritvaldirandi 0:c6c878661d4e 33 // {g=1;data_gps[0]='$';}
ritvaldirandi 0:c6c878661d4e 34 // }
ritvaldirandi 0:c6c878661d4e 35 // else if (g==42){g=0;}
ritvaldirandi 0:c6c878661d4e 36 // else
ritvaldirandi 0:c6c878661d4e 37 // {
ritvaldirandi 0:c6c878661d4e 38 // data_gps[g]=temp;g++;led1=1;
ritvaldirandi 0:c6c878661d4e 39 // if(data_gps[g-1]=='\r')g=0;
ritvaldirandi 0:c6c878661d4e 40 // }
ritvaldirandi 0:c6c878661d4e 41 char temp=gps.getc();
ritvaldirandi 0:c6c878661d4e 42 if (temp=='$') {g=0;}
ritvaldirandi 0:c6c878661d4e 43 data_gps[g]=temp;
ritvaldirandi 0:c6c878661d4e 44 g++;
ritvaldirandi 0:c6c878661d4e 45
ritvaldirandi 0:c6c878661d4e 46
ritvaldirandi 0:c6c878661d4e 47 }
ritvaldirandi 0:c6c878661d4e 48
ritvaldirandi 0:c6c878661d4e 49 float trunc(float v) // pembulatan nilai
ritvaldirandi 0:c6c878661d4e 50 {
ritvaldirandi 0:c6c878661d4e 51 if(v < 0.0) {
ritvaldirandi 0:c6c878661d4e 52 v*= -1.0;
ritvaldirandi 0:c6c878661d4e 53 v = floor(v);
ritvaldirandi 0:c6c878661d4e 54 v*=-1.0;
ritvaldirandi 0:c6c878661d4e 55 } else {
ritvaldirandi 0:c6c878661d4e 56 v = floor(v);
ritvaldirandi 0:c6c878661d4e 57 }
ritvaldirandi 0:c6c878661d4e 58 return v;
ritvaldirandi 0:c6c878661d4e 59 }
ritvaldirandi 0:c6c878661d4e 60
ritvaldirandi 0:c6c878661d4e 61 void get_gps()
ritvaldirandi 0:c6c878661d4e 62 {
ritvaldirandi 0:c6c878661d4e 63 //if(data_gps[0]=='G'&&data_gps[1]=='P'&&data_gps[2]=='G'&&data_gps[3]=='G'&&data_gps[4]=='A'&&data_gps[5]==','){
ritvaldirandi 0:c6c878661d4e 64 //$GPGGA,050749.299,0745.9647,S,11022.3071,E,0,00,,189.5,M,4.3,M,,0000*5E
ritvaldirandi 0:c6c878661d4e 65 sscanf(data_gps,"$GPGGA,%f,%f,%c,%f,%c,%d,%2d,%f,%f",&waktu, &latx,&ns, &longx, &ew, &lock, &satelit, &dilution, &altitude);
ritvaldirandi 0:c6c878661d4e 66
ritvaldirandi 0:c6c878661d4e 67 degrees = (latx / 100.0f);
ritvaldirandi 0:c6c878661d4e 68 lattitude[0]=degrees;
ritvaldirandi 0:c6c878661d4e 69 degrees= degrees-lattitude[0];
ritvaldirandi 0:c6c878661d4e 70 minutes=(degrees*1000);
ritvaldirandi 0:c6c878661d4e 71 lattitude[1]=minutes;
ritvaldirandi 0:c6c878661d4e 72 minutes=minutes-lattitude[1];
ritvaldirandi 0:c6c878661d4e 73 seconds =(minutes*1000);
ritvaldirandi 0:c6c878661d4e 74 lattitude[2]= seconds;
ritvaldirandi 0:c6c878661d4e 75
ritvaldirandi 0:c6c878661d4e 76 degrees = (longx / 100.0f);
ritvaldirandi 0:c6c878661d4e 77 longitude[0]=degrees;
ritvaldirandi 0:c6c878661d4e 78 degrees= degrees-longitude[0];
ritvaldirandi 0:c6c878661d4e 79 minutes=(degrees*1000);
ritvaldirandi 0:c6c878661d4e 80 longitude[1]=minutes;
ritvaldirandi 0:c6c878661d4e 81 minutes=minutes-longitude[1];
ritvaldirandi 0:c6c878661d4e 82 seconds =(minutes*1000);
ritvaldirandi 0:c6c878661d4e 83 longitude[2]= seconds;
ritvaldirandi 0:c6c878661d4e 84
ritvaldirandi 0:c6c878661d4e 85 }
ritvaldirandi 0:c6c878661d4e 86
ritvaldirandi 0:c6c878661d4e 87 void gps_ats()
ritvaldirandi 0:c6c878661d4e 88 {
ritvaldirandi 0:c6c878661d4e 89 //fl waktu;
ritvaldirandi 0:c6c878661d4e 90 if(cek_gps==1){
ritvaldirandi 0:c6c878661d4e 91 sscanf(data_gps, "$GPGGA,%f,%f,%c,%f,%c",&waktu, &laty, &ns, &longy, &ew) ;
ritvaldirandi 0:c6c878661d4e 92
ritvaldirandi 0:c6c878661d4e 93 if(ns == 'S') { laty *= -1.0; }
ritvaldirandi 0:c6c878661d4e 94 if(ew == 'W') { longy *= -1.0; }
ritvaldirandi 0:c6c878661d4e 95 degrees = trunc(laty / 100.0f);
ritvaldirandi 0:c6c878661d4e 96 minutes = laty - (degrees * 100.0f);
ritvaldirandi 0:c6c878661d4e 97 lat_target = degrees + minutes / 60.0f;
ritvaldirandi 0:c6c878661d4e 98 degrees = trunc(longy / 100.0f);
ritvaldirandi 0:c6c878661d4e 99 minutes = longy- (degrees * 100.0f);
ritvaldirandi 0:c6c878661d4e 100 long_target = degrees + minutes / 60.0f;
ritvaldirandi 0:c6c878661d4e 101 }
ritvaldirandi 0:c6c878661d4e 102 }
ritvaldirandi 0:c6c878661d4e 103
ritvaldirandi 0:c6c878661d4e 104 void bin_dec_conv(unsigned int data)// anything to binary
ritvaldirandi 0:c6c878661d4e 105 {
ritvaldirandi 0:c6c878661d4e 106 pc.printf("%d%d%d",(data/100),(data%100/10),(data%10));
ritvaldirandi 0:c6c878661d4e 107 }
ritvaldirandi 0:c6c878661d4e 108
ritvaldirandi 0:c6c878661d4e 109 void telemetry_gps(unsigned int data_1_x,unsigned int data_2_x, unsigned int data_2_y,unsigned int data_3_x, unsigned int data_3_y, unsigned int data_3_z)
ritvaldirandi 0:c6c878661d4e 110 {
ritvaldirandi 0:c6c878661d4e 111
ritvaldirandi 0:c6c878661d4e 112 pc.putc(0x0D);
ritvaldirandi 0:c6c878661d4e 113 bin_dec_conv(106);
ritvaldirandi 0:c6c878661d4e 114 pc.putc(0x20);
ritvaldirandi 0:c6c878661d4e 115
ritvaldirandi 0:c6c878661d4e 116 bin_dec_conv(data_1_x);
ritvaldirandi 0:c6c878661d4e 117 pc.putc(0x20);
ritvaldirandi 0:c6c878661d4e 118
ritvaldirandi 0:c6c878661d4e 119 bin_dec_conv(data_2_x);
ritvaldirandi 0:c6c878661d4e 120 pc.putc(0x20);
ritvaldirandi 0:c6c878661d4e 121
ritvaldirandi 0:c6c878661d4e 122 bin_dec_conv(data_2_y);
ritvaldirandi 0:c6c878661d4e 123 pc.putc(0x20);
ritvaldirandi 0:c6c878661d4e 124
ritvaldirandi 0:c6c878661d4e 125 bin_dec_conv(data_3_x);
ritvaldirandi 0:c6c878661d4e 126 pc.putc(0x20);
ritvaldirandi 0:c6c878661d4e 127
ritvaldirandi 0:c6c878661d4e 128 bin_dec_conv(data_3_y);
ritvaldirandi 0:c6c878661d4e 129 pc.putc(0x20);
ritvaldirandi 0:c6c878661d4e 130
ritvaldirandi 0:c6c878661d4e 131 bin_dec_conv(data_3_z);
ritvaldirandi 0:c6c878661d4e 132
ritvaldirandi 0:c6c878661d4e 133 }
ritvaldirandi 0:c6c878661d4e 134
ritvaldirandi 0:c6c878661d4e 135 int main()
ritvaldirandi 0:c6c878661d4e 136 {
ritvaldirandi 0:c6c878661d4e 137 pc.baud(57600);
ritvaldirandi 0:c6c878661d4e 138 gps.baud(115200);
ritvaldirandi 0:c6c878661d4e 139 gps.attach(&gps_interrupt);
ritvaldirandi 0:c6c878661d4e 140 initialize_gps();
ritvaldirandi 0:c6c878661d4e 141 pc.printf("start!");
ritvaldirandi 0:c6c878661d4e 142 wait(3);
ritvaldirandi 0:c6c878661d4e 143 int i;
ritvaldirandi 0:c6c878661d4e 144 while(1)
ritvaldirandi 0:c6c878661d4e 145 {
ritvaldirandi 0:c6c878661d4e 146 for(i=0;i<=65;i++)
ritvaldirandi 0:c6c878661d4e 147 {
ritvaldirandi 0:c6c878661d4e 148 pc.putc(data_gps[i]);
ritvaldirandi 0:c6c878661d4e 149 }
ritvaldirandi 0:c6c878661d4e 150 pc.putc('\t');
ritvaldirandi 0:c6c878661d4e 151 get_gps();
ritvaldirandi 0:c6c878661d4e 152 //telemetry_gps(lattitude[0],lattitude[1],lattitude[2],longitude[0],longitude[1],longitude[2]);
ritvaldirandi 0:c6c878661d4e 153 //pc.putc('\t');
ritvaldirandi 0:c6c878661d4e 154 //pc.printf("cek=%d ",cek_gps);
ritvaldirandi 0:c6c878661d4e 155 pc.printf("waktu = %f\t",waktu);
ritvaldirandi 0:c6c878661d4e 156 pc.printf("tinggi = %f\t",altitude);
ritvaldirandi 0:c6c878661d4e 157 pc.printf("lock = %d\r",lock);
ritvaldirandi 0:c6c878661d4e 158 wait(0.3);
ritvaldirandi 0:c6c878661d4e 159
ritvaldirandi 0:c6c878661d4e 160 }
ritvaldirandi 0:c6c878661d4e 161 }