HeptaCamera_GPS.cpp

00001 #include "HeptaCamera_GPS.h"
00002 #define WAITIDLE    waitIdle
00003 #define SENDFUNC    sendBytes
00004 #define RECVFUNC    recvBytes
00005 #define WAITFUNC    waitRecv
00006 int num=0;
00007 /**
00008  * Constructor.
00009  *
00010  * @param tx A pin for transmit.
00011  * @param rx A pin for receive.
00012  * @param baud Baud rate. (Default is Baud14400.)
00013  */
00014 HeptaCamera_GPS::HeptaCamera_GPS(PinName tx, PinName rx, PinName CAM, PinName GPS) : serial(tx, rx), CAM_SW(CAM), GPS_SW(GPS)
00015 {
00016 }
00017 
00018 /**
00019  * Destructor.
00020  */
00021 HeptaCamera_GPS::~HeptaCamera_GPS()
00022 {
00023 }
00024 
00025 /**
00026  * Make a sync. for baud rate.
00027  */
00028 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sync()
00029 {
00030     for (int i = 0; i < SYNCMAX; i++) {
00031         if (NoError == sendSync()) {
00032             if (NoError == recvAckOrNck()) {
00033                 if (NoError == recvSync()) {
00034                     if (NoError == sendAck(0x0D, 0x00)) {
00035                         /*
00036                          * After synchronization, the camera needs a little time for AEC and AGC to be stable.
00037                          * Users should wait for 1-2 seconds before capturing the first picture.
00038                          */
00039                         wait(2.0);
00040                         return NoError;
00041                     }
00042                 }
00043             }
00044         }
00045         wait_ms(50);
00046     }
00047     return UnexpectedReply;
00048 }
00049 
00050 /**
00051  * Initialize.
00052  *
00053  * @param baud Camera Interface Speed.
00054  * @param jr JPEG resolution.
00055  */
00056 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::init(Baud baud,JpegResolution jr)
00057 {
00058     int i ;
00059     ErrorNumber en;
00060     WAITIDLE();
00061     setmbedBaud((Baud)(0x07)) ;
00062 
00063     for ( i = 1 ; i < 7 ; i++ ) {
00064         if ( NoError == sendSync() ) {
00065             if ( NoError == recvAckOrNck() ) {
00066                 if ( NoError == recvSync() ) {
00067                     if ( NoError == sendAck(0x0D, 0x00) ) {
00068                         en = sendInitial(baud,jr);
00069                         if (NoError != en) {
00070                             return en;
00071                         }
00072                         en = recvAckOrNck();
00073                         if (NoError != en) {
00074                             return en;
00075                         }
00076                         wait_ms(50) ;
00077                         setmbedBaud(baud);
00078                         //wait_ms(50) ;
00079                         static bool alreadySetupPackageSize = false;
00080                         if (!alreadySetupPackageSize) {
00081                             en = sendSetPackageSize(packageSize);
00082                             if (NoError != en) {
00083                                 return en;
00084                             }
00085                             WAITFUNC();
00086                             en = recvAckOrNck();
00087                             if (NoError != en) {
00088                                 return en;
00089                             }
00090                             alreadySetupPackageSize = true;
00091                         }
00092 
00093                         wait(2.0);
00094                         return (ErrorNumber)NoError;
00095                         /*
00096                          * After synchronization, the camera needs a little time for AEC and AGC to be stable.
00097                          * Users should wait for 1-2 seconds before capturing the first picture.
00098                          */
00099                     }
00100                 }
00101             } else {
00102                 setmbedBaud((Baud)(i+1)) ;
00103             }
00104         }
00105         wait_ms(50);
00106     }
00107     return UnexpectedReply;
00108 }
00109 
00110 
00111 /**
00112  * Get JPEG snapshot picture.
00113  *
00114  * @param func A pointer to a callback function.
00115  *             You can block this function until saving the image datas.
00116  * @return Status of the error.
00117  */
00118 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::getJpegSnapshotPicture(FILE*fp)
00119 {
00120     WAITIDLE();
00121     ErrorNumber en;
00122 
00123 
00124     en = sendSnapshot();
00125     if (NoError != en) {
00126         return en;
00127     }
00128     WAITFUNC();
00129     en = recvAckOrNck();
00130     if (NoError != en) {
00131         return en;
00132     }
00133 
00134     en = sendGetPicture();
00135     if (NoError != en) {
00136         return en;
00137     }
00138     WAITFUNC();
00139     en = recvAckOrNck();
00140     if (NoError != en) {
00141         return en;
00142     }
00143 
00144     /*
00145      * Data : snapshot picture
00146      */
00147     uint32_t length = 0;
00148     WAITFUNC();
00149     en = recvData(&length);
00150     if (NoError != en) {
00151         return en;
00152     }
00153     en = sendAck(0x00, 0);
00154     if (NoError != en) {
00155         return en;
00156     }
00157 
00158     char databuf[packageSize - 6];
00159     uint16_t pkg_total = length / (packageSize - 6);
00160     for (int i = 0; i <= (int)pkg_total; i++) {
00161         uint16_t checksum = 0;
00162         // ID.
00163         char idbuf[2];
00164         WAITFUNC();
00165         if (!RECVFUNC(idbuf, sizeof(idbuf))) {
00166             return (ErrorNumber)UnexpectedReply;
00167         }
00168         checksum += idbuf[0];
00169         checksum += idbuf[1];
00170         uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0);
00171         if (id != i) {
00172             return (ErrorNumber)UnexpectedReply;
00173         }
00174 
00175         // Size of the data.
00176         char dsbuf[2];
00177         WAITFUNC();
00178         if (!RECVFUNC(dsbuf, sizeof(dsbuf))) {
00179             return (ErrorNumber)UnexpectedReply;
00180         }
00181 
00182         // Received the data.
00183         checksum += dsbuf[0];
00184         checksum += dsbuf[1];
00185         uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0);
00186         WAITFUNC();
00187         if (!RECVFUNC(&databuf[0], ds)) {
00188             return (ErrorNumber)UnexpectedReply;
00189         }
00190         for (int j = 0; j < ds; j++) {
00191             checksum += databuf[j];
00192         }
00193 
00194         // Verify code.
00195         char vcbuf[2];
00196         WAITFUNC();
00197         if (!RECVFUNC(vcbuf, sizeof(vcbuf))) {
00198             return (ErrorNumber)UnexpectedReply;
00199         }
00200         uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0);
00201         if (vc != (checksum & 0xff)) {
00202             return (ErrorNumber)UnexpectedReply;
00203         }
00204 
00205         /*
00206          * Call a call back function.
00207          * You can block this function while working.
00208          */
00209         size_t siz = ds;
00210         for (int ii = 0; ii < (int)siz; ii++) {
00211             fprintf(fp, "%c", databuf[ii]);
00212         }
00213         /*
00214          * We should wait for camera working before reply a ACK.
00215          */
00216         wait_ms(100);
00217         en = sendAck(0x00, 1 + i);
00218         if (NoError != en) {
00219             return en;
00220         }
00221     }
00222 
00223     return (ErrorNumber)NoError;
00224 }
00225 
00226 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::getJpegSnapshotPicture_data(FILE*fp)
00227 {
00228     WAITIDLE();
00229     ErrorNumber en;
00230 
00231 
00232     en = sendSnapshot();
00233     if (NoError != en) {
00234         return en;
00235     }
00236     WAITFUNC();
00237     en = recvAckOrNck();
00238     if (NoError != en) {
00239         return en;
00240     }
00241 
00242     en = sendGetPicture();
00243     if (NoError != en) {
00244         return en;
00245     }
00246     WAITFUNC();
00247     en = recvAckOrNck();
00248     if (NoError != en) {
00249         return en;
00250     }
00251 
00252     /*
00253      * Data : snapshot picture
00254      */
00255     uint32_t length = 0;
00256     WAITFUNC();
00257     en = recvData(&length);
00258     if (NoError != en) {
00259         return en;
00260     }
00261     en = sendAck(0x00, 0);
00262     if (NoError != en) {
00263         return en;
00264     }
00265 
00266     char databuf[packageSize - 6];
00267     uint16_t pkg_total = length / (packageSize - 6);
00268     for (int i = 0; i <= (int)pkg_total; i++) {
00269         uint16_t checksum = 0;
00270         // ID.
00271         char idbuf[2];
00272         WAITFUNC();
00273         if (!RECVFUNC(idbuf, sizeof(idbuf))) {
00274             return (ErrorNumber)UnexpectedReply;
00275         }
00276         checksum += idbuf[0];
00277         checksum += idbuf[1];
00278         uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0);
00279         if (id != i) {
00280             return (ErrorNumber)UnexpectedReply;
00281         }
00282 
00283         // Size of the data.
00284         char dsbuf[2];
00285         WAITFUNC();
00286         if (!RECVFUNC(dsbuf, sizeof(dsbuf))) {
00287             return (ErrorNumber)UnexpectedReply;
00288         }
00289 
00290         // Received the data.
00291         checksum += dsbuf[0];
00292         checksum += dsbuf[1];
00293         uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0);
00294         WAITFUNC();
00295         if (!RECVFUNC(&databuf[0], ds)) {
00296             return (ErrorNumber)UnexpectedReply;
00297         }
00298         for (int j = 0; j < ds; j++) {
00299             checksum += databuf[j];
00300         }
00301 
00302         // Verify code.
00303         char vcbuf[2];
00304         WAITFUNC();
00305         if (!RECVFUNC(vcbuf, sizeof(vcbuf))) {
00306             return (ErrorNumber)UnexpectedReply;
00307         }
00308         uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0);
00309         if (vc != (checksum & 0xff)) {
00310             return (ErrorNumber)UnexpectedReply;
00311         }
00312 
00313         /*
00314          * Call a call back function.
00315          * You can block this function while working.
00316          */
00317         size_t siz = ds;
00318         for (int ii = 0; ii < (int)siz; ii++) {
00319             fprintf(fp, "%02X ", databuf[ii]);
00320             if(++num%16==0) fprintf(fp,"\r\n");
00321         }
00322         /*
00323          * We should wait for camera working before reply a ACK.
00324          */
00325         wait_ms(100);
00326         en = sendAck(0x00, 1 + i);
00327         if (NoError != en) {
00328             return en;
00329         }
00330     }
00331 
00332     return (ErrorNumber)NoError;
00333 }
00334 
00335 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendInitial(Baud baud, JpegResolution jr)
00336 {
00337     char send[COMMAND_LENGTH];
00338 
00339     send[0] = 0xAA;
00340     send[1] = 0x01;
00341     send[2] = (char)baud;
00342     send[3] = 0x07;
00343     send[4] = 0x00;
00344     send[5] = (char)jr;
00345 
00346     if (!SENDFUNC(send, sizeof(send))) {
00347         return (ErrorNumber)SendRegisterTimeout;
00348     }
00349 
00350     return (ErrorNumber)NoError;
00351 }
00352 
00353 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendGetPicture(void)
00354 {
00355     char send[COMMAND_LENGTH];
00356 
00357     send[0] = 0xAA;
00358     send[1] = 0x04;
00359     send[2] = 0x01;
00360     send[3] = 0x00;
00361     send[4] = 0x00;
00362     send[5] = 0x00;
00363 
00364     if (!SENDFUNC(send, sizeof(send))) {
00365         return (ErrorNumber)SendRegisterTimeout;
00366     }
00367     return (ErrorNumber)NoError;
00368 }
00369 
00370 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendSnapshot(void)
00371 {
00372     char send[COMMAND_LENGTH];
00373     send[0] = 0xAA;
00374     send[1] = 0x05;
00375     send[2] = 0x00;
00376     send[3] = 0x00;
00377     send[4] = 0x00;
00378     send[5] = 0x00;
00379 
00380     if (!SENDFUNC(send, sizeof(send))) {
00381         return (ErrorNumber)SendRegisterTimeout;
00382     }
00383     return (ErrorNumber)NoError;
00384 }
00385 
00386 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendSetPackageSize(uint16_t packageSize)
00387 {
00388     char send[COMMAND_LENGTH];
00389     send[0] = 0xAA;
00390     send[1] = 0x06;
00391     send[2] = 0x08;
00392     send[3] = (packageSize >> 0) & 0xff;
00393     send[4] = (packageSize >> 8) & 0xff;
00394     send[5] = 0x00;
00395 
00396     if (!SENDFUNC(send, sizeof(send))) {
00397         return (ErrorNumber)SendRegisterTimeout;
00398     }
00399     return (ErrorNumber)NoError;
00400 }
00401 
00402 void HeptaCamera_GPS::setmbedBaud(Baud baud)
00403 {
00404     switch((int)baud) {
00405         case 0x02:
00406             serial._baud(460800);
00407             break;
00408         case 0x03:
00409             serial._baud(230400);
00410             break;
00411         case 0x04:
00412             serial._baud(115200);
00413             break;
00414         case 0x05:
00415             serial._baud(57600);
00416             break;
00417         case 0x06:
00418             serial._baud((int)28800);
00419             break;
00420         case 0x07:
00421             serial._baud(14400);
00422             break;
00423         default:
00424             serial._baud(14400);
00425     }
00426 }
00427 
00428 
00429 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendReset(ResetType specialReset)
00430 {
00431     char send[COMMAND_LENGTH];
00432     send[0] = 0xAA;
00433     send[1] = 0x08;
00434     send[2] = 0x00;
00435     send[3] = 0x00;
00436     send[4] = 0x00;
00437     send[5] = specialReset;
00438     /*
00439      * Special reset : If the parameter is 0xFF, the command is a special Reset command and the firmware responds to it immediately.
00440      */
00441 
00442     if (!SENDFUNC(send, sizeof(send))) {
00443         return (ErrorNumber)SendRegisterTimeout;
00444     }
00445 
00446     return (ErrorNumber)NoError;
00447 }
00448 
00449 
00450 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::recvData(uint32_t *length)
00451 {
00452     char recv[COMMAND_LENGTH];
00453     if (!RECVFUNC(recv, sizeof(recv))) {
00454         return (ErrorNumber)UnexpectedReply;
00455     }
00456     if ((0xAA != recv[0]) || (0x0A != recv[1])) {
00457         return (ErrorNumber)UnexpectedReply;
00458     }
00459     recv[2] = (char)0x01;
00460     *length = (recv[5] << 16) | (recv[4] << 8) | (recv[3] << 0);
00461     return (ErrorNumber)NoError;
00462 }
00463 
00464 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendSync()
00465 {
00466     char send[COMMAND_LENGTH];
00467     send[0] = 0xAA;
00468     send[1] = 0x0D;
00469     send[2] = 0x00;
00470     send[3] = 0x00;
00471     send[4] = 0x00;
00472     send[5] = 0x00;
00473 
00474 
00475     if (!SENDFUNC(send, sizeof(send))) {
00476         return (ErrorNumber)SendRegisterTimeout;
00477     }
00478     return (ErrorNumber)NoError;
00479 }
00480 
00481 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::recvSync()
00482 {
00483     char recv[COMMAND_LENGTH];
00484 
00485     if (!RECVFUNC(recv, sizeof(recv))) {
00486         return (ErrorNumber)UnexpectedReply;
00487     }
00488     if ((0xAA != recv[0]) || (0x0D != recv[1])) {
00489         return (ErrorNumber)UnexpectedReply;
00490     }
00491     return (ErrorNumber)NoError;
00492 }
00493 
00494 /**
00495  * Send ACK.
00496  *
00497  * @param commandId The command with that ID is acknowledged by this command.
00498  * @param packageId For acknowledging Data command, these two bytes represent the requested package ID. While for acknowledging other commands, these two bytes are set to 00h.
00499  */
00500 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendAck(uint8_t commandId, uint16_t packageId)
00501 {
00502     char send[COMMAND_LENGTH];
00503 
00504     send[0] = 0xAA;
00505     send[1] = 0x0E;
00506     send[2] = commandId;
00507     send[3] = 0x00;    // ACK counter is not used.
00508     send[4] = (packageId >> 0) & 0xff;
00509     send[5] = (packageId >> 8) & 0xff;
00510     if (!SENDFUNC(send, sizeof(send))) {
00511         return (ErrorNumber)SendRegisterTimeout;
00512     }
00513     return (ErrorNumber)NoError;
00514 }
00515 
00516 /**
00517  * Receive ACK or NCK.
00518  *
00519  * @return Error number.
00520  */
00521 HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::recvAckOrNck()
00522 {
00523     char recv[COMMAND_LENGTH];
00524 
00525     if (!RECVFUNC(recv, sizeof(recv))) {
00526         return (ErrorNumber)UnexpectedReply;
00527     }
00528     if ((0xAA == recv[0]) && (0x0E == recv[1])) {
00529         return (ErrorNumber)NoError;
00530     }
00531     if ((0xAA == recv[0]) && (0x0F == recv[1]) && (0x00 == recv[2])) {
00532         return (ErrorNumber)NoError;
00533     }
00534     if ((0xAA == recv[0]) && (0x0F == recv[1])) {
00535         return (ErrorNumber)recv[4];
00536     }
00537     return (ErrorNumber)UnexpectedReply;
00538 }
00539 
00540 /**
00541  * Send bytes to camera module.
00542  *
00543  * @param buf Pointer to the data buffer.
00544  * @param len Length of the data buffer.
00545  *
00546  * @return True if the data sended.
00547  */
00548 bool HeptaCamera_GPS::sendBytes(char *buf, size_t len, int timeout_us)
00549 {
00550     for (uint32_t i = 0; i < (uint32_t)len; i++) {
00551         int cnt = 0;
00552         while (!serial.writeable()) {
00553             wait_us(1);
00554             cnt++;
00555             if (timeout_us < cnt) {
00556                 return false;
00557             }
00558         }
00559         serial.putc(buf[i]);
00560     }
00561     return true;
00562 }
00563 
00564 
00565 bool HeptaCamera_GPS::recvBytes(char *buf, size_t len, int timeout_us)
00566 {
00567     for (uint32_t i = 0; i < (uint32_t)len; i++) {
00568         int cnt = 0;
00569         while (!serial.readable()) {
00570             wait_us(1);
00571             cnt++;
00572             if (timeout_us < cnt) {
00573                 return false;
00574             }
00575         }
00576         buf[i] = serial.getc();
00577     }
00578     return true;
00579 }
00580 
00581 
00582 bool HeptaCamera_GPS::waitRecv()
00583 {
00584     while (!serial.readable()) {
00585     }
00586     return true;
00587 }
00588 
00589 bool HeptaCamera_GPS::waitIdle()
00590 {
00591     while (serial.readable()) {
00592         serial.getc();
00593     }
00594     return true;
00595 }
00596 
00597 void HeptaCamera_GPS::camera_setting(void)
00598 {
00599     GPS_SW = 0;
00600     CAM_SW = 1;
00601     serial.setTimeout(1);
00602     flushSerialBuffer();
00603 }
00604 
00605 void HeptaCamera_GPS::Sync(void)
00606 {
00607     camera_setting();
00608     HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
00609     printf("synchro setting now\r\n");
00610     err = sync();
00611     int count = 0;
00612     int fflag = 0;
00613     while(err) {
00614         switch(count) {
00615             case 0:
00616                 printf("Connection of camera and mbed at baudrate 14400\r\n");
00617                 setmbedBaud(HeptaCamera_GPS::Baud14400);
00618                 count++;
00619                 break;
00620             case 1:
00621                 printf("Connection of camera and mbed at baudrate 115200\r\n");
00622                 setmbedBaud(HeptaCamera_GPS::Baud115200);
00623                 count++;
00624                 break;
00625             default:
00626                 count=0;
00627                 fflag = 1;
00628         }
00629         //count++;
00630         err = sync();
00631         printf("synchro setting now\r\n");
00632         if(!err) {
00633             printf("synchro setting finish\r\n");
00634         }
00635         if(fflag==1) {
00636             printf("Synchronization failed\r\n");
00637             fflag = 0;
00638             break;
00639         }
00640     }//while
00641 }
00642 
00643 void HeptaCamera_GPS::test_jpeg_snapshot_picture(const char *filename)
00644 {
00645     FILE*fp_jpeg;
00646     HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
00647     fp_jpeg = fopen(filename, "w");
00648     if(fp_jpeg == NULL) {
00649         printf("Could not open file for write\r\n");
00650     } else {
00651         err = getJpegSnapshotPicture(fp_jpeg);
00652         if (HeptaCamera_GPS::NoError == err) {
00653             printf("[ OK ]:Picture taken\r\n");
00654         } else {
00655             printf("[FAIL]:Picture taken(Error=%02X)\r\n", (int)err);
00656         }
00657         fclose(fp_jpeg);
00658     }
00659 }
00660 
00661 void HeptaCamera_GPS::test_jpeg_snapshot_data(const char *filename)
00662 {
00663     FILE*fp_jpeg;
00664     HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
00665     fp_jpeg = fopen(filename, "w");
00666     if(fp_jpeg == NULL) {
00667         printf("Could not open file for write\r\n");
00668     } else {
00669         err = getJpegSnapshotPicture_data(fp_jpeg);
00670 
00671         if (HeptaCamera_GPS::NoError == err) {
00672             printf("[ OK ]:Picture taken\r\n");
00673         } else {
00674             printf("[FAIL]:Picture taken(Error=%02X)\r\n", (int)err);
00675         }
00676         fclose(fp_jpeg);
00677     }
00678 }
00679 
00680 void HeptaCamera_GPS::initialize(Baud baud,JpegResolution jr)
00681 {
00682     HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;//ErrorNumber define
00683     err = init(baud, jr);//
00684     if (HeptaCamera_GPS::NoError == err) {
00685         printf("[ OK ]:Camera setting\r\n");
00686         setmbedBaud(baud);
00687     } else {
00688         printf("[FAIL]:Camera setting(Error=%02X)\r\n", (int)err);
00689     }
00690 
00691 }
00692 
00693 //*********************serial*********************//
00694 void HeptaCamera_GPS::gps_setting(void)
00695 {
00696     CAM_SW = 0;
00697     GPS_SW = 1;
00698     flushSerialBuffer();
00699     serial._baud(9600);
00700     serial.setTimeout(9999);
00701 }
00702 
00703 char HeptaCamera_GPS::getc()
00704 {
00705     c = serial.getc();
00706     return c;
00707 }
00708 int HeptaCamera_GPS::readable()
00709 {
00710     i = serial.readable();
00711     return i;
00712 }
00713 void HeptaCamera_GPS::flushSerialBuffer(void)
00714 {
00715     ite = 0;
00716     while (serial.readable()) {
00717         serial.getc();
00718         ite++;
00719         if(ite==100) {
00720             break;
00721         };
00722     }
00723     return;
00724 }
00725 void HeptaCamera_GPS::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)
00726 {
00727     int ite = 0;
00728     while(serial.getc()!='$') {
00729         ite++;
00730         if(ite==10000) break;
00731     }
00732     for(int i=0; i<5; i++) {
00733         msg[i] = serial.getc();
00734     }
00735     if((msg[2]=='G')&(msg[3]=='G')&(msg[4]=='A')) {
00736         for(int j=0; j<6; j++) {
00737             if(j==0) {
00738                 for(int i=5; i<256; i++) {
00739                     msg[i] = serial.getc();
00740                     if(msg[i] == '\r') {
00741                         msg[i] = 0;
00742                         break;
00743                     }
00744                 }
00745             } else {
00746                 for(int i=0; i<256; i++) {
00747                     msgd[i] = serial.getc();
00748                     if(msgd[i] == '\r') {
00749                         break;
00750                     }
00751                 }
00752                 if((msgd[4]=='V')&(msgd[5]=='T')&(msgd[6]=='G')) {
00753                     break;
00754                 }
00755             }
00756         }
00757         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) {
00758             if(!(quality)) {
00759                 //latitude(unit transformation)
00760                 *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
00761                 //longitude(unit transformation)
00762                 *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
00763                 *serial_check = 0;
00764             } else {
00765                 //latitude(unit transformation)
00766                 *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
00767                 //longitude(unit transformation)
00768                 *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
00769                 *serial_check = 1;
00770             }
00771             if(!strcmp(ns,"S")){
00772                 *latitude = -*latitude;
00773             }
00774             if(!strcmp(ns,"W")){
00775                 *longitude = -*longitude;
00776             }
00777         } else {
00778             printf("No Data");
00779             *serial_check = 2;
00780         }
00781     } else {
00782         *serial_check = 3;
00783     }
00784 }
00785 
00786 void HeptaCamera_GPS::lat_log_sensing_u16(char *lat, char *log, char *height)
00787 {
00788     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};
00789     char hei1[8]= {0x00},hei2[8]= {0x00};
00790     int i=0,j=0;
00791     while (serial.readable()) {
00792         serial.getc();
00793     }
00794 loop:
00795     while(serial.getc()!='$') {}
00796     for(j=0; j<5; j++) {
00797         gps_data[1][j]=serial.getc();
00798     }
00799     if((gps_data[1][2]==0x47)&(gps_data[1][3]==0x47)&(gps_data[1][4]==0x41)) {
00800         for(j=0; j<1; j++) {
00801             if(j==0) {
00802                 i=0;
00803                 while((gps_data[j+1][i+5] = serial.getc()) != '\r') {
00804                     //pc.putc(gps_data[j+1][i+5]);
00805                     i++;
00806                 }
00807                 gps_data[j+1][i+5]='\0';
00808                 i=0;
00809                 //pc.printf("\n\r");
00810             } else {
00811                 while(serial.getc()!='$') {}
00812                 i=0;
00813                 while((gps_data[j+1][i] = serial.getc()) != '\r') {
00814                     //pc.putc(gps_data[j+1][i]);
00815                     i++;
00816                 }
00817                 gps_data[j+1][i]='\0';
00818                 i=0;
00819                 //pc.printf("\n\r");
00820             }
00821         }
00822     } else {
00823         goto loop;
00824     }
00825     if( sscanf(gps_data[1],"GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", &time, &hokui, &ns, &tokei, &ew, &_quality, &_stnum, &_hacu, &_altitude, &_aunit) >= 1) {
00826         //hokui
00827         d_hokui=int(hokui/100);
00828         m_hokui=(hokui-d_hokui*100);
00829         //m_hokui=(hokui-d_hokui*100)/60;
00830         g_hokui=d_hokui+(hokui-d_hokui*100)/60;
00831         //printf("%f\r\n",hokui);
00832         sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
00833         sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
00834         sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
00835         sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
00836 
00837         //tokei
00838         d_tokei=int(tokei/100);
00839         m_tokei=(tokei-d_tokei*100);
00840         //m_tokei=(tokei-d_tokei*100)/60;
00841         g_tokei=d_tokei+(tokei-d_tokei*100)/60;
00842         //printf("%f\r\n",tokei);
00843         sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
00844         sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
00845         sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
00846         sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
00847 
00848         m_height = int(_altitude);
00849         cm_height = int((_altitude-m_height)*100 );
00850         // printf("%f\r\n",_altitude);
00851         sprintf( hei1, "%02X", (char(m_height)) & 0xFF);
00852         sprintf( hei2, "%02X", (char(cm_height)) & 0xFF);
00853 
00854         lat[0] = gph1[0];
00855         lat[1] = gph1[1];
00856         lat[2] = gph2[0];
00857         lat[3] = gph2[1];
00858         lat[4] = gph3[0];
00859         lat[5] = gph3[1];
00860         lat[6] = gph4[0];
00861         lat[7] = gph4[1];
00862         log[0] = gpt1[0];
00863         log[1] = gpt1[1];
00864         log[2] = gpt2[0];
00865         log[3] = gpt2[1];
00866         log[4] = gpt3[0];
00867         log[5] = gpt3[1];
00868         log[6] = gpt4[0];
00869         log[7] = gpt4[1];
00870         height[0] = hei1[0];
00871         height[1] = hei1[1];
00872         height[2] = hei2[0];
00873         height[3] = hei2[1];
00874     }
00875     //*dsize1 = 8;
00876     //*dsize2 = 4;
00877 }