Diff: main.cpp

Revision:

0:c6c878661d4e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 04 16:01:08 2014 +0000
@@ -0,0 +1,161 @@
+#include "mbed.h"
+#include "gps_stg_venus.h"
+#include "string.h"
+
+Serial pc(USBTX,USBRX);
+Serial gps(p13,p14);
+GPSVenus venus(gps);
+DigitalOut led1(LED1);
+char data_gps[256],ns,ew,sampah[256];
+int g=0, cek_gps,lock,satelit;
+float waktu,latx,longx, laty, longy,dilution, altitude;
+double lat_target,long_target;
+double degrees, minutes, seconds;
+int lattitude[3], longitude[3];
+
+void initialize_gps()
+{
+    venus.setBaud_115200();
+    wait(0.5); 
+    gps.baud(115200);
+    venus.setUpdateRate(20);
+    wait(0.5);
+    venus.setNmeaMessages(true, false, false, false, false, false);
+}
+
+void gps_interrupt()
+{
+//     char temp = gps.getc();
+//     if(g==0)
+//        {   led1=0;
+//            data_gps[g]=temp;
+//            if(data_gps[g]=='$')
+//            {g=1;data_gps[0]='$';}
+//        }
+//    else if (g==42){g=0;}
+//    else
+//        {
+//            data_gps[g]=temp;g++;led1=1;
+//            if(data_gps[g-1]=='\r')g=0;
+//        }
+    char temp=gps.getc();
+    if (temp=='$') {g=0;}
+    data_gps[g]=temp;
+    g++;
+    
+    
+}
+
+float trunc(float v) // pembulatan nilai
+{
+    if(v < 0.0) {
+        v*= -1.0;
+        v = floor(v);
+        v*=-1.0;
+    } else {
+        v = floor(v);
+    }
+    return v;
+}
+
+void get_gps()
+{       
+        //if(data_gps[0]=='G'&&data_gps[1]=='P'&&data_gps[2]=='G'&&data_gps[3]=='G'&&data_gps[4]=='A'&&data_gps[5]==','){
+        //$GPGGA,050749.299,0745.9647,S,11022.3071,E,0,00,,189.5,M,4.3,M,,0000*5E
+        sscanf(data_gps,"$GPGGA,%f,%f,%c,%f,%c,%d,%2d,%f,%f",&waktu, &latx,&ns, &longx, &ew, &lock, &satelit, &dilution, &altitude);
+           
+                degrees = (latx / 100.0f);
+                lattitude[0]=degrees;
+                degrees= degrees-lattitude[0];
+                minutes=(degrees*1000);
+                lattitude[1]=minutes;
+                minutes=minutes-lattitude[1];
+                seconds =(minutes*1000);
+                lattitude[2]= seconds;
+                
+                degrees = (longx / 100.0f);
+                longitude[0]=degrees;
+                degrees= degrees-longitude[0];
+                minutes=(degrees*1000);
+                longitude[1]=minutes;
+                minutes=minutes-longitude[1];
+                seconds =(minutes*1000);
+                longitude[2]= seconds;
+                       
+}
+
+void gps_ats()
+{
+    //fl waktu;
+    if(cek_gps==1){
+                sscanf(data_gps, "$GPGGA,%f,%f,%c,%f,%c",&waktu, &laty, &ns, &longy, &ew) ;
+           
+                if(ns == 'S') {    laty  *= -1.0; }
+                if(ew == 'W') {    longy *= -1.0; }
+                degrees = trunc(laty / 100.0f);
+                minutes = laty - (degrees * 100.0f);
+                lat_target = degrees + minutes / 60.0f;    
+                degrees = trunc(longy / 100.0f);
+                minutes = longy- (degrees * 100.0f);
+                long_target = degrees + minutes / 60.0f;  
+            }
+}
+
+void bin_dec_conv(unsigned int data)// anything to binary
+{
+    pc.printf("%d%d%d",(data/100),(data%100/10),(data%10));
+}
+
+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)
+{
+
+        pc.putc(0x0D);
+        bin_dec_conv(106);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_1_x);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_2_x);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_2_y);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_3_x);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_3_y);
+        pc.putc(0x20);
+        
+        bin_dec_conv(data_3_z);
+
+}
+
+int main()
+{
+    pc.baud(57600);
+    gps.baud(115200);
+    gps.attach(&gps_interrupt);
+    initialize_gps();
+    pc.printf("start!");
+    wait(3);
+    int i;
+    while(1)
+    {
+        for(i=0;i<=65;i++)
+        {
+            pc.putc(data_gps[i]);
+        }
+        pc.putc('\t');
+        get_gps();
+        //telemetry_gps(lattitude[0],lattitude[1],lattitude[2],longitude[0],longitude[1],longitude[2]);
+        //pc.putc('\t');
+        //pc.printf("cek=%d ",cek_gps);
+        pc.printf("waktu = %f\t",waktu);
+        pc.printf("tinggi = %f\t",altitude);
+        pc.printf("lock = %d\r",lock);
+        wait(0.3);
+        
+    }
+}
\ No newline at end of file