Diff: HeptaSerial.cpp

Revision:

6:c11f48bbb567

Parent:

5:50a03fcd9cc0

--- a/HeptaSerial.cpp	Wed Jul 26 06:22:13 2017 +0000
+++ b/HeptaSerial.cpp	Sat Aug 05 13:36:04 2017 +0000
@@ -614,21 +614,11 @@
 {
     serial.setTimeout(1);
     HeptaSerial::ErrorNumber err = HeptaSerial::NoError;
-    serial._baud(14400);
-    err = sync();
-    //setmbedBaud(HeptaSerial::Baud115200);
-    //err = sync();
-    if (HeptaSerial::NoError == err) {
-        printf("[ OK ] : Camera::sync\r\n");
-    } else {
-        printf("[FAIL] : Camera::sync (Error=%02X)\r\n", (int)err);
-    }
-    /*
-     HeptaSerial::ErrorNumber err = HeptaSerial::NoError;
 
     err = sync();
     int count=0;
     while(err) {
+        printf("count = %d\r\n",count);
         switch(count) {
             case 0:
                 setmbedBaud(HeptaSerial::Baud14400);
@@ -636,20 +626,20 @@
             case 1:
                 setmbedBaud(HeptaSerial::Baud115200);
                 break;
-            case 2:
-                setmbedBaud(HeptaSerial::Baud57600);
-                break;
-            case 3:
-                setmbedBaud(HeptaSerial::Baud28800);
-                break;
             default:
                 count=0;
         }
         count++;
         err = sync();
         printf("init err1=%d\r\n", err);
-
-    }*/
+        if(!err) {
+            printf("to 115200\r\n");
+            init(HeptaSerial::Baud115200, HeptaSerial::JpegResolution320x240);
+            printf("init err2=%d\r\n", err);
+            setmbedBaud(HeptaSerial::Baud115200);
+            printf("init err3=%d\r\n", err);
+        }
+    }
 }
 
 void HeptaSerial::test_jpeg_snapshot_picture(int CAPTURE_FRAMES)
@@ -704,68 +694,72 @@
 }
 
 //*********************serial*********************//
+void HeptaSerial::gps_setting(void)
+{
+    serial._baud(9600);
+    serial.setTimeout(9999);
+}
+
+
 char HeptaSerial::getc()
 {
-    serial._baud(9600);
-    serial.setTimeout(999);
+    gps_setting();
     c = serial.getc();
     return c;
 }
 int HeptaSerial::readable()
 {
-    serial._baud(9600);
-    serial.setTimeout(999);
+    gps_setting();
     i = serial.readable();
     return i;
 }
 void HeptaSerial::flushSerialBuffer(void)
 {
-    serial._baud(9600);
-    serial.setTimeout(999);
+    gps_setting();
     ite = 0;
-    while (serial.readable())
-    { 
+    while (serial.readable()) {
         serial.getc();
         ite++;
-        if(ite==100){break;};
+        if(ite==100) {
+            break;
+        };
     }
     return;
 }
 void HeptaSerial::gga_sensing(float *time, float *latitude, char *ns, float *longitude, char *ew, int *quality, int *stnum, float *hacu, float *altitude, char *aunit, int *serial_check)
 {
-    serial._baud(9600);
-    serial.setTimeout(999);
+    gps_setting();
     int ite = 0;
-    while(serial.getc()!='$'){
+    while(serial.getc()!='$') {
         ite++;
         if(ite==10000) break;
     }
-    for(int i=0; i<5; i++){
+    for(int i=0; i<5; i++) {
         msg[i] = serial.getc();
     }
-    if((msg[2]=='G')&(msg[3]=='G')&(msg[4]=='A')){
-        for(int j=0; j<6; j++){
-            if(j==0){
-                for(int i=5; i<256; i++){
+    if((msg[2]=='G')&(msg[3]=='G')&(msg[4]=='A')) {
+        for(int j=0; j<6; j++) {
+            if(j==0) {
+                for(int i=5; i<256; i++) {
                     msg[i] = serial.getc();
                     if(msg[i] == '\r') {
                         msg[i] = 0;
                         break;
                     }
                 }
-            }else{
-                for(int i=0; i<256; i++){
+            } else {
+                for(int i=0; i<256; i++) {
                     msgd[i] = serial.getc();
                     if(msgd[i] == '\r') {
                         break;
                     }
                 }
-                if((msgd[4]=='V')&(msgd[5]=='T')&(msgd[6]=='G')){
+                if((msgd[4]=='V')&(msgd[5]=='T')&(msgd[6]=='G')) {
                     break;
                 }
             }
         }
-        if(sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", time, latitude, ns, longitude, ew, quality, stnum, hacu, altitude, aunit) >= 1) { 
+        if(sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", time, latitude, ns, longitude, ew, quality, stnum, hacu, altitude, aunit) >= 1) {
             if(!(quality)) {
                 //latitude(unit transformation)
                 *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
@@ -779,46 +773,44 @@
                 *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
                 *serial_check = 1;
             }
-        }
-        else{
+        } else {
             printf("No Data");
             *serial_check = 2;
-        }       
-    }
-    else{
+        }
+    } else {
         *serial_check = 3;
     }
 }
-void HeptaSerial::lat_log_sensing_u16(char *lat, char *log, int *dsize)
+void HeptaSerial::lat_log_sensing_u16(char *lat, char *log, char *height, int *dsize1, int *dsize2)
 {
-    serial._baud(9600);
-    serial.setTimeout(999);
-    char gph1[8]={0x00},gph2[8]={0x00},gph3[8]={0x00},gph4[8]={0x00},gpt1[8]={0x00},gpt2[8]={0x00},gpt3[8]={0x00},gpt4[8]={0x00};
+    gps_setting();
+    height_1 = 29.4;
+    char gph1[8]= {0x00},gph2[8]= {0x00},gph3[8]= {0x00},gph4[8]= {0x00},gpt1[8]= {0x00},gpt2[8]= {0x00},gpt3[8]= {0x00},gpt4[8]= {0x00};
+    char hei1[8]= {0x00},hei2[8]= {0x00};
     int i=0,j=0;
-    while (serial.readable()){ 
+    while (serial.readable()) {
         serial.getc();
     }
-    loop:
-    while(serial.getc()!='$'){}
-    for(j=0;j<5;j++){
+loop:
+    while(serial.getc()!='$') {}
+    for(j=0; j<5; j++) {
         gps_data[1][j]=serial.getc();
     }
-    if((gps_data[1][2]==0x52)&(gps_data[1][3]==0x4d)&(gps_data[1][4]==0x43)){
-        for(j=0;j<1;j++){
-            if(j==0){
+    if((gps_data[1][2]==0x52)&(gps_data[1][3]==0x4d)&(gps_data[1][4]==0x43)) {
+        for(j=0; j<1; j++) {
+            if(j==0) {
                 i=0;
-                while((gps_data[j+1][i+5] = serial.getc()) != '\r'){    
+                while((gps_data[j+1][i+5] = serial.getc()) != '\r') {
                     //pc.putc(gps_data[j+1][i+5]);
                     i++;
                 }
                 gps_data[j+1][i+5]='\0';
                 i=0;
                 //pc.printf("\n\r");
-            }
-            else{
-                while(serial.getc()!='$'){}
+            } else {
+                while(serial.getc()!='$') {}
                 i=0;
-                while((gps_data[j+1][i] = serial.getc()) != '\r'){    
+                while((gps_data[j+1][i] = serial.getc()) != '\r') {
                     //pc.putc(gps_data[j+1][i]);
                     i++;
                 }
@@ -827,12 +819,10 @@
                 //pc.printf("\n\r");
             }
         }
-    }
-    else
-    {
+    } else {
         goto loop;
     }
-    if( sscanf(gps_data[1],"GPRMC,%f,%c,%f,%c,%f,%c,%f",&time,&statas,&hokui,&ns,&tokei,&ew,&vel) >= 1){
+    if( sscanf(gps_data[1],"GPRMC,%f,%c,%f,%c,%f,%c,%f",&time,&statas,&hokui,&ns,&tokei,&ew,&vel) >= 1) {
         //hokui
         d_hokui=int(hokui/100);
         m_hokui=(hokui-d_hokui*100);
@@ -842,7 +832,7 @@
         sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
         sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
         sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
-    
+
         //tokei
         d_tokei=int(tokei/100);
         m_tokei=(tokei-d_tokei*100);
@@ -852,6 +842,13 @@
         sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
         sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
         sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
+
+        m_height = int(height_1);
+        cm_height = int((height_1-m_height)*100 );
+        //printf("%d\r\n",cm_height);
+        sprintf( hei1, "%02X", (char(m_height)) & 0xFF);
+        sprintf( hei2, "%02X", (char(cm_height)) & 0xFF);
+
         lat[0] = gph1[0];
         lat[1] = gph1[1];
         lat[2] = gph2[0];
@@ -867,7 +864,12 @@
         log[4] = gpt3[0];
         log[5] = gpt3[1];
         log[6] = gpt4[0];
-        log[7] = gpt4[1];               
+        log[7] = gpt4[1];
+        height[0] = hei1[0];
+        height[1] = hei1[1];
+        height[2] = hei2[0];
+        height[3] = hei2[1];
     }
-    *dsize = 8;
+    *dsize1 = 8;
+    *dsize2 = 4;
 }
\ No newline at end of file