Revision 8:6d9c33df4c09, committed 2017-08-23

Comitter:

umeume

Date:

Wed Aug 23 06:18:42 2017 +0000

Parent:

7:a41100627f55

Child:

9:591580af98be

Commit message:

test

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HeptaCamera_GPS.cpp	Wed Aug 23 06:18:42 2017 +0000
@@ -0,0 +1,1082 @@
+#include "HeptaCamera_GPS.h"
+static FILE *fp_jpeg;
+#define WAITIDLE    waitIdle
+#define SENDFUNC    sendBytes
+#define RECVFUNC    recvBytes
+#define WAITFUNC    waitRecv
+int num=0;
+/**
+ * Constructor.
+ *
+ * @param tx A pin for transmit.
+ * @param rx A pin for receive.
+ * @param baud Baud rate. (Default is Baud14400.)
+ */
+HeptaCamera_GPS::HeptaCamera_GPS(PinName tx, PinName rx, PinName CAM, PinName GPS) : serial(tx, rx), CAM_SW(CAM), GPS_SW(GPS)
+{
+    //serial.baud(baud);
+}
+
+/**
+ * Destructor.
+ */
+HeptaCamera_GPS::~HeptaCamera_GPS()
+{
+}
+
+/**
+ * Make a sync. for baud rate.
+ */
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sync()
+{
+    for (int i = 0; i < SYNCMAX; i++) {
+        if (NoError == sendSync()) {
+            if (NoError == recvAckOrNck()) {
+                if (NoError == recvSync()) {
+                    if (NoError == sendAck(0x0D, 0x00)) {
+                        /*
+                         * After synchronization, the camera needs a little time for AEC and AGC to be stable.
+                         * Users should wait for 1-2 seconds before capturing the first picture.
+                         */
+                        wait(2.0);
+                        return NoError;
+                    }
+                }
+            }
+        }
+        wait_ms(50);
+    }
+    return UnexpectedReply;
+}
+
+/**
+ * Initialize.
+ *
+ * @param baud Camera Interface Speed.
+ * @param jr JPEG resolution.
+ */
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::init(Baud baud,JpegResolution jr)
+{
+    int i ;
+    ErrorNumber en;
+    WAITIDLE();
+    setmbedBaud((Baud)(0x07)) ;
+
+    for ( i = 1 ; i < 7 ; i++ ) {
+        if ( NoError == sendSync() ) {
+            if ( NoError == recvAckOrNck() ) {
+                if ( NoError == recvSync() ) {
+                    if ( NoError == sendAck(0x0D, 0x00) ) {
+                        en = sendInitial(baud,jr);
+                        if (NoError != en) {
+                            return en;
+                        }
+                        en = recvAckOrNck();
+                        if (NoError != en) {
+                            return en;
+                        }
+                        wait_ms(50) ;
+                        setmbedBaud(baud);
+                        //wait_ms(50) ;
+                        static bool alreadySetupPackageSize = false;
+                        if (!alreadySetupPackageSize) {
+                            en = sendSetPackageSize(packageSize);
+                            if (NoError != en) {
+                                return en;
+                            }
+                            WAITFUNC();
+                            en = recvAckOrNck();
+                            if (NoError != en) {
+                                return en;
+                            }
+                            alreadySetupPackageSize = true;
+                        }
+
+                        wait(2.0);
+                        return (ErrorNumber)NoError;
+                        /*
+                         * After synchronization, the camera needs a little time for AEC and AGC to be stable.
+                         * Users should wait for 1-2 seconds before capturing the first picture.
+                         */
+                    }
+                }
+            } else {
+                setmbedBaud((Baud)(i+1)) ;
+            }
+        }
+        wait_ms(50);
+    }
+    return UnexpectedReply;
+}
+
+
+/**
+ * Get JPEG snapshot picture.
+ *
+ * @param func A pointer to a callback function.
+ *             You can block this function until saving the image datas.
+ * @return Status of the error.
+ */
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::getJpegSnapshotPicture()
+{
+    WAITIDLE();
+    ErrorNumber en;
+
+
+    en = sendSnapshot();
+    if (NoError != en) {
+        return en;
+    }
+    WAITFUNC();
+    en = recvAckOrNck();
+    if (NoError != en) {
+        return en;
+    }
+
+    en = sendGetPicture();
+    if (NoError != en) {
+        return en;
+    }
+    WAITFUNC();
+    en = recvAckOrNck();
+    if (NoError != en) {
+        return en;
+    }
+
+    /*
+     * Data : snapshot picture
+     */
+    uint32_t length = 0;
+    WAITFUNC();
+    en = recvData(&length);
+    if (NoError != en) {
+        return en;
+    }
+    en = sendAck(0x00, 0);
+    if (NoError != en) {
+        return en;
+    }
+
+    char databuf[packageSize - 6];
+    uint16_t pkg_total = length / (packageSize - 6);
+    for (int i = 0; i <= (int)pkg_total; i++) {
+        uint16_t checksum = 0;
+        // ID.
+        char idbuf[2];
+        WAITFUNC();
+        if (!RECVFUNC(idbuf, sizeof(idbuf))) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+        checksum += idbuf[0];
+        checksum += idbuf[1];
+        uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0);
+        if (id != i) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+
+        // Size of the data.
+        char dsbuf[2];
+        WAITFUNC();
+        if (!RECVFUNC(dsbuf, sizeof(dsbuf))) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+
+        // Received the data.
+        checksum += dsbuf[0];
+        checksum += dsbuf[1];
+        uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0);
+        WAITFUNC();
+        if (!RECVFUNC(&databuf[0], ds)) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+        for (int j = 0; j < ds; j++) {
+            checksum += databuf[j];
+        }
+
+        // Verify code.
+        char vcbuf[2];
+        WAITFUNC();
+        if (!RECVFUNC(vcbuf, sizeof(vcbuf))) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+        uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0);
+        if (vc != (checksum & 0xff)) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+
+        /*
+         * Call a call back function.
+         * You can block this function while working.
+         */
+        size_t siz = ds;
+        for (int ii = 0; ii < (int)siz; ii++) {
+            fprintf(fp_jpeg, "%c", databuf[ii]);
+        }
+        /*
+         * We should wait for camera working before reply a ACK.
+         */
+        wait_ms(100);
+        en = sendAck(0x00, 1 + i);
+        if (NoError != en) {
+            return en;
+        }
+    }
+
+    return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::getJpegSnapshotPicture_data()
+{
+    WAITIDLE();
+    ErrorNumber en;
+
+
+    en = sendSnapshot();
+    if (NoError != en) {
+        return en;
+    }
+    WAITFUNC();
+    en = recvAckOrNck();
+    if (NoError != en) {
+        return en;
+    }
+
+    en = sendGetPicture();
+    if (NoError != en) {
+        return en;
+    }
+    WAITFUNC();
+    en = recvAckOrNck();
+    if (NoError != en) {
+        return en;
+    }
+
+    /*
+     * Data : snapshot picture
+     */
+    uint32_t length = 0;
+    WAITFUNC();
+    en = recvData(&length);
+    if (NoError != en) {
+        return en;
+    }
+    en = sendAck(0x00, 0);
+    if (NoError != en) {
+        return en;
+    }
+
+    char databuf[packageSize - 6];
+    uint16_t pkg_total = length / (packageSize - 6);
+    for (int i = 0; i <= (int)pkg_total; i++) {
+        uint16_t checksum = 0;
+        // ID.
+        char idbuf[2];
+        WAITFUNC();
+        if (!RECVFUNC(idbuf, sizeof(idbuf))) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+        checksum += idbuf[0];
+        checksum += idbuf[1];
+        uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0);
+        if (id != i) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+
+        // Size of the data.
+        char dsbuf[2];
+        WAITFUNC();
+        if (!RECVFUNC(dsbuf, sizeof(dsbuf))) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+
+        // Received the data.
+        checksum += dsbuf[0];
+        checksum += dsbuf[1];
+        uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0);
+        WAITFUNC();
+        if (!RECVFUNC(&databuf[0], ds)) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+        for (int j = 0; j < ds; j++) {
+            checksum += databuf[j];
+        }
+
+        // Verify code.
+        char vcbuf[2];
+        WAITFUNC();
+        if (!RECVFUNC(vcbuf, sizeof(vcbuf))) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+        uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0);
+        if (vc != (checksum & 0xff)) {
+            return (ErrorNumber)UnexpectedReply;
+        }
+
+        /*
+         * Call a call back function.
+         * You can block this function while working.
+         */
+        size_t siz = ds;
+        for (int ii = 0; ii < (int)siz; ii++) {
+            fprintf(fp_jpeg, "%02X ", databuf[ii]);
+            if(++num%16==0) fprintf(fp_jpeg,"\r\n");
+        }
+        /*
+         * We should wait for camera working before reply a ACK.
+         */
+        wait_ms(100);
+        en = sendAck(0x00, 1 + i);
+        if (NoError != en) {
+            return en;
+        }
+    }
+
+    return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendInitial(Baud baud, JpegResolution jr)
+{
+    char send[COMMAND_LENGTH];
+
+    send[0] = 0xAA;
+    send[1] = 0x01;
+    send[2] = (char)baud;
+    send[3] = 0x07;
+    send[4] = 0x00;
+    send[5] = (char)jr;
+
+    if (!SENDFUNC(send, sizeof(send))) {
+        return (ErrorNumber)SendRegisterTimeout;
+    }
+
+    return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendGetPicture(void)
+{
+    char send[COMMAND_LENGTH];
+
+    send[0] = 0xAA;
+    send[1] = 0x04;
+    send[2] = 0x01;
+    send[3] = 0x00;
+    send[4] = 0x00;
+    send[5] = 0x00;
+
+    if (!SENDFUNC(send, sizeof(send))) {
+        return (ErrorNumber)SendRegisterTimeout;
+    }
+    return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendSnapshot(void)
+{
+    char send[COMMAND_LENGTH];
+    send[0] = 0xAA;
+    send[1] = 0x05;
+    send[2] = 0x00;
+    send[3] = 0x00;
+    send[4] = 0x00;
+    send[5] = 0x00;
+
+    if (!SENDFUNC(send, sizeof(send))) {
+        return (ErrorNumber)SendRegisterTimeout;
+    }
+    return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendSetPackageSize(uint16_t packageSize)
+{
+    char send[COMMAND_LENGTH];
+    send[0] = 0xAA;
+    send[1] = 0x06;
+    send[2] = 0x08;
+    send[3] = (packageSize >> 0) & 0xff;
+    send[4] = (packageSize >> 8) & 0xff;
+    send[5] = 0x00;
+
+    if (!SENDFUNC(send, sizeof(send))) {
+        return (ErrorNumber)SendRegisterTimeout;
+    }
+    return (ErrorNumber)NoError;
+}
+
+void HeptaCamera_GPS::setmbedBaud(Baud baud)
+{
+    switch((int)baud) {
+        case 0x02:
+            serial._baud(460800);
+            break;
+        case 0x03:
+            serial._baud(230400);
+            break;
+        case 0x04:
+            serial._baud(115200);
+            break;
+        case 0x05:
+            serial._baud(57600);
+            break;
+        case 0x06:
+            serial._baud((int)28800);
+            break;
+        case 0x07:
+            serial._baud(14400);
+            break;
+        default:
+            serial._baud(14400);
+    }
+}
+
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendReset(ResetType specialReset)
+{
+    char send[COMMAND_LENGTH];
+    send[0] = 0xAA;
+    send[1] = 0x08;
+    send[2] = 0x00;
+    send[3] = 0x00;
+    send[4] = 0x00;
+    send[5] = specialReset;
+    /*
+     * Special reset : If the parameter is 0xFF, the command is a special Reset command and the firmware responds to it immediately.
+     */
+
+    if (!SENDFUNC(send, sizeof(send))) {
+        return (ErrorNumber)SendRegisterTimeout;
+    }
+
+    return (ErrorNumber)NoError;
+}
+
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::recvData(uint32_t *length)
+{
+    char recv[COMMAND_LENGTH];
+    if (!RECVFUNC(recv, sizeof(recv))) {
+        return (ErrorNumber)UnexpectedReply;
+    }
+    if ((0xAA != recv[0]) || (0x0A != recv[1])) {
+        return (ErrorNumber)UnexpectedReply;
+    }
+    recv[2] = (char)0x01;
+    *length = (recv[5] << 16) | (recv[4] << 8) | (recv[3] << 0);
+    return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendSync()
+{
+    char send[COMMAND_LENGTH];
+    send[0] = 0xAA;
+    send[1] = 0x0D;
+    send[2] = 0x00;
+    send[3] = 0x00;
+    send[4] = 0x00;
+    send[5] = 0x00;
+
+
+    if (!SENDFUNC(send, sizeof(send))) {
+        return (ErrorNumber)SendRegisterTimeout;
+    }
+    return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::recvSync()
+{
+    char recv[COMMAND_LENGTH];
+
+    if (!RECVFUNC(recv, sizeof(recv))) {
+        return (ErrorNumber)UnexpectedReply;
+    }
+    if ((0xAA != recv[0]) || (0x0D != recv[1])) {
+        return (ErrorNumber)UnexpectedReply;
+    }
+    return (ErrorNumber)NoError;
+}
+
+/**
+ * Send ACK.
+ *
+ * @param commandId The command with that ID is acknowledged by this command.
+ * @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.
+ */
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendAck(uint8_t commandId, uint16_t packageId)
+{
+    char send[COMMAND_LENGTH];
+
+    send[0] = 0xAA;
+    send[1] = 0x0E;
+    send[2] = commandId;
+    send[3] = 0x00;    // ACK counter is not used.
+    send[4] = (packageId >> 0) & 0xff;
+    send[5] = (packageId >> 8) & 0xff;
+    if (!SENDFUNC(send, sizeof(send))) {
+        return (ErrorNumber)SendRegisterTimeout;
+    }
+    return (ErrorNumber)NoError;
+}
+
+/**
+ * Receive ACK or NCK.
+ *
+ * @return Error number.
+ */
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::recvAckOrNck()
+{
+    char recv[COMMAND_LENGTH];
+
+    if (!RECVFUNC(recv, sizeof(recv))) {
+        return (ErrorNumber)UnexpectedReply;
+    }
+    if ((0xAA == recv[0]) && (0x0E == recv[1])) {
+        return (ErrorNumber)NoError;
+    }
+    if ((0xAA == recv[0]) && (0x0F == recv[1]) && (0x00 == recv[2])) {
+        return (ErrorNumber)NoError;
+    }
+    if ((0xAA == recv[0]) && (0x0F == recv[1])) {
+        return (ErrorNumber)recv[4];
+    }
+    return (ErrorNumber)UnexpectedReply;
+}
+
+/**
+ * Send bytes to camera module.
+ *
+ * @param buf Pointer to the data buffer.
+ * @param len Length of the data buffer.
+ *
+ * @return True if the data sended.
+ */
+bool HeptaCamera_GPS::sendBytes(char *buf, size_t len, int timeout_us)
+{
+    for (uint32_t i = 0; i < (uint32_t)len; i++) {
+        int cnt = 0;
+        while (!serial.writeable()) {
+            wait_us(1);
+            cnt++;
+            if (timeout_us < cnt) {
+                return false;
+            }
+        }
+        serial.putc(buf[i]);
+    }
+    return true;
+}
+
+
+bool HeptaCamera_GPS::recvBytes(char *buf, size_t len, int timeout_us)
+{
+    for (uint32_t i = 0; i < (uint32_t)len; i++) {
+        int cnt = 0;
+        while (!serial.readable()) {
+            wait_us(1);
+            cnt++;
+            if (timeout_us < cnt) {
+                return false;
+            }
+        }
+        buf[i] = serial.getc();
+    }
+    return true;
+}
+
+
+bool HeptaCamera_GPS::waitRecv()
+{
+    while (!serial.readable()) {
+    }
+    return true;
+}
+
+bool HeptaCamera_GPS::waitIdle()
+{
+    while (serial.readable()) {
+        serial.getc();
+    }
+    return true;
+}
+
+void HeptaCamera_GPS::camera_setting(void)
+{
+    GPS_SW = 0;
+    CAM_SW = 1;
+    serial.setTimeout(1);
+    flushSerialBuffer();
+}
+
+void HeptaCamera_GPS::Sync(void)
+{
+    camera_setting();
+    HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
+    printf("synchro setting now\r\n");
+    err = sync();
+    int count=0;
+    while(err) {
+        //printf("count = %d\r\n",count);
+        switch(count) {
+            case 0:
+                printf("Connection of camera and mbed at baudrate 14400\r\n");
+                setmbedBaud(HeptaCamera_GPS::Baud14400);
+
+                break;
+            case 1:
+                printf("Connection of camera and mbed at baudrate 115200\r\n");
+                setmbedBaud(HeptaCamera_GPS::Baud115200);
+                break;
+            default:
+                count=0;
+        }
+        count++;
+        err = sync();
+        printf("synchro setting now\r\n");
+        if(!err) {
+            printf("synchro setting finish\r\n");
+        }
+        /*  if(!err) {
+              printf("to 115200\r\n");
+              init(HeptaCamera_GPS::Baud115200, HeptaCamera_GPS::JpegResolution320x240);
+              printf("init err2=%d\r\n", err);
+              setmbedBaud(HeptaCamera_GPS::Baud115200);
+              printf("init err3=%d\r\n", err);
+          }*/
+    }
+}
+
+void HeptaCamera_GPS::test_jpeg_snapshot_picture(int CAPTURE_FRAMES)
+{
+    HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
+    for (int i = 0; i < CAPTURE_FRAMES; i++) {
+        char fname[64];
+        snprintf(fname, sizeof(fname), "/fs/test.jpg");
+        fp_jpeg = fopen(fname, "w");
+
+        err = getJpegSnapshotPicture();
+
+        if (HeptaCamera_GPS::NoError == err) {
+            printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
+        } else {
+            printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
+        }
+        fclose(fp_jpeg);
+    }
+}
+
+void HeptaCamera_GPS::test_jpeg_snapshot_data(int CAPTURE_FRAMES)
+{
+    HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
+    for (int i = 0; i < CAPTURE_FRAMES; i++) {
+        char fname[64];
+        snprintf(fname, sizeof(fname), "/fs/test.txt");
+        fp_jpeg = fopen(fname, "w");
+
+        err = getJpegSnapshotPicture_data();
+
+        if (HeptaCamera_GPS::NoError == err) {
+            printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
+        } else {
+            printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
+        }
+        fclose(fp_jpeg);
+    }
+}
+
+void HeptaCamera_GPS::initialize(Baud baud,JpegResolution jr)
+{
+    HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;//ErrorNumber define
+    err = init(baud, jr);//
+    if (HeptaCamera_GPS::NoError == err) {
+        printf("[ OK ] : Camera::init\r\n");
+        setmbedBaud(baud);
+    } else {
+        printf("[FAIL] : Camera::init (Error=%02X)\r\n", (int)err);
+    }
+
+}
+
+//*********************serial*********************//
+void HeptaCamera_GPS::gps_setting(void)
+{
+    CAM_SW = 0;
+    GPS_SW = 1;
+    serial._baud(9600);
+    serial.setTimeout(9999);
+}
+
+
+char HeptaCamera_GPS::getc()
+{
+    //gps_setting();
+    //flushSerialBuffer();
+    c = serial.getc();
+    return c;
+}
+int HeptaCamera_GPS::readable()
+{
+    //gps_setting();
+    i = serial.readable();
+    return i;
+}
+void HeptaCamera_GPS::flushSerialBuffer(void)
+{
+    //gps_setting();
+    ite = 0;
+    while (serial.readable()) {
+        serial.getc();
+        ite++;
+        if(ite==100) {
+            break;
+        };
+    }
+    return;
+}
+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)
+{
+    //flushSerialBuffer();
+    int ite = 0;
+    while(serial.getc()!='$') {
+        ite++;
+        if(ite==10000) break;
+    }
+    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++) {
+                    msg[i] = serial.getc();
+                    if(msg[i] == '\r') {
+                        msg[i] = 0;
+                        break;
+                    }
+                }
+            } 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')) {
+                    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(!(quality)) {
+                //latitude(unit transformation)
+                *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
+                //longitude(unit transformation)
+                *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
+                *serial_check = 0;
+            } else {
+                //latitude(unit transformation)
+                *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
+                //longitude(unit transformation)
+                *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
+                *serial_check = 1;
+            }
+        } else {
+            printf("No Data");
+            *serial_check = 2;
+        }
+    } else {
+        *serial_check = 3;
+    }
+}
+
+void HeptaCamera_GPS::lat_log_sensing_u16(char *lat, char *log, char *height, int *dsize1, int *dsize2)
+{
+    //flushSerialBuffer();
+    //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()) {
+        serial.getc();
+    }
+loop:
+    while(serial.getc()!='$') {}
+    for(j=0; j<5; j++) {
+        gps_data[1][j]=serial.getc();
+    }
+    if((gps_data[1][2]==0x47)&(gps_data[1][3]==0x47)&(gps_data[1][4]==0x41)) {
+        for(j=0; j<1; j++) {
+            if(j==0) {
+                i=0;
+                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()!='$') {}
+                i=0;
+                while((gps_data[j+1][i] = serial.getc()) != '\r') {
+                    //pc.putc(gps_data[j+1][i]);
+                    i++;
+                }
+                gps_data[j+1][i]='\0';
+                i=0;
+                //pc.printf("\n\r");
+            }
+        }
+    } else {
+        goto loop;
+    }
+    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) {
+        //hokui
+        d_hokui=int(hokui/100);
+        m_hokui=(hokui-d_hokui*100);
+        //m_hokui=(hokui-d_hokui*100)/60;
+        g_hokui=d_hokui+(hokui-d_hokui*100)/60;
+        //printf("%f\r\n",hokui);
+        sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
+        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);
+        //m_tokei=(tokei-d_tokei*100)/60;
+        g_tokei=d_tokei+(tokei-d_tokei*100)/60;
+        //printf("%f\r\n",tokei);
+        sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
+        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(_altitude);
+        cm_height = int((height_1-m_height)*100 );
+        // printf("%f\r\n",_altitude);
+        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];
+        lat[3] = gph2[1];
+        lat[4] = gph3[0];
+        lat[5] = gph3[1];
+        lat[6] = gph4[0];
+        lat[7] = gph4[1];
+        log[0] = gpt1[0];
+        log[1] = gpt1[1];
+        log[2] = gpt2[0];
+        log[3] = gpt2[1];
+        log[4] = gpt3[0];
+        log[5] = gpt3[1];
+        log[6] = gpt4[0];
+        log[7] = gpt4[1];
+        height[0] = hei1[0];
+        height[1] = hei1[1];
+        height[2] = hei2[0];
+        height[3] = hei2[1];
+    }
+    *dsize1 = 8;
+    *dsize2 = 4;
+}
+/*
+void HeptaCamera_GPS::lat_log_sensing_u16(char *lat, char *log, char *height, int *dsize1, int *dsize2)
+{
+    flushSerialBuffer();
+    //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()) {
+        serial.getc();
+    }
+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) {
+                i=0;
+                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()!='$') {}
+                i=0;
+                while((gps_data[j+1][i] = serial.getc()) != '\r') {
+                    //pc.putc(gps_data[j+1][i]);
+                    i++;
+                }
+                gps_data[j+1][i]='\0';
+                i=0;
+                //pc.printf("\n\r");
+            }
+        }
+    } else {
+        goto loop;
+    }
+    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);
+        //m_hokui=(hokui-d_hokui*100)/60;
+        g_hokui=d_hokui+(hokui-d_hokui*100)/60;
+        sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
+        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);
+        //m_tokei=(tokei-d_tokei*100)/60;
+        g_tokei=d_tokei+(tokei-d_tokei*100)/60;
+        sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
+        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];
+        lat[3] = gph2[1];
+        lat[4] = gph3[0];
+        lat[5] = gph3[1];
+        lat[6] = gph4[0];
+        lat[7] = gph4[1];
+        log[0] = gpt1[0];
+        log[1] = gpt1[1];
+        log[2] = gpt2[0];
+        log[3] = gpt2[1];
+        log[4] = gpt3[0];
+        log[5] = gpt3[1];
+        log[6] = gpt4[0];
+        log[7] = gpt4[1];
+        height[0] = hei1[0];
+        height[1] = hei1[1];
+        height[2] = hei2[0];
+        height[3] = hei2[1];
+    }
+    *dsize1 = 8;
+    *dsize2 = 4;
+}*/
+/*
+void HeptaCamera_GPS::lat_log_sensing_u16(char *lat, char *log, char *height, int *serial_check ,int *dsize1, int *dsize2)
+{
+    int quality=0,stnum=0;
+    char ns='A',ew='B',aunit='m';
+    float time=0.0,latitude=0.0,longitude=0.0,hacu=0.0,altitude=0.0;
+
+    flushSerialBuffer();
+    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 ite = 0;
+    while(serial.getc()!='$') {
+        ite++;
+        if(ite==10000) break;
+    }
+    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++) {
+                    msg[i] = serial.getc();
+                    if(msg[i] == '\r') {
+                        msg[i] = 0;
+                        break;
+                    }
+                }
+            } 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')) {
+                    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(!(quality)) {
+
+                *serial_check = 0;
+            } else {
+
+                *serial_check = 1;
+            }
+        } else {
+            printf("No Data");
+            *serial_check = 2;
+        }
+    } else {
+        *serial_check = 3;
+    }
+    //hokui
+    d_hokui=int(latitude/100);
+    m_hokui=(latitude-d_hokui*100);
+    //m_hokui=(hokui-d_hokui*100)/60;
+    g_hokui=d_hokui+(latitude-d_hokui*100)/60;
+    printf("%f\r\n",latitude);
+    sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
+    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(longitude/100);
+    m_tokei=(longitude-d_tokei*100);
+    //m_tokei=(tokei-d_tokei*100)/60;
+    g_tokei=d_tokei+(longitude-d_tokei*100)/60;
+    printf("%f\r\n",longitude);
+    sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
+    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(altitude);
+    cm_height = int((altitude-m_height)*100 );
+    printf("%f\r\n",altitude);
+    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];
+    lat[3] = gph2[1];
+    lat[4] = gph3[0];
+    lat[5] = gph3[1];
+    lat[6] = gph4[0];
+    lat[7] = gph4[1];
+    log[0] = gpt1[0];
+    log[1] = gpt1[1];
+    log[2] = gpt2[0];
+    log[3] = gpt2[1];
+    log[4] = gpt3[0];
+    log[5] = gpt3[1];
+    log[6] = gpt4[0];
+    log[7] = gpt4[1];
+    height[0] = hei1[0];
+    height[1] = hei1[1];
+    height[2] = hei2[0];
+    height[3] = hei2[1];
+    *dsize1 = 8;
+    *dsize2 = 4;
+}*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HeptaCamera_GPS.h	Wed Aug 23 06:18:42 2017 +0000
@@ -0,0 +1,107 @@
+#include "mbed.h"
+#include "SerialBuffered.h"
+
+#ifndef HEPTA_SERIAL_H
+#define HEPTA_SERIAL_H
+
+class HeptaCamera_GPS
+{
+public:
+
+    enum JpegResolution {
+        JpegResolution80x64 = 0x01,   // unofficial
+        JpegResolution160x128 = 0x03, // unofficial
+        JpegResolution320x240 = 0x05, // QVGA
+        JpegResolution640x480 = 0x07  // VGA
+    };
+
+    enum ErrorNumber {
+        NoError = 0x00,
+        UnexpectedReply = 0x04,
+        ParameterError = 0x0b,
+        SendRegisterTimeout = 0x0c,
+        CommandIdError = 0x0d,
+        CommandHeaderError = 0xf0,
+        SetTransferPackageSizeWrong = 0x11
+    };
+
+    enum Baud {
+        Baud460800 = 0x02,
+        Baud230400 = 0x03,
+        Baud115200 = 0x04,
+        Baud57600  = 0x05,
+        Baud28800  = 0x06,
+        Baud14400  = 0x07  // Default.
+    };
+
+    enum ResetType {
+        Nomal = 0x00,
+        High  = 0xff
+    };
+
+    HeptaCamera_GPS(PinName tx, PinName rx, PinName CAM, PinName GPS);
+
+    ~HeptaCamera_GPS();
+
+    ErrorNumber sync();
+    ErrorNumber init(Baud baud, JpegResolution jr);
+    ErrorNumber getJpegSnapshotPicture();
+    ErrorNumber getJpegSnapshotPicture_data();
+    void setmbedBaud(Baud baud);
+    void camera_setting(void);
+    void Sync(void);
+    void initialize(Baud baud,JpegResolution jr);
+    void test_jpeg_snapshot_picture(int CAPTURE_FRAMES);
+    void test_jpeg_snapshot_data(int CAPTURE_FRAMES);
+    void jpeg_callback(char *buf, size_t siz);
+    void gps_setting(void);
+    char getc();
+    int  readable();
+    void flushSerialBuffer(void);
+    void gga_sensing(float *time, float *latitude, char *ns, float *longitude, char *ew, int *quality, int *stnum, float *hacu, float *altitude, char *aunit, int *gps_check);
+    void lat_log_sensing_u16(char *lat, char *log, char * height, int *dsize1, int *dsize2);
+
+private:
+    SerialBuffered serial;
+    DigitalOut CAM_SW;
+    DigitalOut GPS_SW;
+    static const int COMMAND_LENGTH = 6;
+    static const int SYNCMAX = 60;
+    static const int packageSize = 256;
+    //static const int CAPTURE_FRAMES = 3;
+
+    char msg[256],msgd[256];
+    int i,ite,rlock,stn;
+    int _quality,_stnum;
+    float _hacu;
+    float _altitude; 
+    char _aunit; 
+    int _gps_check;
+    
+    char c;
+    char gps_data[7][1000];
+    char ns,ew,statas;
+    float time,hokui,tokei,vel;
+    float g_hokui,g_tokei;
+    float d_hokui,m_hokui,d_tokei,m_tokei;
+    int h_time,m_time,s_time;
+    float height_1;
+    int m_height,cm_height;
+
+    ErrorNumber sendInitial(Baud band, JpegResolution jr);
+    ErrorNumber sendGetPicture(void);
+    ErrorNumber sendSnapshot(void);
+    ErrorNumber sendSetPackageSize(uint16_t packageSize);
+    ErrorNumber sendReset(ResetType specialReset);
+    ErrorNumber recvData(uint32_t *length);
+    ErrorNumber sendSync();
+    ErrorNumber recvSync();
+    ErrorNumber sendAck(uint8_t commandId, uint16_t packageId);
+    ErrorNumber recvAckOrNck();
+
+    bool sendBytes(char *buf, size_t len, int timeout_us = 20000);
+    bool recvBytes(char *buf, size_t len, int timeout_us = 20000);
+    bool waitRecv();
+    bool waitIdle();
+};
+#endif

--- a/HeptaSerial.cpp	Sat Aug 05 14:20:51 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,883 +0,0 @@
-#include "HeptaSerial.h"
-static FILE *fp_jpeg;
-#define WAITIDLE    waitIdle
-#define SENDFUNC    sendBytes
-#define RECVFUNC    recvBytes
-#define WAITFUNC    waitRecv
-int num=0;
-/**
- * Constructor.
- *
- * @param tx A pin for transmit.
- * @param rx A pin for receive.
- * @param baud Baud rate. (Default is Baud14400.)
- */
-HeptaSerial::HeptaSerial(PinName tx, PinName rx, PinName CAM, PinName GPS) : serial(tx, rx), CAM_SW(CAM), GPS_SW(GPS)
-{
-    //serial.baud(baud);
-}
-
-/**
- * Destructor.
- */
-HeptaSerial::~HeptaSerial()
-{
-}
-
-/**
- * Make a sync. for baud rate.
- */
-HeptaSerial::ErrorNumber HeptaSerial::sync()
-{
-    for (int i = 0; i < SYNCMAX; i++) {
-        if (NoError == sendSync()) {
-            if (NoError == recvAckOrNck()) {
-                if (NoError == recvSync()) {
-                    if (NoError == sendAck(0x0D, 0x00)) {
-                        /*
-                         * After synchronization, the camera needs a little time for AEC and AGC to be stable.
-                         * Users should wait for 1-2 seconds before capturing the first picture.
-                         */
-                        wait(2.0);
-                        return NoError;
-                    }
-                }
-            }
-        }
-        wait_ms(50);
-    }
-    return UnexpectedReply;
-}
-
-/**
- * Initialize.
- *
- * @param baud Camera Interface Speed.
- * @param jr JPEG resolution.
- */
-HeptaSerial::ErrorNumber HeptaSerial::init(Baud baud,JpegResolution jr)
-{
-    int i ;
-    ErrorNumber en;
-    WAITIDLE();
-    setmbedBaud((Baud)(0x07)) ;
-
-    for ( i = 1 ; i < 7 ; i++ ) {
-        if ( NoError == sendSync() ) {
-            if ( NoError == recvAckOrNck() ) {
-                if ( NoError == recvSync() ) {
-                    if ( NoError == sendAck(0x0D, 0x00) ) {
-                        en = sendInitial(baud,jr);
-                        if (NoError != en) {
-                            return en;
-                        }
-                        en = recvAckOrNck();
-                        if (NoError != en) {
-                            return en;
-                        }
-                        wait_ms(50) ;
-                        setmbedBaud(baud);
-                        //wait_ms(50) ;
-                        static bool alreadySetupPackageSize = false;
-                        if (!alreadySetupPackageSize) {
-                            en = sendSetPackageSize(packageSize);
-                            if (NoError != en) {
-                                return en;
-                            }
-                            WAITFUNC();
-                            en = recvAckOrNck();
-                            if (NoError != en) {
-                                return en;
-                            }
-                            alreadySetupPackageSize = true;
-                        }
-
-                        wait(2.0);
-                        return (ErrorNumber)NoError;
-                        /*
-                         * After synchronization, the camera needs a little time for AEC and AGC to be stable.
-                         * Users should wait for 1-2 seconds before capturing the first picture.
-                         */
-                    }
-                }
-            } else {
-                setmbedBaud((Baud)(i+1)) ;
-            }
-        }
-        wait_ms(50);
-    }
-    return UnexpectedReply;
-}
-
-
-/**
- * Get JPEG snapshot picture.
- *
- * @param func A pointer to a callback function.
- *             You can block this function until saving the image datas.
- * @return Status of the error.
- */
-HeptaSerial::ErrorNumber HeptaSerial::getJpegSnapshotPicture()
-{
-    WAITIDLE();
-    ErrorNumber en;
-
-
-    en = sendSnapshot();
-    if (NoError != en) {
-        return en;
-    }
-    WAITFUNC();
-    en = recvAckOrNck();
-    if (NoError != en) {
-        return en;
-    }
-
-    en = sendGetPicture();
-    if (NoError != en) {
-        return en;
-    }
-    WAITFUNC();
-    en = recvAckOrNck();
-    if (NoError != en) {
-        return en;
-    }
-
-    /*
-     * Data : snapshot picture
-     */
-    uint32_t length = 0;
-    WAITFUNC();
-    en = recvData(&length);
-    if (NoError != en) {
-        return en;
-    }
-    en = sendAck(0x00, 0);
-    if (NoError != en) {
-        return en;
-    }
-
-    char databuf[packageSize - 6];
-    uint16_t pkg_total = length / (packageSize - 6);
-    for (int i = 0; i <= (int)pkg_total; i++) {
-        uint16_t checksum = 0;
-        // ID.
-        char idbuf[2];
-        WAITFUNC();
-        if (!RECVFUNC(idbuf, sizeof(idbuf))) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-        checksum += idbuf[0];
-        checksum += idbuf[1];
-        uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0);
-        if (id != i) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-
-        // Size of the data.
-        char dsbuf[2];
-        WAITFUNC();
-        if (!RECVFUNC(dsbuf, sizeof(dsbuf))) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-
-        // Received the data.
-        checksum += dsbuf[0];
-        checksum += dsbuf[1];
-        uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0);
-        WAITFUNC();
-        if (!RECVFUNC(&databuf[0], ds)) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-        for (int j = 0; j < ds; j++) {
-            checksum += databuf[j];
-        }
-
-        // Verify code.
-        char vcbuf[2];
-        WAITFUNC();
-        if (!RECVFUNC(vcbuf, sizeof(vcbuf))) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-        uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0);
-        if (vc != (checksum & 0xff)) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-
-        /*
-         * Call a call back function.
-         * You can block this function while working.
-         */
-        size_t siz = ds;
-        for (int ii = 0; ii < (int)siz; ii++) {
-            fprintf(fp_jpeg, "%c", databuf[ii]);
-        }
-        /*
-         * We should wait for camera working before reply a ACK.
-         */
-        wait_ms(100);
-        en = sendAck(0x00, 1 + i);
-        if (NoError != en) {
-            return en;
-        }
-    }
-
-    return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::getJpegSnapshotPicture_data()
-{
-    WAITIDLE();
-    ErrorNumber en;
-
-
-    en = sendSnapshot();
-    if (NoError != en) {
-        return en;
-    }
-    WAITFUNC();
-    en = recvAckOrNck();
-    if (NoError != en) {
-        return en;
-    }
-
-    en = sendGetPicture();
-    if (NoError != en) {
-        return en;
-    }
-    WAITFUNC();
-    en = recvAckOrNck();
-    if (NoError != en) {
-        return en;
-    }
-
-    /*
-     * Data : snapshot picture
-     */
-    uint32_t length = 0;
-    WAITFUNC();
-    en = recvData(&length);
-    if (NoError != en) {
-        return en;
-    }
-    en = sendAck(0x00, 0);
-    if (NoError != en) {
-        return en;
-    }
-
-    char databuf[packageSize - 6];
-    uint16_t pkg_total = length / (packageSize - 6);
-    for (int i = 0; i <= (int)pkg_total; i++) {
-        uint16_t checksum = 0;
-        // ID.
-        char idbuf[2];
-        WAITFUNC();
-        if (!RECVFUNC(idbuf, sizeof(idbuf))) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-        checksum += idbuf[0];
-        checksum += idbuf[1];
-        uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0);
-        if (id != i) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-
-        // Size of the data.
-        char dsbuf[2];
-        WAITFUNC();
-        if (!RECVFUNC(dsbuf, sizeof(dsbuf))) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-
-        // Received the data.
-        checksum += dsbuf[0];
-        checksum += dsbuf[1];
-        uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0);
-        WAITFUNC();
-        if (!RECVFUNC(&databuf[0], ds)) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-        for (int j = 0; j < ds; j++) {
-            checksum += databuf[j];
-        }
-
-        // Verify code.
-        char vcbuf[2];
-        WAITFUNC();
-        if (!RECVFUNC(vcbuf, sizeof(vcbuf))) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-        uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0);
-        if (vc != (checksum & 0xff)) {
-            return (ErrorNumber)UnexpectedReply;
-        }
-
-        /*
-         * Call a call back function.
-         * You can block this function while working.
-         */
-        size_t siz = ds;
-        for (int ii = 0; ii < (int)siz; ii++) {
-            fprintf(fp_jpeg, "%02X ", databuf[ii]);
-            if(++num%16==0) fprintf(fp_jpeg,"\r\n");
-        }
-        /*
-         * We should wait for camera working before reply a ACK.
-         */
-        wait_ms(100);
-        en = sendAck(0x00, 1 + i);
-        if (NoError != en) {
-            return en;
-        }
-    }
-
-    return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::sendInitial(Baud baud, JpegResolution jr)
-{
-    char send[COMMAND_LENGTH];
-
-    send[0] = 0xAA;
-    send[1] = 0x01;
-    send[2] = (char)baud;
-    send[3] = 0x07;
-    send[4] = 0x00;
-    send[5] = (char)jr;
-
-    if (!SENDFUNC(send, sizeof(send))) {
-        return (ErrorNumber)SendRegisterTimeout;
-    }
-
-    return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::sendGetPicture(void)
-{
-    char send[COMMAND_LENGTH];
-
-    send[0] = 0xAA;
-    send[1] = 0x04;
-    send[2] = 0x01;
-    send[3] = 0x00;
-    send[4] = 0x00;
-    send[5] = 0x00;
-
-    if (!SENDFUNC(send, sizeof(send))) {
-        return (ErrorNumber)SendRegisterTimeout;
-    }
-    return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::sendSnapshot(void)
-{
-    char send[COMMAND_LENGTH];
-    send[0] = 0xAA;
-    send[1] = 0x05;
-    send[2] = 0x00;
-    send[3] = 0x00;
-    send[4] = 0x00;
-    send[5] = 0x00;
-
-    if (!SENDFUNC(send, sizeof(send))) {
-        return (ErrorNumber)SendRegisterTimeout;
-    }
-    return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::sendSetPackageSize(uint16_t packageSize)
-{
-    char send[COMMAND_LENGTH];
-    send[0] = 0xAA;
-    send[1] = 0x06;
-    send[2] = 0x08;
-    send[3] = (packageSize >> 0) & 0xff;
-    send[4] = (packageSize >> 8) & 0xff;
-    send[5] = 0x00;
-
-    if (!SENDFUNC(send, sizeof(send))) {
-        return (ErrorNumber)SendRegisterTimeout;
-    }
-    return (ErrorNumber)NoError;
-}
-
-void HeptaSerial::setmbedBaud(Baud baud)
-{
-    switch((int)baud) {
-        case 0x02:
-            serial._baud(460800);
-            break;
-        case 0x03:
-            serial._baud(230400);
-            break;
-        case 0x04:
-            serial._baud(115200);
-            break;
-        case 0x05:
-            serial._baud(57600);
-            break;
-        case 0x06:
-            serial._baud((int)28800);
-            break;
-        case 0x07:
-            serial._baud(14400);
-            break;
-        default:
-            serial._baud(14400);
-    }
-}
-
-
-HeptaSerial::ErrorNumber HeptaSerial::sendReset(ResetType specialReset)
-{
-    char send[COMMAND_LENGTH];
-    send[0] = 0xAA;
-    send[1] = 0x08;
-    send[2] = 0x00;
-    send[3] = 0x00;
-    send[4] = 0x00;
-    send[5] = specialReset;
-    /*
-     * Special reset : If the parameter is 0xFF, the command is a special Reset command and the firmware responds to it immediately.
-     */
-
-    if (!SENDFUNC(send, sizeof(send))) {
-        return (ErrorNumber)SendRegisterTimeout;
-    }
-
-    return (ErrorNumber)NoError;
-}
-
-
-HeptaSerial::ErrorNumber HeptaSerial::recvData(uint32_t *length)
-{
-    char recv[COMMAND_LENGTH];
-    if (!RECVFUNC(recv, sizeof(recv))) {
-        return (ErrorNumber)UnexpectedReply;
-    }
-    if ((0xAA != recv[0]) || (0x0A != recv[1])) {
-        return (ErrorNumber)UnexpectedReply;
-    }
-    recv[2] = (char)0x01;
-    *length = (recv[5] << 16) | (recv[4] << 8) | (recv[3] << 0);
-    return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::sendSync()
-{
-    char send[COMMAND_LENGTH];
-    send[0] = 0xAA;
-    send[1] = 0x0D;
-    send[2] = 0x00;
-    send[3] = 0x00;
-    send[4] = 0x00;
-    send[5] = 0x00;
-
-
-    if (!SENDFUNC(send, sizeof(send))) {
-        return (ErrorNumber)SendRegisterTimeout;
-    }
-    return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::recvSync()
-{
-    char recv[COMMAND_LENGTH];
-
-    if (!RECVFUNC(recv, sizeof(recv))) {
-        return (ErrorNumber)UnexpectedReply;
-    }
-    if ((0xAA != recv[0]) || (0x0D != recv[1])) {
-        return (ErrorNumber)UnexpectedReply;
-    }
-    return (ErrorNumber)NoError;
-}
-
-/**
- * Send ACK.
- *
- * @param commandId The command with that ID is acknowledged by this command.
- * @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.
- */
-HeptaSerial::ErrorNumber HeptaSerial::sendAck(uint8_t commandId, uint16_t packageId)
-{
-    char send[COMMAND_LENGTH];
-
-    send[0] = 0xAA;
-    send[1] = 0x0E;
-    send[2] = commandId;
-    send[3] = 0x00;    // ACK counter is not used.
-    send[4] = (packageId >> 0) & 0xff;
-    send[5] = (packageId >> 8) & 0xff;
-    if (!SENDFUNC(send, sizeof(send))) {
-        return (ErrorNumber)SendRegisterTimeout;
-    }
-    return (ErrorNumber)NoError;
-}
-
-/**
- * Receive ACK or NCK.
- *
- * @return Error number.
- */
-HeptaSerial::ErrorNumber HeptaSerial::recvAckOrNck()
-{
-    char recv[COMMAND_LENGTH];
-
-    if (!RECVFUNC(recv, sizeof(recv))) {
-        return (ErrorNumber)UnexpectedReply;
-    }
-    if ((0xAA == recv[0]) && (0x0E == recv[1])) {
-        return (ErrorNumber)NoError;
-    }
-    if ((0xAA == recv[0]) && (0x0F == recv[1]) && (0x00 == recv[2])) {
-        return (ErrorNumber)NoError;
-    }
-    if ((0xAA == recv[0]) && (0x0F == recv[1])) {
-        return (ErrorNumber)recv[4];
-    }
-    return (ErrorNumber)UnexpectedReply;
-}
-
-/**
- * Send bytes to camera module.
- *
- * @param buf Pointer to the data buffer.
- * @param len Length of the data buffer.
- *
- * @return True if the data sended.
- */
-bool HeptaSerial::sendBytes(char *buf, size_t len, int timeout_us)
-{
-    for (uint32_t i = 0; i < (uint32_t)len; i++) {
-        int cnt = 0;
-        while (!serial.writeable()) {
-            wait_us(1);
-            cnt++;
-            if (timeout_us < cnt) {
-                return false;
-            }
-        }
-        serial.putc(buf[i]);
-    }
-    return true;
-}
-
-/**
- * Receive bytes from camera module.
- *
- * @param buf Pointer to the data buffer.
- * @param len Length of the data buffer.
- *
- * @return True if the data received.
- */
-bool HeptaSerial::recvBytes(char *buf, size_t len, int timeout_us)
-{
-    for (uint32_t i = 0; i < (uint32_t)len; i++) {
-        int cnt = 0;
-        while (!serial.readable()) {
-            wait_us(1);
-            cnt++;
-            if (timeout_us < cnt) {
-                return false;
-            }
-        }
-        buf[i] = serial.getc();
-    }
-    return true;
-}
-
-/**
- * Wait received.
- *
- * @return True if the data received.
- */
-bool HeptaSerial::waitRecv()
-{
-    while (!serial.readable()) {
-    }
-    return true;
-}
-
-/**
- * Wait idle state.
- */
-bool HeptaSerial::waitIdle()
-{
-    while (serial.readable()) {
-        serial.getc();
-    }
-    return true;
-}
-
-void HeptaSerial::camera_setting(void)
-{
-    CAM_SW = 1;
-    GPS_SW = 0;
-    serial.setTimeout(1);
-}
-
-void HeptaSerial::Sync(void)
-{
-    camera_setting();
-    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);
-                break;
-            case 1:
-                setmbedBaud(HeptaSerial::Baud115200);
-                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)
-{
-    HeptaSerial::ErrorNumber err = HeptaSerial::NoError;
-    for (int i = 0; i < CAPTURE_FRAMES; i++) {
-        char fname[64];
-        snprintf(fname, sizeof(fname), "/fs/test.jpg");
-        fp_jpeg = fopen(fname, "w");
-
-        err = getJpegSnapshotPicture();
-
-        if (HeptaSerial::NoError == err) {
-            printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
-        } else {
-            printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
-        }
-        fclose(fp_jpeg);
-    }
-}
-
-void HeptaSerial::test_jpeg_snapshot_data(int CAPTURE_FRAMES)
-{
-    HeptaSerial::ErrorNumber err = HeptaSerial::NoError;
-    for (int i = 0; i < CAPTURE_FRAMES; i++) {
-        char fname[64];
-        snprintf(fname, sizeof(fname), "/fs/test.txt");
-        fp_jpeg = fopen(fname, "w");
-
-        err = getJpegSnapshotPicture_data();
-
-        if (HeptaSerial::NoError == err) {
-            printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
-        } else {
-            printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
-        }
-        fclose(fp_jpeg);
-    }
-}
-
-void HeptaSerial::initialize(Baud baud,JpegResolution jr)
-{
-    HeptaSerial::ErrorNumber err = HeptaSerial::NoError;//ErrorNumber define
-    err = init(baud, jr);//
-    if (HeptaSerial::NoError == err) {
-        printf("[ OK ] : Camera::init\r\n");
-        setmbedBaud(baud);
-    } else {
-        printf("[FAIL] : Camera::init (Error=%02X)\r\n", (int)err);
-    }
-
-}
-
-//*********************serial*********************//
-void HeptaSerial::gps_setting(void)
-{
-    CAM_SW = 0;
-    GPS_SW = 1;
-    serial._baud(9600);
-    serial.setTimeout(9999);
-}
-
-
-char HeptaSerial::getc()
-{
-    gps_setting();
-    c = serial.getc();
-    return c;
-}
-int HeptaSerial::readable()
-{
-    gps_setting();
-    i = serial.readable();
-    return i;
-}
-void HeptaSerial::flushSerialBuffer(void)
-{
-    gps_setting();
-    ite = 0;
-    while (serial.readable()) {
-        serial.getc();
-        ite++;
-        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)
-{
-    gps_setting();
-    int ite = 0;
-    while(serial.getc()!='$') {
-        ite++;
-        if(ite==10000) break;
-    }
-    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++) {
-                    msg[i] = serial.getc();
-                    if(msg[i] == '\r') {
-                        msg[i] = 0;
-                        break;
-                    }
-                }
-            } 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')) {
-                    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(!(quality)) {
-                //latitude(unit transformation)
-                *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
-                //longitude(unit transformation)
-                *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
-                *serial_check = 0;
-            } else {
-                //latitude(unit transformation)
-                *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
-                //longitude(unit transformation)
-                *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
-                *serial_check = 1;
-            }
-        } else {
-            printf("No Data");
-            *serial_check = 2;
-        }
-    } else {
-        *serial_check = 3;
-    }
-}
-void HeptaSerial::lat_log_sensing_u16(char *lat, char *log, char *height, int *dsize1, int *dsize2)
-{
-    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()) {
-        serial.getc();
-    }
-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) {
-                i=0;
-                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()!='$') {}
-                i=0;
-                while((gps_data[j+1][i] = serial.getc()) != '\r') {
-                    //pc.putc(gps_data[j+1][i]);
-                    i++;
-                }
-                gps_data[j+1][i]='\0';
-                i=0;
-                //pc.printf("\n\r");
-            }
-        }
-    } else {
-        goto loop;
-    }
-    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);
-        //m_hokui=(hokui-d_hokui*100)/60;
-        g_hokui=d_hokui+(hokui-d_hokui*100)/60;
-        sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
-        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);
-        //m_tokei=(tokei-d_tokei*100)/60;
-        g_tokei=d_tokei+(tokei-d_tokei*100)/60;
-        sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
-        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];
-        lat[3] = gph2[1];
-        lat[4] = gph3[0];
-        lat[5] = gph3[1];
-        lat[6] = gph4[0];
-        lat[7] = gph4[1];
-        log[0] = gpt1[0];
-        log[1] = gpt1[1];
-        log[2] = gpt2[0];
-        log[3] = gpt2[1];
-        log[4] = gpt3[0];
-        log[5] = gpt3[1];
-        log[6] = gpt4[0];
-        log[7] = gpt4[1];
-        height[0] = hei1[0];
-        height[1] = hei1[1];
-        height[2] = hei2[0];
-        height[3] = hei2[1];
-    }
-    *dsize1 = 8;
-    *dsize2 = 4;
-}
\ No newline at end of file

--- a/HeptaSerial.h	Sat Aug 05 14:20:51 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,101 +0,0 @@
-#include "mbed.h"
-#include "SerialBuffered.h"
-
-#ifndef HEPTA_SERIAL_H
-#define HEPTA_SERIAL_H
-
-class HeptaSerial
-{
-public:
-
-    enum JpegResolution {
-        JpegResolution80x64 = 0x01,   // unofficial
-        JpegResolution160x128 = 0x03, // unofficial
-        JpegResolution320x240 = 0x05, // QVGA
-        JpegResolution640x480 = 0x07  // VGA
-    };
-
-    enum ErrorNumber {
-        NoError = 0x00,
-        UnexpectedReply = 0x04,
-        ParameterError = 0x0b,
-        SendRegisterTimeout = 0x0c,
-        CommandIdError = 0x0d,
-        CommandHeaderError = 0xf0,
-        SetTransferPackageSizeWrong = 0x11
-    };
-
-    enum Baud {
-        Baud460800 = 0x02,
-        Baud230400 = 0x03,
-        Baud115200 = 0x04,
-        Baud57600  = 0x05,
-        Baud28800  = 0x06,
-        Baud14400  = 0x07  // Default.
-    };
-
-    enum ResetType {
-        Nomal = 0x00,
-        High  = 0xff
-    };
-
-    HeptaSerial(PinName tx, PinName rx, PinName CAM, PinName GPS);
-
-    ~HeptaSerial();
-
-    ErrorNumber sync();
-    ErrorNumber init(Baud baud, JpegResolution jr);
-    ErrorNumber getJpegSnapshotPicture();
-    ErrorNumber getJpegSnapshotPicture_data();
-    void setmbedBaud(Baud baud);
-    void camera_setting(void);
-    void Sync(void);
-    void initialize(Baud baud,JpegResolution jr);
-    void test_jpeg_snapshot_picture(int CAPTURE_FRAMES);
-    void test_jpeg_snapshot_data(int CAPTURE_FRAMES);
-    void jpeg_callback(char *buf, size_t siz);
-    void gps_setting(void);
-    char getc();
-    int  readable();
-    void flushSerialBuffer(void);
-    void gga_sensing(float *time, float *latitude, char *ns, float *longitude, char *ew, int *quality, int *stnum, float *hacu, float *altitude, char *aunit, int *gps_check);
-    void lat_log_sensing_u16(char *lat, char *log, char * height, int *dsize1, int *dsize2);
-
-private:
-    SerialBuffered serial;
-    DigitalOut CAM_SW;
-    DigitalOut GPS_SW;
-    static const int COMMAND_LENGTH = 6;
-    static const int SYNCMAX = 60;
-    static const int packageSize = 256;
-    //static const int CAPTURE_FRAMES = 3;
-
-    char msg[256],msgd[256];
-    int i,ite,rlock,stn;
-    char c;
-    char gps_data[7][1000];
-    char ns,ew,statas;
-    float time,hokui,tokei,vel;
-    float g_hokui,g_tokei;
-    float d_hokui,m_hokui,d_tokei,m_tokei;
-    int h_time,m_time,s_time;
-    float height_1;
-    int m_height,cm_height;
-
-    ErrorNumber sendInitial(Baud band, JpegResolution jr);
-    ErrorNumber sendGetPicture(void);
-    ErrorNumber sendSnapshot(void);
-    ErrorNumber sendSetPackageSize(uint16_t packageSize);
-    ErrorNumber sendReset(ResetType specialReset);
-    ErrorNumber recvData(uint32_t *length);
-    ErrorNumber sendSync();
-    ErrorNumber recvSync();
-    ErrorNumber sendAck(uint8_t commandId, uint16_t packageId);
-    ErrorNumber recvAckOrNck();
-
-    bool sendBytes(char *buf, size_t len, int timeout_us = 20000);
-    bool recvBytes(char *buf, size_t len, int timeout_us = 20000);
-    bool waitRecv();
-    bool waitIdle();
-};
-#endif